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

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

Revision: 7452
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7452&view=rev
Author:   tfoote
Date:     2008-12-02 07:54:46 +0000 (Tue, 02 Dec 2008)

Log Message:
-----------
a few more patches to clean things up in bullet, + testcode 

Modified Paths:
--------------
    pkg/trunk/3rdparty/bullet/quaternion.patch
    pkg/trunk/util/tf/test/bullet_unittest.cpp

Modified: pkg/trunk/3rdparty/bullet/quaternion.patch
===================================================================
--- pkg/trunk/3rdparty/bullet/quaternion.patch	2008-12-02 06:53:45 UTC (rev 7451)
+++ pkg/trunk/3rdparty/bullet/quaternion.patch	2008-12-02 07:54:46 UTC (rev 7452)
@@ -1,3 +1,21 @@
+Index: src/LinearMath/btTransformUtil.h
+===================================================================
+--- src/LinearMath/btTransformUtil.h	(revision 1497)
++++ src/LinearMath/btTransformUtil.h	(working copy)
+@@ -115,12 +115,9 @@
+ 		}
+ 	}
+ 
+-	static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
++	static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1,btVector3& axis,btScalar& angle)
+ 	{
+-		btQuaternion orn1 = orn0.farthest(orn1a);
+ 		btQuaternion dorn = orn1 * orn0.inverse();
+-		///floating point inaccuracy can lead to w component > 1..., which breaks 
+-		dorn.normalize();
+ 		angle = dorn.getAngle();
+ 		axis = btVector3(dorn.x(),dorn.y(),dorn.z());
+ 		axis[3] = btScalar(0.);
 Index: src/LinearMath/btQuaternion.h
 ===================================================================
 --- src/LinearMath/btQuaternion.h	(revision 1497)
@@ -2,3 +20,3 @@
 +++ src/LinearMath/btQuaternion.h	(working copy)
-@@ -199,7 +199,12 @@
+@@ -199,15 +199,26 @@
  	{
@@ -16,7 +34,9 @@
  	}
    /**@brief Return the angle of rotation represented by this quaternion */
  	btScalar getAngle() const 
-@@ -208,6 +213,12 @@
+ 	{
+-		btScalar s = btScalar(2.) * btAcos(m_floats[3]);
++		btScalar s = btScalar(2.) * btAcos(GEN_clamped(m_floats[3], (btScalar)-1.0, (btScalar)1.0));
  		return s;
  	}
  

Modified: pkg/trunk/util/tf/test/bullet_unittest.cpp
===================================================================
--- pkg/trunk/util/tf/test/bullet_unittest.cpp	2008-12-02 06:53:45 UTC (rev 7451)
+++ pkg/trunk/util/tf/test/bullet_unittest.cpp	2008-12-02 07:54:46 UTC (rev 7452)
@@ -30,6 +30,7 @@
 #include <gtest/gtest.h>
 #include <sys/time.h>
 #include "LinearMath/btTransform.h"
+#include "LinearMath/btTransformUtil.h"
 #include "LinearMath/btVector3.h"
 
 void seed_rand()
@@ -353,6 +354,66 @@
 
 }
 
+TEST(Bullet, SetEulerZYX)
+{
+  btMatrix3x3 mat;
+  mat.setEulerZYX(M_PI/2, 0, 0);
+  double yaw, pitch, roll;
+  mat.getEulerZYX(yaw, pitch, roll);
+  EXPECT_NEAR(yaw, M_PI/2, 0.1);
+  EXPECT_NEAR(pitch, 0, 0.1);
+  EXPECT_NEAR(roll, 0, 0.1);
+//  printf("%f %f %f\n", yaw, pitch, roll);
+  btQuaternion q;
+  mat.getRotation(q);
+  EXPECT_NEAR(q.z(), sqrt(2)/2, 0.1);
+  EXPECT_NEAR(q.y(), 0, 0.1);
+  EXPECT_NEAR(q.x(), 0, 0.1);
+  EXPECT_NEAR(q.w(), sqrt(2)/2, 0.1);
+  //  printf("%f %f %f %f\n", q.x(), q.y(), q.z(), q.w());
+}
+
+
+TEST(Bullet, calculateDiffAxisAngleQuaternion)
+{
+  btVector3 vec;
+  btScalar ang;
+  for (unsigned int i = 0 ; i < 1000 ; i++)
+  {
+    btQuaternion q1(M_PI*2 *(double) i / 1000, 0, 0);
+    btQuaternion q2(M_PI/2*0, 0,0);
+    btTransformUtil::calculateDiffAxisAngleQuaternion(q1, q2, vec, ang);
+    //    printf("%f %f %f, ang %f\n", vec.x(), vec.y(), vec.z(), ang);
+    EXPECT_NEAR(M_PI*2 *(double) i / 1000, ang, 0.001);
+    btTransformUtil::calculateDiffAxisAngleQuaternion(q2, q1, vec, ang);
+    //    printf("%f %f %f, ang %f\n", vec.x(), vec.y(), vec.z(), ang);
+    EXPECT_NEAR(M_PI*2 *(double) i / 1000, ang, 0.001);
+  }
+  for (unsigned int i = 0 ; i < 1000 ; i++)
+  {
+    btQuaternion q1(0, M_PI*2 *(double) i / 1000,1);
+    btQuaternion q2(0, 0, 1);
+    btTransformUtil::calculateDiffAxisAngleQuaternion(q1, q2, vec, ang);
+    //    printf("%f %f %f, ang %f\n", vec.x(), vec.y(), vec.z(), ang);
+    EXPECT_NEAR(M_PI*2 *(double) i / 1000, ang, 0.001);
+    btTransformUtil::calculateDiffAxisAngleQuaternion(q2, q1, vec, ang);
+    //    printf("%f %f %f, ang %f\n", vec.x(), vec.y(), vec.z(), ang);
+    EXPECT_NEAR(M_PI*2 *(double) i / 1000, ang, 0.001);
+  }
+  for (unsigned int i = 0 ; i < 1000 ; i++)
+  {
+    btQuaternion q1(0, 0, M_PI*2 *(double) i / 1000);
+    btQuaternion q2(0, 0,0);
+    btTransformUtil::calculateDiffAxisAngleQuaternion(q1, q2, vec, ang);
+    //    printf("%f %f %f, ang %f\n", vec.x(), vec.y(), vec.z(), ang);
+    EXPECT_NEAR(M_PI*2 *(double) i / 1000, ang, 0.001);
+    btTransformUtil::calculateDiffAxisAngleQuaternion(q2, q1, vec, ang);
+    //    printf("%f %f %f, ang %f\n", vec.x(), vec.y(), vec.z(), ang);
+    EXPECT_NEAR(M_PI*2 *(double) i / 1000, ang, 0.001);
+  }
+}
+
+
 int main(int argc, char **argv){
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();


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