web-dev-qa-db-fra.com

Comment convertir un vecteur de direction en angles d'Euler?

Je cherche un moyen de convertir le vecteur de direction (X, Y, Z) en angles d'Euler (cap, tangage, banque). Je sais que le vecteur direction en lui-même ne suffit pas pour obtenir l'angle d'inclinaison, il y a donc aussi un autre soi-disant vecteur Up.

Ayant un vecteur direction (X, Y, Z) et un vecteur haut (X, Y, Z) comment puis-je convertir cela en angles d'Euler?

mathgeometryeuler-angles
17
Kromster

Voyons si je comprends bien. Il s'agit de l'orientation d'un corps rigide dans un espace tridimensionnel, comme un avion en vol. Le nez de cet avion pointe vers le vecteur de direction

D=(XD,YD,ZD) .

Vers le toit se trouve le vecteur haut

U=(XU,YU,ZU) .

Alors en-têteH serait le vecteur de direction D projeté sur la surface terrestre:

H=(XD,YD,0) ,

avec un angle associé

angle_H=atan2(YD,XD) .

Pitch P serait l'angle haut/bas du nez par rapport à l'horizon, si le vecteur de direction D est normalisé, vous l'obtiendrez

ZD=sin(angle_P)

résultant en

angle_P=asin(ZD) .

Enfin, pour l'angle d'inclinaison, nous considérons la direction des ailes, en supposant que les ailes sont perpendiculaires au corps. Si l'avion vole droit vers D, les ailes pointent perpendiculairement à D et parallèlement à la surface de la terre:

W0 = ( -YD, XD, 0 )

Ce serait un angle d'inclinaison de 0. Le vecteur haut attendu serait perpendiculaire à W0 et perpendiculaire à D

U0 = W0 × D

avec × désignant le produit croisé. U est égal à U0 si l'angle d'inclinaison est nul, sinon l'angle entre U et U0 est l'angle d'inclinaison angle_B, qui peut être calculé à partir de

cos(angle_B) = Dot(U0,U) / abs(U0) / abs(U)
sin(angle_B) = Dot(W0,U) / abs(W0) / abs(U) .

Ici, "abs" calcule la longueur du vecteur. De là, vous obtenez l'angle d'inclinaison comme

angle_B = atan2( Dot(W0,U) / abs(W0), Dot(U0,U) / abs(U0) ) .

Les facteurs de normalisation s'annulent si U et D sont normalisés.

24
pentadecagon

nous avons besoin de trois vecteurs: X1, Y1, Z1 du système de coordonnées local (LCS) exprimé en termes de système de coordonnées mondial (WCS). Le code ci-dessous présente comment calculer trois angles d'Euler en fonction de ces 3 vecteurs.

#include <math.h> 
#include <float.h> 

#define PI 3.141592653589793 
/**
 * @param X1x
 * @param X1y
 * @param X1z X1 vector coordinates
 * @param Y1x
 * @param Y1y
 * @param Y1z Y1 vector coordinates
 * @param Z1x
 * @param Z1y
 * @param Z1z Z1 vector coordinates
 * @param pre precession rotation
 * @param nut nutation rotation
 * @param rot intrinsic rotation
 */
void lcs2Euler(
        double X1x, double X1y, double X1z,
        double Y1x, double Y1y, double Y1z,
        double Z1x, double Z1y, double Z1z,
        double *pre, double *nut, double *rot) {
    double Z1xy = sqrt(Z1x * Z1x + Z1y * Z1y);
    if (Z1xy > DBL_EPSILON) {
        *pre = atan2(Y1x * Z1y - Y1y*Z1x, X1x * Z1y - X1y * Z1x);
        *nut = atan2(Z1xy, Z1z);
        *rot = -atan2(-Z1x, Z1y);
    }
    else {
        *pre = 0.;
        *nut = (Z1z > 0.) ? 0. : PI;
        *rot = -atan2(X1y, X1x);
    }
}
5
4pie0