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

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

Revision: 157
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=157&view=rev
Author:   tfoote
Date:     2008-04-22 16:14:22 -0700 (Tue, 22 Apr 2008)

Log Message:
-----------
getting rid of zero distance interpolation case

Modified Paths:
--------------
    pkg/trunk/libTF/include/libTF/Quaternion3D.h
    pkg/trunk/libTF/src/simple/Quaternion3D.cpp
    pkg/trunk/libTF/src/test/main.cpp

Modified: pkg/trunk/libTF/include/libTF/Quaternion3D.h
===================================================================
--- pkg/trunk/libTF/include/libTF/Quaternion3D.h	2008-04-22 21:42:52 UTC (rev 156)
+++ pkg/trunk/libTF/include/libTF/Quaternion3D.h	2008-04-22 23:14:22 UTC (rev 157)
@@ -52,6 +52,7 @@
 class Quaternion3D {
 
 public:
+  static const int MIN_INTERPOLATION_DISTANCE = 5; //Number of micro seconds to not interpolate below.
   // Storage class
   class Quaternion3DStorage
   {

Modified: pkg/trunk/libTF/src/simple/Quaternion3D.cpp
===================================================================
--- pkg/trunk/libTF/src/simple/Quaternion3D.cpp	2008-04-22 21:42:52 UTC (rev 156)
+++ pkg/trunk/libTF/src/simple/Quaternion3D.cpp	2008-04-22 23:14:22 UTC (rev 157)
@@ -514,6 +514,14 @@
   // Calculate the ration of time betwen target and the two end points.
   long long total_diff = two.time - one.time;
   long long target_diff = target_time - one.time;
+
+  //Check for zero distance case and just return
+  if (total_diff < 10 || target_diff < 10)
+    {
+      output = one;
+      return;
+    } 
+
   double t = (double)target_diff / (double) total_diff; //ratio between first and 2nd position interms of time
 
   // Interpolate the translation

Modified: pkg/trunk/libTF/src/test/main.cpp
===================================================================
--- pkg/trunk/libTF/src/test/main.cpp	2008-04-22 21:42:52 UTC (rev 156)
+++ pkg/trunk/libTF/src/test/main.cpp	2008-04-22 23:14:22 UTC (rev 157)
@@ -27,6 +27,7 @@
   mTR.setWithEulers(3,5,dx,dy,dz,dyaw,dp,dr,atime);
   mTR.setWithEulers(5,1,dx,dy,dz,dyaw,dp,dr,atime);
   mTR.setWithEulers(6,5,dx,dy,dz,dyaw,dp,dr,atime);
+  mTR.setWithEulers(6,5,dx,dy,dz,dyaw,dp,dr,atime);
   mTR.setWithEulers(7,6,1,1,1,dyaw,dp,dr,atime);
   mTR.setWithDH(8,7,1.0,1.0,1.0,dyaw,atime);
   //mTR.setWithEulers(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above


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