Orientation and angle displacement in 3D (8)

Convert from orah to Quaternary

To convert the angle displacement from the Euclidean angle to the Quaternary angle, you can use a method similar to the Euclidean angle matrix. First, convert the three rotations to the four elements. This is a simple operation. Then, we concatenate these three Enis into one. Like a matrix, there are two situations to consider. The first is inertia-Object four-element, and the second is the object-inertial four-element. Because they are mutually bounded, we only derive the inertia-object Quaternary.

Set ouarla to variables H, P, and B.H,P,BRound-axis Y, X, and Z. Remember, use negative rotation because they specify the rotation angle in the coordinate system.

Connect them in the correct order to get the formula 10.24:

(Remember, the four-element multiplication is defined as multiplication from left to right in the order of rotation .)

The object-inertial four-element is the inertia-the object's four-element concatenation. For details, see formula 10.25:

　

Convert from the Quaternary element to the ouarla corner

Based on the previous formula, we found that:

Now you can directly convert it to the code, as shown in program list 10.5, which converts inertia-object Quaternary element into a euclidean angle.

Listing 10.5: converting an inertial-to-object Quaternion to Euler Angles

// Use global variables for Input and Output
Float w, x, y, z;
Float H, P, B;

// Extract sin (pitch)
Float sp =-2.0f * (y * z + W * X );

// Check for gimbal lock, giving slight tolerance for numerical impretries
If (FABS (SP)> 0.9999f ){
// Looking straight up or down
P = 1.570796f * sp; // PI/2

// Compute heading, slam bank to zero
H = atan2 (-x * z-W * y, 0.5f-y * Y-z * z );
B = 0.0f;
} Else {
// Compute angles
P = Asin (SP );
H = atan2 (x * z-W * y, 0.5f-x * x-y * y );
B = atan2 (x * Y-W * z, 0.5f-x * X-z * z );
}

The code used to convert an object, the inertial four-element, to the Euclidean angle is very similar to that used above. Only the values of X, Y, and Z are changed to negative values, because the object-inertial four-element is inertial-the object's four-element concatenation.

Listing 10.6: converting an object-to-inertial Quaternion to Euler Angles

// Extract sin (pitch)
Float sp =-2.0f * (y * z-W * X );

// Check for gimbal lock, giving slight tolerance for numerical impretries
If (FABS (SP)> 0.9999f ){
// Looking straight up or down
P = 1.570796f * sp; // PI/2

// Compute heading, slam bank to zero
H = atan2 (-x * z + W * y, 0.5f-y * Y-z * z );
B = 0.0f;
} Else {
// Compute angles
P = Asin (SP );
H = atan2 (x * z + W * y, 0.5f-x * x-y * y );
B = atan2 (x * Y + W * z, 0.5f-x * X-z * z );
}