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

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

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

Log Message:
-----------
cleaning up header and adding Inverse method

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

Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc	2008-04-18 00:20:12 UTC (rev 124)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc	2008-04-18 00:32:02 UTC (rev 125)
@@ -261,7 +261,12 @@
   return temp.asMatrix();
 }  
 
+NEWMAT::Matrix Quaternion3D::getInverseMatrix(unsigned long long time)
+{
+  return getMatrix(time).i();
 
+};
+
 NEWMAT::Matrix Quaternion3D::Quaternion3DStorage::asMatrix()
 {
   

Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh	2008-04-18 00:20:12 UTC (rev 124)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh	2008-04-18 00:32:02 UTC (rev 125)
@@ -53,7 +53,7 @@
 class Quaternion3D {
 
 public:
-  // Storage 
+  // Storage class
   class Quaternion3DStorage
   {
   public:
@@ -82,7 +82,6 @@
   /** Mutators **/
   // Set the values manually
   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);
   // Set the values using Euler angles
@@ -91,26 +90,31 @@
   void fromDH(double theta, double length, double distance, double alpha, unsigned long long time);
 
   
+  /** Interpolated Accessors **/
+  // Return a Quaternion
+  Quaternion3D::Quaternion3DStorage getQuaternion(unsigned long long time);
+  // Return a Matrix
+  NEWMAT::Matrix getMatrix(unsigned long long time);
+  // Return the inverse matrix
+  NEWMAT::Matrix getInverseMatrix(unsigned long long time);
+
   /**** Utility Functions ****/
+  // this is a function to return the current time in microseconds from the beginning of 1970
+  static  unsigned long long Qgettime(void);
+
+  // Convert DH Parameters to a Homogeneous Transformation Matrix
   static NEWMAT::Matrix matrixFromDH(double theta,
 			      double length, double distance, double alpha);
+  // Convert Euler Angles to a Homogeneous Transformation Matrix
   static NEWMAT::Matrix matrixFromEuler(double ax,
 				 double ay, double az, double yaw,
 				 double pitch, double roll);
   
-
-  /** Interpolated Accessors **/
-  // Return a Matrix
-  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
   void printStorage(const Quaternion3DStorage &storage); //Do i need this now that i've got the ostream method??
-
-  // this is a function to return the current time in microseconds from the beginning of 1970
-  static  unsigned long long Qgettime(void);
-
+  
+  
 private:
   /**** Linked List stuff ****/
   static const long long MAX_STORAGE_TIME = 1000000000; // max of 100 seconds storage


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