[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [20905] branches/ge_dev/intern/itasc/kdl/ frames.cpp: KDL: detect 180 degre rotation around axis.

[Benoit Bolsee]

Revision: 20905
          http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=20905
Author:   ben2610
Date:     2009-06-16 00:18:44 +0200 (Tue, 16 Jun 2009)

Log Message:
-----------
KDL: detect 180 degre rotation around axis.

Modified Paths:
--------------
    branches/ge_dev/intern/itasc/kdl/frames.cpp

Modified: branches/ge_dev/intern/itasc/kdl/frames.cpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/frames.cpp	2009-06-15 20:28:49 UTC (rev 20904)
+++ branches/ge_dev/intern/itasc/kdl/frames.cpp	2009-06-15 22:18:44 UTC (rev 20905)
@@ -300,8 +300,23 @@
        double alfa;
        if (sa > epsilon)
            alfa = ::atan2(sa,ca)/sa;
-       else
-           alfa = 1;
+	   else {
+		   if (ca < 0.0) {
+			   alfa = KDL::PI;
+			   axis.data[0] = 0.0;
+			   axis.data[1] = 0.0;
+			   axis.data[2] = 0.0;
+			   if (data[0] > 0.0) {
+				   axis.data[0] = 1.0;
+			   } else if (data[4] > 0.0) {
+				   axis.data[1] = 1.0;
+			   } else {
+				   axis.data[2] = 1.0;
+			   }
+		   } else {
+			   alfa = 0.0;
+		   }
+	   }
        return axis * alfa;
      }