compute Euler angles from rotation matrix

Common/Context.cpp

-// $Id: Context.cpp,v 1.50 2004-02-07 01:40:17 geuzaine Exp $
+// $Id: Context.cpp,v 1.51 2004-10-11 17:21:16 geuzaine Exp $
 //
 // Copyright (C) 1997-2004 C. Geuzaine, J.-F. Remacle
 //
@@ -29,102 +29,54 @@
 #include "DefaultOptions.h"
 #include "Trackball.h"
 
-/*
-  3 successive rotations along x, y and z:
-
-           c(y)c(z)    s(x)s(y)c(z)+c(x)s(z)   -c(x)s(y)c(z)+s(x)s(z) 
-  t[][] = -c(y)s(z)   -s(x)s(y)s(z)+c(x)c(z)    c(x)s(y)s(z)+s(x)c(z)
-           s(y)       -s(x)c(y)                 c(x)c(y)
-
-  get the position angles:
-
-  y = asin(t31)
-      Pi - asin(t31)
- 
-  si y != +- Pi/2 :
-
-  x = atan(-t32/t33)    si t33 cos y > 0
-      atan(-t32/t33)+Pi si t33 cos y < 0
-
-  z = atan(-t21/t11)    si t11 cos y > 0
-      atan(-t21/t11)+Pi si t11 cos y < 0
-
-*/
-
 void Context_T::buildRotmatrix(void)
 {
-  double x, y, z;
-
   if(useTrackball) {
     build_rotmatrix(rot, quaternion);
 
-#if defined(HAVE_FLTK)
-    // We should reconstruct the Euler angles from the rotation
-    // matrix. I'm too lazy to do it :-(
-    extern void set_r(int i, double val);
-    set_r(0, 0.);
-    set_r(1, 0.);
-    set_r(2, 0.);
-    /*
-       double x=0., y=0., z=0.
-
-       y = asin(rot[2][0]) ; y = Pi - asin(rot[2][0]) ; // choix ???
-
-       if(fabs(y) != Pi/2.){
-       if(rot[2][2]*cos(y) > 0.) x = atan2(-rot[2][1],rot[2][2]);
-       else                      x = atan2(-rot[2][1],rot[2][2]) + Pi;
-       if(rot[0][0]*cos(y) > 0.) z = atan2(-rot[1][0],rot[0][0]);
-       else                      z = atan2(-rot[1][0],rot[0][0]) + Pi;
+    // get Euler angles from rotation matrix
+    r[1] = asin(rot[2][0]); // Calculate Y-axis angle
+    double C =  cos(r[1]);
+    r[1] *=  180. / Pi;
+    if(fabs(C) > 0.005){ // Gimball lock?
+      double tmpx =  rot[2][2] / C; // No, so get X-axis angle
+      double tmpy = -rot[2][1] / C;
+      r[0] = atan2(tmpy, tmpx) * 180. / Pi;
+      tmpx =  rot[0][0] / C; // Get Z-axis angle
+      tmpy = -rot[1][0] / C;
+      r[2] = atan2(tmpy, tmpx) * 180. / Pi;
     }
-       set_r(0, x * 180./Pi);
-       set_r(1, y * 180./Pi);
-       set_r(2, z * 180./Pi);
-     */
-    /*
-       double r0, r1, r2;
-
-       r1 = atan2(-rot[0][2],sqrt(rot[1][2]*rot[1][2] + rot[2][2]*rot[2][2]));
-
-       double c = cos(r1);  
-       if(c != 0.0){
-       r0 = atan2(rot[1][2]/c,rot[2][2]/c) ;
-       r2 = atan2(-rot[1][0]/c,rot[0][0]/c) ;
-       r0 *= 180./(Pi);
-       r2 *= 180./(Pi);
+    else{ // Gimball lock has occurred
+      r[0] = 0.; // Set X-axis angle to zero
+      double tmpx = rot[1][1]; // And calculate Z-axis angle
+      double tmpy = rot[0][1];
+      r[2] = atan2(tmpy, tmpx) * 180. / Pi;
     }
-       set_r(0, r0);
-       set_r(1, r1 * 180./(Pi));  // lazyyyyyy
-       set_r(2, r2);
-     */
-#endif
-
+    // return only positive angles in [0,360]
+    if(r[0] < 0.) r[0] += 360.;
+    if(r[1] < 0.) r[1] += 360.;
+    if(r[2] < 0.) r[2] += 360.;
   }
   else {
-    x = r[0] * Pi / 180.;
-    y = r[1] * Pi / 180.;
-    z = r[2] * Pi / 180.;
-
-    rot[0][0] = cos(y) * cos(z);
-    rot[0][1] = sin(x) * sin(y) * cos(z) + cos(x) * sin(z);
-    rot[0][2] = -cos(x) * sin(y) * cos(z) + sin(x) * sin(z);
-    rot[0][3] = 0.0;
-
-    rot[1][0] = -cos(y) * sin(z);
-    rot[1][1] = -sin(x) * sin(y) * sin(z) + cos(x) * cos(z);
-    rot[1][2] = cos(x) * sin(y) * sin(z) + sin(x) * cos(z);
-    rot[1][3] = 0.0;
-
-    rot[2][0] = sin(y);
-    rot[2][1] = -sin(x) * cos(y);
-    rot[2][2] = cos(x) * cos(y);
-    rot[2][3] = 0.0;
-
-    rot[3][0] = 0.0;
-    rot[3][1] = 0.0;
-    rot[3][2] = 0.0;
-    rot[3][3] = 1.0;
+    double x = r[0] * Pi / 180.;
+    double y = r[1] * Pi / 180.;
+    double z = r[2] * Pi / 180.;
+    double A = cos(x);
+    double B = sin(x);
+    double C = cos(y);
+    double D = sin(y);
+    double E = cos(z);
+    double F = sin(z);
+    double AD = A * D;
+    double BD = B * D;
+    rot[0][0] = C*E; rot[0][1] = BD*E+A*F; rot[0][2] =-AD*E+B*F; rot[0][3] = 0.;
+    rot[1][0] =-C*F; rot[1][1] =-BD*F+A*E; rot[1][2] = AD*F+B*E; rot[1][3] = 0.;
+    rot[2][0] = D;   rot[2][1] =-B*C;	   rot[2][2] = A*C;      rot[2][3] = 0.;
+    rot[3][0] = 0.;  rot[3][1] = 0.;       rot[3][2] = 0.;       rot[3][3] = 1.;
+
+    // get the quaternion from the Euler angles
+    // todo
   }
-
 }
 
 void Context_T::addQuaternion(float p1x, float p1y, float p2x, float p2y)