Merge pull request #199 from maverick-long/fix_rotation_angle_numerical_error

back to use a more numerically stable method to compute the rotation angle

Author: Ruben Smits

orocos_kdl/src/frames.cpp

@@ -413,15 +413,13 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const {
         return angle; // return 180 deg rotation
     }
 
-    // If the matrix is slightly non-orthogonal, `f` may be out of the (-1, +1) range.
-    // Therefore, clamp it between those values to avoid NaNs.
     double f = (data[0] + data[4] + data[8] - 1) / 2;
-    angle = acos(std::max(-1.0, std::min(1.0, f)));
 
     x = (data[7] - data[5]);
     y = (data[2] - data[6]);
     z = (data[3] - data[1]);
     axis = KDL::Vector(x, y, z);
+    angle = atan2(axis.Norm()/2,f);
     axis.Normalize();
     return angle;
 }