[ODE] euler angles from matrix

[Daniel Monteiro Basso]

Hi,

on 19 Aug 2004, Andrzej Szombierski posted a code snippet for euler
angle extraction from a rotation matrix, but it was not consistent with
the ODE's dRFromEulerAngles, so I paste the corrected code below, for
those interested:

void get_euler(dReal * matrix,dReal &kx,dReal &ky,dReal &kz)
{       
   const dReal epsilon=0.0000001;
   if(matrix[2] < 1-epsilon && matrix[2] > -1+epsilon)
   {
      ky=-asin(matrix[2]);
      dReal c=cos(ky);
      kx= atan2(matrix[6]/c,matrix[10]/c);
      kz= atan2(matrix[1]/c,matrix[0]/c);
   } else {       
      kz=0;   
      ky=-atan2(matrix[2],0);
      kx= atan2(-matrix[9],matrix[5]);
   }       
}       

Thanks Andrzej, your code was very useful.

Cheers,

Daniel