[Personalrobots-commit] SF.net SVN: personalrobots: [107] pkg/trunk/libTF/simple

[<tf...@us...>]

Revision: 107
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=107&view=rev
Author:   tfoote
Date:     2008-04-16 15:39:37 -0700 (Wed, 16 Apr 2008)

Log Message:
-----------
adding an import from homogeneous method

Modified Paths:
--------------
    pkg/trunk/libTF/simple/Quaternion3D.cc
    pkg/trunk/libTF/simple/Quaternion3D.hh
    pkg/trunk/libTF/simple/testQuat.cc

Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc	2008-04-16 22:16:17 UTC (rev 106)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc	2008-04-16 22:39:37 UTC (rev 107)
@@ -14,7 +14,60 @@
   return;
 };
 
+Quaternion3D::Quaternion3D(NEWMAT::Matrix matIn)
+{
+  // math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
 
+  double * mat = matIn.Store();
+  //Get the translations
+  xt = mat[3];
+  yt = mat[7];
+  zt = mat[11];
+
+  //TODO ASSERT others are zero and one as they should be
+
+
+  double T  = 1 + mat[0] + mat[5] + mat[10];
+
+
+  //  If the trace of the matrix is greater than zero, then
+  //  perform an "instant" calculation.
+  //	      Important note wrt. rouning errors:
+
+  if ( T > 0.00000001 ) //to avoid large distortions!
+    {
+      double S = sqrt(T) * 2;
+      xr = ( mat[9] - mat[6] ) / S;
+      yr = ( mat[2] - mat[8] ) / S;
+      zr = ( mat[4] - mat[1] ) / S;
+      w = 0.25 * S;
+    }
+  //If the trace of the matrix is equal to zero then identify
+  // which major diagonal element has the greatest value.
+  //  Depending on this, calculate the following:
+
+      if ( mat[0] > mat[5] && mat[0] > mat[10] ) {// Column 0: 
+        double S  = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
+        xr = 0.25 * S;
+        yr = (mat[1] + mat[4] ) / S;
+        zr = (mat[8] + mat[2] ) / S;
+        w = (mat[6] - mat[9] ) / S;
+      } else if ( mat[5] > mat[10] ) {// Column 1: 
+        double S  = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
+        xr = (mat[1] + mat[4] ) / S;
+        yr = 0.25 * S;
+        zr = (mat[6] + mat[9] ) / S;
+        w = (mat[8] - mat[2] ) / S;
+      } else {// Column 2:
+        double S  = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
+        xr = (mat[8] + mat[2] ) / S;
+        yr = (mat[6] + mat[9] ) / S;
+        zr = 0.25 * S;
+        w = (mat[1] - mat[4] ) / S;
+      }
+};
+
+
 void Quaternion3D::Normalize()
 {
   double mag = getMagnitude();
@@ -36,7 +89,7 @@
   
   double * mat = outMat.Store();
 
-  // math from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+  // math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
   double xx      = xr * xr;
   double xy      = xr * yr;
   double xz      = xr * zr;

Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh	2008-04-16 22:16:17 UTC (rev 106)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh	2008-04-16 22:39:37 UTC (rev 107)
@@ -22,6 +22,7 @@
 public:
   //Constructor
   Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w);
+  Quaternion3D(NEWMAT::Matrix matIn);
 
   inline void Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w)
   {xt = _xt; yt = _yt; zt = _zt; xr = _xr; yr = _yr; zr = _zr; w = _w;} ;

Modified: pkg/trunk/libTF/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/simple/testQuat.cc	2008-04-16 22:16:17 UTC (rev 106)
+++ pkg/trunk/libTF/simple/testQuat.cc	2008-04-16 22:39:37 UTC (rev 107)
@@ -7,7 +7,11 @@
 {
   Quaternion3D myquat(1,1,1,1,1,1,1);
 
+  Quaternion3D my2ndquat(myquat.asMatrix());
+  std::cout << "Quat1"<<std::endl;
   myquat.printMatrix();
+  std::cout << "Quat2"<<std::endl;
+  my2ndquat.printMatrix();
 
   return 0;
 }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.