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
.