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

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

Revision: 123
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=123&view=rev
Author:   tfoote
Date:     2008-04-17 17:02:02 -0700 (Thu, 17 Apr 2008)

Log Message:
-----------
cleaning up API

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

Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc	2008-04-17 23:52:04 UTC (rev 122)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc	2008-04-18 00:02:02 UTC (rev 123)
@@ -50,7 +50,7 @@
   return;
 };
 
-void Quaternion3D::Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time)
+void Quaternion3D::fromQuaternion(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time)
 {
  Quaternion3DStorage temp;
  temp.xt = _xt; temp.yt = _yt; temp.zt = _zt; temp.xr = _xr; temp.yr = _yr; temp.zr = _zr; temp.w = _w; temp.time = time;
@@ -238,7 +238,7 @@
   return mystream;
 };
 
-Quaternion3D::Quaternion3DStorage Quaternion3D::asQuaternion(unsigned long long time)
+Quaternion3D::Quaternion3DStorage Quaternion3D::getQuaternion(unsigned long long time)
 {
   Quaternion3DStorage temp;
   long long diff_time; //todo Find a way to use this offset. pass storage by reference and return diff_time??
@@ -247,31 +247,38 @@
 };
 
 
-NEWMAT::Matrix Quaternion3D::asMatrix(unsigned long long time)
+NEWMAT::Matrix Quaternion3D::getMatrix(unsigned long long time)
 {
   Quaternion3DStorage temp;
   long long diff_time;
   getValue(temp, time, diff_time);
 
-  //  std::cout << temp;
+  std::cout << temp;
   //  printStorage(temp);
   // std::cout <<"Locally: "<< xt <<", "<< yt  <<", "<< zt <<", "<< xr <<", "<<yr  <<", "<<zr  <<", "<<w<<std::endl;
   //std::cout << "time Difference: "<< diff_time<<std::endl;
+
+  return temp.asMatrix();
+}  
+
+
+NEWMAT::Matrix Quaternion3D::Quaternion3DStorage::asMatrix()
+{
   
   NEWMAT::Matrix outMat(4,4);
   
   double * mat = outMat.Store();
 
   // math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
-  double xx      = temp.xr * temp.xr;
-  double xy      = temp.xr * temp.yr;
-  double xz      = temp.xr * temp.zr;
-  double xw      = temp.xr * temp.w;
-  double yy      = temp.yr * temp.yr;
-  double yz      = temp.yr * temp.zr;
-  double yw      = temp.yr * temp.w;
-  double zz      = temp.zr * temp.zr;
-  double zw      = temp.zr * temp.w;
+  double xx      = xr * xr;
+  double xy      = xr * yr;
+  double xz      = xr * zr;
+  double xw      = xr * w;
+  double yy      = yr * yr;
+  double yz      = yr * zr;
+  double yw      = yr * w;
+  double zz      = zr * zr;
+  double zw      = zr * w;
   mat[0]  = 1 - 2 * ( yy + zz );
   mat[4]  =     2 * ( xy - zw );
   mat[8]  =     2 * ( xz + yw );
@@ -282,9 +289,9 @@
   mat[6]  =     2 * ( yz + xw );
   mat[10] = 1 - 2 * ( xx + yy );
   mat[12]  = mat[13] = mat[14] = 0;
-  mat[3] = temp.xt;
-  mat[7] = temp.yt;
-  mat[11] = temp.zt;
+  mat[3] = xt;
+  mat[7] = yt;
+  mat[11] = zt;
   mat[15] = 1;
     
 
@@ -294,7 +301,7 @@
 
 void Quaternion3D::printMatrix(unsigned long long time)
 {
-  std::cout << asMatrix(time);
+  std::cout << getMatrix(time);
 };
 
 void Quaternion3D::printStorage(const Quaternion3DStorage& storage)

Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh	2008-04-17 23:52:04 UTC (rev 122)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh	2008-04-18 00:02:02 UTC (rev 123)
@@ -67,6 +67,9 @@
     Quaternion3DStorage & operator=(const Quaternion3DStorage & input);
 
 
+    /* accessors */
+    NEWMAT::Matrix asMatrix();
+
     /* Internal Data */    
     double xt, yt, zt, xr, yr, zr, w;
     unsigned long long time;
@@ -78,7 +81,7 @@
   
   /** Mutators **/
   // Set the values manually
-  void Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time);
+  void fromQuaternion(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time);
 
   //Set the values from a matrix
   void fromMatrix(const NEWMAT::Matrix& matIn, unsigned long long time);
@@ -98,8 +101,8 @@
 
   /** Interpolated Accessors **/
   // Return a Matrix
-  Quaternion3D::Quaternion3DStorage asQuaternion(unsigned long long time);
-  NEWMAT::Matrix asMatrix(unsigned long long time);
+  Quaternion3D::Quaternion3DStorage getQuaternion(unsigned long long time);
+  NEWMAT::Matrix getMatrix(unsigned long long time);
 
   //Print as a matrix
   void printMatrix(unsigned long long time);  //Not a critical part either

Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc	2008-04-17 23:52:04 UTC (rev 122)
+++ pkg/trunk/libTF/simple/libTF.cc	2008-04-18 00:02:02 UTC (rev 123)
@@ -42,7 +42,7 @@
 /* Quaternion 3D version */
 void RefFrame::setParamsQuaternion3D(double a,double b,double c,double d,double e,double f, double g, unsigned long long time)
 {
-  myQuat.Set(a,b,c,d,e,f,g,time);
+  myQuat.fromQuaternion(a,b,c,d,e,f,g,time);
 };
 
 /* Six DOF version */
@@ -60,12 +60,12 @@
 
 NEWMAT::Matrix RefFrame::getMatrix(unsigned long long time)
 {
-  return myQuat.asMatrix(time);
+  return myQuat.getMatrix(time);
 }
 
 NEWMAT::Matrix RefFrame::getInverseMatrix(unsigned long long time)
 {
-  return myQuat.asMatrix(time).i();
+  return myQuat.getMatrix(time).i();
 };
 
 


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