Device Attitude (Euler Angle) to 3D Coordinates

Question

Background

Here is a drawing of my device's Roll/Pitch/Yaw values:

Note: values aren't exact but for illustrative purposes only

• PI_2 is half of pi or roughly 1.57
• PI_4 is a quarter of pi or roughly 0.78

Scenario

I'm using the Euler Angles to orientate the camera in VTK 3D space. The 3D space per my model is different from my device XYZ:

• Z in the model is real world Y and vice-versa, X is same in both.

I've researched Rotation Matrices and found an articles on Wikipedia and even found some c++ code on SOF which I converted to ObjC with no luck. It's close but the axis are always off. I've watched the RPY and XYZ as I've moved the device around but I don't see any relation.

Question

How can I get my Euler Angles from my XYZ device to work in my VTK XZY space?

Code

const float DISTANCE = 10;
typedef float Matrix[3][3];
struct EulerAngle { float X,Y,Z; };
enum EEulerOrder
{
    ORDER_XYZ,
    ORDER_YZX,
    ORDER_ZXY,
    ORDER_ZYX,
    ORDER_YXZ,
    ORDER_XZY
};

...

vesVector3f cpv = self->mKiwiApp->cameraPosition();
vesVector3f fpv = self->mKiwiApp->cameraFocalPoint();
vesVector3f cuv = self->mKiwiApp->cameraViewUp();

double pitch = self.motionData.attitude.pitch;
double roll = self.motionData.attitude.roll;
double yaw = self.motionData.attitude.yaw;

NSArray *eaArr = [NSArray arrayWithObjects:[NSNumber numberWithFloat:r],
                    [NSNumber numberWithFloat:p],
                    [NSNumber numberWithFloat:y],
                    nil];
//I've tried mixtures of pry, ypy, rpr, etc into `eulerAnglesInOrder`
//I've also tried all various orders into `eulerAnglesInOrder`
NSArray *xyz = [self eulerAnglesInOrder:eaArr withOrder:ORDER_XYZ];

cpv(0, 0) = 0;
cpv(1, 0) = 0;
cpv(2, 0) = 0;
self->mKiwiApp->setCameraPosition(cpv);

fpv(0, 0) = [[xyz objectAtIndex:0] doubleValue] * DISTANCE;
fpv(1, 0) = [[xyz objectAtIndex:1] doubleValue] * DISTANCE;
fpv(2, 0) = [[xyz objectAtIndex:2] doubleValue] * DISTANCE;
self->mKiwiApp->setCameraFocalPoint(fpv);

cuv(0, 0) = [[xyz objectAtIndex:3] doubleValue];
cuv(1, 0) = [[xyz objectAtIndex:4] doubleValue];
cuv(2, 0) = [[xyz objectAtIndex:5] doubleValue];
self->mKiwiApp->setCameraViewUp(cuv);

...



- (NSArray *)eulerAnglesInOrder:(const NSArray *)inEulerAngle withOrder:(const int)eulerOrder
{
    // Convert Euler Angles passed in a vector of Radians
    // into a rotation matrix.  The individual Euler Angles are
    // processed in the order requested.
    float x = [[inEulerAngle objectAtIndex:0] floatValue];
    float y = [[inEulerAngle objectAtIndex:1] floatValue];
    float z = [[inEulerAngle objectAtIndex:2] floatValue];

    float x0, x1, x2;
    float y0, y1, y2;
    float z0, z1, z2;

    const float    Sx    = sin(x);
    const float    Sy    = sin(y);
    const float    Sz    = sin(z);
    const float    Cx    = cos(x);
    const float    Cy    = cos(y);
    const float    Cz    = cos(z);

    switch(eulerOrder)
    {
        case ORDER_XYZ:
            x0=Cy*Cz;
            x1=-Cy*Sz;
            x2=Sy;
            y0=Cz*Sx*Sy+Cx*Sz;
            y1=Cx*Cz-Sx*Sy*Sz;
            y2=-Cy*Sx;
            z0=-Cx*Cz*Sy+Sx*Sz;
            z1=Cz*Sx+Cx*Sy*Sz;
            z2=Cx*Cy;
            break;

        case ORDER_YZX:
            x0=Cy*Cz;
            x1=Sx*Sy-Cx*Cy*Sz;
            x2=Cx*Sy+Cy*Sx*Sz;
            y0=Sz;
            y1=Cx*Cz;
            y2=-Cz*Sx;
            z0=-Cz*Sy;
            z1=Cy*Sx+Cx*Sy*Sz;
            z2=Cx*Cy-Sx*Sy*Sz;
            break;

        case ORDER_ZXY:
            x0=Cy*Cz-Sx*Sy*Sz;
            x1=-Cx*Sz;
            x2=Cz*Sy+Cy*Sx*Sz;
            y0=Cz*Sx*Sy+Cy*Sz;
            y1=Cx*Cz;
            y2=-Cy*Cz*Sx+Sy*Sz;
            z0=-Cx*Sy;
            z1=Sx;
            z2=Cx*Cy;
            break;

        case ORDER_ZYX:
            x0=Cy*Cz;
            x1=Cz*Sx*Sy-Cx*Sz;
            x2=Cx*Cz*Sy+Sx*Sz;
            y0=Cy*Sz;
            y1=Cx*Cz+Sx*Sy*Sz;
            y2=-Cz*Sx+Cx*Sy*Sz;
            z0=-Sy;
            z1=Cy*Sx;
            z2=Cx*Cy;
            break;

        case ORDER_YXZ:
            x0=Cy*Cz+Sx*Sy*Sz;
            x1=Cz*Sx*Sy-Cy*Sz;
            x2=Cx*Sy;
            y0=Cx*Sz;
            y1=Cx*Cz;
            y2=-Sx;
            z0=-Cz*Sy+Cy*Sx*Sz;
            z1=Cy*Cz*Sx+Sy*Sz;
            z2=Cx*Cy;
            break;

        case ORDER_XZY:
            x0=Cy*Cz;
            x1=-Sz;
            x2=Cz*Sy;
            y0=Sx*Sy+Cx*Cy*Sz;
            y1=Cx*Cz;
            y2=-Cy*Sx+Cx*Sy*Sz;
            z0=-Cx*Sy+Cy*Sx*Sz;
            z1=Cz*Sx;
            z2=Cx*Cy+Sx*Sy*Sz;
            break;
    }

    NSArray *arr = [NSArray arrayWithObjects:
                      [NSNumber numberWithFloat:x0+x1+x2]
                    , [NSNumber numberWithFloat:y0+y1+y2]
                    , [NSNumber numberWithFloat:z0+z1+z2]
                    , [NSNumber numberWithFloat:x1] // for camera up
                    , [NSNumber numberWithFloat:y1] // for camera up
                    , [NSNumber numberWithFloat:z1] // for camera up
                    , nil];

    return(arr);
}