# [PD] Calculate rotation from translation & Gimbal Fix issue (euler, quaternion)

Roch Jub rochjub at gmail.com
Wed Jun 20 11:14:52 CEST 2012

```Hello PD list !

My first message here !

I have a little problem and I need some of your logic, because I've took my
project from paper scratch to the end pd patch like I thought it should,
but obviously it's not working right !

So I got a kinect ... keeewl ! I managed to install it on Ubuntu then on
Pure Data with pix_openni, thanks to Matthias !

I want to calculate the angles of rotations of each joints of the skeleton
in order to apply it on a 3D character rig. I just want the rotations, no
translations, because the guy in front of camera might not have the same
proportions as the model, I just want to reproduce the rotation of the
angles !

Now I'm gona tell you what I do, and if one know what's wrong ... please
tell me :) I'm writting in normal code so it's clearer here

That's what I do first to get each angle in degree

function getAngle(AB,BC) {

AC = sqrt((AB*AB)+(BC*BC));
BD = AC - AB;
angle = atan(AB/BC)+atan(BD/BC)
angle = ( angle *180/PI)*2
return angle%360;

}

_X = getAngle((hand.x- shoulder .x),( hand .y- shoulder .y) );
_Y = getAngle(( hand .x- shoulder .x),( hand .z- shoulder .z) );
_Z = getAngle(( hand .y- shoulder .y),( hand .z- shoulder .z) );

When _X, _Y and _Z are applied to the rotation values of a cube, it's kinda
matching with X and Y but when I plug Z it goes really weird.
After some readings, I found out about Gimbal Lock. So I've read this
http://www.cs.princeton.edu/~gewang/projects/darth/stuff/quat_faq.html#Q26 wich
is great info !
And I used the sources they give, simplified them to do just what I wan,
and translated it in PD graphical language. I checked many time every
calculation so it's same result as the original sources of the link above.
The degrees seems to be transformed correctly, but when applied to my cube,
still not matching moves !!! Brrrrr ....

Other fact I don't know how to take, when I compare the results of the
sources from the link above to the one on a online calculator
euler->quaternion->euler, I have different results in one case, if abc(C) >
0.005, when Y angle is almost 90°. But even by translating the source of
this online calculator to a PD patch and applying it to my cube, it's still
not right !!

Here is the sources of the gimbal lock fix I use

Y = -Math.asin(-sin_Y);
C = Math.cos(Y);

if (Math.abs(C) > 0.005) {

RX = (cos_X*cos_Y)/ C
RY = -(-sin_X*cos_Y)/ C

X = Math.atan2(RY,RX);

RX = (cos_Y*cos_Z)/ C
RY = -(-cos_Y*sin_Z)/ C

Z = Math.atan2(RY,RX);

}else{

X = 0;

RX = ((sin_X*sin_Y)*sin_Z)+(cos_X*cos_Z);
RY = -(sin_X*sin_Y)*cos_Z+cos_X*sin_Z;

Z = Math.atan2(RX,RY);

}

If anyone can tell me what's wrong would be great, I'm really stuck, but
still looking for solutions everywhere, so I'll keep you updated, and
hopefully share the patch when it's finished :)