Euler's angle rotation

Question

From a three dimensional cartesian coordinate, object A's coordinate can be expressed as xyzwpr (green arrow). And from object A's coordinate world, object B can be also expressed as xyzwpr (blue arrow).

Then can anyone write down the C# code for calculating xyzwpr of object B relative to the original coordinate system (red arrow)?

Say A's coordinate is (30,50,70, -15,44,-80) B (60,90,110, 33,150,-90).

And say the order of the rotation is yaw(z)-> pitch(x) -> roll(y)

--- EDIT ---

Can anyone validate below assumptions?

Assumption for xyz of point B.

xyz of point B, the smaller airplane, can be calculated by adding xyz of point A, the first airplane, and the xyz of B and then applying the 3d rotation of A's wpr on the A's xyz.

The order of doing this is;

1) translate the A point to the origin (subtract A which is translate by -Ax,-Ay,-Az)

2) rotate about the origin (can use 3×3 matrix R0 of A)

3) then translate back. (add A which is translate by +Ax,+Ay,+Az)

Assumption for wpr of point B is simply succession of rotations of two points. AwApArBwBpBr.

--- SOLVED. A few references with detailed explanation and codes ---

Global frame-of-reference VS Local frame-of-reference

3D matrix rotation about an arbitrary point

Euler to matrix conversion

Answer 1

This question has some issues.

First, I think it's not good practice to request directly for code. Instead, show code you tried, ask for errors in your code, or a better approach, or libraries that may help you.

I would suggest rephrasing your question. Now it looks like "Can anyone do my homework, please?".

What problems are you facing? Maybe you don't want to implement matrix multiplication and you would like to know libraries that already do it, or you don't know how to make the call to atan2.

Once you get matrix multiplication, translation matrix build up, rotation matrix build up and atan2 (made by yourself or by a library), you just have to (pseudocode):

Matrix c = a;
Matrix yaw, pitch, roll;
Matrix pos;

buildTranslationMatrix(pos, x, y, z);
buildRotationZMatrix(yaw, w);
buildRotationXMatrix(pitch, p);
buildRotationYMatrix(roll, r);

mult (c, c, pos);    //c = c*pos

mult (c, c, yaw);    //c = c*yaw
mult (c, c, pitch);
mult (c, c, roll);

decomposePos(c, x, y, z);  // obtain final xyz from c
decomposeAngles(c, w, p, r);   // obtain final wpr from c

Note the post-multiplication.

Hope I made a constructive criticism. :)

EDIT

Second assumption is correct.

Maybe I misunderstood the first one, but I think it's wrong. As I am more used to transformation matrices than to euler angles (and you pointed that link), I understand it this way:

To obtain xyz (as well as wpr) I would compute the transformation matrix, which contains all the values. The final transformation matrix of the second plane, in the original coordinate system, is computed as:

M = TA * RA * TB * RB

(TA is A translation matrix of plane A and RA is its rotation matrix)

Transformation matrices can be understood this way:

    r r r t
    r r r t
M = r r r t
    s s s w

We only care about rotation and translation. If you multiply TA*RA:

1 0 0 x   r r r 0   r r r x
0 1 0 y   r r r 0   r r r y
0 0 1 z * r r r 0 = r r r z
0 0 0 1   0 0 0 1   0 0 0 1 

which is how we understand the coordinate system of A. Remember that this means first rotating, as if it were at the origin, and then translating to position x, y, z. Post-multiplicating means intern transformation, transformation in the mobile coordinate system. So, if we continue post-multiplicating, we will composite the final transformation matrix.

Also, matrices are associative, so

M = (TA * RA) * (TB * RB)

is the same as

M = ((TA * RA) * TB) * RB

Recapitulation

xyz will be in the last column of M and wpr will have to be decomposed from 3*3 submatrix of M.