Computing the Upward Direction(Z-Axis) Vector from a 3D Rotation

18 Views Asked by At

I was tasked on creating a C Program that needs to process this case:

  1. Convert the Initial Rotation Matrix(InitialRotationMatrix) into 3D Rotation(Rotation3DResult1)
  2. Convert the 3D Rotation(Rotation3DResult1) back to its Rotation Matrix(ConvertedRotationMatrix)
  3. Convert the Converted Rotation Matrix(ConvertedRotationMatrix) back to its 3D Rotation(Rotation3DResult2)
  4. Rotation3DResult2's components must be equal to Rotation3DResult1's components
  5. 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) Vector

3D 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.