[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/15 15:53:10

Modified files:
        .              : so3.h 

Log message:
        this is the accurate version, not fast yet

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

Patches:
Index: so3.h
===================================================================
RCS file: /cvsroot/toon/TooN/so3.h,v
retrieving revision 1.23.2.3
retrieving revision 1.23.2.4
diff -u -b -r1.23.2.3 -r1.23.2.4
--- so3.h       12 Mar 2009 18:16:59 -0000      1.23.2.3
+++ so3.h       15 Mar 2009 15:53:10 -0000      1.23.2.4
@@ -210,53 +210,44 @@
 inline Vector<3> SO3::ln() const{
   Vector<3> result;
 
-  // 2 * cos(theta) + 1 == trace
-  const double trace = my_matrix[0][0] + my_matrix[1][1] + my_matrix[2][2];
+  const double cos_angle = (my_matrix[0][0] + my_matrix[1][1] + 
my_matrix[2][2] - 1.0) * 0.5;
   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 > 1e-6 && trace >= 1) { // cannot use small angle 
approximation
-         double angle;
-         if(sin_angle_abs > 1.0) {
-             sin_angle_abs = 1.0;
-             angle = M_PI_2;
-         } else {
-             angle = asin(sin_angle_abs);
-             if (trace < 1)
-                 angle = M_PI - angle;
+    if (cos_angle > M_SQRT1_2) {            // [0 - Pi/4[ use asin
+        if(sin_angle_abs > 0){
+            result *= asin(sin_angle_abs) / sin_angle_abs;
          }
+    } else if( cos_angle > -M_SQRT1_2) {    // [Pi/4 - 3Pi/4[ use acos, but 
antisymmetric part
+        double angle = acos(cos_angle);
          result *= angle / sin_angle_abs;
-      } else if( trace < 1) { // antisymmetric part vanishes, but still large 
rotation, need information from symmetric part
-       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
-       );
+    } else {  // rest use symmetric part
+        // antisymmetric part vanishes, but still large rotation, need 
information from symmetric part
+       const double angle = M_PI - asin(sin_angle_abs);
+    const double d0 = my_matrix[0][0] - cos_angle,
+                    d1 = my_matrix[1][1] - cos_angle,
+                        d2 = my_matrix[2][2] - cos_angle;
        TooN::Vector<3> r2;
-       if(fabs(diag[0]) > fabs(diag[1]) && fabs(diag[0]) > fabs(diag[2])){ // 
first is largest, fill with first column
-               r2[0] = diag[0];
+       if(fabs(d0) > fabs(d1) && fabs(d0) > fabs(d2)){ // first is largest, 
fill with first column
+               r2[0] = d0;
                r2[1] = (my_matrix[1][0]+my_matrix[0][1])/2;
                r2[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
+       } else if(fabs(d1) > fabs(d2)) {                            // second 
is largest, fill with second column
                r2[0] = (my_matrix[1][0]+my_matrix[0][1])/2;
-               r2[1] = diag[1];
+               r2[1] = d1;
                r2[2] = (my_matrix[2][1]+my_matrix[1][2])/2;
        } else {                                                            // 
third is largest, fill with third column
                r2[0] = (my_matrix[0][2]+my_matrix[2][0])/2;
                r2[1] = (my_matrix[2][1]+my_matrix[1][2])/2;
-               r2[2] = diag[2];
+               r2[2] = d2;
        }
        // flip, if we point in the wrong direction!
        if(r2 * result < 0)
                r2 *= -1;
        normalize(r2);
        result = angle * r2;
-      } else { // can use small angle approximation, because we are around zero
-              // nothing to do here
       }
       return result;
 }