Update Quaternion3f API

include/hpp/fcl/math/transform.h

@@ -58,14 +58,6 @@ public:
     data[3] = 0;
   }
 
-  Quaternion3f(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d)
-  {
-    data[0] = a;
-    data[1] = b;
-    data[2] = c;
-    data[3] = d;
-  }
-
   /// @brief Whether the rotation is identity
   bool isIdentity() const
   {
@@ -127,20 +119,6 @@ public:
   /// @brief rotate a vector
   Vec3f transform(const Vec3f& v) const;
 
-  inline const FCL_REAL& getW() const { return data[0]; }
-  inline const FCL_REAL& getX() const { return data[1]; }
-  inline const FCL_REAL& getY() const { return data[2]; }
-  inline const FCL_REAL& getZ() const { return data[3]; }
-
-  inline FCL_REAL& getW() { return data[0]; }
-  inline FCL_REAL& getX() { return data[1]; }
-  inline FCL_REAL& getY() { return data[2]; }
-  inline FCL_REAL& getZ() { return data[3]; }
-
-  Vec3f getColumn(std::size_t i) const;
-
-  Vec3f getRow(std::size_t i) const;
-
   bool operator == (const Quaternion3f& other) const
   {
     for(std::size_t i = 0; i < 4; ++i)
@@ -161,8 +139,26 @@ public:
     return data[i];
   }
 
+  inline FCL_REAL& w() { return data[0]; }
+  inline FCL_REAL& x() { return data[1]; }
+  inline FCL_REAL& y() { return data[2]; }
+  inline FCL_REAL& z() { return data[3]; }
+
+  inline const FCL_REAL& w() const { return data[0]; }
+  inline const FCL_REAL& x() const { return data[1]; }
+  inline const FCL_REAL& y() const { return data[2]; }
+  inline const FCL_REAL& z() const { return data[3]; }
+
 private:
 
+  Quaternion3f(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d)
+  {
+    data[0] = a;
+    data[1] = b;
+    data[2] = c;
+    data[3] = d;
+  }
+
   FCL_REAL data[4];
 };
 

src/math/transform.cpp

@@ -316,8 +316,8 @@ Quaternion3f& Quaternion3f::inverse()
 
 Vec3f Quaternion3f::transform(const Vec3f& v) const
 {
-  Vec3f u(getX(), getY(), getZ());
-  double s = getW();
+  Vec3f u(x(), y(), z());
+  double s = w();
   Vec3f vprime = 2*u.dot(v)*u + (s*s - u.dot(u))*v + 2*s*u.cross(v);
   return vprime;
 }
@@ -366,49 +366,6 @@ void Quaternion3f::toEuler(FCL_REAL& a, FCL_REAL& b, FCL_REAL& c) const
 }
 
 
-Vec3f Quaternion3f::getColumn(std::size_t i) const
-{
-  switch(i)
-  {
-  case 0:
-    return Vec3f(data[0] * data[0] + data[1] * data[1] - data[2] * data[2] - data[3] * data[3],
-                 2 * (- data[0] * data[3] + data[1] * data[2]),
-                 2 * (data[1] * data[3] + data[0] * data[2]));
-  case 1:
-    return Vec3f(2 * (data[1] * data[2] + data[0] * data[3]),
-                 data[0] * data[0] - data[1] * data[1] + data[2] * data[2] - data[3] * data[3],
-                 2 * (data[2] * data[3] - data[0] * data[1]));
-  case 2:
-    return Vec3f(2 * (data[1] * data[3] - data[0] * data[2]),
-                 2 * (data[2] * data[3] + data[0] * data[1]),
-                 data[0] * data[0] - data[1] * data[1] - data[2] * data[2] + data[3] * data[3]);
-  default:
-    return Vec3f();
-  }
-}
-
-Vec3f Quaternion3f::getRow(std::size_t i) const
-{
-  switch(i)
-  {
-  case 0:
-    return Vec3f(data[0] * data[0] + data[1] * data[1] - data[2] * data[2] - data[3] * data[3],
-                 2 * (data[0] * data[3] + data[1] * data[2]),
-                 2 * (data[1] * data[3] - data[0] * data[2]));
-  case 1:
-    return Vec3f(2 * (data[1] * data[2] - data[0] * data[3]),
-                 data[0] * data[0] - data[1] * data[1] + data[2] * data[2] - data[3] * data[3],
-                 2 * (data[2] * data[3] + data[0] * data[1]));
-  case 2:
-    return Vec3f(2 * (data[1] * data[3] + data[0] * data[2]),
-                 2 * (data[2] * data[3] - data[0] * data[1]),
-                 data[0] * data[0] - data[1] * data[1] - data[2] * data[2] + data[3] * data[3]);
-  default:
-    return Vec3f();
-  }
-}
-
-
 const Matrix3f& Transform3f::getRotationInternal() const
 {
   boost::mutex::scoped_lock slock(const_cast<boost::mutex&>(lock_));

src/narrowphase/gjk_libccd.cpp

@@ -509,7 +509,7 @@ static void shapeToGJK(const ShapeBase& s, const Transform3f& tf, ccd_obj_t* o)
   const Quaternion3f& q = tf.getQuatRotation();
   const Vec3f& T = tf.getTranslation();
   ccdVec3Set(&o->pos, T[0], T[1], T[2]);
-  ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
+  ccdQuatSet(&o->rot, q.x(), q.y(), q.z(), q.w());
   ccdQuatInvert2(&o->rot_inv, &o->rot);
 }
 
@@ -1016,7 +1016,7 @@ void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, cons
   const Quaternion3f& q = tf.getQuatRotation();
   const Vec3f& T = tf.getTranslation();
   ccdVec3Set(&o->pos, T[0], T[1], T[2]);
-  ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
+  ccdQuatSet(&o->rot, q.x(), q.y(), q.z(), q.w());
   ccdQuatInvert2(&o->rot_inv, &o->rot);
 
   return o;

test/CMakeLists.txt

@@ -32,11 +32,11 @@ add_fcl_test(test_fcl_frontlist test_fcl_frontlist.cpp test_fcl_utility.cpp)
 #add_fcl_test(test_fcl_math test_fcl_math.cpp test_fcl_utility.cpp)
 
 add_fcl_test(test_fcl_sphere_capsule test_fcl_sphere_capsule.cpp)
-add_fcl_test(test_fcl_capsule_capsule test_fcl_capsule_capsule.cpp)
-add_fcl_test(test_fcl_box_box_distance test_fcl_box_box_distance.cpp)
+add_fcl_test(test_fcl_capsule_capsule test_fcl_capsule_capsule.cpp test_fcl_utility.cpp)
+add_fcl_test(test_fcl_box_box_distance test_fcl_box_box_distance.cpp test_fcl_utility.cpp)
 add_fcl_test(test_fcl_simple test_fcl_simple.cpp)
-add_fcl_test(test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp)
-add_fcl_test(test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp)
+add_fcl_test(test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp test_fcl_utility.cpp)
+add_fcl_test(test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp test_fcl_utility.cpp)
 #add_fcl_test(test_fcl_obb test_fcl_obb.cpp)
 
 add_fcl_test(test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp)

test/test_fcl_box_box_distance.cpp

@@ -46,10 +46,11 @@
 #include <hpp/fcl/collision_object.h>
 #include <hpp/fcl/shape/geometric_shapes.h>
 
+#include "test_fcl_utility.h"
+
 typedef boost::shared_ptr <fcl::CollisionGeometry> CollisionGeometryPtr_t;
 
 using fcl::Transform3f;
-using fcl::Quaternion3f;
 using fcl::Vec3f;
 using fcl::CollisionObject;
 using fcl::DistanceResult;
@@ -104,7 +105,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2)
   CollisionGeometryPtr_t s2 (new fcl::Box (2, 2, 2));
   static double pi = M_PI;
   Transform3f tf1;
-  Transform3f tf2 (Quaternion3f (cos (pi/8), sin(pi/8)/sqrt(3),
+  Transform3f tf2 (fcl::makeQuat (cos (pi/8), sin(pi/8)/sqrt(3),
 				 sin(pi/8)/sqrt(3), sin(pi/8)/sqrt(3)),
 		   Vec3f(0, 0, 10));
 
@@ -144,9 +145,9 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3)
   CollisionGeometryPtr_t s1 (new fcl::Box (1, 1, 1));
   CollisionGeometryPtr_t s2 (new fcl::Box (1, 1, 1));
   static double pi = M_PI;
-  Transform3f tf1 (Quaternion3f (cos (pi/8), 0, 0, sin (pi/8)),
+  Transform3f tf1 (fcl::makeQuat (cos (pi/8), 0, 0, sin (pi/8)),
 		   Vec3f (-2, 1, .5));
-  Transform3f tf2 (Quaternion3f (cos (pi/8), 0, sin(pi/8),0),
+  Transform3f tf2 (fcl::makeQuat (cos (pi/8), 0, sin(pi/8),0),
 		   Vec3f (2, .5, .5));
 
   CollisionObject o1 (s1, tf1);
@@ -182,7 +183,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3)
   BOOST_CHECK_CLOSE (p2 [2], p2Ref [2], 1e-4);
 
   // Apply the same global transform to both objects and recompute
-  Transform3f tf3 (Quaternion3f (0.435952844074,-0.718287018243,
+  Transform3f tf3 (fcl::makeQuat (0.435952844074,-0.718287018243,
 				 0.310622451066, 0.444435113443),
 		   Vec3f (4, 5, 6));
   tf1 = tf3*tf1;

test/test_fcl_capsule_box_1.cpp

@@ -46,6 +46,8 @@
 #include <hpp/fcl/collision_object.h>
 #include <hpp/fcl/shape/geometric_shapes.h>
 
+#include "test_fcl_utility.h"
+
 BOOST_AUTO_TEST_CASE(distance_capsule_box)
 {
   typedef boost::shared_ptr <fcl::CollisionGeometry> CollisionGeometryPtr_t;
@@ -96,7 +98,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
 
   // Rotate capsule around y axis by pi/2 and move it behind box
   tf1.setTranslation (fcl::Vec3f (-10., 0., 0.));
-  tf1.setQuatRotation (fcl::Quaternion3f (sqrt(2)/2, 0, sqrt(2)/2, 0));
+  tf1.setQuatRotation (fcl::makeQuat (sqrt(2)/2, 0, sqrt(2)/2, 0));
   capsule.setTransform (tf1);
 
   // test distance

test/test_fcl_capsule_box_2.cpp

@@ -39,6 +39,8 @@
 #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))
 #include <boost/test/included/unit_test.hpp>
 
+#include "test_fcl_utility.h"
+
 #include <cmath>
 #include <hpp/fcl/distance.h>
 #include <hpp/fcl/math/transform.h>
@@ -59,7 +61,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
   fcl::DistanceResult distanceResult;
 
   // Rotate capsule around y axis by pi/2 and move it behind box
-  fcl::Transform3f tf1 (fcl::Quaternion3f (sqrt(2)/2, 0, sqrt(2)/2, 0),
+  fcl::Transform3f tf1 (fcl::makeQuat (sqrt(2)/2, 0, sqrt(2)/2, 0),
 			fcl::Vec3f (-10., 0.8, 1.5));
   fcl::Transform3f tf2;
   fcl::CollisionObject capsule (capsuleGeometry, tf1);

test/test_fcl_capsule_capsule.cpp

@@ -47,6 +47,8 @@
 #include <hpp/fcl/collision_object.h>
 #include <hpp/fcl/shape/geometric_shapes.h>
 
+#include "test_fcl_utility.h"
+
 using namespace fcl;
 typedef boost::shared_ptr <CollisionGeometry> CollisionGeometryPtr_t;
 
@@ -139,7 +141,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2)
   CollisionGeometryPtr_t s2 (new Capsule (5, 10));
 
   Transform3f tf1;
-  Transform3f tf2 (Quaternion3f (sqrt (2)/2, 0, sqrt (2)/2, 0),
+  Transform3f tf2 (makeQuat (sqrt (2)/2, 0, sqrt (2)/2, 0),
 		   Vec3f(0,0,25.1));
 
   CollisionObject o1 (s1, tf1);

test/test_fcl_utility.cpp

@@ -454,4 +454,14 @@ std::string getGJKSolverName(GJKSolverType solver_type)
     return std::string("invalid");
 }
 
+Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z)
+{
+  Quaternion3f q;
+  q.w() = w;
+  q.x() = x;
+  q.y() = y;
+  q.z() = z;
+  return q;
+}
+
 }

test/test_fcl_utility.h

@@ -187,6 +187,8 @@ std::string getNodeTypeName(NODE_TYPE node_type);
 
 std::string getGJKSolverName(GJKSolverType solver_type);
 
+Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z);
+
 }
 
 #endif