[Toon-members] TooN so3.h [Maintenance_Branch_1_x]

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    TooN
Branch:         Maintenance_Branch_1_x
Changes by:     Gerhard Reitmayr <gerhard>      09/03/12 16:49:45

Modified files:
        .              : so3.h 

Log message:
        really stable SO3::ln now

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/so3.h?cvsroot=toon&only_with_tag=Maintenance_Branch_1_x&r1=1.23.2.1&r2=1.23.2.2

Patches:
Index: so3.h
===================================================================
RCS file: /cvsroot/toon/TooN/so3.h,v
retrieving revision 1.23.2.1
retrieving revision 1.23.2.2
diff -u -b -r1.23.2.1 -r1.23.2.2
--- so3.h       11 Mar 2009 15:30:03 -0000      1.23.2.1
+++ so3.h       12 Mar 2009 16:49:45 -0000      1.23.2.2
@@ -210,15 +210,14 @@
 inline Vector<3> SO3::ln() const{
   Vector<3> result;
 
-  const double trace = my_matrix[0][0] + my_matrix[1][1] + my_matrix[2][2];
-
   // 2 * cos(theta) + 1 == trace
+  const double trace = my_matrix[0][0] + my_matrix[1][1] + my_matrix[2][2];
   result[0] = (my_matrix[2][1]-my_matrix[1][2])/2;
   result[1] = (my_matrix[0][2]-my_matrix[2][0])/2;
   result[2] = (my_matrix[1][0]-my_matrix[0][1])/2;
 
       double sin_angle_abs = sqrt(result*result);
-      if (sin_angle_abs > 0.000001 ) { // cannot use small angle approximation
+      if (sin_angle_abs > 1e-6 && trace >= 1) { // cannot use small angle 
approximation
          double angle;
          if(sin_angle_abs > 1.0) {
              sin_angle_abs = 1.0;
@@ -230,26 +229,30 @@
          }
          result *= angle / sin_angle_abs;
       } else if( trace < 1) { // antisymmetric part vanishes, but still large 
rotation, need information from symmetric part
-       // diagonal elements provide squares of rotation axis elements
-       const double ANom = 1.0/(3.0 - trace);
-       // explicit solution of the linear system for the squares of rotation 
axis elements
-       TooN::Vector<3> r2 = TooN::makeVector(
-               sqrt((+ my_matrix[0][0] - my_matrix[1][1] - my_matrix[2][2]  + 
1.0) * ANom),
-               sqrt((- my_matrix[0][0] + my_matrix[1][1] - my_matrix[2][2]  + 
1.0) * ANom),
-               sqrt((- my_matrix[0][0] - my_matrix[1][1] + my_matrix[2][2]  + 
1.0) * ANom)
+       const double cos_angle = (trace - 3) * 0.5 + 1; // cos(angle)
+       const double angle = M_PI - ((sin_angle_abs > 
1e-6)?asin(sin_angle_abs):sin_angle_abs);  // small angle approximation, if 
possible
+       const TooN::Vector<3> diag = TooN::makeVector(
+               my_matrix[0][0] - cos_angle,
+               my_matrix[1][1] - cos_angle,
+               my_matrix[2][2] - cos_angle
        );
-       // antisymmetric part constrains sign (where possible)
-       if(result[0] < 0.0)
-               r2[0] *= -1;
-       if(result[1] < 0.0)
-               r2[1] *= -1;
-       if(result[2] < 0.0)
-               r2[2] *= -1;
-       // r2 contains now the rotation axis as unit vector, need to scale by 
correct angle !
-       const double angle = acos((trace - 1.0)* 0.5);
-       result = r2 * angle;
+       if(fabs(diag[0]) > fabs(diag[1]) && fabs(diag[0]) > fabs(diag[2])){ // 
first is largest, fill with first column
+               result[0] = diag[0];
+               result[1] = (my_matrix[1][0]+my_matrix[0][1])/2;
+               result[2] = (my_matrix[0][2]+my_matrix[2][0])/2;
+       } else if(fabs(diag[1]) > fabs(diag[2])) {                          // 
second is largest, fill with second column
+               result[0] = (my_matrix[1][0]+my_matrix[0][1])/2;
+               result[1] = diag[1];
+               result[2] = (my_matrix[2][1]+my_matrix[1][2])/2;
+       } else {                                                            // 
third is largest, fill with third column
+               result[0] = (my_matrix[0][2]+my_matrix[2][0])/2;
+               result[1] = (my_matrix[2][1]+my_matrix[1][2])/2;
+               result[2] = diag[2];
+       }       
+       normalize(result);
+       result *= angle;
       } else { // can use small angle approximation, because we are around zero
-       // nothing todo
+              // nothing to do here
       }
       return result;
 }