I was tasked on creating a C Program that needs to process this case:
- Convert the Initial Rotation Matrix(InitialRotationMatrix) into 3D Rotation(Rotation3DResult1)
- Convert the 3D Rotation(Rotation3DResult1) back to its Rotation Matrix(ConvertedRotationMatrix)
- Convert the Converted Rotation Matrix(ConvertedRotationMatrix) back to its 3D Rotation(Rotation3DResult2)
- Rotation3DResult2's components must be equal to Rotation3DResult1's components
- ConvertedRotationMatrix's components must be equal to InitialRotationMatrix's components
Note that: Rotation Matrix Format:
[a1 a2 a3] <----- Rightward Direction(X-Axis) Vector [b1 b2 b3] <------- Forward Direction(Y-Axis) Vector [c1 c2 c3] <-------- Upward Direction(Z-Axis) Vector3D Rotation Format:
-Pitch <- Tilt Downwards , +Pitch <- Tilt Upwards -Roll <- Tilt Leftwards , +Roll <- Tilt Rightwards -Yaw <- Point Rightwards , +Yaw <- Point Leftwards
But Im Struggling to figure out how to compute the Upward Direction(Z-Axis) Vector in my C Program:
#include <stdio.h>
#include <stdint.h>
#include <math.h>
typedef struct {
float Pitch;
float Roll;
float Yaw;
} Rotation3D;
typedef struct {
float X;
float Y;
float Z;
} Vector3D;
typedef struct {
Vector3D RightwardDirection;
Vector3D ForwardDirection;
Vector3D UpwardDirection;
} RotationMatrix;
void Get3DRotation(RotationMatrix *inputRotationMatrix, Rotation3D *outputRotation) {
outputRotation->Pitch = atan2(sqrt((inputRotationMatrix->ForwardDirection.X*inputRotationMatrix->ForwardDirection.X)
+(inputRotationMatrix->ForwardDirection.Y*inputRotationMatrix->ForwardDirection.Y))
, inputRotationMatrix->ForwardDirection.Y);
outputRotation->Yaw = atan2(inputRotationMatrix->ForwardDirection.Y, inputRotationMatrix->ForwardDirection.X);
outputRotation->Roll = atan2(inputRotationMatrix->UpwardDirection.Z, inputRotationMatrix->RightwardDirection.Z);
}
void Set3DRotation(Rotation3D *inputRotation, RotationMatrix *outputRotationMatrix) {
outputRotationMatrix->RightwardDirection.X = cos(-inputRotation->Roll) * cos(inputRotation->Yaw - M_PI_2);
outputRotationMatrix->RightwardDirection.Y = cos(-inputRotation->Roll) * sin(inputRotation->Yaw - M_PI_2);
outputRotationMatrix->RightwardDirection.Z = -sin(-inputRotation->Roll);
outputRotationMatrix->ForwardDirection.X = cos(inputRotation->Pitch) * cos(inputRotation->Yaw);
outputRotationMatrix->ForwardDirection.Y = cos(inputRotation->Pitch) * sin(inputRotation->Yaw);
outputRotationMatrix->ForwardDirection.Z = -sin(inputRotation->Pitch);
// outputRotationMatrix->UpwardDirection.X = ? how to compute Upward Direction X?
// outputRotationMatrix->UpwardDirection.Y = ? how to compute Upward Direction Y?
// outputRotationMatrix->UpwardDirection.Z = ? how to compute Upward Direction Z?
}
int main() {
RotationMatrix initialRotationMatrix = {
{ 0.0769, -0.5747, 0.8147},
{ 0.6699, 0.6350, 0.3847},
{-0.7385, 0.5162, 0.4338}
};
printf("Initial RightwardDirection: %f %f %f\n", initialRotationMatrix.RightwardDirection.X, initialRotationMatrix.RightwardDirection.Y, initialRotationMatrix.RightwardDirection.Z);
printf("Initial ForwardDirection: %f %f %f\n", initialRotationMatrix.ForwardDirection.X, initialRotationMatrix.ForwardDirection.Y, initialRotationMatrix.ForwardDirection.Z);
printf("Initial UpwardDirection: %f %f %f\n\n", initialRotationMatrix.UpwardDirection.X, initialRotationMatrix.UpwardDirection.Y, initialRotationMatrix.UpwardDirection.Z);
Rotation3D rotationResult = {0, 0, 0};
Get3DRotation(&initialRotationMatrix, &rotationResult);
printf("Rotation Result: Pitch: %f , Roll: %f , Yaw: %f\n\n", rotationResult.Pitch, rotationResult.Roll, rotationResult.Yaw);
RotationMatrix rotationMatrixResult = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
Set3DRotation(&rotationResult, &rotationMatrixResult);
printf("Converted RightwardDirection: %f %f %f\n", rotationMatrixResult.RightwardDirection.X, rotationMatrixResult.RightwardDirection.Y, rotationMatrixResult.RightwardDirection.Z);
printf("Converted ForwardDirection: %f %f %f\n", rotationMatrixResult.ForwardDirection.X, rotationMatrixResult.ForwardDirection.Y, rotationMatrixResult.ForwardDirection.Z);
printf("Converted UpwardDirection: %f %f %f\n\n", rotationMatrixResult.UpwardDirection.X, rotationMatrixResult.UpwardDirection.Y, rotationMatrixResult.UpwardDirection.Z);
Rotation3D rotationResult2 = {0, 0, 0};
Get3DRotation(&rotationMatrixResult, &rotationResult2);
printf("Second Convertion Rotation Result: Pitch: %f , Roll: %f , Yaw: %f\n\n", rotationResult2.Pitch, rotationResult2.Roll, rotationResult2.Yaw);
return 0;
}
Result:
Initial RightwardDirection: 0.076900 -0.574700 0.814700
Initial ForwardDirection: 0.669900 0.635000 0.384700
Initial UpwardDirection: -0.738500 0.516200 0.433800
Rotation Result: Pitch: 0.968204 , Roll: 0.489282 , Yaw: 0.758659 <---- these 3 results are correct
Converted RightwardDirection: 0.607232 -0.640606 0.469992
Converted ForwardDirection: 0.411346 0.389916 -0.823869
Converted UpwardDirection: 0.000000 0.000000 0.000000
Second Convertion Rotation Result: Pitch: 0.968204 , Roll: 0.000000 , Yaw: 0.758659
As you can clearly see on the result, I'm on the right track when it comes to the calculation of the rightward and forward direction because the pitch and yaw rotations matches the correct results. What's missing in my code is the calculation of the Upward Direction Vector to properly convert the 3d rotation back to its original rotation matrix.
I appreciate if someone could provide me the formula to calculate it properly.