use a better 3x3 matrix, avoid the previous bad one

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@48 253336fb-580f-4252-a368-f3cef5a2a82b

trunk/fcl/include/fcl/BVH_utility.h

@@ -83,10 +83,10 @@ void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, BVH_REAL r);
 void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty* ucs);
 
 /** \brief Compute the covariance matrix for a set or subset of points. */
-void getCovariance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f M[3]);
+void getCovariance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Matrix3f& M);
 
 /** \brief Compute the covariance matrix for a set or subset of triangles. */
-void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f M[3]);
+void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M);
 
 
 /** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin.

trunk/fcl/include/fcl/OBB.h

@@ -39,6 +39,7 @@
 
 #include "fcl/BVH_internal.h"
 #include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
 
 /** \brief Main namespace */
 namespace fcl
@@ -141,12 +142,12 @@ private:
 
 public:
   /** Kernel check whether two OBB are disjoint */
-  static bool obbDisjoint(const Vec3f B[3], const Vec3f& T, const Vec3f& a, const Vec3f& b);
+  static bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b);
 
 };
 
 
-bool overlap(const Vec3f R0[3], const Vec3f& T0, const OBB& b1, const OBB& b2);
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2);
 
 }
 

trunk/fcl/include/fcl/RSS.h

@@ -39,6 +39,7 @@
 
 #include "fcl/BVH_internal.h"
 #include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
 
 namespace fcl
 {
@@ -159,7 +160,7 @@ public:
   /** \brief distance between two oriented rectangles
    * P and Q (optional return values) are the closest points in the rectangles, both are in the local frame of the first rectangle
    */
-  static BVH_REAL rectDistance(const Vec3f Rab[3], Vec3f const& Tab, const BVH_REAL a[2], const BVH_REAL b[2], Vec3f* P = NULL, Vec3f* Q = NULL);
+  static BVH_REAL rectDistance(const Matrix3f& Rab, const Vec3f& Tab, const BVH_REAL a[2], const BVH_REAL b[2], Vec3f* P = NULL, Vec3f* Q = NULL);
 
 };
 
@@ -167,9 +168,9 @@ public:
  * P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points
  * Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1)
  */
-BVH_REAL distance(const Vec3f R0[3], const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
+BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
 
-bool overlap(const Vec3f R0[3], const Vec3f& T0, const RSS& b1, const RSS& b2);
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2);
 
 
 }

trunk/fcl/include/fcl/collision_object.h

@@ -89,7 +89,7 @@ public:
     return t.getTranslation();
   }
 
-  inline const Vec3f* getRotation() const
+  inline const Matrix3f& getRotation() const
   {
     return t.getRotation();
   }
@@ -99,7 +99,7 @@ public:
     return t.getQuatRotation();
   }
 
-  void setRotation(const Vec3f R[3])
+  void setRotation(const Matrix3f& R)
   {
     t.setRotation(R);
   }
@@ -114,7 +114,7 @@ public:
     t.setQuatRotation(q);
   }
 
-  void setTransform(const Vec3f R[3], const Vec3f& T)
+  void setTransform(const Matrix3f& R, const Vec3f& T)
   {
     t.setTransform(R, T);
   }

trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h

@@ -79,8 +79,8 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
 
   for(unsigned int i = 0; i < points.size(); ++i)
   {
-    Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
-    v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+    Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
+    v = shape.getRotation() * v + shape.getTranslation();
     points[i] = v;
   }
 
@@ -146,8 +146,8 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
 
   for(unsigned int i = 0; i < points.size(); ++i)
   {
-    Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
-    v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+    Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
+    v = shape.getRotation() * v + shape.getTranslation();
     points[i] = v;
   }
 
@@ -221,8 +221,8 @@ void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, unsigned int n_
 
   for(unsigned int i = 0; i < points.size(); ++i)
   {
-    Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
-    v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+    Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
+    v = shape.getRotation() * v + shape.getTranslation();
     points[i] = v;
   }
 
@@ -298,8 +298,8 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
 
   for(unsigned int i = 0; i < points.size(); ++i)
   {
-    Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
-    v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+    Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
+    v = shape.getRotation() * v + shape.getTranslation();
     points[i] = v;
   }
 
@@ -378,8 +378,8 @@ void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, unsigned int
 
   for(unsigned int i = 0; i < points.size(); ++i)
   {
-    Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
-    v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+    Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
+    v = shape.getRotation() * v + shape.getTranslation();
     points[i] = v;
   }
 
@@ -453,8 +453,8 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, unsigned int tot =
 
   for(unsigned int i = 0; i < points.size(); ++i)
   {
-    Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
-    v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+    Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
+    v = shape.getRotation() * v + shape.getTranslation();
     points[i] = v;
   }
 

trunk/fcl/include/fcl/geometric_shapes.h

@@ -53,26 +53,20 @@ public:
   /** \brief Default Constructor */
   ShapeBase()
   {
-    Rloc[0][0] = 1;
-    Rloc[1][1] = 1;
-    Rloc[2][2] = 1;
+    Rloc.setIdentity();
   }
 
   /** \brief Set the local frame of the shape */
-  void setLocalTransform(const Vec3f R_[3], const Vec3f& T_)
+  void setLocalTransform(const Matrix3f& R_, const Vec3f& T_)
   {
-    Rloc[0] = R_[0];
-    Rloc[1] = R_[1];
-    Rloc[2] = R_[2];
+    Rloc = R_;
     Tloc = T_;
   }
 
   /** \brief Set the local orientation of the shape */
-  void setLocalRotation(const Vec3f R[3])
+  void setLocalRotation(const Matrix3f& R)
   {
-    Rloc[0] = R[0];
-    Rloc[1] = R[1];
-    Rloc[2] = R[2];
+    Rloc = R;
   }
 
   /** \brief Set the local position of the shape */
@@ -82,22 +76,17 @@ public:
   }
 
   /** \brief Append additional transform to the local transform */
-  void appendLocalTransform(const Vec3f R[3], const Vec3f& T)
+  void appendLocalTransform(const Matrix3f& R, const Vec3f& T)
   {
-    Vec3f R0[3];
-    for(int i = 0; i < 3; ++i)
-      R0[i] = Rloc[i];
-    matMulMat(R, R0, Rloc);
-    Tloc = matMulVec(R, Tloc) + T;
+    Rloc = R * Rloc;
+    Tloc = R * Tloc + T;
   }
 
   /** \brief Get local transform */
-  void getLocalTransform(Vec3f R[3], Vec3f& T) const
+  void getLocalTransform(Matrix3f& R, Vec3f& T) const
   {
     T = Tloc;
-    R[0] = Rloc[0];
-    R[1] = Rloc[1];
-    R[2] = Rloc[2];
+    R = Rloc;
   }
 
   /** \brief Get local position */
@@ -107,7 +96,7 @@ public:
   }
 
   /** \brief Get local orientation */
-  inline const Vec3f* getLocalRotation() const
+  inline const Matrix3f& getLocalRotation() const
   {
     return Rloc;
   }
@@ -117,7 +106,7 @@ public:
 
 protected:
 
-  Vec3f Rloc[3];
+  Matrix3f Rloc;
   Vec3f Tloc;
 };
 

trunk/fcl/include/fcl/geometric_shapes_intersect.h

@@ -145,7 +145,7 @@ GJKCenterFunction triGetCenterFunction();
 
 void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3);
 
-void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f R[3], const Vec3f& T);
+void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T);
 
 void triDeleteGJKObject(void* o);
 
@@ -187,7 +187,7 @@ bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const
 
 /** \brief intersection checking between one shape and a triangle with transformation */
 template<typename S>
-bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f R[3], const Vec3f& T,
+bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
                             Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
 {
   void* o1 = GJKInitializer<S>::createGJKObject(s);

trunk/fcl/include/fcl/intersect.h

@@ -137,7 +137,7 @@ public:
 
   static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
                                  const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
-                                 const Vec3f R[3], const Vec3f& T,
+                                 const Matrix3f& R, const Vec3f& T,
                                  Vec3f* contact_points = NULL,
                                  unsigned int* num_contact_points = NULL,
                                  BVH_REAL* penetration_depth = NULL,
@@ -151,14 +151,14 @@ public:
 
   static BVH_REAL intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
                                         Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
-                                        const Vec3f R[3], const Vec3f& T, const CloudClassifierParam& solver, bool scaling = true);
+                                        const Matrix3f& R, const Vec3f& T, const CloudClassifierParam& solver, bool scaling = true);
 
   static BVH_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
                                                 const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3);
 
   static BVH_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
                                                 const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
-                                                const Vec3f R[3], const Vec3f& T);
+                                                const Matrix3f& R, const Vec3f& T);
 #endif
 
 private:
@@ -320,12 +320,12 @@ public:
    *  The returned P and Q are both in the coordinate of the first triangle's coordinate
    */
   static BVH_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
-                              const Vec3f R[3], const Vec3f& Tl,
+                              const Matrix3f& R, const Vec3f& Tl,
                               Vec3f& P, Vec3f& Q);
 
   static BVH_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
                               const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
-                              const Vec3f R[3], const Vec3f& Tl,
+                              const Matrix3f& R, const Vec3f& Tl,
                               Vec3f& P, Vec3f& Q);
 };
 

trunk/fcl/include/fcl/matrix_3f.h

@@ -54,7 +54,7 @@ namespace fcl
              BVH_REAL zx, BVH_REAL zy, BVH_REAL zz)
     {
       setValue(xx, xy, xz,
-               yz, yy, yz,
+               yx, yy, yz,
                zx, zy, zz);
     }
 
@@ -95,6 +95,8 @@ namespace fcl
 
     Matrix3f& operator *= (const Matrix3f& other);
 
+    Matrix3f& operator += (BVH_REAL c);
+
     void setIdentity()
     {
       setValue((BVH_REAL)1.0, (BVH_REAL)0.0, (BVH_REAL)0.0,
@@ -102,6 +104,11 @@ namespace fcl
                (BVH_REAL)0.0, (BVH_REAL)0.0, (BVH_REAL)1.0);
     }
 
+    void setZero()
+    {
+      setValue((BVH_REAL)0.0);
+    }
+
     static const Matrix3f& getIdentity()
     {
       static const Matrix3f I((BVH_REAL)1.0, (BVH_REAL)0.0, (BVH_REAL)0.0,
@@ -113,6 +120,7 @@ namespace fcl
     BVH_REAL determinant() const;
     Matrix3f transpose() const;
     Matrix3f inverse() const;
+    Matrix3f abs() const;
 
     Matrix3f transposeTimes(const Matrix3f& m) const;
     Matrix3f timesTranspose(const Matrix3f& m) const;
@@ -146,15 +154,20 @@ namespace fcl
       return v_[0][2] * v[0] + v_[1][2] * v[1] + v_[2][2] * v[2];
     }
 
-    inline Matrix3f& setValue(BVH_REAL xx, BVH_REAL xy, BVH_REAL xz,
-                              BVH_REAL yx, BVH_REAL yy, BVH_REAL yz,
-                              BVH_REAL zx, BVH_REAL zy, BVH_REAL zz)
+    inline void setValue(BVH_REAL xx, BVH_REAL xy, BVH_REAL xz,
+                         BVH_REAL yx, BVH_REAL yy, BVH_REAL yz,
+                         BVH_REAL zx, BVH_REAL zy, BVH_REAL zz)
     {
       v_[0].setValue(xx, xy, xz);
       v_[1].setValue(yx, yy, yz);
       v_[2].setValue(zx, zy, zz);
+    }
 
-      return *this;
+    inline void setValue(BVH_REAL x)
+    {
+      v_[0].setValue(x);
+      v_[1].setValue(x);
+      v_[2].setValue(x);
     }
   };
 

trunk/fcl/include/fcl/motion.h

@@ -39,6 +39,7 @@
 #define FCL_MOTION_H
 
 #include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
 #include "fcl/RSS.h"
 #include "fcl/transform.h"
 #include "fcl/motion_base.h"
@@ -138,20 +139,15 @@ public:
   }
 
   /** \brief Get the rotation and translation in current step */
-  void getCurrentTransform(Vec3f R[3], Vec3f& T) const
+  void getCurrentTransform(Matrix3f& R, Vec3f& T) const
   {
-    for(int i = 0; i < 3; ++i)
-    {
-      R[i] = tf.getRotation()[i];
-    }
-
+    R = tf.getRotation();
     T = tf.getTranslation();
   }
 
-  void getCurrentRotation(Vec3f R[3]) const
+  void getCurrentRotation(Matrix3f& R) const
   {
-    for(int i = 0; i < 3; ++i)
-      R[i] = tf.getRotation()[i];
+    R = tf.getRotation();
   }
 
   void getCurrentTranslation(Vec3f& T) const
@@ -333,8 +329,8 @@ public:
   }
 
   /** \brief Construct motion from the initial rotation/translation and goal rotation/translation */
-  ScrewMotion(const Vec3f R1[3], const Vec3f& T1,
-              const Vec3f R2[3], const Vec3f& T2)
+  ScrewMotion(const Matrix3f& R1, const Vec3f& T1,
+              const Matrix3f& R2, const Vec3f& T2)
   {
     tf1 = SimpleTransform(R1, T1);
     tf2 = SimpleTransform(R2, T2);
@@ -384,20 +380,15 @@ public:
   }
 
   /** \brief Get the rotation and translation in current step */
-  void getCurrentTransform(Vec3f R[3], Vec3f& T) const
+  void getCurrentTransform(Matrix3f& R, Vec3f& T) const
   {
-    for(int i = 0; i < 3; ++i)
-    {
-      R[i] = tf.getRotation()[i];
-    }
-
+    R = tf.getRotation();
     T = tf.getTranslation();
   }
 
-  void getCurrentRotation(Vec3f R[3]) const
+  void getCurrentRotation(Matrix3f& R) const
   {
-    for(int i = 0; i < 3; ++i)
-      R[i] = tf.getRotation()[i];
+    R = tf.getRotation();
   }
 
   void getCurrentTranslation(Vec3f& T) const
@@ -491,7 +482,7 @@ public:
   InterpMotion()
   {
     /** Default angular velocity is zero */
-    angular_axis = Vec3f(1, 0, 0);
+    angular_axis.setValue(1, 0, 0);
     angular_vel = 0;
 
     /** Default reference point is local zero point */
@@ -500,8 +491,8 @@ public:
   }
 
   /** \brief Construct motion from the initial rotation/translation and goal rotation/translation */
-  InterpMotion(const Vec3f R1[3], const Vec3f& T1,
-               const Vec3f R2[3], const Vec3f& T2)
+  InterpMotion(const Matrix3f& R1, const Vec3f& T1,
+               const Matrix3f& R2, const Vec3f& T2)
   {
     tf1 = SimpleTransform(R1, T1);
     tf2 = SimpleTransform(R2, T2);
@@ -517,8 +508,8 @@ public:
 
   /** \brief Construct motion from the initial rotation/translation and goal rotation/translation related to some rotation center
    */
-  InterpMotion(const Vec3f R1[3], const Vec3f& T1,
-               const Vec3f R2[3], const Vec3f& T2,
+  InterpMotion(const Matrix3f& R1, const Vec3f& T1,
+               const Matrix3f& R2, const Vec3f& T2,
                const Vec3f& O)
   {
     tf1 = SimpleTransform(R1, T1);
@@ -574,20 +565,15 @@ public:
   }
 
   /** \brief Get the rotation and translation in current step */
-  void getCurrentTransform(Vec3f R[3], Vec3f& T) const
+  void getCurrentTransform(Matrix3f& R, Vec3f& T) const
   {
-    for(int i = 0; i < 3; ++i)
-    {
-      R[i] = tf.getRotation()[i];
-    }
-
+    R = tf.getRotation();
     T = tf.getTranslation();
   }
 
-  void getCurrentRotation(Vec3f R[3]) const
+  void getCurrentRotation(Matrix3f& R) const
   {
-    for(int i = 0; i < 3; ++i)
-      R[i] = tf.getRotation()[i];
+    R = tf.getRotation();
   }
 
   void getCurrentTranslation(Vec3f& T) const

trunk/fcl/include/fcl/motion_base.h

@@ -39,6 +39,7 @@
 #define FCL_MOTION_BASE_H
 
 #include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
 #include "fcl/RSS.h"
 namespace fcl
 {
@@ -59,9 +60,9 @@ public:
   virtual BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const = 0;
 
   /** \brief Get the rotation and translation in current step */
-  virtual void getCurrentTransform(Vec3f R[3], Vec3f& T) const = 0;
+  virtual void getCurrentTransform(Matrix3f& R, Vec3f& T) const = 0;
 
-  virtual void getCurrentRotation(Vec3f R[3]) const = 0;
+  virtual void getCurrentRotation(Matrix3f& R) const = 0;
 
   virtual void getCurrentTranslation(Vec3f& T) const = 0;
 };

trunk/fcl/include/fcl/primitive.h

@@ -39,6 +39,7 @@
 
 #include "fcl/BVH_internal.h"
 #include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
 
 /** \brief Main namespace */
 namespace fcl
@@ -49,10 +50,8 @@ struct Uncertainty
 {
   Uncertainty() {}
 
-  Uncertainty(Vec3f Sigma_[3])
+  Uncertainty(Matrix3f& Sigma_) : Sigma(Sigma_)
   {
-    for(int i = 0; i < 3; ++i)
-      Sigma[i] = Sigma_[i];
     preprocess();
   }
 
@@ -74,14 +73,7 @@ struct Uncertainty
     }
 
 
-    for(int i = 0; i < 3; ++i)
-    {
-      for(int j = 0; j < 3; ++j)
-      {
-        Sigma[i][j] = 0;
-      }
-    }
-
+    Sigma.setZero();
     for(int i = 0; i < 3; ++i)
     {
       for(int j = 0; j < 3; ++j)
@@ -94,7 +86,7 @@ struct Uncertainty
   }
 
   /** \brief Variation matrix for uncertainty */
-  Vec3f Sigma[3];
+  Matrix3f Sigma;
 
   /** \brief Variations along the eigen axes */
   BVH_REAL sigma[3];

trunk/fcl/include/fcl/simple_setup.h

@@ -75,7 +75,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& mode
     for(int i = 0; i < model1.num_vertices; ++i)
     {
       Vec3f& p = model1.vertices[i];
-      Vec3f new_v = matMulVec(model1.getRotation(), p) + model1.getTranslation();
+      Vec3f new_v = model1.getRotation() * p + model1.getTranslation();
       vertices_transformed[i] = new_v;
     }
 
@@ -114,7 +114,7 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV>& node, const S& model1, B
     for(int i = 0; i < model2.num_vertices; ++i)
     {
       Vec3f& p = model2.vertices[i];
-      Vec3f new_v = matMulVec(model2.getRotation(), p) + model2.getTranslation();
+      Vec3f new_v = model2.getRotation() * p  + model2.getTranslation();
       vertices_transformed[i] = new_v;
     }
 
@@ -152,8 +152,7 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>
   node.exhaustive = exhaustive;
   node.enable_contact = enable_contact;
 
-  for(int i = 0; i < 3; ++i)
-    node.R[i] = model1.getRotation()[i];
+  node.R = model1.getRotation();
   node.T = model1.getTranslation();
 
   return true;
@@ -176,8 +175,7 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBB<S>& node, const S& model1, co
   node.exhaustive = exhaustive;
   node.enable_contact = enable_contact;
 
-  for(int i = 0; i < 3; ++i)
-    node.R[i] = model2.getRotation()[i];
+  node.R = model2.getRotation();
   node.T = model2.getTranslation();
 
   return true;
@@ -198,7 +196,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
     for(int i = 0; i < model1.num_vertices; ++i)
     {
       Vec3f& p = model1.vertices[i];
-      Vec3f new_v = matMulVec(model1.getRotation(), p) + model1.getTranslation();
+      Vec3f new_v = model1.getRotation() *  p + model1.getTranslation();
       vertices_transformed1[i] = new_v;
     }
 
@@ -215,7 +213,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
     for(int i = 0; i < model2.num_vertices; ++i)
     {
       Vec3f& p = model2.vertices[i];