How to convert direction vector to euler angles?
Let's see if I understand correctly. This is about the orientation of a rigid body in three dimensional space, like an air plane during flight. The nose of that airplane points towards the direction vector
D=(XD,YD,ZD) .
Towards the roof is the up vector
U=(XU,YU,ZU) .
Then heading H
would be the direction vector D
projected onto the earth surface:
H=(XD,YD,0) ,
with an associated angle
angle_H=atan2(YD,XD) .
Pitch P would be the up/down angle of the nose with respect to the horizon, if the direction vector D
is normalized you get it from
ZD=sin(angle_P)
resulting in
angle_P=asin(ZD) .
Finally, for the bank angle we consider the direction of the wings, assuming the wings are perpendicular to the body. If the plane flies straight towards D
, the wings point perpendicular to D
and parallel to the earth surface:
W0 = ( -YD, XD, 0 )
This would be a bank angle of 0. The expected Up Vector would be perpendicular to W0
and perpendicular to D
U0 = W0 × D
with ×
denoting the cross product. U
equals U0
if the bank angle is zero, otherwise the angle between U
and U0
is the bank angle angle_B
, which can be calculated from
cos(angle_B) = Dot(U0,U) / abs(U0) / abs(U)
sin(angle_B) = Dot(W0,U) / abs(W0) / abs(U) .
Here 'abs' calculates the length of the vector. From that you get the bank angle as
angle_B = atan2( Dot(W0,U) / abs(W0), Dot(U0,U) / abs(U0) ) .
The normalization factors cancel each other if U
and D
are normalized.
we need three vectors: X1, Y1, Z1 of local coordinate system (LCS) expressed in terms of world coordinate system (WCS). The code below presents how to calculate three Euler angles based on these 3 vectors.
#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);
}
}