I have a task to calculate yaw, pitch and roll between 2 3d-vectors
2 examples with same destination vector, but different source:
1) First example:
Source vector: x=-626.264 y=-233.745 z=2270.071
Dest vector: x=-753.13 y=-1680.23 z=1881.34
Original rotation matrix 3x3:
-0.082 -0.021 -0.996
-0.964 -0.255 0.080
-0.256 0.967 -0.000
Must be: yaw=1.655 pitch=-0.321 Roll=0.0
2) Second example:
Source vector: x=-168.809 y=-1121.165 z=1880.855
Dest vector: x=-753.13 y=-1680.23 z=1881.34
Original rotation matrix 3x3:
-0.720 0.001 -0.694
-0.694 0.001 0.720
0.002 1.000 -0.000
Must be: yaw=2.375 pitch=0.0 (at the same height) Roll=0.0
I've tried:
1) Restoring original matrix and recalculating angles back:
pitch = -atan2f(-rot[1][2], rot[2][2]);
yaw = -atan2f(-rot[0][1], rot[0][0]);
edit: valid yaw, but inversed. pitch not valid, PI/2 all the time. roll invalid - must be always 0
2) Calculate angles from deltas:
yaw = atan2(delta.x, delta.y);
float padj = sqrt(pow(delta.x, 2) + pow(delta.x, 2));
pitch = atan2(padj, delta.z)
edit: valid yaw. pitch is dynamic, but not valid. roll is not calculated (like it must be)
The trouble is nothing working right:
1) roll must be always null
2) yaw is not problem anymore
3) pitch must be correct and be dependent on distance and z-axe difference, original value is clamped between -1.484 and 1.134...
Thank you!
