From cd66479c62f8dc1a5835e6820974ede6493df14e Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Fri, 6 Jan 2012 04:13:43 +0000
Subject: [PATCH] change the interface of collide: separate geometry and pose.
 See collide.h and related files

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@55 253336fb-580f-4252-a368-f3cef5a2a82b
---
 .../src/environmentFCL.cpp                    |  85 +-
 trunk/fcl/include/fcl/BVH_model.h             |   2 +-
 trunk/fcl/include/fcl/collision.h             |   5 +
 trunk/fcl/include/fcl/collision_data.h        |   8 +-
 trunk/fcl/include/fcl/collision_func_matrix.h |   2 +-
 trunk/fcl/include/fcl/collision_object.h      | 175 +++-
 .../include/fcl/conservative_advancement.h    |   4 +-
 .../fcl/geometric_shape_to_BVH_model.h        |  24 +-
 trunk/fcl/include/fcl/geometric_shapes.h      |   2 +-
 .../include/fcl/geometric_shapes_intersect.h  |  32 +-
 .../include/fcl/geometric_shapes_utility.h    |  42 +-
 trunk/fcl/include/fcl/motion.h                |  15 +
 trunk/fcl/include/fcl/motion_base.h           |   3 +
 trunk/fcl/include/fcl/simple_setup.h          | 190 ++--
 trunk/fcl/include/fcl/traversal_node_base.h   |   5 +
 .../include/fcl/traversal_node_bvh_shape.h    |  24 +-
 trunk/fcl/include/fcl/traversal_node_shapes.h |   5 +-
 trunk/fcl/src/BVH_model.cpp                   |   6 +-
 trunk/fcl/src/collision.cpp                   |  81 +-
 trunk/fcl/src/collision_func_matrix.cpp       | 628 +++----------
 trunk/fcl/src/conservative_advancement.cpp    |  24 +-
 trunk/fcl/src/geometric_shapes_intersect.cpp  |  54 +-
 trunk/fcl/src/geometric_shapes_utility.cpp    |  99 +-
 trunk/fcl/src/intersect.cpp                   |  41 +-
 trunk/fcl/src/simple_setup.cpp                |  57 +-
 trunk/fcl/test/test_core_broad_phase.cpp      |  12 +-
 trunk/fcl/test/test_core_collision.cpp        |  43 +-
 trunk/fcl/test/test_core_collision_point.cpp  |  54 +-
 ..._core_collision_shape_mesh_consistency.cpp | 852 +++++++++---------
 .../test_core_conservative_advancement.cpp    |  32 +-
 .../test/test_core_continuous_collision.cpp   |  12 +-
 .../fcl/test/test_core_deformable_object.cpp  |  20 +-
 trunk/fcl/test/test_core_distance.cpp         |  29 +-
 trunk/fcl/test/test_core_front_list.cpp       |  39 +-
 trunk/fcl/test/test_core_geometric_shapes.cpp | 323 +++----
 trunk/fcl/test/timing_test.cpp                |   8 +-
 36 files changed, 1505 insertions(+), 1532 deletions(-)

diff --git a/trunk/collision_space_fcl/src/environmentFCL.cpp b/trunk/collision_space_fcl/src/environmentFCL.cpp
index 60a2aa1c..14453697 100644
--- a/trunk/collision_space_fcl/src/environmentFCL.cpp
+++ b/trunk/collision_space_fcl/src/environmentFCL.cpp
@@ -266,9 +266,92 @@ fcl::CollisionObject* EnvironmentModelFCL::createGeom(const shapes::Shape *shape
       ROS_FATAL_STREAM("This shape type is not supported using FCL yet");
   }
 
-  return g;
+  fcl::CollisionObject* co = new fcl::CollisionObject(g);
+  return co;
 }
 
+/*
+fcl::CollisionObject* EnvironmentModelFCL::createGeom(const shapes::Shape *shape, double scale, double padding)
+{
+  fcl::CollisionObject* g = NULL;
+  switch(shape->type)
+  {
+    case shapes::SPHERE:
+    {
+      g = new fcl::Sphere(static_cast<const shapes::Sphere*>(shape)->radius * scale + padding);
+    }
+    break;
+    case shapes::BOX:
+    {
+      const double *size = static_cast<const shapes::Box*>(shape)->size;
+      g = new fcl::Box(size[0] * scale + padding * 2.0, size[1] * scale + padding * 2.0, size[2] * scale + padding * 2.0);
+    }
+    break;
+    case shapes::CYLINDER:
+    {
+      g = new fcl::Cylinder(static_cast<const shapes::Cylinder*>(shape)->radius * scale + padding,
+                            static_cast<const shapes::Cylinder*>(shape)->length * scale + padding * 2.0);
+    }
+    break;
+    case shapes::MESH:
+    {
+      fcl::BVHModel<fcl::OBB>* gobb = new fcl::BVHModel<fcl::OBB>();
+      const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
+      if(mesh->vertexCount > 0 && mesh->triangleCount > 0)
+      {
+        std::vector<fcl::Triangle> tri_indices(mesh->triangleCount);
+        for(unsigned int i = 0; i < mesh->triangleCount; ++i)
+          tri_indices[i] = fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]);
+
+        std::vector<fcl::Vec3f> points(mesh->vertexCount);
+        double sx = 0.0, sy = 0.0, sz = 0.0;
+        for(unsigned int i = 0; i < mesh->vertexCount; ++i)
+        {
+          points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
+          sx += points[i][0];
+          sy += points[i][1];
+          sz += points[i][2];
+        }
+        // the center of the mesh
+        sx /= (double)mesh->vertexCount;
+        sy /= (double)mesh->vertexCount;
+        sz /= (double)mesh->vertexCount;
+
+        // scale the mesh
+        for(unsigned int i = 0; i < mesh->vertexCount; ++i)
+        {
+          // vector from center to the vertex
+          double dx = points[i][0] - sx;
+          double dy = points[i][1] - sy;
+          double dz = points[i][2] - sz;
+
+          // length of vector
+          //double norm = sqrt(dx * dx + dy * dy + dz * dz);
+
+          double ndx = ((dx > 0) ? dx+padding : dx-padding);
+          double ndy = ((dy > 0) ? dy+padding : dy-padding);
+          double ndz = ((dz > 0) ? dz+padding : dz-padding);
+
+          // the new distance of the vertex from the center
+          //double fact = scale + padding/norm;
+          points[i] = fcl::Vec3f(sx + ndx, sy + ndy, sz + ndz);
+        }
+
+        gobb->beginModel();
+        gobb->addSubModel(points, tri_indices);
+        gobb->endModel();
+        gobb->computeLocalAABB();
+        g = gobb;
+      }
+    }
+    break;
+    default:
+      ROS_FATAL_STREAM("This shape type is not supported using FCL yet");
+  }
+
+  return g;
+}
+*/
 
 void EnvironmentModelFCL::updateGeom(fcl::CollisionObject* geom,  const btTransform &pose) const
 {
diff --git a/trunk/fcl/include/fcl/BVH_model.h b/trunk/fcl/include/fcl/BVH_model.h
index a1489bb0..38aaa40e 100644
--- a/trunk/fcl/include/fcl/BVH_model.h
+++ b/trunk/fcl/include/fcl/BVH_model.h
@@ -54,7 +54,7 @@ namespace fcl
 
 /** \brief A class describing the bounding hierarchy of a mesh model */
 template<typename BV>
-class BVHModel : public CollisionObject
+class BVHModel : public CollisionGeometry
 {
 private:
   int num_tris_allocated;
diff --git a/trunk/fcl/include/fcl/collision.h b/trunk/fcl/include/fcl/collision.h
index 44f4375e..2b53a291 100644
--- a/trunk/fcl/include/fcl/collision.h
+++ b/trunk/fcl/include/fcl/collision.h
@@ -54,6 +54,11 @@ namespace fcl
 int collide(const CollisionObject* o1, const CollisionObject* o2,
              int num_max_contacts, bool exhaustive, bool enable_contact,
              std::vector<Contact>& contacts);
+
+int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
+            const CollisionGeometry* o2, const SimpleTransform& tf2,
+            int num_max_contacts, bool exhaustive, bool enable_contact,
+            std::vector<Contact>& contacts);
 }
 
 #endif
diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index 164ac241..c39e1151 100644
--- a/trunk/fcl/include/fcl/collision_data.h
+++ b/trunk/fcl/include/fcl/collision_data.h
@@ -14,8 +14,8 @@ struct Contact
   BVH_REAL penetration_depth;
   Vec3f normal;
   Vec3f pos;
-  const CollisionObject* o1;
-  const CollisionObject* o2;
+  const CollisionGeometry* o1;
+  const CollisionGeometry* o2;
   int b1;
   int b2;
 
@@ -27,7 +27,7 @@ struct Contact
     b2 = -1;
   }
 
-  Contact(const CollisionObject* o1_, const CollisionObject* o2_, int b1_, int b2_)
+  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
   {
     o1 = o1_;
     o2 = o2_;
@@ -35,7 +35,7 @@ struct Contact
     b2 = b2_;
   }
 
-  Contact(const CollisionObject* o1_, const CollisionObject* o2_, int b1_, int b2_,
+  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_,
           const Vec3f& pos_, const Vec3f& normal_, BVH_REAL depth_)
   {
     o1 = o1_;
diff --git a/trunk/fcl/include/fcl/collision_func_matrix.h b/trunk/fcl/include/fcl/collision_func_matrix.h
index 9fdb2649..32582fb3 100644
--- a/trunk/fcl/include/fcl/collision_func_matrix.h
+++ b/trunk/fcl/include/fcl/collision_func_matrix.h
@@ -46,7 +46,7 @@ namespace fcl
 {
 
 
-typedef int (*CollisionFunc)(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
+typedef int (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
 
 
 struct CollisionFunctionMatrix
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index 48bcee67..9996a4c7 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -40,6 +40,7 @@
 
 #include "fcl/AABB.h"
 #include "fcl/transform.h"
+#include <boost/shared_ptr.hpp>
 
 namespace fcl
 {
@@ -49,11 +50,181 @@ enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM};
 enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
                 GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE};
 
-/** \brief Base class for all objects participating in collision */
+class CollisionGeometry
+{
+public:
+  virtual ~CollisionGeometry() {}
+
+  virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
+
+  virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
+
+  virtual void computeLocalAABB() = 0;
+
+  /** AABB center in local coordinate */
+  Vec3f aabb_center;
+
+  /** AABB radius */
+  BVH_REAL aabb_radius;
+};
+
 class CollisionObject
 {
 public:
-  virtual ~CollisionObject() {}
+  CollisionObject(CollisionGeometry* cgeom_)
+  {
+    cgeom.reset(cgeom_);
+    computeAABB();
+  }
+
+  CollisionObject(CollisionGeometry* cgeom_, const SimpleTransform& tf)
+  {
+    cgeom.reset(cgeom_);
+    t = tf;
+    computeAABB();
+  }
+
+  CollisionObject(CollisionGeometry* cgeom_, const Matrix3f& R, const Vec3f& T)
+  {
+    cgeom.reset(cgeom_);
+    t = SimpleTransform(R, T);
+    computeAABB();
+  }
+
+  CollisionObject()
+  {
+  }
+
+  ~CollisionObject()
+  {
+  }
+
+  OBJECT_TYPE getObjectType() const
+  {
+    return cgeom->getObjectType();
+  }
+
+  NODE_TYPE getNodeType() const
+  {
+    return cgeom->getNodeType();
+  }
+
+  inline const AABB& getAABB() const
+  {
+    return aabb;
+  }
+
+  inline void computeAABB()
+  {
+    Vec3f center = t.transform(cgeom->aabb_center);
+    Vec3f delta(cgeom->aabb_radius, cgeom->aabb_radius, cgeom->aabb_radius);
+    aabb.min_ = center - delta;
+    aabb.max_ = center + delta;
+  }
+
+  inline void computeAABB2()
+  {
+    Vec3f center = t.transform(cgeom->aabb_center);
+    // compute new r1, r2, r3
+  }
+
+  void* getUserData() const
+  {
+    return user_data;
+  }
+
+  void setUserData(void *data)
+  {
+    user_data = data;
+  }
+
+  inline const Vec3f& getTranslation() const
+  {
+    return t.getTranslation();
+  }
+
+  inline const Matrix3f& getRotation() const
+  {
+    return t.getRotation();
+  }
+
+  inline const SimpleQuaternion& getQuatRotation() const
+  {
+    return t.getQuatRotation();
+  }
+
+  inline const SimpleTransform& getTransform() const
+  {
+    return t;
+  }
+
+  void setRotation(const Matrix3f& R)
+  {
+    t.setRotation(R);
+  }
+
+  void setTranslation(const Vec3f& T)
+  {
+    t.setTranslation(T);
+  }
+
+  void setQuatRotation(const SimpleQuaternion& q)
+  {
+    t.setQuatRotation(q);
+  }
+
+  void setTransform(const Matrix3f& R, const Vec3f& T)
+  {
+    t.setTransform(R, T);
+  }
+
+  void setTransform(const SimpleQuaternion& q, const Vec3f& T)
+  {
+    t.setTransform(q, T);
+  }
+
+  void setTransform(const SimpleTransform& tf)
+  {
+    t = tf;
+  }
+
+  bool isIdentityTransform() const
+  {
+    return t.isIdentity();
+  }
+
+  void setIdentityTransform()
+  {
+    t.setIdentity();
+  }
+
+  const CollisionGeometry* getCollisionGeometry() const
+  {
+    return cgeom.get();
+  }
+
+protected:
+
+  // const CollisionGeometry* cgeom;
+  boost::shared_ptr<CollisionGeometry> cgeom;
+
+  SimpleTransform t;
+
+  /** AABB in global coordinate */
+  mutable AABB aabb;
+
+  /** pointer to user defined data specific to this object */
+  void *user_data;
+};
+
+
+
+
+/** \brief Base class for all objects participating in collision */
+class CollisionObject2
+{
+public:
+  virtual ~CollisionObject2() {}
 
   virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
 
diff --git a/trunk/fcl/include/fcl/conservative_advancement.h b/trunk/fcl/include/fcl/conservative_advancement.h
index 0fe7b2d3..9ef9f322 100644
--- a/trunk/fcl/include/fcl/conservative_advancement.h
+++ b/trunk/fcl/include/fcl/conservative_advancement.h
@@ -48,9 +48,9 @@ namespace fcl
 {
 
 template<typename BV>
-int conservativeAdvancement(const CollisionObject* o1,
+int conservativeAdvancement(const CollisionGeometry* o1,
                             MotionBase<BV>* motion1,
-                            const CollisionObject* o2,
+                            const CollisionGeometry* o2,
                             MotionBase<BV>* motion2,
                             int num_max_contacts, bool exhaustive, bool enable_contact,
                             std::vector<Contact>& contacts,
diff --git a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h b/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
index 914708fe..563a1bf3 100644
--- a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
+++ b/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
@@ -48,7 +48,7 @@ namespace fcl
 
 /** \brief Generate BVH model from box */
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box& shape)
+void generateBVHModel(BVHModel<BV>& model, const Box& shape, const SimpleTransform& pose = SimpleTransform())
 {
   double a = shape.side[0];
   double b = shape.side[1];
@@ -80,7 +80,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
   for(unsigned int i = 0; i < points.size(); ++i)
   {
     Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
-    v = shape.getRotation() * v + shape.getTranslation();
+    v = pose.transform(v);
     points[i] = v;
   }
 
@@ -92,7 +92,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
 
 /** Generate BVH model from sphere */
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg = 16, unsigned int ring = 16)
+void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int seg = 16, unsigned int ring = 16)
 {
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
@@ -147,7 +147,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
   for(unsigned int i = 0; i < points.size(); ++i)
   {
     Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
-    v = shape.getRotation() * v + shape.getTranslation();
+    v = pose.transform(v);
     points[i] = v;
   }
 
@@ -162,7 +162,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
  * then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
  */
 template<typename BV>
-void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, unsigned int n_faces_for_unit_sphere)
+void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int n_faces_for_unit_sphere = 100)
 {
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
@@ -222,7 +222,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, unsigned int n_
   for(unsigned int i = 0; i < points.size(); ++i)
   {
     Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
-    v = shape.getRotation() * v + shape.getTranslation();
+    v = pose.transform(v);
     points[i] = v;
   }
 
@@ -235,7 +235,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, unsigned int n_
 
 /** \brief Generate BVH model from cylinder */
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int tot = 16)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int tot = 16)
 {
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
@@ -299,7 +299,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
   for(unsigned int i = 0; i < points.size(); ++i)
   {
     Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
-    v = shape.getRotation() * v + shape.getTranslation();
+    v = pose.transform(v);
     points[i] = v;
   }
 
@@ -314,7 +314,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
  * larger radius, the number of circle split number is r * tot.
  */
 template<typename BV>
-void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, unsigned int tot_for_unit_cylinder)
+void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int tot_for_unit_cylinder = 100)
 {
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
@@ -379,7 +379,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, unsigned int
   for(unsigned int i = 0; i < points.size(); ++i)
   {
     Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
-    v = shape.getRotation() * v + shape.getTranslation();
+    v = pose.transform(v);
     points[i] = v;
   }
 
@@ -392,7 +392,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, unsigned int
 
 /** \brief Generate BVH model from cone */
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone& shape, unsigned int tot = 16)
+void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int tot = 16)
 {
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
@@ -454,7 +454,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, unsigned int tot =
   for(unsigned int i = 0; i < points.size(); ++i)
   {
     Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
-    v = shape.getRotation() * v + shape.getTranslation();
+    v = pose.transform(v);
     points[i] = v;
   }
 
diff --git a/trunk/fcl/include/fcl/geometric_shapes.h b/trunk/fcl/include/fcl/geometric_shapes.h
index dc9d6ead..7e461a75 100644
--- a/trunk/fcl/include/fcl/geometric_shapes.h
+++ b/trunk/fcl/include/fcl/geometric_shapes.h
@@ -47,7 +47,7 @@ namespace fcl
 {
 
 /** \brief Base class for all basic geometric shapes */
-class ShapeBase : public CollisionObject
+class ShapeBase : public CollisionGeometry
 {
 public:
   /** \brief Default Constructor */
diff --git a/trunk/fcl/include/fcl/geometric_shapes_intersect.h b/trunk/fcl/include/fcl/geometric_shapes_intersect.h
index e53a3a2a..30d49f0c 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_intersect.h
+++ b/trunk/fcl/include/fcl/geometric_shapes_intersect.h
@@ -66,7 +66,7 @@ public:
    * Notice that only local transformation is applied.
    * Gloal transformation are considered later
    */
-  static void* createGJKObject(const T& s) { return NULL; }
+  static void* createGJKObject(const T& s, const SimpleTransform& tf) { return NULL; }
 
   /** \brief Delete GJK object */
   static void deleteGJKObject(void* o) {}
@@ -79,7 +79,7 @@ class GJKInitializer<Cylinder>
 public:
   static GJKSupportFunction getSupportFunction();
   static GJKCenterFunction getCenterFunction();
-  static void* createGJKObject(const Cylinder& s);
+  static void* createGJKObject(const Cylinder& s, const SimpleTransform& tf);
   static void deleteGJKObject(void* o);
 };
 
@@ -90,7 +90,7 @@ class GJKInitializer<Sphere>
 public:
   static GJKSupportFunction getSupportFunction();
   static GJKCenterFunction getCenterFunction();
-  static void* createGJKObject(const Sphere& s);
+  static void* createGJKObject(const Sphere& s, const SimpleTransform& tf);
   static void deleteGJKObject(void* o);
 };
 
@@ -101,7 +101,7 @@ class GJKInitializer<Box>
 public:
   static GJKSupportFunction getSupportFunction();
   static GJKCenterFunction getCenterFunction();
-  static void* createGJKObject(const Box& s);
+  static void* createGJKObject(const Box& s, const SimpleTransform& tf);
   static void deleteGJKObject(void* o);
 };
 
@@ -112,7 +112,7 @@ class GJKInitializer<Capsule>
 public:
   static GJKSupportFunction getSupportFunction();
   static GJKCenterFunction getCenterFunction();
-  static void* createGJKObject(const Capsule& s);
+  static void* createGJKObject(const Capsule& s, const SimpleTransform& tf);
   static void deleteGJKObject(void* o);
 };
 
@@ -123,7 +123,7 @@ class GJKInitializer<Cone>
 public:
   static GJKSupportFunction getSupportFunction();
   static GJKCenterFunction getCenterFunction();
-  static void* createGJKObject(const Cone& s);
+  static void* createGJKObject(const Cone& s, const SimpleTransform& tf);
   static void deleteGJKObject(void* o);
 };
 
@@ -134,7 +134,7 @@ class GJKInitializer<Convex>
 public:
   static GJKSupportFunction getSupportFunction();
   static GJKCenterFunction getCenterFunction();
-  static void* createGJKObject(const Convex& s);
+  static void* createGJKObject(const Convex& s, const SimpleTransform& tf);
   static void deleteGJKObject(void* o);
 };
 
@@ -157,10 +157,12 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
 
 /** intersection checking between two shapes */
 template<typename S1, typename S2>
-bool shapeIntersect(const S1& s1, const S2& s2, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
+                    const S2& s2, const SimpleTransform& tf2,
+                    Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
 {
-  void* o1 = GJKInitializer<S1>::createGJKObject(s1);
-  void* o2 = GJKInitializer<S2>::createGJKObject(s2);
+  void* o1 = GJKInitializer<S1>::createGJKObject(s1, tf1);
+  void* o2 = GJKInitializer<S2>::createGJKObject(s2, tf2);
 
   return GJKCollide(o1, GJKInitializer<S1>::getSupportFunction(), GJKInitializer<S1>::getCenterFunction(),
                     o2, GJKInitializer<S2>::getSupportFunction(), GJKInitializer<S2>::getCenterFunction(),
@@ -172,9 +174,10 @@ bool shapeIntersect(const S1& s1, const S2& s2, Vec3f* contact_points = NULL, BV
 
 /** \brief intersection checking between one shape and a triangle */
 template<typename S>
-bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
+                            const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
 {
-  void* o1 = GJKInitializer<S>::createGJKObject(s);
+  void* o1 = GJKInitializer<S>::createGJKObject(s, tf);
   void* o2 = triCreateGJKObject(P1, P2, P3);
 
   return GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
@@ -187,10 +190,11 @@ 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 Matrix3f& R, const Vec3f& T,
+bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
+                            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);
+  void* o1 = GJKInitializer<S>::createGJKObject(s, tf);
   void* o2 = triCreateGJKObject(P1, P2, P3, R, T);
 
   return GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
diff --git a/trunk/fcl/include/fcl/geometric_shapes_utility.h b/trunk/fcl/include/fcl/geometric_shapes_utility.h
index 8fe6a290..2e286807 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_utility.h
+++ b/trunk/fcl/include/fcl/geometric_shapes_utility.h
@@ -44,71 +44,71 @@
 namespace fcl
 {
   template<typename BV>
-  void computeBV(const Box& s, BV& bv) {}
+  void computeBV(const Box& s, const SimpleTransform& tf, BV& bv) {}
 
   template<typename BV>
-  void computeBV(const Sphere& s, BV& bv) {}
+  void computeBV(const Sphere& s, const SimpleTransform& tf, BV& bv) {}
 
   template<typename BV>
-  void computeBV(const Capsule& s, BV& bv) {}
+  void computeBV(const Capsule& s, const SimpleTransform& tf, BV& bv) {}
 
   template<typename BV>
-  void computeBV(const Cone& s, BV& bv) {}
+  void computeBV(const Cone& s, const SimpleTransform& tf, BV& bv) {}
 
   template<typename BV>
-  void computeBV(const Cylinder& s, BV& bv) {}
+  void computeBV(const Cylinder& s, const SimpleTransform& tf, BV& bv) {}
 
   template<typename BV>
-  void computeBV(const Convex& s, BV& bv) {}
+  void computeBV(const Convex& s, const SimpleTransform& tf, BV& bv) {}
 
   /** the bounding volume for half space back of plane
    * for OBB, it is the plane itself
    */
   template<typename BV>
-  void computeBV(const Plane& s, BV& bv) {}
+  void computeBV(const Plane& s, const SimpleTransform& tf, BV& bv) {}
 
   /** For AABB */
   template<>
-  void computeBV<AABB>(const Box& s, AABB& bv);
+  void computeBV<AABB>(const Box& s, const SimpleTransform& tf, AABB& bv);
 
   template<>
-  void computeBV<AABB>(const Sphere& s, AABB& bv);
+  void computeBV<AABB>(const Sphere& s, const SimpleTransform& tf, AABB& bv);
 
   template<>
-  void computeBV<AABB>(const Capsule& s, AABB& bv);
+  void computeBV<AABB>(const Capsule& s, const SimpleTransform& tf, AABB& bv);
 
   template<>
-  void computeBV<AABB>(const Cone& s, AABB& bv);
+  void computeBV<AABB>(const Cone& s, const SimpleTransform& tf, AABB& bv);
 
   template<>
-  void computeBV<AABB>(const Cylinder& s, AABB& bv);
+  void computeBV<AABB>(const Cylinder& s, const SimpleTransform& tf, AABB& bv);
 
   template<>
-  void computeBV<AABB>(const Convex& s, AABB& bv);
+  void computeBV<AABB>(const Convex& s, const SimpleTransform& tf, AABB& bv);
 
   template<>
-  void computeBV<AABB>(const Plane& s, AABB& bv);
+  void computeBV<AABB>(const Plane& s, const SimpleTransform& tf, AABB& bv);
 
   template<>
-  void computeBV<OBB>(const Box& s, OBB& bv);
+  void computeBV<OBB>(const Box& s, const SimpleTransform& tf, OBB& bv);
 
   template<>
-  void computeBV<OBB>(const Sphere& s, OBB& bv);
+  void computeBV<OBB>(const Sphere& s, const SimpleTransform& tf, OBB& bv);
 
   template<>
-  void computeBV<OBB>(const Capsule& s, OBB& bv);
+  void computeBV<OBB>(const Capsule& s, const SimpleTransform& tf, OBB& bv);
 
   template<>
-  void computeBV<OBB>(const Cone& s, OBB& bv);
+  void computeBV<OBB>(const Cone& s, const SimpleTransform& tf, OBB& bv);
 
   template<>
-  void computeBV<OBB>(const Cylinder& s, OBB& bv);
+  void computeBV<OBB>(const Cylinder& s, const SimpleTransform& tf, OBB& bv);
 
   template<>
-  void computeBV<OBB>(const Convex& s, OBB& bv);
+  void computeBV<OBB>(const Convex& s, const SimpleTransform& tf, OBB& bv);
 
   template<>
-  void computeBV<OBB>(const Plane& s, OBB& bv);
+  void computeBV<OBB>(const Plane& s, const SimpleTransform& tf, OBB& bv);
 
   // TODO: implement computeBV for RSS and KDOP
 }
diff --git a/trunk/fcl/include/fcl/motion.h b/trunk/fcl/include/fcl/motion.h
index 450799db..b8953e29 100644
--- a/trunk/fcl/include/fcl/motion.h
+++ b/trunk/fcl/include/fcl/motion.h
@@ -155,6 +155,11 @@ public:
     T = tf.getTranslation();
   }
 
+  void getCurrentTransform(SimpleTransform& tf_) const
+  {
+    tf_ = tf;
+  }
+
 protected:
   void computeSplineParameter()
   {
@@ -396,6 +401,11 @@ public:
     T = tf.getTranslation();
   }
 
+  void getCurrentTransform(SimpleTransform& tf_) const
+  {
+    tf_ = tf;
+  }
+
 protected:
   void computeScrewParameter()
   {
@@ -581,6 +591,11 @@ public:
     T = tf.getTranslation();
   }
 
+  void getCurrentTransform(SimpleTransform& tf_) const
+  {
+    tf_ = tf;
+  }
+
 protected:
 
   void computeVelocity()
diff --git a/trunk/fcl/include/fcl/motion_base.h b/trunk/fcl/include/fcl/motion_base.h
index 3f17afff..1725ab50 100644
--- a/trunk/fcl/include/fcl/motion_base.h
+++ b/trunk/fcl/include/fcl/motion_base.h
@@ -40,6 +40,7 @@
 
 #include "fcl/vec_3f.h"
 #include "fcl/matrix_3f.h"
+#include "fcl/transform.h"
 #include "fcl/RSS.h"
 namespace fcl
 {
@@ -65,6 +66,8 @@ public:
   virtual void getCurrentRotation(Matrix3f& R) const = 0;
 
   virtual void getCurrentTranslation(Vec3f& T) const = 0;
+
+  virtual void getCurrentTransform(SimpleTransform& tf) const = 0;
 };
 
 
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index 4740c146..801554c2 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -49,33 +49,37 @@ namespace fcl
 
 /** \brief Initialize traversal node for collision between two geometric shapes */
 template<typename S1, typename S2>
-bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, const S2& shape2, bool enable_contact = false)
+bool initialize(ShapeCollisionTraversalNode<S1, S2>& node,
+                const S1& shape1, const SimpleTransform& tf1,
+                const S2& shape2, const SimpleTransform& tf2,
+                bool enable_contact = false)
 {
   node.enable_contact = enable_contact;
   node.model1 = &shape1;
+  node.tf1 = tf1;
   node.model2 = &shape2;
+  node.tf2 = tf2;
   return true;
 }
 
 /** \brief Initialize traversal node for collision between one mesh and one shape, given current object transform */
 template<typename BV, typename S>
-bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& model1, const S& model2,
+bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
+                BVHModel<BV>& model1, SimpleTransform& tf1,
+                const S& model2, const SimpleTransform& tf2,
                 int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
 
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  if(!model1.isIdentityTransform())
+  if(!tf1.isIdentity())
   {
     std::vector<Vec3f> vertices_transformed(model1.num_vertices);
     for(int i = 0; i < model1.num_vertices; ++i)
     {
       Vec3f& p = model1.vertices[i];
-      Vec3f new_v = model1.getRotation() * p + model1.getTranslation();
+      Vec3f new_v = tf1.transform(p);
       vertices_transformed[i] = new_v;
     }
 
@@ -83,9 +87,14 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& mode
     model1.replaceSubModel(vertices_transformed);
     model1.endReplaceModel(use_refit, refit_bottomup);
 
-    model1.setIdentityTransform();
+    tf1.setIdentity();
   }
 
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+
   node.vertices = model1.vertices;
   node.tri_indices = model1.tri_indices;
   node.num_max_contacts = num_max_contacts;
@@ -98,23 +107,22 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& mode
 
 /** \brief Initialize traversal node for collision between one mesh and one shape, given current object transform */
 template<typename S, typename BV>
-bool initialize(ShapeMeshCollisionTraversalNode<S, BV>& node, const S& model1, BVHModel<BV>& model2,
+bool initialize(ShapeMeshCollisionTraversalNode<S, BV>& node,
+                const S& model1, const SimpleTransform& tf1,
+                BVHModel<BV>& model2, SimpleTransform& tf2,
                 int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
 
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  if(!model2.isIdentityTransform())
+  if(!tf2.isIdentity())
   {
     std::vector<Vec3f> vertices_transformed(model2.num_vertices);
     for(int i = 0; i < model2.num_vertices; ++i)
     {
       Vec3f& p = model2.vertices[i];
-      Vec3f new_v = model2.getRotation() * p  + model2.getTranslation();
+      Vec3f new_v = tf2.transform(p);
       vertices_transformed[i] = new_v;
     }
 
@@ -122,9 +130,14 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV>& node, const S& model1, B
     model2.replaceSubModel(vertices_transformed);
     model2.endReplaceModel(use_refit, refit_bottomup);
 
-    model2.setIdentityTransform();
+    tf2.setIdentity();
   }
 
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+
   node.vertices = model2.vertices;
   node.tri_indices = model2.tri_indices;
   node.num_max_contacts = num_max_contacts;
@@ -138,13 +151,18 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV>& node, const S& model1, B
 
 /** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type */
 template<typename S>
-bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>& model1, const S& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node,
+                const BVHModel<OBB>& model1, const SimpleTransform& tf1,
+                const S& model2, const SimpleTransform& tf2,
+                int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices = model1.vertices;
   node.tri_indices = model1.tri_indices;
@@ -152,8 +170,8 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>
   node.exhaustive = exhaustive;
   node.enable_contact = enable_contact;
 
-  node.R = model1.getRotation();
-  node.T = model1.getTranslation();
+  node.R = tf1.getRotation();
+  node.T = tf1.getTranslation();
 
   return true;
 }
@@ -161,13 +179,18 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>
 
 /** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type */
 template<typename S>
-bool initialize(ShapeMeshCollisionTraversalNodeOBB<S>& node, const S& model1, const BVHModel<OBB>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+bool initialize(ShapeMeshCollisionTraversalNodeOBB<S>& node,
+                const S& model1, const SimpleTransform& tf1,
+                const BVHModel<OBB>& model2, const SimpleTransform& tf2,
+                int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
 {
   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices = model2.vertices;
   node.tri_indices = model2.tri_indices;
@@ -175,28 +198,30 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBB<S>& node, const S& model1, co
   node.exhaustive = exhaustive;
   node.enable_contact = enable_contact;
 
-  node.R = model2.getRotation();
-  node.T = model2.getTranslation();
+  node.R = tf2.getRotation();
+  node.T = tf2.getTranslation();
 
   return true;
 }
 
 /** \brief Initialize traversal node for collision between two meshes, given the current transforms */
 template<typename BV>
-bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
+bool initialize(MeshCollisionTraversalNode<BV>& node,
+                BVHModel<BV>& model1, SimpleTransform& tf1,
+                BVHModel<BV>& model2, SimpleTransform& tf2,
                 int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
 
-  if(!model1.isIdentityTransform())
+  if(!tf1.isIdentity())
   {
     std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
     for(int i = 0; i < model1.num_vertices; ++i)
     {
       Vec3f& p = model1.vertices[i];
-      Vec3f new_v = model1.getRotation() *  p + model1.getTranslation();
+      Vec3f new_v = tf1.transform(p);
       vertices_transformed1[i] = new_v;
     }
 
@@ -204,16 +229,16 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
     model1.replaceSubModel(vertices_transformed1);
     model1.endReplaceModel(use_refit, refit_bottomup);
 
-    model1.setIdentityTransform();
+    tf1.setIdentity();
   }
 
-  if(!model2.isIdentityTransform())
+  if(!tf2.isIdentity())
   {
     std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
     for(int i = 0; i < model2.num_vertices; ++i)
     {
       Vec3f& p = model2.vertices[i];
-      Vec3f new_v = model2.getRotation() * p + model2.getTranslation();
+      Vec3f new_v = tf2.transform(p);
       vertices_transformed2[i] = new_v;
     }
 
@@ -221,11 +246,13 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
     model2.replaceSubModel(vertices_transformed2);
     model2.endReplaceModel(use_refit, refit_bottomup);
 
-    model2.setIdentityTransform();
+    tf2.setIdentity();
   }
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -242,10 +269,14 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
 
 
 /** \brief Initialize traversal node for collision between two meshes, specialized for OBB type */
-bool initialize(MeshCollisionTraversalNodeOBB& node, const BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
+bool initialize(MeshCollisionTraversalNodeOBB& node,
+                const BVHModel<OBB>& model1, const SimpleTransform& tf1,
+                const BVHModel<OBB>& model2, const SimpleTransform& tf2,
                 int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
 
-bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
+bool initialize(MeshCollisionTraversalNodeRSS& node,
+                const BVHModel<RSS>& model1, const SimpleTransform& tf1,
+                const BVHModel<RSS>& model2, const SimpleTransform& tf2,
                 int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
 
 
@@ -253,7 +284,9 @@ bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel<RSS>& model1
 
 /** \brief Initialize traversal node for collision between two point clouds, given current transforms */
 template<typename BV, bool use_refit, bool refit_bottomup>
-bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
+bool initialize(PointCloudCollisionTraversalNode<BV>& node,
+                BVHModel<BV>& model1, SimpleTransform& tf1,
+                BVHModel<BV>& model2, SimpleTransform& tf2,
                 BVH_REAL collision_prob_threshold = 0.5,
                 int leaf_size_threshold = 1,
                 int num_max_contacts = 1,
@@ -265,13 +298,13 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
       || !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
     return false;
 
-  if(!model1.isIdentityTransform())
+  if(!tf1.isIdentity())
   {
     std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
     for(int i = 0; i < model1.num_vertices; ++i)
     {
       Vec3f& p = model1.vertices[i];
-      Vec3f new_v = model1.getRotation() * p + model1.getTranslation();
+      Vec3f new_v = tf1.transform(p);
       vertices_transformed1[i] = new_v;
     }
 
@@ -279,16 +312,16 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
     model1.replaceSubModel(vertices_transformed1);
     model1.endReplaceModel(use_refit, refit_bottomup);
 
-    model1.setIdentityTransform();
+    tf1.setIdentity();
   }
 
-  if(!model2.isIdentityTransform())
+  if(!tf2.isIdentity())
   {
     std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
     for(int i = 0; i < model2.num_vertices; ++i)
     {
       Vec3f& p = model2.vertices[i];
-      Vec3f new_v = model2.getRotation() * p + model2.getTranslation();
+      Vec3f new_v = tf2.transform(p);
       vertices_transformed2[i] = new_v;
     }
 
@@ -296,11 +329,13 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
     model2.replaceSubModel(vertices_transformed2);
     model2.endReplaceModel(use_refit, refit_bottomup);
 
-    model2.setIdentityTransform();
+    tf2.setIdentity();
   }
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -324,7 +359,9 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
 }
 
 /** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for OBB type */ 
-bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, BVHModel<OBB>& model2,
+bool initialize(PointCloudCollisionTraversalNodeOBB& node,
+                BVHModel<OBB>& model1, const SimpleTransform& tf1,
+                BVHModel<OBB>& model2, const SimpleTransform& tf2,
                 BVH_REAL collision_prob_threshold = 0.5,
                 int leaf_size_threshold = 1,
                 int num_max_contacts = 1,
@@ -333,7 +370,9 @@ bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1
                 BVH_REAL expand_r = 1);
 
 /** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for RSS type */
-bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, BVHModel<RSS>& model2,
+bool initialize(PointCloudCollisionTraversalNodeRSS& node,
+                BVHModel<RSS>& model1, const SimpleTransform& tf1,
+                BVHModel<RSS>& model2, const SimpleTransform& tf2,
                 BVH_REAL collision_prob_threshold = 0.5,
                 int leaf_size_threshold = 1,
                 int num_max_contacts = 1,
@@ -343,7 +382,9 @@ bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1
 
 /** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms */
 template<typename BV, bool use_refit, bool refit_bottomup>
-bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
+bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node,
+                BVHModel<BV>& model1, SimpleTransform& tf1,
+                BVHModel<BV>& model2, SimpleTransform& tf2,
                 BVH_REAL collision_prob_threshold = 0.5,
                 int leaf_size_threshold = 1,
                 int num_max_contacts = 1,
@@ -354,13 +395,13 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
   if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
 
-  if(!model1.isIdentityTransform())
+  if(!tf1.isIdentity())
   {
     std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
     for(int i = 0; i < model1.num_vertices; ++i)
     {
       Vec3f& p = model1.vertices[i];
-      Vec3f new_v = model1.getRotation() * p + model1.getTranslation();
+      Vec3f new_v = tf1.transform(p);
       vertices_transformed1[i] = new_v;
     }
 
@@ -368,16 +409,16 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
     model1.replaceSubModel(vertices_transformed1);
     model1.endReplaceModel(use_refit, refit_bottomup);
 
-    model1.setIdentityTransform();
+    tf1.setIdentity();
   }
 
-  if(!model2.isIdentityTransform())
+  if(!tf2.isIdentity())
   {
     std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
     for(int i = 0; i < model2.num_vertices; ++i)
     {
       Vec3f& p = model2.vertices[i];
-      Vec3f new_v = model2.getRotation() * p + model2.getTranslation();
+      Vec3f new_v = tf2.transform(p);
       vertices_transformed2[i] = new_v;
     }
 
@@ -385,11 +426,13 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
     model2.replaceSubModel(vertices_transformed2);
     model2.endReplaceModel(use_refit, refit_bottomup);
 
-    model2.setIdentityTransform();
+    tf2.setIdentity();
   }
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -412,7 +455,9 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
 
 
 /** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for OBB type */
-bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
+bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
+                BVHModel<OBB>& model1, const SimpleTransform& tf1,
+                const BVHModel<OBB>& model2, const SimpleTransform& tf2,
                 BVH_REAL collision_prob_threshold = 0.5,
                 int leaf_size_threshold = 1,
                 int num_max_contacts = 1,
@@ -421,7 +466,9 @@ bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& mo
                 BVH_REAL expand_r = 1);
 
 /** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for RSS type */
-bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
+bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
+                BVHModel<RSS>& model1, const SimpleTransform& tf1,
+                const BVHModel<RSS>& model2, const SimpleTransform& tf2,
                 BVH_REAL collision_prob_threshold = 0.5,
                 int leaf_size_threshold = 1,
                 int num_max_contacts = 1,
@@ -434,19 +481,21 @@ bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& mo
 
 /** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
 template<typename BV>
-bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
+bool initialize(MeshDistanceTraversalNode<BV>& node,
+                BVHModel<BV>& model1, SimpleTransform& tf1,
+                BVHModel<BV>& model2, SimpleTransform& tf2,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
 
-  if(!model1.isIdentityTransform())
+  if(!tf1.isIdentity())
   {
     std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
     for(int i = 0; i < model1.num_vertices; ++i)
     {
       Vec3f& p = model1.vertices[i];
-      Vec3f new_v = model1.getRotation() * p + model1.getTranslation();
+      Vec3f new_v = tf1.transform(p);
       vertices_transformed1[i] = new_v;
     }
 
@@ -454,16 +503,16 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHMo
     model1.replaceSubModel(vertices_transformed1);
     model1.endReplaceModel(use_refit, refit_bottomup);
 
-    model1.setIdentityTransform();
+    tf1.setIdentity();
   }
 
-  if(!model2.isIdentityTransform())
+  if(!tf2.isIdentity())
   {
     std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
     for(int i = 0; i < model2.num_vertices; ++i)
     {
       Vec3f& p = model2.vertices[i];
-      Vec3f new_v = model2.getRotation() * p + model2.getTranslation();
+      Vec3f new_v = tf2.transform(p);
       vertices_transformed2[i] = new_v;
     }
 
@@ -471,11 +520,13 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHMo
     model2.replaceSubModel(vertices_transformed2);
     model2.endReplaceModel(use_refit, refit_bottomup);
 
-    model2.setIdentityTransform();
+    tf2.setIdentity();
   }
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -488,19 +539,26 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHMo
 
 
 /** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
-bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2);
+bool initialize(MeshDistanceTraversalNodeRSS& node,
+                const BVHModel<RSS>& model1, const SimpleTransform& tf1,
+                const BVHModel<RSS>& model2, const SimpleTransform& tf2);
 
 
 
 /** \brief Initialize traversal node for continuous collision detection between two meshes */
 template<typename BV>
-bool initialize(MeshContinuousCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
+                const BVHModel<BV>& model1, const SimpleTransform& tf1,
+                const BVHModel<BV>& model2, const SimpleTransform& tf2,
+                int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -520,13 +578,18 @@ bool initialize(MeshContinuousCollisionTraversalNode<BV>& node, const BVHModel<B
 
 /** \brief Initialize traversal node for continuous collision detection between one mesh and one point cloud */
 template<typename BV>
-bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node,
+                const BVHModel<BV>& model1, const SimpleTransform& tf1,
+                const BVHModel<BV>& model2, const SimpleTransform& tf2,
+                int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES || !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
     return false;
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -544,13 +607,18 @@ bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node, const
 
 /** \brief Initialize traversal node for continuous collision detection between one point cloud and one mesh */
 template<typename BV>
-bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node,
+                const BVHModel<BV>& model1, const SimpleTransform& tf1,
+                const BVHModel<BV>& model2, const SimpleTransform& tf2,
+                int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
 {
   if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -571,7 +639,9 @@ bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node, const
 
 /** \brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms */
 template<typename BV>
-bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
+bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
+                BVHModel<BV>& model1,
+                BVHModel<BV>& model2,
                 const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, BVH_REAL w = 1,
                 bool use_refit = false, bool refit_bottomup = false)
 {
diff --git a/trunk/fcl/include/fcl/traversal_node_base.h b/trunk/fcl/include/fcl/traversal_node_base.h
index e8c0c1d3..feb3f210 100644
--- a/trunk/fcl/include/fcl/traversal_node_base.h
+++ b/trunk/fcl/include/fcl/traversal_node_base.h
@@ -38,6 +38,7 @@
 #define FCL_TRAVERSAL_NODE_BASE_H
 
 #include "fcl/primitive.h"
+#include "fcl/transform.h"
 
 /** \brief Main namespace */
 namespace fcl
@@ -74,6 +75,10 @@ public:
   void enableStatistics(bool enable) { enable_statistics = enable; }
 
   bool enable_statistics;
+
+  SimpleTransform tf1;
+
+  SimpleTransform tf2;
 };
 
 class CollisionTraversalNodeBase : public TraversalNodeBase
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
index 73ee6423..90292ca9 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -79,7 +79,7 @@ public:
   {
     if(this->enable_statistics) num_bv_tests++;
     BV bv_shape;
-    computeBV(*model2, bv_shape);
+    computeBV(*model2, tf2, bv_shape);
     return !model1->getBV(b1).bv.overlap(bv_shape);
   }
 
@@ -130,7 +130,7 @@ public:
   {
     if(this->enable_statistics) num_bv_tests++;
     BV bv_shape;
-    computeBV(*model1, bv_shape);
+    computeBV(*model1, tf1, bv_shape);
     return !model2->getBV(b2).bv.overlap(bv_shape);
   }
 
@@ -209,14 +209,14 @@ public:
 
     if(!enable_contact) // only interested in collision or not
     {
-      if(shapeTriangleIntersect(*(this->model2), p1, p2, p3))
+      if(shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3))
       {
         pairs.push_back(BVHShapeCollisionPair(primitive_id));
       }
     }
     else
     {
-      if(shapeTriangleIntersect(*(this->model2), p1, p2, p3, &contactp, &penetration, &normal))
+      if(shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal))
       {
         pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
       }
@@ -251,7 +251,7 @@ public:
   {
     if(this->enable_statistics) this->num_bv_tests++;
     OBB bv_shape;
-    computeBV(*this->model2, bv_shape);
+    computeBV(*this->model2, this->tf2, bv_shape);
     return !overlap(R, T, bv_shape, this->model1->getBV(b1).bv);
   }
 
@@ -274,14 +274,14 @@ public:
 
     if(!this->enable_contact) // only interested in collision or not
     {
-      if(shapeTriangleIntersect(*(this->model2), p1, p2, p3, R, T))
+      if(shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, R, T))
       {
         this->pairs.push_back(BVHShapeCollisionPair(primitive_id));
       }
     }
     else
     {
-      if(shapeTriangleIntersect(*(this->model2), p1, p2, p3, R, T, &contactp, &penetration, &normal))
+      if(shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, R, T, &contactp, &penetration, &normal))
       {
         this->pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
       }
@@ -327,14 +327,14 @@ public:
 
     if(!enable_contact) // only interested in collision or not
     {
-      if(shapeTriangleIntersect(*(this->model1), p1, p2, p3))
+      if(shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3))
       {
         pairs.push_back(BVHShapeCollisionPair(primitive_id));
       }
     }
     else
     {
-      if(shapeTriangleIntersect(*(this->model1), p1, p2, p3, &contactp, &penetration, &normal))
+      if(shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal))
       {
         pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
       }
@@ -369,7 +369,7 @@ public:
   {
     if(this->enable_statistics) this->num_bv_tests++;
     OBB bv_shape;
-    computeBV(*this->model1, bv_shape);
+    computeBV(*this->model1, this->tf1, bv_shape);
     return !overlap(R, T, bv_shape, this->model2->getBV(b2).bv);
   }
 
@@ -392,14 +392,14 @@ public:
 
     if(!this->enable_contact) // only interested in collision or not
     {
-      if(shapeTriangleIntersect(*(this->model1), p1, p2, p3, R, T))
+      if(shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, R, T))
       {
         this->pairs.push_back(BVHShapeCollisionPair(primitive_id));
       }
     }
     else
     {
-      if(shapeTriangleIntersect(*(this->model1), p1, p2, p3, R, T, &contactp, &penetration, &normal))
+      if(shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, R, T, &contactp, &penetration, &normal))
       {
         this->pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
       }
diff --git a/trunk/fcl/include/fcl/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal_node_shapes.h
index 172bca87..214a25cb 100644
--- a/trunk/fcl/include/fcl/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal_node_shapes.h
@@ -65,12 +65,13 @@ public:
   void leafTesting(int, int) const
   {
     if(enable_contact)
-      is_collision = shapeIntersect(*model1, *model2, &contact_point, &penetration_depth, &normal);
+      is_collision = shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal);
     else
-      is_collision = shapeIntersect(*model1, *model2);
+      is_collision = shapeIntersect(*model1, tf1, *model2, tf2);
   }
 
   const S1* model1;
+
   const S2* model2;
 
   mutable Vec3f normal;
diff --git a/trunk/fcl/src/BVH_model.cpp b/trunk/fcl/src/BVH_model.cpp
index bcc0bbeb..cec7809d 100644
--- a/trunk/fcl/src/BVH_model.cpp
+++ b/trunk/fcl/src/BVH_model.cpp
@@ -43,7 +43,7 @@ namespace fcl
 {
 
 template<typename BV>
-BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : CollisionObject(other)
+BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : CollisionGeometry(other)
 {
   num_tris = num_tris_allocated = other.num_tris;
   num_vertices = num_vertices_allocated = other.num_vertices;
@@ -861,9 +861,7 @@ void BVHModel<BV>::computeLocalAABB()
     aabb_ += vertices[i];
   }
 
-  aabb = aabb_;
-
-  aabb_center = aabb.center();
+  aabb_center = aabb_.center();
 
   aabb_radius = 0;
   for(int i = 0; i < num_vertices; ++i)
diff --git a/trunk/fcl/src/collision.cpp b/trunk/fcl/src/collision.cpp
index bedb7b30..6f5d8d0c 100644
--- a/trunk/fcl/src/collision.cpp
+++ b/trunk/fcl/src/collision.cpp
@@ -46,8 +46,8 @@ namespace fcl
 static CollisionFunctionMatrix CollisionFunctionLookTable;
 
 int collide(const CollisionObject* o1, const CollisionObject* o2,
-             int num_max_contacts, bool exhaustive, bool enable_contact,
-             std::vector<Contact>& contacts)
+            int num_max_contacts, bool exhaustive, bool enable_contact,
+            std::vector<Contact>& contacts)
 {
   if(num_max_contacts <= 0 && !exhaustive)
   {
@@ -70,19 +70,17 @@ int collide(const CollisionObject* o1, const CollisionObject* o2,
       return 0;
     }
 
-    return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, o2, num_max_contacts, exhaustive, enable_contact, contacts);
+    return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), num_max_contacts, exhaustive, enable_contact, contacts);
   }
   else if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
   {
-    //if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
     if(!CollisionFunctionLookTable.collision_matrix[node_type2][node_type1])
     {
       std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
       return 0;
     }
 
-    //return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, o2, num_max_contacts, exhaustive, enable_contact, contacts);
-    return CollisionFunctionLookTable.collision_matrix[node_type2][node_type1](o2, o1, num_max_contacts, exhaustive, enable_contact, contacts);
+    return CollisionFunctionLookTable.collision_matrix[node_type2][node_type1](o2->getCollisionGeometry(), o2->getTransform(), o1->getCollisionGeometry(), o1->getTransform(), num_max_contacts, exhaustive, enable_contact, contacts);
   }
   else if(object_type1 == OT_BVH && object_type2 == OT_GEOM)
   {
@@ -92,7 +90,7 @@ int collide(const CollisionObject* o1, const CollisionObject* o2,
       return 0;
     }
 
-    return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, o2, num_max_contacts, exhaustive, enable_contact, contacts);
+    return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), num_max_contacts, exhaustive, enable_contact, contacts);
   }
   else if(object_type1 == OT_BVH && object_type2 == OT_BVH)
   {
@@ -104,11 +102,78 @@ int collide(const CollisionObject* o1, const CollisionObject* o2,
 
     if(node_type1 == node_type2)
     {
-      return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, o2, num_max_contacts, exhaustive, enable_contact, contacts);
+      return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), num_max_contacts, exhaustive, enable_contact, contacts);
     }
   }
 
   return 0;
 }
 
+
+int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
+            const CollisionGeometry* o2, const SimpleTransform& tf2,
+            int num_max_contacts, bool exhaustive, bool enable_contact,
+            std::vector<Contact>& contacts)
+{
+  if(num_max_contacts <= 0 && !exhaustive)
+  {
+    std::cerr << "Warning: should stop early as num_max_contact is " << num_max_contacts << " !" << std::endl;
+    return 0;
+  }
+
+  const OBJECT_TYPE object_type1 = o1->getObjectType();
+  const NODE_TYPE node_type1 = o1->getNodeType();
+
+  const OBJECT_TYPE object_type2 = o2->getObjectType();
+  const NODE_TYPE node_type2 = o2->getNodeType();
+
+
+  if(object_type1 == OT_GEOM && object_type2 == OT_GEOM)
+  {
+    if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
+    {
+      std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
+      return 0;
+    }
+
+    return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, num_max_contacts, exhaustive, enable_contact, contacts);
+  }
+  else if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
+  {
+    if(!CollisionFunctionLookTable.collision_matrix[node_type2][node_type1])
+    {
+      std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
+      return 0;
+    }
+
+    return CollisionFunctionLookTable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, num_max_contacts, exhaustive, enable_contact, contacts);
+  }
+  else if(object_type1 == OT_BVH && object_type2 == OT_GEOM)
+  {
+    if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
+    {
+      std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
+      return 0;
+    }
+
+    return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, num_max_contacts, exhaustive, enable_contact, contacts);
+  }
+  else if(object_type1 == OT_BVH && object_type2 == OT_BVH)
+  {
+    if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
+    {
+      std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
+      return 0;
+    }
+
+    if(node_type1 == node_type2)
+    {
+      return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, num_max_contacts, exhaustive, enable_contact, contacts);
+    }
+  }
+
+  return 0;
+}
+
+
 }
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index ad5098a4..709057da 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -51,7 +51,7 @@ namespace fcl
 /** \brief Hey, I know it is ugly... but it is the best way I can find now... */
 
 #define SHAPESHAPE_COMMON_CODE() do{ \
-                                     initialize(node, *obj1, *obj2, enable_contact); \
+                                     initialize(node, *obj1, tf1, *obj2, tf2, enable_contact); \
                                      collide(&node); \
                                      if(!node.is_collision) return 0; \
                                      contacts.resize(1); \
@@ -62,7 +62,7 @@ namespace fcl
 
 
 #define MESHSHAPE_COMMON_CODE() do{ \
-                                    initialize(node, *obj1_tmp, *obj2, num_max_contacts, exhaustive, enable_contact); \
+                                    initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, num_max_contacts, exhaustive, enable_contact); \
                                     collide(&node); \
                                     int num_contacts = node.pairs.size(); \
                                     if(num_contacts > 0) \
@@ -87,7 +87,7 @@ namespace fcl
 
 
 #define SHAPEMESH_COMMON_CODE() do{ \
-                                    initialize(node, *obj1, *obj2_tmp, num_max_contacts, exhaustive, enable_contact); \
+                                    initialize(node, *obj1, tf1, *obj2_tmp, tf2_tmp, num_max_contacts, exhaustive, enable_contact); \
                                     collide(&node); \
                                     int num_contacts = node.pairs.size(); \
                                     if(num_contacts > 0) \
@@ -111,7 +111,7 @@ namespace fcl
                                    } while(0)
 
 #define MESHSHAPEOBBRSS_COMMON_CODE() do{ \
-                                    initialize(node, *obj1, *obj2, num_max_contacts, exhaustive, enable_contact); \
+                                    initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact); \
                                     collide(&node); \
                                     int num_contacts = node.pairs.size(); \
                                     if(num_contacts > 0) \
@@ -133,7 +133,7 @@ namespace fcl
                                    } while(0)
 
 #define MESHMESH_COMMON_CODE() do{ \
-                                    initialize(node, *obj1_tmp, *obj2_tmp, num_max_contacts, exhaustive, enable_contact); \
+                                    initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, num_max_contacts, exhaustive, enable_contact); \
                                     collide(&node); \
                                     int num_contacts = node.pairs.size(); \
                                     if(num_contacts > 0) \
@@ -160,7 +160,7 @@ namespace fcl
 
 
 #define MESHMESHOBBRSS_COMMON_CODE() do{ \
-                                         initialize(node, *obj1, *obj2, num_max_contacts, exhaustive, enable_contact); \
+                                         initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact); \
                                          collide(&node); \
                                          int num_contacts = node.pairs.size(); \
                                          if(num_contacts > 0) \
@@ -176,8 +176,8 @@ namespace fcl
                                            { \
                                              for(int i = 0; i < num_contacts; ++i) \
                                              { \
-                                               Vec3f normal = obj1->getRotation() * node.pairs[i].normal; \
-                                               Vec3f contact_point = obj1->getRotation() * node.pairs[i].contact_point + obj1->getTranslation(); \
+                                               Vec3f normal = tf1.getRotation() * node.pairs[i].normal; \
+                                               Vec3f contact_point = tf1.transform(node.pairs[i].contact_point); \
                                                contacts[i] = Contact(obj1, obj2, node.pairs[i].id1, node.pairs[i].id2, contact_point, normal, node.pairs[i].penetration_depth); \
                                              } \
                                            } \
@@ -187,7 +187,7 @@ namespace fcl
 
 
 
-int BoxBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int BoxBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Box, Box> node;
   const Box* obj1 = (Box*)o1;
@@ -195,7 +195,7 @@ int BoxBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_
   SHAPESHAPE_COMMON_CODE();
 }
 
-int BoxSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int BoxSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Box, Sphere> node;
   const Box* obj1 = (Box*)o1;
@@ -203,7 +203,7 @@ int BoxSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   SHAPESHAPE_COMMON_CODE();
 }
 
-int BoxCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int BoxCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Box, Capsule> node;
   const Box* obj1 = (Box*)o1;
@@ -211,7 +211,7 @@ int BoxCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_
   SHAPESHAPE_COMMON_CODE();
 }
 
-int BoxConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int BoxConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Box, Cone> node;
   const Box* obj1 = (Box*)o1;
@@ -219,7 +219,7 @@ int BoxConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num
   SHAPESHAPE_COMMON_CODE();
 }
 
-int BoxCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int BoxCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Box, Cylinder> node;
   const Box* obj1 = (Box*)o1;
@@ -227,7 +227,7 @@ int BoxCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int BoxConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int BoxConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Box, Convex> node;
   const Box* obj1 = (Box*)o1;
@@ -235,7 +235,7 @@ int BoxConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   SHAPESHAPE_COMMON_CODE();
 }
 
-int BoxPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int BoxPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Box, Plane> node;
   const Box* obj1 = (Box*)o1;
@@ -243,7 +243,7 @@ int BoxPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int nu
   SHAPESHAPE_COMMON_CODE();
 }
 
-int SphereBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int SphereBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Sphere, Box> node;
   const Sphere* obj1 = (Sphere*)o1;
@@ -251,7 +251,7 @@ int SphereBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   SHAPESHAPE_COMMON_CODE();
 }
 
-int SphereSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int SphereSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Sphere, Sphere> node;
   const Sphere* obj1 = (Sphere*)o1;
@@ -259,7 +259,7 @@ int SphereSphereCollide(const CollisionObject* o1, const CollisionObject* o2, in
   SHAPESHAPE_COMMON_CODE();
 }
 
-int SphereCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int SphereCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Sphere, Capsule> node;
   const Sphere* obj1 = (Sphere*)o1;
@@ -267,7 +267,7 @@ int SphereCapCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   SHAPESHAPE_COMMON_CODE();
 }
 
-int SphereConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int SphereConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Sphere, Cone> node;
   const Sphere* obj1 = (Sphere*)o1;
@@ -275,7 +275,7 @@ int SphereConeCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int SphereCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int SphereCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Sphere, Cylinder> node;
   const Sphere* obj1 = (Sphere*)o1;
@@ -283,7 +283,7 @@ int SphereCylinderCollide(const CollisionObject* o1, const CollisionObject* o2,
   SHAPESHAPE_COMMON_CODE();
 }
 
-int SphereConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int SphereConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Sphere, Convex> node;
   const Sphere* obj1 = (Sphere*)o1;
@@ -291,7 +291,7 @@ int SphereConvexCollide(const CollisionObject* o1, const CollisionObject* o2, in
   SHAPESHAPE_COMMON_CODE();
 }
 
-int SpherePlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int SpherePlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Sphere, Plane> node;
   const Sphere* obj1 = (Sphere*)o1;
@@ -299,7 +299,7 @@ int SpherePlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CapBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CapBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Capsule, Box> node;
   const Capsule* obj1 = (Capsule*)o1;
@@ -307,7 +307,7 @@ int CapBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CapSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CapSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Capsule, Sphere> node;
   const Capsule* obj1 = (Capsule*)o1;
@@ -315,7 +315,7 @@ int CapSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CapCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CapCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Capsule, Capsule> node;
   const Capsule* obj1 = (Capsule*)o1;
@@ -323,7 +323,7 @@ int CapCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CapConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CapConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Capsule, Cone> node;
   const Capsule* obj1 = (Capsule*)o1;
@@ -331,7 +331,7 @@ int CapConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CapCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CapCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Capsule, Cylinder> node;
   const Capsule* obj1 = (Capsule*)o1;
@@ -339,7 +339,7 @@ int CapCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CapConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CapConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Capsule, Convex> node;
   const Capsule* obj1 = (Capsule*)o1;
@@ -347,7 +347,7 @@ int CapConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CapPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CapPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Capsule, Plane> node;
   const Capsule* obj1 = (Capsule*)o1;
@@ -355,7 +355,7 @@ int CapPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int nu
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConeBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConeBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cone, Box> node;
   const Cone* obj1 = (Cone*)o1;
@@ -363,7 +363,7 @@ int ConeBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConeSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConeSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cone, Sphere> node;
   const Cone* obj1 = (Cone*)o1;
@@ -371,7 +371,7 @@ int ConeSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConeCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConeCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cone, Capsule> node;
   const Cone* obj1 = (Cone*)o1;
@@ -379,7 +379,7 @@ int ConeCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConeConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConeConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cone, Cone> node;
   const Cone* obj1 = (Cone*)o1;
@@ -387,7 +387,7 @@ int ConeConeCollide(const CollisionObject* o1, const CollisionObject* o2, int nu
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConeCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConeCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cone, Cylinder> node;
   const Cone* obj1 = (Cone*)o1;
@@ -395,7 +395,7 @@ int ConeCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, in
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConeConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConeConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cone, Convex> node;
   const Cone* obj1 = (Cone*)o1;
@@ -403,7 +403,7 @@ int ConeConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConePlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConePlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cone, Plane> node;
   const Cone* obj1 = (Cone*)o1;
@@ -411,7 +411,7 @@ int ConePlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CylinderBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CylinderBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cylinder, Box> node;
   const Cylinder* obj1 = (Cylinder*)o1;
@@ -419,7 +419,7 @@ int CylinderBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CylinderSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CylinderSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cylinder, Sphere> node;
   const Cylinder* obj1 = (Cylinder*)o1;
@@ -427,7 +427,7 @@ int CylinderSphereCollide(const CollisionObject* o1, const CollisionObject* o2,
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CylinderCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CylinderCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cylinder, Capsule> node;
   const Cylinder* obj1 = (Cylinder*)o1;
@@ -435,7 +435,7 @@ int CylinderCapCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CylinderConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CylinderConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cylinder, Cone> node;
   const Cylinder* obj1 = (Cylinder*)o1;
@@ -443,7 +443,7 @@ int CylinderConeCollide(const CollisionObject* o1, const CollisionObject* o2, in
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CylinderCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CylinderCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cylinder, Cylinder> node;
   const Cylinder* obj1 = (Cylinder*)o1;
@@ -451,7 +451,7 @@ int CylinderCylinderCollide(const CollisionObject* o1, const CollisionObject* o2
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CylinderConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CylinderConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cylinder, Convex> node;
   const Cylinder* obj1 = (Cylinder*)o1;
@@ -459,7 +459,7 @@ int CylinderConvexCollide(const CollisionObject* o1, const CollisionObject* o2,
   SHAPESHAPE_COMMON_CODE();
 }
 
-int CylinderPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int CylinderPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Cylinder, Plane> node;
   const Cylinder* obj1 = (Cylinder*)o1;
@@ -467,7 +467,7 @@ int CylinderPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, i
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConvexBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConvexBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Convex, Box> node;
   const Convex* obj1 = (Convex*)o1;
@@ -475,7 +475,7 @@ int ConvexBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConvexSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConvexSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Convex, Sphere> node;
   const Convex* obj1 = (Convex*)o1;
@@ -483,7 +483,7 @@ int ConvexSphereCollide(const CollisionObject* o1, const CollisionObject* o2, in
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConvexCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConvexCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Convex, Capsule> node;
   const Convex* obj1 = (Convex*)o1;
@@ -491,7 +491,7 @@ int ConvexCapCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConvexConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConvexConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Convex, Cone> node;
   const Convex* obj1 = (Convex*)o1;
@@ -499,7 +499,7 @@ int ConvexConeCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConvexCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConvexCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Convex, Cylinder> node;
   const Convex* obj1 = (Convex*)o1;
@@ -507,7 +507,7 @@ int ConvexCylinderCollide(const CollisionObject* o1, const CollisionObject* o2,
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConvexConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConvexConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Convex, Convex> node;
   const Convex* obj1 = (Convex*)o1;
@@ -515,7 +515,7 @@ int ConvexConvexCollide(const CollisionObject* o1, const CollisionObject* o2, in
   SHAPESHAPE_COMMON_CODE();
 }
 
-int ConvexPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int ConvexPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Convex, Plane> node;
   const Convex* obj1 = (Convex*)o1;
@@ -523,7 +523,7 @@ int ConvexPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int PlaneBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int PlaneBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Plane, Box> node;
   const Plane* obj1 = (Plane*)o1;
@@ -531,7 +531,7 @@ int PlaneBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int nu
   SHAPESHAPE_COMMON_CODE();
 }
 
-int PlaneSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int PlaneSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Plane, Sphere> node;
   const Plane* obj1 = (Plane*)o1;
@@ -539,7 +539,7 @@ int PlaneSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int PlaneCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int PlaneCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Plane, Capsule> node;
   const Plane* obj1 = (Plane*)o1;
@@ -547,7 +547,7 @@ int PlaneCapCollide(const CollisionObject* o1, const CollisionObject* o2, int nu
   SHAPESHAPE_COMMON_CODE();
 }
 
-int PlaneConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int PlaneConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Plane, Cone> node;
   const Plane* obj1 = (Plane*)o1;
@@ -555,7 +555,7 @@ int PlaneConeCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   SHAPESHAPE_COMMON_CODE();
 }
 
-int PlaneCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int PlaneCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Plane, Cylinder> node;
   const Plane* obj1 = (Plane*)o1;
@@ -563,7 +563,7 @@ int PlaneCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, i
   SHAPESHAPE_COMMON_CODE();
 }
 
-int PlaneConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int PlaneConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Plane, Convex> node;
   const Plane* obj1 = (Plane*)o1;
@@ -571,7 +571,7 @@ int PlaneConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int PlanePlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int PlanePlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   ShapeCollisionTraversalNode<Plane, Plane> node;
   const Plane* obj1 = (Plane*)o1;
@@ -579,70 +579,77 @@ int PlanePlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int
   SHAPESHAPE_COMMON_CODE();
 }
 
-int AABBBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int AABBBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<AABB, Box> node;
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Box* obj2 = (Box*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int AABBSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int AABBSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<AABB, Sphere> node;
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Sphere* obj2 = (Sphere*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int AABBCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int AABBCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<AABB, Capsule> node;
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Capsule* obj2 = (Capsule*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int AABBConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int AABBConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<AABB, Cone> node;
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Cone* obj2 = (Cone*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int AABBCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int AABBCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<AABB, Cylinder> node;
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Cylinder* obj2 = (Cylinder*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int AABBConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int AABBConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<AABB, Convex> node;
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Convex* obj2 = (Convex*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int AABBPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int AABBPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<AABB, Plane> node;
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Plane* obj2 = (Plane*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int OBBBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int OBBBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNodeOBB<Box> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
@@ -650,7 +657,7 @@ int OBBBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
-int OBBSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int OBBSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNodeOBB<Sphere> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
@@ -658,7 +665,7 @@ int OBBSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
-int OBBCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int OBBCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNodeOBB<Capsule> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
@@ -666,7 +673,7 @@ int OBBCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
-int OBBConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int OBBConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNodeOBB<Cone> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
@@ -674,7 +681,7 @@ int OBBConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
-int OBBCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int OBBCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNodeOBB<Cylinder> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
@@ -682,7 +689,7 @@ int OBBCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
-int OBBConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int OBBConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNodeOBB<Convex> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
@@ -690,7 +697,7 @@ int OBBConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
-int OBBPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int OBBPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNodeOBB<Plane> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
@@ -698,645 +705,300 @@ int OBBPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int nu
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
-int RSSBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int RSSBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<RSS, Box> node;
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Box* obj2 = (Box*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int RSSSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int RSSSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<RSS, Sphere> node;
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Sphere* obj2 = (Sphere*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int RSSCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int RSSCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<RSS, Capsule> node;
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Capsule* obj2 = (Capsule*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int RSSConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int RSSConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<RSS, Cone> node;
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Cone* obj2 = (Cone*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int RSSCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int RSSCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<RSS, Cylinder> node;
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Cylinder* obj2 = (Cylinder*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int RSSConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int RSSConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<RSS, Convex> node;
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Convex* obj2 = (Convex*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int RSSPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int RSSPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<RSS, Plane> node;
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Plane* obj2 = (Plane*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP16BoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP16BoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<16> , Box> node;
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Box* obj2 = (Box*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP16SphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP16SphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<16> , Sphere> node;
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Sphere* obj2 = (Sphere*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP16CapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP16CapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<16> , Capsule> node;
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Capsule* obj2 = (Capsule*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP16ConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP16ConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<16> , Cone> node;
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Cone* obj2 = (Cone*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP16CylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP16CylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<16> , Cylinder> node;
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Cylinder* obj2 = (Cylinder*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP16ConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP16ConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<16> , Convex> node;
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Convex* obj2 = (Convex*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP16PlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP16PlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<16> , Plane> node;
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Plane* obj2 = (Plane*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP18BoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP18BoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<18> , Box> node;
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Box* obj2 = (Box*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP18SphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP18SphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<18> , Sphere> node;
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Sphere* obj2 = (Sphere*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP18CapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP18CapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<18> , Capsule> node;
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Capsule* obj2 = (Capsule*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP18ConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP18ConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<18> , Cone> node;
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Cone* obj2 = (Cone*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP18CylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP18CylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<18> , Cylinder> node;
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Cylinder* obj2 = (Cylinder*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP18ConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP18ConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<18> , Convex> node;
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Convex* obj2 = (Convex*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP18PlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP18PlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<18> , Plane> node;
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Plane* obj2 = (Plane*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP24BoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP24BoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<24> , Box> node;
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Box* obj2 = (Box*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP24SphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP24SphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<24> , Sphere> node;
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Sphere* obj2 = (Sphere*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP24CapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP24CapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<24> , Capsule> node;
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Capsule* obj2 = (Capsule*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP24ConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP24ConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<24> , Cone> node;
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Cone* obj2 = (Cone*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP24CylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP24CylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<24> , Cylinder> node;
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Cylinder* obj2 = (Cylinder*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP24ConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP24ConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<24> , Convex> node;
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Convex* obj2 = (Convex*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-int KDOP24PlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP24PlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshShapeCollisionTraversalNode<KDOP<24> , Plane> node;
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   const Plane* obj2 = (Plane*)o2;
   MESHSHAPE_COMMON_CODE();
 }
 
-// use MESH SHAPE
-/*
-int BoxAABBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Box, AABB> node;
-  const Box* obj1 = (Box*)o1;
-  const BVHModel<AABB>* obj2 = (BVHModel<AABB>*)o2;
-  BVHModel<AABB>* obj2_tmp = new BVHModel<AABB>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int SphereAABBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Sphere, AABB> node;
-  const Sphere* obj1 = (Sphere*)o1;
-  const BVHModel<AABB>* obj2 = (BVHModel<AABB>*)o2;
-  BVHModel<AABB>* obj2_tmp = new BVHModel<AABB>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int CapAABBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Capsule, AABB> node;
-  const Capsule* obj1 = (Capsule*)o1;
-  const BVHModel<AABB>* obj2 = (BVHModel<AABB>*)o2;
-  BVHModel<AABB>* obj2_tmp = new BVHModel<AABB>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int ConeAABBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Cone, AABB> node;
-  const Cone* obj1 = (Cone*)o1;
-  const BVHModel<AABB>* obj2 = (BVHModel<AABB>*)o2;
-  BVHModel<AABB>* obj2_tmp = new BVHModel<AABB>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int CylinderAABBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Cylinder, AABB> node;
-  const Cylinder* obj1 = (Cylinder*)o1;
-  const BVHModel<AABB>* obj2 = (BVHModel<AABB>*)o2;
-  BVHModel<AABB>* obj2_tmp = new BVHModel<AABB>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int ConvexAABBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Convex, AABB> node;
-  const Convex* obj1 = (Convex*)o1;
-  const BVHModel<AABB>* obj2 = (BVHModel<AABB>*)o2;
-  BVHModel<AABB>* obj2_tmp = new BVHModel<AABB>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int PlaneAABBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Plane, AABB> node;
-  const Plane* obj1 = (Plane*)o1;
-  const BVHModel<AABB>* obj2 = (BVHModel<AABB>*)o2;
-  BVHModel<AABB>* obj2_tmp = new BVHModel<AABB>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int BoxOBBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNodeOBB<Box> node;
-  const Box* obj1 = (Box*)o1;
-  const BVHModel<OBB>* obj2 = (BVHModel<OBB>*)o2;
-  MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int SphereOBBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNodeOBB<Sphere> node;
-  const Sphere* obj1 = (Sphere*)o1;
-  const BVHModel<OBB>* obj2 = (BVHModel<OBB>*)o2;
-  MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int CapOBBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNodeOBB<Capsule> node;
-  const Capsule* obj1 = (Capsule*)o1;
-  const BVHModel<OBB>* obj2 = (BVHModel<OBB>*)o2;
-  MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int ConeOBBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNodeOBB<Cone> node;
-  const Cone* obj1 = (Cone*)o1;
-  const BVHModel<OBB>* obj2 = (BVHModel<OBB>*)o2;
-  MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int CylinderOBBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNodeOBB<Cylinder> node;
-  const Cylinder* obj1 = (Cylinder*)o1;
-  const BVHModel<OBB>* obj2 = (BVHModel<OBB>*)o2;
-  MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int ConvexOBBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNodeOBB<Convex> node;
-  const Convex* obj1 = (Convex*)o1;
-  const BVHModel<OBB>* obj2 = (BVHModel<OBB>*)o2;
-  MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int PlaneOBBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNodeOBB<Plane> node;
-  const Plane* obj1 = (Plane*)o1;
-  const BVHModel<OBB>* obj2 = (BVHModel<OBB>*)o2;
-  MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int BoxRSSCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Box, RSS> node;
-  const Box* obj1 = (Box*)o1;
-  const BVHModel<RSS>* obj2 = (BVHModel<RSS>*)o2;
-  BVHModel<RSS>* obj2_tmp = new BVHModel<RSS>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int SphereRSSCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Sphere, RSS> node;
-  const Sphere* obj1 = (Sphere*)o1;
-  const BVHModel<RSS>* obj2 = (BVHModel<RSS>*)o2;
-  BVHModel<RSS>* obj2_tmp = new BVHModel<RSS>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int CapRSSCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Capsule, RSS> node;
-  const Capsule* obj1 = (Capsule*)o1;
-  const BVHModel<RSS>* obj2 = (BVHModel<RSS>*)o2;
-  BVHModel<RSS>* obj2_tmp = new BVHModel<RSS>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int ConeRSSCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Cone, RSS> node;
-  const Cone* obj1 = (Cone*)o1;
-  const BVHModel<RSS>* obj2 = (BVHModel<RSS>*)o2;
-  BVHModel<RSS>* obj2_tmp = new BVHModel<RSS>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int CylinderRSSCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Cylinder, RSS> node;
-  const Cylinder* obj1 = (Cylinder*)o1;
-  const BVHModel<RSS>* obj2 = (BVHModel<RSS>*)o2;
-  BVHModel<RSS>* obj2_tmp = new BVHModel<RSS>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int ConvexRSSCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Convex, RSS> node;
-  const Convex* obj1 = (Convex*)o1;
-  const BVHModel<RSS>* obj2 = (BVHModel<RSS>*)o2;
-  BVHModel<RSS>* obj2_tmp = new BVHModel<RSS>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int PlaneRSSCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Plane, RSS> node;
-  const Plane* obj1 = (Plane*)o1;
-  const BVHModel<RSS>* obj2 = (BVHModel<RSS>*)o2;
-  BVHModel<RSS>* obj2_tmp = new BVHModel<RSS>(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int BoxKDOP16Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Box, KDOP<16> > node;
-  const Box* obj1 = (Box*)o1;
-  const BVHModel<KDOP<16> >* obj2 = (BVHModel<KDOP<16> >*)o2;
-  BVHModel<KDOP<16> >* obj2_tmp = new BVHModel<KDOP<16> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int SphereKDOP16Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Sphere, KDOP<16> > node;
-  const Sphere* obj1 = (Sphere*)o1;
-  const BVHModel<KDOP<16> >* obj2 = (BVHModel<KDOP<16> >*)o2;
-  BVHModel<KDOP<16> >* obj2_tmp = new BVHModel<KDOP<16> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int CapKDOP16Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Capsule, KDOP<16> > node;
-  const Capsule* obj1 = (Capsule*)o1;
-  const BVHModel<KDOP<16> >* obj2 = (BVHModel<KDOP<16> >*)o2;
-  BVHModel<KDOP<16> >* obj2_tmp = new BVHModel<KDOP<16> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int ConeKDOP16Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Cone, KDOP<16> > node;
-  const Cone* obj1 = (Cone*)o1;
-  const BVHModel<KDOP<16> >* obj2 = (BVHModel<KDOP<16> >*)o2;
-  BVHModel<KDOP<16> >* obj2_tmp = new BVHModel<KDOP<16> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int CylinderKDOP16Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Cylinder, KDOP<16> > node;
-  const Cylinder* obj1 = (Cylinder*)o1;
-  const BVHModel<KDOP<16> >* obj2 = (BVHModel<KDOP<16> >*)o2;
-  BVHModel<KDOP<16> >* obj2_tmp = new BVHModel<KDOP<16> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int ConvexKDOP16Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Convex, KDOP<16> > node;
-  const Convex* obj1 = (Convex*)o1;
-  const BVHModel<KDOP<16> >* obj2 = (BVHModel<KDOP<16> >*)o2;
-  BVHModel<KDOP<16> >* obj2_tmp = new BVHModel<KDOP<16> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int PlaneKDOP16Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Plane, KDOP<16> > node;
-  const Plane* obj1 = (Plane*)o1;
-  const BVHModel<KDOP<16> >* obj2 = (BVHModel<KDOP<16> >*)o2;
-  BVHModel<KDOP<16> >* obj2_tmp = new BVHModel<KDOP<16> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int BoxKDOP18Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Box, KDOP<18> > node;
-  const Box* obj1 = (Box*)o1;
-  const BVHModel<KDOP<18> >* obj2 = (BVHModel<KDOP<18> >*)o2;
-  BVHModel<KDOP<18> >* obj2_tmp = new BVHModel<KDOP<18> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int SphereKDOP18Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Sphere, KDOP<18> > node;
-  const Sphere* obj1 = (Sphere*)o1;
-  const BVHModel<KDOP<18> >* obj2 = (BVHModel<KDOP<18> >*)o2;
-  BVHModel<KDOP<18> >* obj2_tmp = new BVHModel<KDOP<18> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int CapKDOP18Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Capsule, KDOP<18> > node;
-  const Capsule* obj1 = (Capsule*)o1;
-  const BVHModel<KDOP<18> >* obj2 = (BVHModel<KDOP<18> >*)o2;
-  BVHModel<KDOP<18> >* obj2_tmp = new BVHModel<KDOP<18> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int ConeKDOP18Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Cone, KDOP<18> > node;
-  const Cone* obj1 = (Cone*)o1;
-  const BVHModel<KDOP<18> >* obj2 = (BVHModel<KDOP<18> >*)o2;
-  BVHModel<KDOP<18> >* obj2_tmp = new BVHModel<KDOP<18> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int CylinderKDOP18Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Cylinder, KDOP<18> > node;
-  const Cylinder* obj1 = (Cylinder*)o1;
-  const BVHModel<KDOP<18> >* obj2 = (BVHModel<KDOP<18> >*)o2;
-  BVHModel<KDOP<18> >* obj2_tmp = new BVHModel<KDOP<18> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int ConvexKDOP18Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Convex, KDOP<18> > node;
-  const Convex* obj1 = (Convex*)o1;
-  const BVHModel<KDOP<18> >* obj2 = (BVHModel<KDOP<18> >*)o2;
-  BVHModel<KDOP<18> >* obj2_tmp = new BVHModel<KDOP<18> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int PlaneKDOP18Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Plane, KDOP<18> > node;
-  const Plane* obj1 = (Plane*)o1;
-  const BVHModel<KDOP<18> >* obj2 = (BVHModel<KDOP<18> >*)o2;
-  BVHModel<KDOP<18> >* obj2_tmp = new BVHModel<KDOP<18> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int BoxKDOP24Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Box, KDOP<24> > node;
-  const Box* obj1 = (Box*)o1;
-  const BVHModel<KDOP<24> >* obj2 = (BVHModel<KDOP<24> >*)o2;
-  BVHModel<KDOP<24> >* obj2_tmp = new BVHModel<KDOP<24> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int SphereKDOP24Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Sphere, KDOP<24> > node;
-  const Sphere* obj1 = (Sphere*)o1;
-  const BVHModel<KDOP<24> >* obj2 = (BVHModel<KDOP<24> >*)o2;
-  BVHModel<KDOP<24> >* obj2_tmp = new BVHModel<KDOP<24> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int CapKDOP24Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Capsule, KDOP<24> > node;
-  const Capsule* obj1 = (Capsule*)o1;
-  const BVHModel<KDOP<24> >* obj2 = (BVHModel<KDOP<24> >*)o2;
-  BVHModel<KDOP<24> >* obj2_tmp = new BVHModel<KDOP<24> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int ConeKDOP24Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Cone, KDOP<24> > node;
-  const Cone* obj1 = (Cone*)o1;
-  const BVHModel<KDOP<24> >* obj2 = (BVHModel<KDOP<24> >*)o2;
-  BVHModel<KDOP<24> >* obj2_tmp = new BVHModel<KDOP<24> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int CylinderKDOP24Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Cylinder, KDOP<24> > node;
-  const Cylinder* obj1 = (Cylinder*)o1;
-  const BVHModel<KDOP<24> >* obj2 = (BVHModel<KDOP<24> >*)o2;
-  BVHModel<KDOP<24> >* obj2_tmp = new BVHModel<KDOP<24> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int ConvexKDOP24Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Convex, KDOP<24> > node;
-  const Convex* obj1 = (Convex*)o1;
-  const BVHModel<KDOP<24> >* obj2 = (BVHModel<KDOP<24> >*)o2;
-  BVHModel<KDOP<24> >* obj2_tmp = new BVHModel<KDOP<24> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-int PlaneKDOP24Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
-  ShapeMeshCollisionTraversalNode<Plane, KDOP<24> > node;
-  const Plane* obj1 = (Plane*)o1;
-  const BVHModel<KDOP<24> >* obj2 = (BVHModel<KDOP<24> >*)o2;
-  BVHModel<KDOP<24> >* obj2_tmp = new BVHModel<KDOP<24> >(*obj2);
-  SHAPEMESH_COMMON_CODE();
-}
-
-*/
-
 
-int AABBAABBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int AABBAABBCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshCollisionTraversalNode<AABB> node;
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   const BVHModel<AABB>* obj2 = (BVHModel<AABB>*)o2;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   BVHModel<AABB>* obj2_tmp = new BVHModel<AABB>(*obj2);
+  SimpleTransform tf2_tmp = tf2;
   MESHMESH_COMMON_CODE();
 }
 
-int OBBOBBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int OBBOBBCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshCollisionTraversalNodeOBB node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
@@ -1344,7 +1006,7 @@ int OBBOBBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_
   MESHMESHOBBRSS_COMMON_CODE();
 }
 
-int RSSRSSCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int RSSRSSCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshCollisionTraversalNodeRSS node;
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
@@ -1352,33 +1014,39 @@ int RSSRSSCollide(const CollisionObject* o1, const CollisionObject* o2, int num_
   MESHMESHOBBRSS_COMMON_CODE();
 }
 
-int KDOP16KDOP16Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP16KDOP16Collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshCollisionTraversalNode<KDOP<16> > node;
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   const BVHModel<KDOP<16> >* obj2 = (BVHModel<KDOP<16> >*)o2;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   BVHModel<KDOP<16> >* obj2_tmp = new BVHModel<KDOP<16> >(*obj2);
+  SimpleTransform tf2_tmp = tf2;
   MESHMESH_COMMON_CODE();
 }
 
-int KDOP18KDOP18Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP18KDOP18Collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshCollisionTraversalNode<KDOP<18> > node;
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   const BVHModel<KDOP<18> >* obj2 = (BVHModel<KDOP<18> >*)o2;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   BVHModel<KDOP<18> >* obj2_tmp = new BVHModel<KDOP<18> >(*obj2);
+  SimpleTransform tf2_tmp = tf2;
   MESHMESH_COMMON_CODE();
 }
 
-int KDOP24KDOP24Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int KDOP24KDOP24Collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshCollisionTraversalNode<KDOP<24> > node;
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   const BVHModel<KDOP<24> >* obj2 = (BVHModel<KDOP<24> >*)o2;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
+  SimpleTransform tf1_tmp = tf1;
   BVHModel<KDOP<24> >* obj2_tmp = new BVHModel<KDOP<24> >(*obj2);
+  SimpleTransform tf2_tmp = tf2;
   MESHMESH_COMMON_CODE();
 }
 
diff --git a/trunk/fcl/src/conservative_advancement.cpp b/trunk/fcl/src/conservative_advancement.cpp
index 62ce70d6..2435cee8 100644
--- a/trunk/fcl/src/conservative_advancement.cpp
+++ b/trunk/fcl/src/conservative_advancement.cpp
@@ -47,9 +47,9 @@ namespace fcl
 {
 
 template<typename BV>
-int conservativeAdvancement(const CollisionObject* o1,
+int conservativeAdvancement(const CollisionGeometry* o1,
                             MotionBase<BV>* motion1,
-                            const CollisionObject* o2,
+                            const CollisionGeometry* o2,
                             MotionBase<BV>* motion2,
                             int num_max_contacts, bool exhaustive, bool enable_contact,
                             std::vector<Contact>& contacts,
@@ -77,18 +77,16 @@ int conservativeAdvancement(const CollisionObject* o1,
   const BVHModel<RSS>* model1 = (const BVHModel<RSS>*)o1;
   const BVHModel<RSS>* model2 = (const BVHModel<RSS>*)o2;
 
+  SimpleTransform tf1, tf2;
+  motion1->getCurrentTransform(tf1);
+  motion2->getCurrentTransform(tf2);
+
   // whether the first start configuration is in collision
   MeshCollisionTraversalNodeRSS cnode;
-  if(!initialize(cnode, *model1, *model2))
+  if(!initialize(cnode, *model1, tf1, *model2, tf2))
     std::cout << "initialize error" << std::endl;
 
-  Matrix3f R1_0, R2_0;
-  Vec3f T1_0, T2_0;
-
-  motion1->getCurrentTransform(R1_0, T1_0);
-  motion2->getCurrentTransform(R2_0, T2_0);
-
-  relativeTransform(R1_0, T1_0, R2_0, T2_0, cnode.R, cnode.T);
+  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), cnode.R, cnode.T);
 
   cnode.enable_statistics = false;
   cnode.num_max_contacts = num_max_contacts;
@@ -109,7 +107,7 @@ int conservativeAdvancement(const CollisionObject* o1,
 
   MeshConservativeAdvancementTraversalNodeRSS node;
 
-  initialize(node, *model1, *model2, R1_0, T1_0, R2_0, T2_0);
+  initialize(node, *model1, *model2, tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation());
 
   node.motion1 = motion1;
   node.motion2 = motion2;
@@ -158,9 +156,9 @@ int conservativeAdvancement(const CollisionObject* o1,
 }
 
 
-template int conservativeAdvancement<RSS>(const CollisionObject* o1,
+template int conservativeAdvancement<RSS>(const CollisionGeometry* o1,
                                           MotionBase<RSS>* motion1,
-                                          const CollisionObject* o2,
+                                          const CollisionGeometry* o2,
                                           MotionBase<RSS>* motion2,
                                           int num_max_contacts, bool exhaustive, bool enable_contact,
                                           std::vector<Contact>& contacts,
diff --git a/trunk/fcl/src/geometric_shapes_intersect.cpp b/trunk/fcl/src/geometric_shapes_intersect.cpp
index 2c99d86a..acc554e6 100644
--- a/trunk/fcl/src/geometric_shapes_intersect.cpp
+++ b/trunk/fcl/src/geometric_shapes_intersect.cpp
@@ -102,13 +102,13 @@ void transformGJKObject(void* obj, const Matrix3f& R, const Vec3f& T)
 }
 
 /** Basic shape to ccd shape */
-static void shapeToGJK(const ShapeBase& s, ccd_obj_t* o)
+static void shapeToGJK(const ShapeBase& s, const SimpleTransform& tf, ccd_obj_t* o)
 {
 
   SimpleQuaternion q;
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
   q.fromRotation(R);
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
   ccdVec3Set(&o->pos, T[0], T[1], T[2]);
   ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
   ccdQuatInvert2(&o->rot_inv, &o->rot);
@@ -124,44 +124,44 @@ static void shapeToGJK(const ShapeBase& s, ccd_obj_t* o)
 */
 }
 
-static void boxToGJK(const Box& s, ccd_box_t* box)
+static void boxToGJK(const Box& s, const SimpleTransform& tf, ccd_box_t* box)
 {
-  shapeToGJK(s, box);
+  shapeToGJK(s, tf, box);
   box->dim[0] = s.side[0] / 2.0;
   box->dim[1] = s.side[1] / 2.0;
   box->dim[2] = s.side[2] / 2.0;
 }
 
-static void capToGJK(const Capsule& s, ccd_cap_t* cap)
+static void capToGJK(const Capsule& s, const SimpleTransform& tf, ccd_cap_t* cap)
 {
-  shapeToGJK(s, cap);
+  shapeToGJK(s, tf, cap);
   cap->radius = s.radius;
   cap->height = s.lz / 2;
 }
 
-static void cylToGJK(const Cylinder& s, ccd_cyl_t* cyl)
+static void cylToGJK(const Cylinder& s, const SimpleTransform& tf, ccd_cyl_t* cyl)
 {
-  shapeToGJK(s, cyl);
+  shapeToGJK(s, tf, cyl);
   cyl->radius = s.radius;
   cyl->height = s.lz / 2;
 }
 
-static void coneToGJK(const Cone& s, ccd_cone_t* cone)
+static void coneToGJK(const Cone& s, const SimpleTransform& tf, ccd_cone_t* cone)
 {
-  shapeToGJK(s, cone);
+  shapeToGJK(s, tf, cone);
   cone->radius = s.radius;
   cone->height = s.lz / 2;
 }
 
-static void sphereToGJK(const Sphere& s, ccd_sphere_t* sph)
+static void sphereToGJK(const Sphere& s, const SimpleTransform& tf, ccd_sphere_t* sph)
 {
-  shapeToGJK(s, sph);
+  shapeToGJK(s, tf, sph);
   sph->radius = s.radius;
 }
 
-static void convexToGJK(const Convex& s, ccd_convex_t* conv)
+static void convexToGJK(const Convex& s, const SimpleTransform& tf, ccd_convex_t* conv)
 {
-  shapeToGJK(s, conv);
+  shapeToGJK(s, tf, conv);
   conv->convex = &s;
 }
 
@@ -411,10 +411,10 @@ GJKCenterFunction GJKInitializer<Cylinder>::getCenterFunction()
 }
 
 
-void* GJKInitializer<Cylinder>::createGJKObject(const Cylinder& s)
+void* GJKInitializer<Cylinder>::createGJKObject(const Cylinder& s, const SimpleTransform& tf)
 {
   ccd_cyl_t* o = new ccd_cyl_t;
-  cylToGJK(s, o);
+  cylToGJK(s, tf, o);
   return o;
 }
 
@@ -438,10 +438,10 @@ GJKCenterFunction GJKInitializer<Sphere>::getCenterFunction()
 }
 
 
-void* GJKInitializer<Sphere>::createGJKObject(const Sphere& s)
+void* GJKInitializer<Sphere>::createGJKObject(const Sphere& s, const SimpleTransform& tf)
 {
   ccd_sphere_t* o = new ccd_sphere_t;
-  sphereToGJK(s, o);
+  sphereToGJK(s, tf, o);
   return o;
 }
 
@@ -463,10 +463,10 @@ GJKCenterFunction GJKInitializer<Box>::getCenterFunction()
 }
 
 
-void* GJKInitializer<Box>::createGJKObject(const Box& s)
+void* GJKInitializer<Box>::createGJKObject(const Box& s, const SimpleTransform& tf)
 {
   ccd_box_t* o = new ccd_box_t;
-  boxToGJK(s, o);
+  boxToGJK(s, tf, o);
   return o;
 }
 
@@ -490,10 +490,10 @@ GJKCenterFunction GJKInitializer<Capsule>::getCenterFunction()
 }
 
 
-void* GJKInitializer<Capsule>::createGJKObject(const Capsule& s)
+void* GJKInitializer<Capsule>::createGJKObject(const Capsule& s, const SimpleTransform& tf)
 {
   ccd_cap_t* o = new ccd_cap_t;
-  capToGJK(s, o);
+  capToGJK(s, tf, o);
   return o;
 }
 
@@ -517,10 +517,10 @@ GJKCenterFunction GJKInitializer<Cone>::getCenterFunction()
 }
 
 
-void* GJKInitializer<Cone>::createGJKObject(const Cone& s)
+void* GJKInitializer<Cone>::createGJKObject(const Cone& s, const SimpleTransform& tf)
 {
   ccd_cone_t* o = new ccd_cone_t;
-  coneToGJK(s, o);
+  coneToGJK(s, tf, o);
   return o;
 }
 
@@ -544,10 +544,10 @@ GJKCenterFunction GJKInitializer<Convex>::getCenterFunction()
 }
 
 
-void* GJKInitializer<Convex>::createGJKObject(const Convex& s)
+void* GJKInitializer<Convex>::createGJKObject(const Convex& s, const SimpleTransform& tf)
 {
   ccd_convex_t* o = new ccd_convex_t;
-  convexToGJK(s, o);
+  convexToGJK(s, tf, o);
   return o;
 }
 
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index 71551bd1..0f91c576 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -42,10 +42,10 @@ namespace fcl
 {
 
 template<>
-void computeBV<AABB>(const Box& s, AABB& bv)
+void computeBV<AABB>(const Box& s, const SimpleTransform& tf, AABB& bv)
 {
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   BVH_REAL x_range = 0.5 * (fabs(R[0][0] * s.side[0]) + fabs(R[0][1] * s.side[1]) + fabs(R[0][2] * s.side[2]));
   BVH_REAL y_range = 0.5 * (fabs(R[1][0] * s.side[0]) + fabs(R[1][1] * s.side[1]) + fabs(R[1][2] * s.side[2]));
@@ -56,19 +56,19 @@ void computeBV<AABB>(const Box& s, AABB& bv)
 }
 
 template<>
-void computeBV<AABB>(const Sphere& s, AABB& bv)
+void computeBV<AABB>(const Sphere& s, const SimpleTransform& tf, AABB& bv)
 {
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   bv.max_ = T + Vec3f(s.radius, s.radius, s.radius);
   bv.min_ = T + Vec3f(-s.radius, -s.radius, -s.radius);
 }
 
 template<>
-void computeBV<AABB>(const Capsule& s, AABB& bv)
+void computeBV<AABB>(const Capsule& s, const SimpleTransform& tf, AABB& bv)
 {
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   BVH_REAL x_range = 0.5 * fabs(R[0][2] * s.lz) + s.radius;
   BVH_REAL y_range = 0.5 * fabs(R[1][2] * s.lz) + s.radius;
@@ -79,10 +79,10 @@ void computeBV<AABB>(const Capsule& s, AABB& bv)
 }
 
 template<>
-void computeBV<AABB>(const Cone& s, AABB& bv)
+void computeBV<AABB>(const Cone& s, const SimpleTransform& tf, AABB& bv)
 {
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   BVH_REAL x_range = fabs(R[0][0] * s.radius) + fabs(R[0][1] * s.radius) + 0.5 * fabs(R[0][2] * s.lz);
   BVH_REAL y_range = fabs(R[1][0] * s.radius) + fabs(R[1][1] * s.radius) + 0.5 * fabs(R[1][2] * s.lz);
@@ -93,10 +93,10 @@ void computeBV<AABB>(const Cone& s, AABB& bv)
 }
 
 template<>
-void computeBV<AABB>(const Cylinder& s, AABB& bv)
+void computeBV<AABB>(const Cylinder& s, const SimpleTransform& tf, AABB& bv)
 {
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   BVH_REAL x_range = fabs(R[0][0] * s.radius) + fabs(R[0][1] * s.radius) + 0.5 * fabs(R[0][2] * s.lz);
   BVH_REAL y_range = fabs(R[1][0] * s.radius) + fabs(R[1][1] * s.radius) + 0.5 * fabs(R[1][2] * s.lz);
@@ -107,10 +107,10 @@ void computeBV<AABB>(const Cylinder& s, AABB& bv)
 }
 
 template<>
-void computeBV<AABB>(const Convex& s, AABB& bv)
+void computeBV<AABB>(const Convex& s, const SimpleTransform& tf, AABB& bv)
 {
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   AABB bv_;
   for(int i = 0; i < s.num_points; ++i)
@@ -123,9 +123,9 @@ void computeBV<AABB>(const Convex& s, AABB& bv)
 }
 
 template<>
-void computeBV<AABB>(const Plane& s, AABB& bv)
+void computeBV<AABB>(const Plane& s, const SimpleTransform& tf, AABB& bv)
 {
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
 
   Vec3f n = R * n;
 
@@ -154,10 +154,10 @@ void computeBV<AABB>(const Plane& s, AABB& bv)
 
 
 template<>
-void computeBV<OBB>(const Box& s, OBB& bv)
+void computeBV<OBB>(const Box& s, const SimpleTransform& tf, OBB& bv)
 {
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   bv.To = T;
   bv.axis[0] = R.getColumn(0);
@@ -167,9 +167,9 @@ void computeBV<OBB>(const Box& s, OBB& bv)
 }
 
 template<>
-void computeBV<OBB>(const Sphere& s, OBB& bv)
+void computeBV<OBB>(const Sphere& s, const SimpleTransform& tf, OBB& bv)
 {
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   bv.To = T;
   bv.axis[0].setValue(1, 0, 0);
@@ -179,10 +179,10 @@ void computeBV<OBB>(const Sphere& s, OBB& bv)
 }
 
 template<>
-void computeBV<OBB>(const Capsule& s, OBB& bv)
+void computeBV<OBB>(const Capsule& s, const SimpleTransform& tf, OBB& bv)
 {
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   bv.To = T;
   bv.axis[0] = R.getColumn(0);
@@ -192,10 +192,10 @@ void computeBV<OBB>(const Capsule& s, OBB& bv)
 }
 
 template<>
-void computeBV<OBB>(const Cone& s, OBB& bv)
+void computeBV<OBB>(const Cone& s, const SimpleTransform& tf, OBB& bv)
 {
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   bv.To = T;
   bv.axis[0] = R.getColumn(0);
@@ -205,10 +205,10 @@ void computeBV<OBB>(const Cone& s, OBB& bv)
 }
 
 template<>
-void computeBV<OBB>(const Cylinder& s, OBB& bv)
+void computeBV<OBB>(const Cylinder& s, const SimpleTransform& tf, OBB& bv)
 {
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   bv.To = T;
   bv.axis[0] = R.getColumn(0);
@@ -218,10 +218,10 @@ void computeBV<OBB>(const Cylinder& s, OBB& bv)
 }
 
 template<>
-void computeBV<OBB>(const Convex& s, OBB& bv)
+void computeBV<OBB>(const Convex& s, const SimpleTransform& tf, OBB& bv)
 {
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   fit(s.points, s.num_points, bv);
 
@@ -234,10 +234,10 @@ void computeBV<OBB>(const Convex& s, OBB& bv)
 }
 
 template<>
-void computeBV<OBB>(const Plane& s, OBB& bv)
+void computeBV<OBB>(const Plane& s, const SimpleTransform& tf, OBB& bv)
 {
-  Matrix3f R = s.getRotation() * s.getLocalRotation();
-  Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
+  Matrix3f R = tf.getRotation() * s.getLocalRotation();
+  Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
 
   // generate other two axes orthonormal to plane normal
   generateCoordinateSystem(s.n, bv.axis[1], bv.axis[2]);
@@ -251,49 +251,56 @@ void computeBV<OBB>(const Plane& s, OBB& bv)
 
 void Box::computeLocalAABB()
 {
-  computeBV<AABB>(*this, aabb);
+  AABB aabb;
+  computeBV<AABB>(*this, SimpleTransform(), aabb);
   aabb_center = aabb.center();
   aabb_radius = (aabb.min_ - aabb_center).length();
 }
 
 void Sphere::computeLocalAABB()
 {
-  computeBV<AABB>(*this, aabb);
+  AABB aabb;
+  computeBV<AABB>(*this, SimpleTransform(), aabb);
   aabb_center = aabb.center();
   aabb_radius = radius;
 }
 
 void Capsule::computeLocalAABB()
 {
-  computeBV<AABB>(*this, aabb);
+  AABB aabb;
+  computeBV<AABB>(*this, SimpleTransform(), aabb);
   aabb_center = aabb.center();
   aabb_radius = (aabb.min_ - aabb_center).length();
 }
 
 void Cone::computeLocalAABB()
 {
-  computeBV<AABB>(*this, aabb);
+  AABB aabb;
+  computeBV<AABB>(*this, SimpleTransform(), aabb);
   aabb_center = aabb.center();
   aabb_radius = (aabb.min_ - aabb_center).length();
 }
 
 void Cylinder::computeLocalAABB()
 {
-  computeBV<AABB>(*this, aabb);
+  AABB aabb;
+  computeBV<AABB>(*this, SimpleTransform(), aabb);
   aabb_center = aabb.center();
   aabb_radius = (aabb.min_ - aabb_center).length();
 }
 
 void Convex::computeLocalAABB()
 {
-  computeBV<AABB>(*this, aabb);
+  AABB aabb;
+  computeBV<AABB>(*this, SimpleTransform(), aabb);
   aabb_center = aabb.center();
   aabb_radius = (aabb.min_ - aabb_center).length();
 }
 
 void Plane::computeLocalAABB()
 {
-  computeBV<AABB>(*this, aabb);
+  AABB aabb;
+  computeBV<AABB>(*this, SimpleTransform(), aabb);
   aabb_center = aabb.center();
   aabb_radius = (aabb.min_ - aabb_center).length();
 }
diff --git a/trunk/fcl/src/intersect.cpp b/trunk/fcl/src/intersect.cpp
index 5f89cad6..436833b8 100644
--- a/trunk/fcl/src/intersect.cpp
+++ b/trunk/fcl/src/intersect.cpp
@@ -920,59 +920,59 @@ bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f
   Vec3f e1 = p2 - p1;
   Vec3f e2 = p3 - p2;
   Vec3f n1 = e1.cross(e2);
-  if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f f1 = q2 - q1;
   Vec3f f2 = q3 - q2;
   Vec3f m1 = f1.cross(f2);
-  if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f ef11 = e1.cross(f1);
-  if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f ef12 = e1.cross(f2);
-  if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f f3 = q1 - q3;
   Vec3f ef13 = e1.cross(f3);
-  if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f ef21 = e2.cross(f1);
-  if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f ef22 = e2.cross(f2);
-  if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f ef23 = e2.cross(f3);
-  if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f e3 = p1 - p3;
   Vec3f ef31 = e3.cross(f1);
-  if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f ef32 = e3.cross(f2);
-  if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f ef33 = e3.cross(f3);
-  if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f g1 = e1.cross(n1);
-  if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f g2 = e2.cross(n1);
-  if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f g3 = e3.cross(n1);
-  if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f h1 = f1.cross(m1);
-  if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f h2 = f2.cross(m1);
-  if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false;
 
   Vec3f h3 = f3.cross(m1);
-  if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0;
+  if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false;
 
   if(contact_points && num_contact_points && penetration_depth && normal)
   {
@@ -1018,7 +1018,7 @@ bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f
     }
   }
 
-  return 1;
+  return true;
 }
 #endif
 
@@ -1251,12 +1251,13 @@ int Intersect::project6(const Vec3f& ax,
   BVH_REAL Q2 = ax.dot(q2);
   BVH_REAL Q3 = ax.dot(q3);
 
-  BVH_REAL mx1 = std::max(P1, std::max(P2, P3));
   BVH_REAL mn1 = std::min(P1, std::min(P2, P3));
   BVH_REAL mx2 = std::max(Q1, std::max(Q2, Q3));
+  if(mn1 > mx2) return 0;
+
+  BVH_REAL mx1 = std::max(P1, std::max(P2, P3));
   BVH_REAL mn2 = std::min(Q1, std::min(Q2, Q3));
 
-  if(mn1 > mx2) return 0;
   if(mn2 > mx1) return 0;
   return 1;
 }
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/simple_setup.cpp
index 272f92e1..fa2fb1f6 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/simple_setup.cpp
@@ -40,7 +40,9 @@
 namespace fcl
 {
 
-bool initialize(MeshCollisionTraversalNodeOBB& node, const BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
+bool initialize(MeshCollisionTraversalNodeOBB& node,
+                const BVHModel<OBB>& model1, const SimpleTransform& tf1,
+                const BVHModel<OBB>& model2, const SimpleTransform& tf2,
                 int num_max_contacts, bool exhaustive, bool enable_contact)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -53,19 +55,23 @@ bool initialize(MeshCollisionTraversalNodeOBB& node, const BVHModel<OBB>& model1
   node.tri_indices2 = model2.tri_indices;
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.num_max_contacts = num_max_contacts;
   node.exhaustive = exhaustive;
   node.enable_contact = enable_contact;
 
-  relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
+  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
 
   return true;
 }
 
 
-bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
+bool initialize(MeshCollisionTraversalNodeRSS& node,
+                const BVHModel<RSS>& model1, const SimpleTransform& tf1,
+                const BVHModel<RSS>& model2, const SimpleTransform& tf2,
                 int num_max_contacts, bool exhaustive, bool enable_contact)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -78,20 +84,25 @@ bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel<RSS>& model1
   node.tri_indices2 = model2.tri_indices;
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
+
 
   node.num_max_contacts = num_max_contacts;
   node.exhaustive = exhaustive;
   node.enable_contact = enable_contact;
 
-  relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
+  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
 
   return true;
 }
 
 #if USE_SVMLIGHT
 
-bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, BVHModel<OBB>& model2,
+bool initialize(PointCloudCollisionTraversalNodeOBB& node,
+                BVHModel<OBB>& model1, const SimpleTransform& tf1,
+                BVHModel<OBB>& model2, const SimpleTransform& tf2,
                 BVH_REAL collision_prob_threshold,
                 int leaf_size_threshold,
                 int num_max_contacts,
@@ -104,7 +115,9 @@ bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1
     return false;
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -124,13 +137,15 @@ bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1
   node.collision_prob_threshold = collision_prob_threshold;
   node.leaf_size_threshold = leaf_size_threshold;
 
-  relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
+  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
 
   return true;
 }
 
 
-bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, BVHModel<RSS>& model2,
+bool initialize(PointCloudCollisionTraversalNodeRSS& node,
+                BVHModel<RSS>& model1, const SimpleTransform& tf1,
+                BVHModel<RSS>& model2, const SimpleTransform& tf2,
                 BVH_REAL collision_prob_threshold,
                 int leaf_size_threshold,
                 int num_max_contacts,
@@ -143,7 +158,9 @@ bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1
     return false;
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -163,12 +180,14 @@ bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1
   node.collision_prob_threshold = collision_prob_threshold;
   node.leaf_size_threshold = leaf_size_threshold;
 
-  relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
+  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
 
   return true;
 }
 
-bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
+bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
+                BVHModel<OBB>& model1, const SimpleTransform& tf1,
+                const BVHModel<OBB>& model2, const SimpleTransform& tf2,
                 BVH_REAL collision_prob_threshold,
                 int leaf_size_threshold,
                 int num_max_contacts,
@@ -180,7 +199,9 @@ bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& mo
     return false;
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -198,13 +219,15 @@ bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& mo
   node.collision_prob_threshold = collision_prob_threshold;
   node.leaf_size_threshold = leaf_size_threshold;
 
-  relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
+  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
 
   return true;
 }
 
 
-bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
+bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
+                BVHModel<RSS>& model1, const SimpleTransform& tf1,
+                const BVHModel<RSS>& model2, const SimpleTransform& tf2,
                 BVH_REAL collision_prob_threshold,
                 int leaf_size_threshold,
                 int num_max_contacts,
@@ -216,7 +239,9 @@ bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& mo
     return false;
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -234,20 +259,24 @@ bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& mo
   node.collision_prob_threshold = collision_prob_threshold;
   node.leaf_size_threshold = leaf_size_threshold;
 
-  relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
+  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
 
   return true;
 }
 
 #endif
 
-bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2)
+bool initialize(MeshDistanceTraversalNodeRSS& node,
+                const BVHModel<RSS>& model1, const SimpleTransform& tf1,
+                const BVHModel<RSS>& model2, const SimpleTransform& tf2)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
 
   node.model1 = &model1;
+  node.tf1 = tf1;
   node.model2 = &model2;
+  node.tf2 = tf2;
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -255,7 +284,7 @@ bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel<RSS>& model1,
   node.tri_indices1 = model1.tri_indices;
   node.tri_indices2 = model2.tri_indices;
 
-  relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
+  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
 
   return true;
 }
diff --git a/trunk/fcl/test/test_core_broad_phase.cpp b/trunk/fcl/test/test_core_broad_phase.cpp
index 5a6f91af..63d33f7c 100644
--- a/trunk/fcl/test/test_core_broad_phase.cpp
+++ b/trunk/fcl/test/test_core_broad_phase.cpp
@@ -221,8 +221,8 @@ void generateEnvironments(std::vector<CollisionObject*>& env, int n)
   {
     BVHModel<OBB>* model = new BVHModel<OBB>();
     box.setLocalTransform(transforms[i].R, transforms[i].T);
-    generateBVHModel(*model, box);
-    env.push_back(model);
+    generateBVHModel(*model, box, SimpleTransform());
+    env.push_back(new CollisionObject(model));
   }
 
   generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
@@ -231,8 +231,8 @@ void generateEnvironments(std::vector<CollisionObject*>& env, int n)
   {
     BVHModel<OBB>* model = new BVHModel<OBB>();
     sphere.setLocalTransform(transforms[i].R, transforms[i].T);
-    generateBVHModel(*model, sphere);
-    env.push_back(model);
+    generateBVHModel(*model, sphere, SimpleTransform());
+    env.push_back(new CollisionObject(model));
   }
 
   generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
@@ -241,7 +241,7 @@ void generateEnvironments(std::vector<CollisionObject*>& env, int n)
   {
     BVHModel<OBB>* model = new BVHModel<OBB>();
     cylinder.setLocalTransform(transforms[i].R, transforms[i].T);
-    generateBVHModel(*model, cylinder);
-    env.push_back(model);
+    generateBVHModel(*model, cylinder, SimpleTransform());
+    env.push_back(new CollisionObject(model));
   }
 }
diff --git a/trunk/fcl/test/test_core_collision.cpp b/trunk/fcl/test/test_core_collision.cpp
index d198d50d..6623189f 100644
--- a/trunk/fcl/test/test_core_collision.cpp
+++ b/trunk/fcl/test/test_core_collision.cpp
@@ -562,9 +562,11 @@ bool collide_Test2(const Transform& tf,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
+  SimpleTransform pose1, pose2;
+
   MeshCollisionTraversalNode<BV> node;
 
-  if(!initialize<BV>(node, m1, m2))
+  if(!initialize<BV>(node, m1, pose1, m2, pose2))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -629,16 +631,11 @@ bool collide_Test(const Transform& tf,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf.R, tf.T), pose2;
 
   MeshCollisionTraversalNode<BV> node;
 
-  if(!initialize<BV>(node, m1, m2))
+  if(!initialize<BV>(node, m1, pose1, m2, pose2))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -700,15 +697,10 @@ bool collide_Test_OBB(const Transform& tf,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf.R, tf.T), pose2;
 
   MeshCollisionTraversalNodeOBB node;
-  if(!initialize(node, (const BVHModel<OBB>&)m1, (const BVHModel<OBB>&)m2))
+  if(!initialize(node, (const BVHModel<OBB>&)m1, pose1, (const BVHModel<OBB>&)m2, pose2))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -770,15 +762,10 @@ bool collide_Test_RSS(const Transform& tf,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf.R, tf.T), pose2;
 
   MeshCollisionTraversalNodeRSS node;
-  if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2))
+  if(!initialize(node, (const BVHModel<RSS>&)m1, pose1, (const BVHModel<RSS>&)m2, pose2))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -841,18 +828,10 @@ bool test_collide_func(const Transform& tf,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setRotation(tf.R);
-  m1.setTranslation(tf.T);
-  m2.setRotation(R2);
-  m2.setTranslation(T2);
-
+  SimpleTransform pose1(tf.R, tf.T), pose2;
 
   std::vector<Contact> contacts;
-  int num_contacts = collide(&m1, &m2, num_max_contacts, enable_contact, exhaustive, contacts);
+  int num_contacts = collide(&m1, pose1, &m2, pose2, num_max_contacts, enable_contact, exhaustive, contacts);
   global_pairs_now.resize(num_contacts);
 
   for(int i = 0; i < num_contacts; ++i)
diff --git a/trunk/fcl/test/test_core_collision_point.cpp b/trunk/fcl/test/test_core_collision_point.cpp
index ac7bbe08..832f8cc7 100644
--- a/trunk/fcl/test/test_core_collision_point.cpp
+++ b/trunk/fcl/test/test_core_collision_point.cpp
@@ -161,16 +161,11 @@ bool collide_Test_PP(const Transform& tf,
   m2.addSubModel(vertices2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf.R, tf.T), pose2;
 
   PointCloudCollisionTraversalNode<BV> node;
 
-  if(!initialize<BV, false, false>(node, m1, m2, 0.6, 20))
+  if(!initialize<BV, false, false>(node, m1, pose1, m2, pose2, 0.6, 20))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -224,16 +219,11 @@ bool collide_Test_PP_OBB(const Transform& tf,
   m2.addSubModel(vertices2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf.R, tf.T), pose2;
 
   PointCloudCollisionTraversalNodeOBB node;
 
-  if(!initialize(node, m1, m2, 0.6, 20))
+  if(!initialize(node, m1, pose1, m2, pose2, 0.6, 20))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -291,16 +281,11 @@ bool collide_Test_MP(const Transform& tf,
   m2.addSubModel(vertices2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf.R, tf.T), pose2;
 
   PointCloudMeshCollisionTraversalNode<BV> node;
 
-  if(!initialize<BV, false, false>(node, m2, m1, 0.6, 20))
+  if(!initialize<BV, false, false>(node, m2, pose2, m1, pose1, 0.6, 20))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -357,16 +342,11 @@ bool collide_Test_MP_OBB(const Transform& tf,
   m2.addSubModel(vertices2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf.R, tf.T), pose2;
 
   PointCloudMeshCollisionTraversalNodeOBB node;
 
-  if(!initialize(node, m2, m1, 0.6, 20))
+  if(!initialize(node, m2, pose2, m1, pose1, 0.6, 20))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -424,16 +404,11 @@ bool collide_Test_PM(const Transform& tf,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf.R, tf.T), pose2;
 
   PointCloudMeshCollisionTraversalNode<BV> node;
 
-  if(!initialize<BV, false, false>(node, m1, m2, 0.6, 20))
+  if(!initialize<BV, false, false>(node, m1, pose1, m2, pose2, 0.6, 20))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -489,16 +464,11 @@ bool collide_Test_PM_OBB(const Transform& tf,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf.R, tf.T), pose2;
 
   PointCloudMeshCollisionTraversalNodeOBB node;
 
-  if(!initialize(node, m1, m2, 0.6, 20))
+  if(!initialize(node, m1, pose1, m2, pose2, 0.6, 20))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
diff --git a/trunk/fcl/test/test_core_collision_shape_mesh_consistency.cpp b/trunk/fcl/test/test_core_collision_shape_mesh_consistency.cpp
index ce68dc21..cfa608fa 100644
--- a/trunk/fcl/test/test_core_collision_shape_mesh_consistency.cpp
+++ b/trunk/fcl/test/test_core_collision_shape_mesh_consistency.cpp
@@ -50,202 +50,209 @@ TEST(consistency_shapemesh, spheresphere)
   BVHModel<OBB> s1_obb;
   BVHModel<OBB> s2_obb;
 
-  generateBVHModel(s1_aabb, s1);
-  generateBVHModel(s2_aabb, s2);
-  generateBVHModel(s1_obb, s1);
-  generateBVHModel(s2_obb, s2);
+  generateBVHModel(s1_aabb, s1, SimpleTransform());
+  generateBVHModel(s2_aabb, s2, SimpleTransform());
+  generateBVHModel(s1_obb, s1, SimpleTransform());
+  generateBVHModel(s2_obb, s2, SimpleTransform());
 
   std::vector<Contact> contacts;
   bool res;
 
+  SimpleTransform pose, pose_aabb, pose_obb;
+
+
   // s2 is within s1
   // both are shapes --> collision
   // s1 is shape, s2 is mesh --> in collision
   // s1 is mesh, s2 is shape --> collision free
   // all are reasonable
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
+  pose.setTranslation(Vec3f(40, 0, 0));
+  pose_aabb.setTranslation(Vec3f(40, 0, 0));
+  pose_obb.setTranslation(Vec3f(40, 0, 0));
 
-  s2.setTranslation(Vec3f(40, 0, 0));
-  s2_aabb.setTranslation(Vec3f(40, 0, 0));
-  s2_obb.setTranslation(Vec3f(40, 0, 0));
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s2.setTranslation(Vec3f(30, 0, 0));
-  s2_aabb.setTranslation(Vec3f(30, 0, 0));
-  s2_obb.setTranslation(Vec3f(30, 0, 0));
+  pose.setTranslation(Vec3f(30, 0, 0));
+  pose_aabb.setTranslation(Vec3f(30, 0, 0));
+  pose_obb.setTranslation(Vec3f(30, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s2.setTranslation(Vec3f(29.9, 0, 0));
-  s2_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
-  s2_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose.setTranslation(Vec3f(29.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
 
-  s2.setTranslation(Vec3f(-29.9, 0, 0));
-  s2_aabb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
-  s2_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose.setTranslation(Vec3f(-29.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s2.setTranslation(Vec3f(-30, 0, 0));
-  s2_aabb.setTranslation(Vec3f(-30, 0, 0));
-  s2_obb.setTranslation(Vec3f(-30, 0, 0));
+  pose.setTranslation(Vec3f(-30, 0, 0));
+  pose_aabb.setTranslation(Vec3f(-30, 0, 0));
+  pose_obb.setTranslation(Vec3f(-30, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 }
 
@@ -261,110 +268,113 @@ TEST(consistency_shapemesh, boxbox)
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_aabb, s1);
-  generateBVHModel(s2_aabb, s2);
-  generateBVHModel(s1_obb, s1);
-  generateBVHModel(s2_obb, s2);
-  generateBVHModel(s1_rss, s1);
-  generateBVHModel(s2_rss, s2);
+  generateBVHModel(s1_aabb, s1, SimpleTransform());
+  generateBVHModel(s2_aabb, s2, SimpleTransform());
+  generateBVHModel(s1_obb, s1, SimpleTransform());
+  generateBVHModel(s2_obb, s2, SimpleTransform());
+  generateBVHModel(s1_rss, s1, SimpleTransform());
+  generateBVHModel(s2_rss, s2, SimpleTransform());
 
   std::vector<Contact> contacts;
   bool res;
 
+  SimpleTransform pose, pose_aabb, pose_obb;
+
   // s2 is within s1
   // both are shapes --> collision
   // s1 is shape, s2 is mesh --> in collision
   // s1 is mesh, s2 is shape --> collision free
   // all are reasonable
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
+  pose.setTranslation(Vec3f(15.01, 0, 0));
+  pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
+  pose_obb.setTranslation(Vec3f(15.01, 0, 0));
 
-  s2.setTranslation(Vec3f(15.01, 0, 0));
-  s2_aabb.setTranslation(Vec3f(15.01, 0, 0));
-  s2_obb.setTranslation(Vec3f(15.01, 0, 0));
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s2.setTranslation(Vec3f(14.99, 0, 0));
-  s2_aabb.setTranslation(Vec3f(14.99, 0, 0));
-  s2_obb.setTranslation(Vec3f(14.99, 0, 0));
+  pose.setTranslation(Vec3f(14.99, 0, 0));
+  pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
+  pose_obb.setTranslation(Vec3f(14.99, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 }
 
@@ -380,109 +390,113 @@ TEST(consistency_shapemesh, spherebox)
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_aabb, s1);
-  generateBVHModel(s2_aabb, s2);
-  generateBVHModel(s1_obb, s1);
-  generateBVHModel(s2_obb, s2);
-  generateBVHModel(s1_rss, s1);
-  generateBVHModel(s2_rss, s2);
+  generateBVHModel(s1_aabb, s1, SimpleTransform());
+  generateBVHModel(s2_aabb, s2, SimpleTransform());
+  generateBVHModel(s1_obb, s1, SimpleTransform());
+  generateBVHModel(s2_obb, s2, SimpleTransform());
+  generateBVHModel(s1_rss, s1, SimpleTransform());
+  generateBVHModel(s2_rss, s2, SimpleTransform());
 
   std::vector<Contact> contacts;
   bool res;
 
+  SimpleTransform pose, pose_aabb, pose_obb;
+
   // s2 is within s1
   // both are shapes --> collision
   // s1 is shape, s2 is mesh --> in collision
   // s1 is mesh, s2 is shape --> collision free
   // all are reasonable
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s2.setTranslation(Vec3f(22.4, 0, 0));
-  s2_aabb.setTranslation(Vec3f(22.4, 0, 0));
-  s2_obb.setTranslation(Vec3f(22.4, 0, 0));
+  pose.setTranslation(Vec3f(22.4, 0, 0));
+  pose_aabb.setTranslation(Vec3f(22.4, 0, 0));
+  pose_obb.setTranslation(Vec3f(22.4, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s2.setTranslation(Vec3f(22.51, 0, 0));
-  s2_aabb.setTranslation(Vec3f(22.51, 0, 0));
-  s2_obb.setTranslation(Vec3f(22.51, 0, 0));
+  pose.setTranslation(Vec3f(22.51, 0, 0));
+  pose_aabb.setTranslation(Vec3f(22.51, 0, 0));
+  pose_obb.setTranslation(Vec3f(22.51, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 }
 
@@ -498,76 +512,80 @@ TEST(consistency_shapemesh, cylindercylinder)
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_aabb, s1);
-  generateBVHModel(s2_aabb, s2);
-  generateBVHModel(s1_obb, s1);
-  generateBVHModel(s2_obb, s2);
-  generateBVHModel(s1_rss, s1);
-  generateBVHModel(s2_rss, s2);
+  generateBVHModel(s1_aabb, s1, SimpleTransform());
+  generateBVHModel(s2_aabb, s2, SimpleTransform());
+  generateBVHModel(s1_obb, s1, SimpleTransform());
+  generateBVHModel(s2_obb, s2, SimpleTransform());
+  generateBVHModel(s1_rss, s1, SimpleTransform());
+  generateBVHModel(s2_rss, s2, SimpleTransform());
 
   std::vector<Contact> contacts;
   bool res;
 
-  s2.setTranslation(Vec3f(9.99, 0, 0));
-  s2_aabb.setTranslation(Vec3f(9.99, 0, 0));
-  s2_obb.setTranslation(Vec3f(9.99, 0, 0));
+  SimpleTransform pose, pose_aabb, pose_obb;
+
+  pose.setTranslation(Vec3f(9.99, 0, 0));
+  pose_aabb.setTranslation(Vec3f(9.99, 0, 0));
+  pose_obb.setTranslation(Vec3f(9.99, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s2.setTranslation(Vec3f(10.01, 0, 0));
-  s2_aabb.setTranslation(Vec3f(10.01, 0, 0));
-  s2_obb.setTranslation(Vec3f(10.01, 0, 0));
+  pose.setTranslation(Vec3f(10.01, 0, 0));
+  pose_aabb.setTranslation(Vec3f(10.01, 0, 0));
+  pose_obb.setTranslation(Vec3f(10.01, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 }
 
@@ -583,138 +601,144 @@ TEST(consistency_shapemesh, conecone)
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_aabb, s1);
-  generateBVHModel(s2_aabb, s2);
-  generateBVHModel(s1_obb, s1);
-  generateBVHModel(s2_obb, s2);
-  generateBVHModel(s1_rss, s1);
-  generateBVHModel(s2_rss, s2);
+  generateBVHModel(s1_aabb, s1, SimpleTransform());
+  generateBVHModel(s2_aabb, s2, SimpleTransform());
+  generateBVHModel(s1_obb, s1, SimpleTransform());
+  generateBVHModel(s2_obb, s2, SimpleTransform());
+  generateBVHModel(s1_rss, s1, SimpleTransform());
+  generateBVHModel(s2_rss, s2, SimpleTransform());
 
   std::vector<Contact> contacts;
   bool res;
 
-  s2.setTranslation(Vec3f(9.9, 0, 0));
-  s2_aabb.setTranslation(Vec3f(9.9, 0, 0));
-  s2_obb.setTranslation(Vec3f(9.9, 0, 0));
+  SimpleTransform pose, pose_aabb, pose_obb;
+
+  pose.setTranslation(Vec3f(9.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(9.9, 0, 0));
+  pose_obb.setTranslation(Vec3f(9.9, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s2.setTranslation(Vec3f(10.1, 0, 0));
-  s2_aabb.setTranslation(Vec3f(10.1, 0, 0));
-  s2_obb.setTranslation(Vec3f(10.1, 0, 0));
+  pose.setTranslation(Vec3f(10.1, 0, 0));
+  pose_aabb.setTranslation(Vec3f(10.1, 0, 0));
+  pose_obb.setTranslation(Vec3f(10.1, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s2.setTranslation(Vec3f(0, 0, 9.9));
-  s2_aabb.setTranslation(Vec3f(0, 0, 9.9));
-  s2_obb.setTranslation(Vec3f(0, 0, 9.9));
+  pose.setTranslation(Vec3f(0, 0, 9.9));
+  pose_aabb.setTranslation(Vec3f(0, 0, 9.9));
+  pose_obb.setTranslation(Vec3f(0, 0, 9.9));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s2.setTranslation(Vec3f(0, 0, 10.1));
-  s2_aabb.setTranslation(Vec3f(0, 0, 10.1));
-  s2_obb.setTranslation(Vec3f(0, 0, 10.1));
+  pose.setTranslation(Vec3f(0, 0, 10.1));
+  pose_aabb.setTranslation(Vec3f(0, 0, 10.1));
+  pose_obb.setTranslation(Vec3f(0, 0, 10.1));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_aabb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s2, pose, &s1_obb, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 }
 
@@ -729,116 +753,124 @@ TEST(consistency, spheresphere)
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_aabb, s1);
-  generateBVHModel(s2_aabb, s2);
-  generateBVHModel(s1_obb, s1);
-  generateBVHModel(s2_obb, s2);
-  generateBVHModel(s1_rss, s1);
-  generateBVHModel(s2_rss, s2);
+  generateBVHModel(s1_aabb, s1, SimpleTransform());
+  generateBVHModel(s2_aabb, s2, SimpleTransform());
+  generateBVHModel(s1_obb, s1, SimpleTransform());
+  generateBVHModel(s2_obb, s2, SimpleTransform());
+  generateBVHModel(s1_rss, s1, SimpleTransform());
+  generateBVHModel(s2_rss, s2, SimpleTransform());
 
   std::vector<Contact> contacts;
   bool res;
 
-  s2.setTranslation(Vec3f(40, 0, 0));
-  s2_aabb.setTranslation(Vec3f(40, 0, 0));
-  s2_obb.setTranslation(Vec3f(40, 0, 0));
-  s2_rss.setTranslation(Vec3f(40, 0, 0));
+  SimpleTransform pose, pose_aabb, pose_obb, pose_rss;
+
+  pose.setTranslation(Vec3f(40, 0, 0));
+  pose_aabb.setTranslation(Vec3f(40, 0, 0));
+  pose_obb.setTranslation(Vec3f(40, 0, 0));
+  pose_rss.setTranslation(Vec3f(40, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s2.setTranslation(Vec3f(30, 0, 0));
-  s2_aabb.setTranslation(Vec3f(30, 0, 0));
-  s2_obb.setTranslation(Vec3f(30, 0, 0));
-  s2_rss.setTranslation(Vec3f(30, 0, 0));
+  pose.setTranslation(Vec3f(30, 0, 0));
+  pose_aabb.setTranslation(Vec3f(30, 0, 0));
+  pose_obb.setTranslation(Vec3f(30, 0, 0));
+  pose_rss.setTranslation(Vec3f(30, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s2.setTranslation(Vec3f(29.9, 0, 0));
-  s2_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
-  s2_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
-  s2_rss.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose.setTranslation(Vec3f(29.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose_rss.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s2.setTranslation(Vec3f(0, 0, 0));
-  s2_aabb.setTranslation(Vec3f(0, 0, 0)); // mesh can not detect collision when ball contains ball
-  s2_obb.setTranslation(Vec3f(0, 0, 0)); // mesh can not detect collision when ball contains ball
-  s2_rss.setTranslation(Vec3f(0, 0, 0)); // mesh can not detect collision when ball contains ball
+  pose.setTranslation(Vec3f(0, 0, 0));
+  pose_aabb.setTranslation(Vec3f(0, 0, 0)); // mesh can not detect collision when ball contains ball
+  pose_obb.setTranslation(Vec3f(0, 0, 0)); // mesh can not detect collision when ball contains ball
+  pose_rss.setTranslation(Vec3f(0, 0, 0)); // mesh can not detect collision when ball contains ball
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s2.setTranslation(Vec3f(-29.9, 0, 0));
-  s2_aabb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
-  s2_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
-  s2_rss.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose.setTranslation(Vec3f(-29.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose_rss.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s2.setTranslation(Vec3f(-30, 0, 0));
-  s2_aabb.setTranslation(Vec3f(-30, 0, 0));
-  s2_obb.setTranslation(Vec3f(-30, 0, 0));
-  s2_rss.setTranslation(Vec3f(-30, 0, 0));
+  pose.setTranslation(Vec3f(-30, 0, 0));
+  pose_aabb.setTranslation(Vec3f(-30, 0, 0));
+  pose_obb.setTranslation(Vec3f(-30, 0, 0));
+  pose_rss.setTranslation(Vec3f(-30, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 }
 
@@ -854,61 +886,65 @@ TEST(consistency, boxbox)
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_aabb, s1);
-  generateBVHModel(s2_aabb, s2);
-  generateBVHModel(s1_obb, s1);
-  generateBVHModel(s2_obb, s2);
-  generateBVHModel(s1_rss, s1);
-  generateBVHModel(s2_rss, s2);
+  generateBVHModel(s1_aabb, s1, SimpleTransform());
+  generateBVHModel(s2_aabb, s2, SimpleTransform());
+  generateBVHModel(s1_obb, s1, SimpleTransform());
+  generateBVHModel(s2_obb, s2, SimpleTransform());
+  generateBVHModel(s1_rss, s1, SimpleTransform());
+  generateBVHModel(s2_rss, s2, SimpleTransform());
 
   std::vector<Contact> contacts;
   bool res;
 
+  SimpleTransform pose, pose_aabb, pose_obb, pose_rss;
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res); // mesh can not detect collision when box contains box
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res); // mesh can not detect collision when box contains box
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res); // mesh can not detect collision when box contains box
 
-  s2.setTranslation(Vec3f(15.01, 0, 0));
-  s2_aabb.setTranslation(Vec3f(15.01, 0, 0));
-  s2_obb.setTranslation(Vec3f(15.01, 0, 0));
-  s2_rss.setTranslation(Vec3f(15.01, 0, 0));
+  pose.setTranslation(Vec3f(15.01, 0, 0));
+  pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
+  pose_obb.setTranslation(Vec3f(15.01, 0, 0));
+  pose_rss.setTranslation(Vec3f(15.01, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s2.setTranslation(Vec3f(14.99, 0, 0));
-  s2_aabb.setTranslation(Vec3f(14.99, 0, 0));
-  s2_obb.setTranslation(Vec3f(14.99, 0, 0));
-  s2_rss.setTranslation(Vec3f(14.99, 0, 0));
+  pose.setTranslation(Vec3f(14.99, 0, 0));
+  pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
+  pose_obb.setTranslation(Vec3f(14.99, 0, 0));
+  pose_rss.setTranslation(Vec3f(14.99, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 }
 
@@ -924,61 +960,65 @@ TEST(consistency, spherebox)
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_aabb, s1);
-  generateBVHModel(s2_aabb, s2);
-  generateBVHModel(s1_obb, s1);
-  generateBVHModel(s2_obb, s2);
-  generateBVHModel(s1_rss, s1);
-  generateBVHModel(s2_rss, s2);
+  generateBVHModel(s1_aabb, s1, SimpleTransform());
+  generateBVHModel(s2_aabb, s2, SimpleTransform());
+  generateBVHModel(s1_obb, s1, SimpleTransform());
+  generateBVHModel(s2_obb, s2, SimpleTransform());
+  generateBVHModel(s1_rss, s1, SimpleTransform());
+  generateBVHModel(s2_rss, s2, SimpleTransform());
 
   std::vector<Contact> contacts;
   bool res;
 
+  SimpleTransform pose, pose_aabb, pose_obb, pose_rss;
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res); // mesh can not detect collision when box contains box
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res); // mesh can not detect collision when box contains box
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res); // mesh can not detect collision when box contains box
 
-  s2.setTranslation(Vec3f(22.4, 0, 0));
-  s2_aabb.setTranslation(Vec3f(22.4, 0, 0));
-  s2_obb.setTranslation(Vec3f(22.4, 0, 0));
-  s2_rss.setTranslation(Vec3f(22.4, 0, 0));
+  pose.setTranslation(Vec3f(22.4, 0, 0));
+  pose_aabb.setTranslation(Vec3f(22.4, 0, 0));
+  pose_obb.setTranslation(Vec3f(22.4, 0, 0));
+  pose_rss.setTranslation(Vec3f(22.4, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s2.setTranslation(Vec3f(22.51, 0, 0));
-  s2_aabb.setTranslation(Vec3f(22.51, 0, 0));
-  s2_obb.setTranslation(Vec3f(22.51, 0, 0));
-  s2_rss.setTranslation(Vec3f(22.51, 0, 0));
+  pose.setTranslation(Vec3f(22.51, 0, 0));
+  pose_aabb.setTranslation(Vec3f(22.51, 0, 0));
+  pose_obb.setTranslation(Vec3f(22.51, 0, 0));
+  pose_rss.setTranslation(Vec3f(22.51, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 }
 
@@ -994,48 +1034,52 @@ TEST(consistency, cylindercylinder)
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_aabb, s1);
-  generateBVHModel(s2_aabb, s2);
-  generateBVHModel(s1_obb, s1);
-  generateBVHModel(s2_obb, s2);
-  generateBVHModel(s1_rss, s1);
-  generateBVHModel(s2_rss, s2);
+  generateBVHModel(s1_aabb, s1, SimpleTransform());
+  generateBVHModel(s2_aabb, s2, SimpleTransform());
+  generateBVHModel(s1_obb, s1, SimpleTransform());
+  generateBVHModel(s2_obb, s2, SimpleTransform());
+  generateBVHModel(s1_rss, s1, SimpleTransform());
+  generateBVHModel(s2_rss, s2, SimpleTransform());
 
   std::vector<Contact> contacts;
   bool res;
 
-  s2.setTranslation(Vec3f(9.99, 0, 0));
-  s2_aabb.setTranslation(Vec3f(9.99, 0, 0));
-  s2_obb.setTranslation(Vec3f(9.99, 0, 0));
-  s2_rss.setTranslation(Vec3f(9.99, 0, 0));
+  SimpleTransform pose, pose_aabb, pose_obb, pose_rss;
+
+  pose.setTranslation(Vec3f(9.99, 0, 0));
+  pose_aabb.setTranslation(Vec3f(9.99, 0, 0));
+  pose_obb.setTranslation(Vec3f(9.99, 0, 0));
+  pose_rss.setTranslation(Vec3f(9.99, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s2.setTranslation(Vec3f(10.01, 0, 0));
-  s2_aabb.setTranslation(Vec3f(10.01, 0, 0));
-  s2_obb.setTranslation(Vec3f(10.01, 0, 0));
-  s2_rss.setTranslation(Vec3f(10.01, 0, 0));
+  pose.setTranslation(Vec3f(10.01, 0, 0));
+  pose_aabb.setTranslation(Vec3f(10.01, 0, 0));
+  pose_obb.setTranslation(Vec3f(10.01, 0, 0));
+  pose_rss.setTranslation(Vec3f(10.01, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 }
 
@@ -1051,82 +1095,88 @@ TEST(consistency, conecone)
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_aabb, s1);
-  generateBVHModel(s2_aabb, s2);
-  generateBVHModel(s1_obb, s1);
-  generateBVHModel(s2_obb, s2);
-  generateBVHModel(s1_rss, s1);
-  generateBVHModel(s2_rss, s2);
+  generateBVHModel(s1_aabb, s1, SimpleTransform());
+  generateBVHModel(s2_aabb, s2, SimpleTransform());
+  generateBVHModel(s1_obb, s1, SimpleTransform());
+  generateBVHModel(s2_obb, s2, SimpleTransform());
+  generateBVHModel(s1_rss, s1, SimpleTransform());
+  generateBVHModel(s2_rss, s2, SimpleTransform());
 
   std::vector<Contact> contacts;
   bool res;
 
-  s2.setTranslation(Vec3f(9.9, 0, 0));
-  s2_aabb.setTranslation(Vec3f(9.9, 0, 0));
-  s2_obb.setTranslation(Vec3f(9.9, 0, 0));
-  s2_rss.setTranslation(Vec3f(9.9, 0, 0));
+  SimpleTransform pose, pose_aabb, pose_obb, pose_rss;
+
+  pose.setTranslation(Vec3f(9.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(9.9, 0, 0));
+  pose_obb.setTranslation(Vec3f(9.9, 0, 0));
+  pose_rss.setTranslation(Vec3f(9.9, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s2.setTranslation(Vec3f(10.1, 0, 0));
-  s2_aabb.setTranslation(Vec3f(10.1, 0, 0));
-  s2_obb.setTranslation(Vec3f(10.1, 0, 0));
-  s2_rss.setTranslation(Vec3f(10.1, 0, 0));
+  pose.setTranslation(Vec3f(10.1, 0, 0));
+  pose_aabb.setTranslation(Vec3f(10.1, 0, 0));
+  pose_obb.setTranslation(Vec3f(10.1, 0, 0));
+  pose_rss.setTranslation(Vec3f(10.1, 0, 0));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s2.setTranslation(Vec3f(0, 0, 9.9));
-  s2_aabb.setTranslation(Vec3f(0, 0, 9.9));
-  s2_obb.setTranslation(Vec3f(0, 0, 9.9));
-  s2_rss.setTranslation(Vec3f(0, 0, 9.9));
+  pose.setTranslation(Vec3f(0, 0, 9.9));
+  pose_aabb.setTranslation(Vec3f(0, 0, 9.9));
+  pose_obb.setTranslation(Vec3f(0, 0, 9.9));
+  pose_rss.setTranslation(Vec3f(0, 0, 9.9));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s2.setTranslation(Vec3f(0, 0, 10.1));
-  s2_aabb.setTranslation(Vec3f(0, 0, 10.1));
-  s2_obb.setTranslation(Vec3f(0, 0, 10.1));
-  s2_rss.setTranslation(Vec3f(0, 0, 10.1));
+  pose.setTranslation(Vec3f(0, 0, 10.1));
+  pose_aabb.setTranslation(Vec3f(0, 0, 10.1));
+  pose_obb.setTranslation(Vec3f(0, 0, 10.1));
+  pose_rss.setTranslation(Vec3f(0, 0, 10.1));
+
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, pose, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_aabb, &s2_aabb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_aabb, SimpleTransform(), &s2_aabb, pose_aabb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_obb, &s2_obb, 1, false, false, contacts) > 0);
+  res = (collide(&s1_obb, SimpleTransform(), &s2_obb, pose_obb, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1_rss, &s2_rss, 1, false, false, contacts) > 0);
+  res = (collide(&s1_rss, SimpleTransform(), &s2_rss, pose_rss, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 }
 
diff --git a/trunk/fcl/test/test_core_conservative_advancement.cpp b/trunk/fcl/test/test_core_conservative_advancement.cpp
index 91dba4a6..d59131f2 100644
--- a/trunk/fcl/test/test_core_conservative_advancement.cpp
+++ b/trunk/fcl/test/test_core_conservative_advancement.cpp
@@ -295,16 +295,12 @@ bool linear_interp_Test(const Transform& tf1, const Transform& tf2,
   {
     BVH_REAL curt = i / (BVH_REAL)nsamples;
 
-    Matrix3f R;
-    Vec3f T;
+    SimpleTransform pose;
     motion1.integrate(curt);
-    motion1.getCurrentTransform(R, T);
-
-    m1.setTransform(R, T);
-    m2.setTransform(R2, T2);
+    motion1.getCurrentTransform(pose);
 
     MeshCollisionTraversalNodeRSS node;
-    if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2))
+    if(!initialize(node, (const BVHModel<RSS>&)m1, pose, (const BVHModel<RSS>&)m2, SimpleTransform(R2, T2)))
       std::cout << "initialize error" << std::endl;
 
     node.enable_statistics = false;
@@ -361,16 +357,12 @@ bool screw_interp_Test(const Transform& tf1, const Transform& tf2,
   {
     BVH_REAL curt = i / (BVH_REAL)nsamples;
 
-    Matrix3f R;
-    Vec3f T;
+    SimpleTransform pose;
     motion1.integrate(curt);
-    motion1.getCurrentTransform(R, T);
-
-    m1.setTransform(R, T);
-    m2.setTransform(R2, T2);
+    motion1.getCurrentTransform(pose);
 
     MeshCollisionTraversalNodeRSS node;
-    if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2))
+    if(!initialize(node, (const BVHModel<RSS>&)m1, pose, (const BVHModel<RSS>&)m2, SimpleTransform(R2, T2)))
       std::cout << "initialize error" << std::endl;
 
     node.enable_statistics = false;
@@ -516,19 +508,15 @@ bool spline_interp_Test(const Transform& tf1, const Transform& tf2,
   {
     BVH_REAL curt = i / (BVH_REAL)nsamples;
 
-    Matrix3f R;
-    Vec3f T;
     motion1.integrate(curt);
     motion2.integrate(curt);
 
-    motion1.getCurrentTransform(R, T);
-    m1.setTransform(R, T);
-
-    motion2.getCurrentTransform(R, T);
-    m2.setTransform(R, T);
+    SimpleTransform tf1, tf2;
+    motion1.getCurrentTransform(tf1);
+    motion2.getCurrentTransform(tf2);
 
     MeshCollisionTraversalNodeRSS node;
-    if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2))
+    if(!initialize(node, (const BVHModel<RSS>&)m1, tf1, (const BVHModel<RSS>&)m2, tf2))
       std::cout << "initialize error" << std::endl;
 
     node.enable_statistics = false;
diff --git a/trunk/fcl/test/test_core_continuous_collision.cpp b/trunk/fcl/test/test_core_continuous_collision.cpp
index 45dee43a..45556607 100644
--- a/trunk/fcl/test/test_core_continuous_collision.cpp
+++ b/trunk/fcl/test/test_core_continuous_collision.cpp
@@ -184,9 +184,11 @@ bool continuous_collide_Test(const Transform& tf1, const Transform& tf2,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
+  SimpleTransform pose1, pose2;
+
   MeshCollisionTraversalNode<BV> node0;
 
-  if(!initialize<BV>(node0, m1, m2))
+  if(!initialize<BV>(node0, m1, pose1, m2, pose2))
     std::cout << "initialize error" << std::endl;
 
   node0.enable_statistics = verbose;
@@ -212,9 +214,11 @@ bool continuous_collide_Test(const Transform& tf1, const Transform& tf2,
   m2.updateSubModel(vertices2);
   m2.endUpdateModel(true, refit_bottomup);
 
+  SimpleTransform pose11, pose21;
+
   MeshContinuousCollisionTraversalNode<BV> node;
 
-  if(!initialize<BV>(node, m1, m2))
+  if(!initialize<BV>(node, m1, pose11, m2, pose21))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -272,8 +276,10 @@ bool discrete_continuous_collide_Test(const Transform& tf1, const Transform& tf2
     m2.addSubModel(vertices2, triangles2);
     m2.endModel();
 
+    SimpleTransform pose1, pose2;
+
     MeshCollisionTraversalNode<BV> node;
-    if(!initialize<BV>(node, m1, m2))
+    if(!initialize<BV>(node, m1, pose1, m2, pose2))
       std::cout << "initialize error" << std::endl;
 
     node.enable_statistics = verbose;
diff --git a/trunk/fcl/test/test_core_deformable_object.cpp b/trunk/fcl/test/test_core_deformable_object.cpp
index a520fad1..ef208829 100644
--- a/trunk/fcl/test/test_core_deformable_object.cpp
+++ b/trunk/fcl/test/test_core_deformable_object.cpp
@@ -99,18 +99,18 @@ void lionTest()
       m.beginModel();
       m.addSubModel(p, t);
       m.endModel();
-      m.setIdentityTransform();
     }
     else
     {
       m.beginUpdateModel();
       m.updateSubModel(p);
       m.endUpdateModel(true, true);
-      m.setIdentityTransform();
     }
 
+    SimpleTransform tf1, tf2;
+
     MeshCollisionTraversalNode<AABB> node;
-    initialize(node, m, m);
+    initialize(node, m, tf1, m, tf2);
     selfCollide(&node);
 
     timing += timer.elapsed();
@@ -169,8 +169,10 @@ void lionTest_DCD()
       m.addSubModel(p, t1);
       m.endModel();
 
+      SimpleTransform tf1, tf2;
+
       MeshCollisionTraversalNode<AABB> node;
-      initialize(node, m, m);
+      initialize(node, m, tf1, m, tf2);
       selfCollide(&node);
 
       if(node.pairs.size() > 0) break;
@@ -214,18 +216,18 @@ void ballTest()
       m.beginModel();
       m.addSubModel(p, t);
       m.endModel();
-      m.setIdentityTransform();
     }
     else
     {
       m.beginUpdateModel();
       m.updateSubModel(p);
       m.endUpdateModel(true, true);
-      m.setIdentityTransform();
     }
 
+    SimpleTransform tf1, tf2;
+
     MeshCollisionTraversalNode<AABB> node;
-    initialize(node, m, m);
+    initialize(node, m, tf1, m, tf2);
     selfCollide(&node);
 
     timing += timer.elapsed();
@@ -284,8 +286,10 @@ void ballTest_DCD()
       m.addSubModel(p, t1);
       m.endModel();
 
+      SimpleTransform tf1, tf2;
+
       MeshCollisionTraversalNode<AABB> node;
-      initialize(node, m, m);
+      initialize(node, m, tf1, m, tf2);
       selfCollide(&node);
 
       if(node.pairs.size() > 0) break;
diff --git a/trunk/fcl/test/test_core_distance.cpp b/trunk/fcl/test/test_core_distance.cpp
index 87870aab..62a57431 100644
--- a/trunk/fcl/test/test_core_distance.cpp
+++ b/trunk/fcl/test/test_core_distance.cpp
@@ -217,16 +217,9 @@ void distance_Test(const Transform& tf,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
-
   MeshDistanceTraversalNodeRSS node;
 
-  if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2))
+  if(!initialize(node, (const BVHModel<RSS>&)m1, SimpleTransform(tf.R, tf.T), (const BVHModel<RSS>&)m2, SimpleTransform()))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -235,7 +228,7 @@ void distance_Test(const Transform& tf,
 
   // points are in local coordinate, to global coordinate
   Vec3f p1 = tf.R * node.p1 + tf.T;
-  Vec3f p2 = R2 * node.p2 + T2;
+  Vec3f p2 = node.p2;
 
 
   distance_result.distance = node.min_distance;
@@ -274,16 +267,11 @@ void distance_Test2(const Transform& tf,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf.R, tf.T), pose2;
 
   MeshDistanceTraversalNode<BV> node;
 
-  if(!initialize<BV>(node, m1, m2))
+  if(!initialize<BV>(node, m1, pose1, m2, pose2))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -322,15 +310,8 @@ bool collide_Test_OBB(const Transform& tf,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
-
   MeshCollisionTraversalNodeOBB node;
-  if(!initialize(node, (const BVHModel<OBB>&)m1, (const BVHModel<OBB>&)m2))
+  if(!initialize(node, (const BVHModel<OBB>&)m1, SimpleTransform(tf.R, tf.T), (const BVHModel<OBB>&)m2, SimpleTransform()))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
diff --git a/trunk/fcl/test/test_core_front_list.cpp b/trunk/fcl/test/test_core_front_list.cpp
index d133ff83..6f146989 100644
--- a/trunk/fcl/test/test_core_front_list.cpp
+++ b/trunk/fcl/test/test_core_front_list.cpp
@@ -226,9 +226,11 @@ bool collide_front_list_Test(const Transform& tf1, const Transform& tf2,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
+  SimpleTransform pose1, pose2;
+
   MeshCollisionTraversalNode<BV> node;
 
-  if(!initialize<BV>(node, m1, m2))
+  if(!initialize<BV>(node, m1, pose1, m2, pose2))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -285,16 +287,11 @@ bool collide_front_list_OBB_Test(const Transform& tf1, const Transform& tf2,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf1.R, tf1.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf1.R, tf1.T), pose2;
 
   MeshCollisionTraversalNodeOBB node;
 
-  if(!initialize(node, (const BVHModel<OBB>&)m1, (const BVHModel<OBB>&)m2))
+  if(!initialize(node, (const BVHModel<OBB>&)m1, pose1, (const BVHModel<OBB>&)m2, pose2))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -308,8 +305,8 @@ bool collide_front_list_OBB_Test(const Transform& tf1, const Transform& tf2,
 
 
   // update the mesh
-  m1.setTransform(tf2.R, tf2.T);
-  if(!initialize(node, (const BVHModel<OBB>&)m1, (const BVHModel<OBB>&)m2))
+  pose1.setTransform(tf2.R, tf2.T);
+  if(!initialize(node, (const BVHModel<OBB>&)m1, pose1, (const BVHModel<OBB>&)m2, pose2))
     std::cout << "initialize error" << std::endl;
 
   node.pairs.clear();
@@ -342,16 +339,11 @@ bool collide_front_list_RSS_Test(const Transform& tf1, const Transform& tf2,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf1.R, tf1.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf1.R, tf1.T), pose2;
 
   MeshCollisionTraversalNodeRSS node;
 
-  if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2))
+  if(!initialize(node, (const BVHModel<RSS>&)m1, pose1, (const BVHModel<RSS>&)m2, pose2))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -365,8 +357,8 @@ bool collide_front_list_RSS_Test(const Transform& tf1, const Transform& tf2,
 
 
   // update the mesh
-  m1.setTransform(tf2.R, tf2.T);
-  if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2))
+  pose1.setTransform(tf2.R, tf2.T);
+  if(!initialize(node, (const BVHModel<RSS>&)m1, pose1, (const BVHModel<RSS>&)m2, pose2))
     std::cout << "initialize error" << std::endl;
 
   node.pairs.clear();
@@ -397,16 +389,11 @@ bool collide_Test(const Transform& tf,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Matrix3f R2;
-  R2.setIdentity();
-  Vec3f T2;
-
-  m1.setTransform(tf.R, tf.T);
-  m2.setTransform(R2, T2);
+  SimpleTransform pose1(tf.R, tf.T), pose2;
 
   MeshCollisionTraversalNode<BV> node;
 
-  if(!initialize<BV>(node, m1, m2))
+  if(!initialize<BV>(node, m1, pose1, m2, pose2))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
diff --git a/trunk/fcl/test/test_core_geometric_shapes.cpp b/trunk/fcl/test/test_core_geometric_shapes.cpp
index 0c3eba3d..8a44b8a9 100644
--- a/trunk/fcl/test/test_core_geometric_shapes.cpp
+++ b/trunk/fcl/test/test_core_geometric_shapes.cpp
@@ -57,107 +57,82 @@ TEST(shapeIntersection, spheresphere)
   bool res;
 
   s2.setLocalTranslation(Vec3f(40, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(30, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(29.9, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(0, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(-29.9, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(-30, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
-
 }
 
 TEST(shapeIntersection, boxbox)
@@ -173,61 +148,47 @@ TEST(shapeIntersection, boxbox)
 
   bool res;
 
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(15, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
-
 
   SimpleQuaternion q;
   q.fromAxisAngle(Vec3f(0, 0, 1), (BVH_REAL)3.140 / 6);
   Matrix3f R;
   q.toRotation(R);
   s2.setLocalRotation(R);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
-
 }
 
 TEST(shapeIntersection, spherebox)
@@ -243,57 +204,43 @@ TEST(shapeIntersection, spherebox)
 
   bool res;
 
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
-
 
   s2.setLocalTranslation(Vec3f(22.5, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(22.4, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
-
 }
 
 TEST(shapeIntersection, cylindercylinder)
@@ -309,55 +256,43 @@ TEST(shapeIntersection, cylindercylinder)
 
   bool res;
 
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(9.9, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(10, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 }
 
 TEST(shapeIntersection, conecone)
@@ -373,89 +308,69 @@ TEST(shapeIntersection, conecone)
 
   bool res;
 
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(9.9, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(10.001, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(0, 0, 9.9));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(0, 0, 10));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 }
 
 TEST(shapeIntersection, conecylinder)
@@ -471,89 +386,69 @@ TEST(shapeIntersection, conecylinder)
 
   bool res;
 
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(9.9, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(10, 0, 0));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(0, 0, 9.9));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_TRUE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 
   s2.setLocalTranslation(Vec3f(0, 0, 10));
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(), s2, SimpleTransform());
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(), &s2, SimpleTransform(), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
 
-  s1.setTransform(transform.R, transform.T);
-  s2.setTransform(transform.R, transform.T);
-  res = shapeIntersect(s1, s2);
+  res = shapeIntersect(s1, SimpleTransform(transform.R, transform.T), s2, SimpleTransform(transform.R, transform.T));
   ASSERT_FALSE(res);
   contacts.clear();
-  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  res = (collide(&s1, SimpleTransform(transform.R, transform.T), &s2, SimpleTransform(transform.R, transform.T), 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
-  s1.setTransform(identity.R, identity.T);
-  s2.setTransform(identity.R, identity.T);
 }
 
 TEST(shapeIntersection, spheretriangle)
@@ -570,35 +465,29 @@ TEST(shapeIntersection, spheretriangle)
 
   bool res;
 
-  res = shapeTriangleIntersect(s, t[0], t[1], t[2]);
+  res = shapeTriangleIntersect(s, SimpleTransform(), t[0], t[1], t[2]);
   ASSERT_TRUE(res);
 
-  s.setTransform(transform.R, transform.T);
-  res =  shapeTriangleIntersect(s, t[0], t[1], t[2], transform.R, transform.T);
+  res =  shapeTriangleIntersect(s, SimpleTransform(transform.R, transform.T), t[0], t[1], t[2], transform.R, transform.T);
   ASSERT_TRUE(res);
-  s.setTransform(identity.R, identity.T);
 
   t[0].setValue(30, 0, 0);
   t[1].setValue(10, -20, 0);
   t[2].setValue(10, 20, 0);
-  res = shapeTriangleIntersect(s, t[0], t[1], t[2]);
+  res = shapeTriangleIntersect(s, SimpleTransform(), t[0], t[1], t[2]);
   ASSERT_FALSE(res);
 
-  s.setTransform(transform.R, transform.T);
-  res =  shapeTriangleIntersect(s, t[0], t[1], t[2], transform.R, transform.T);
+  res =  shapeTriangleIntersect(s, SimpleTransform(transform.R, transform.T), t[0], t[1], t[2], transform.R, transform.T);
   ASSERT_FALSE(res);
-  s.setTransform(identity.R, identity.T);
 
   t[0].setValue(30, 0, 0);
   t[1].setValue(9.9, -20, 0);
   t[2].setValue(9.9, 20, 0);
-  res = shapeTriangleIntersect(s, t[0], t[1], t[2]);
+  res = shapeTriangleIntersect(s, SimpleTransform(), t[0], t[1], t[2]);
   ASSERT_TRUE(res);
 
-  s.setTransform(transform.R, transform.T);
-  res =  shapeTriangleIntersect(s, t[0], t[1], t[2], transform.R, transform.T);
+  res =  shapeTriangleIntersect(s, SimpleTransform(transform.R, transform.T), t[0], t[1], t[2], transform.R, transform.T);
   ASSERT_TRUE(res);
-  s.setTransform(identity.R, identity.T);
 }
 
 
diff --git a/trunk/fcl/test/timing_test.cpp b/trunk/fcl/test/timing_test.cpp
index 051ff046..d02db47d 100644
--- a/trunk/fcl/test/timing_test.cpp
+++ b/trunk/fcl/test/timing_test.cpp
@@ -90,11 +90,9 @@ int main()
     for(unsigned int i = 0; i < transforms.size(); ++i)
     {
       Transform& tf = transforms[i];
-      m1.setTransform(tf.R, tf.T);
-      m2.setTransform(R2, T2);
 
       MeshCollisionTraversalNodeOBB node;
-      if(!initialize(node, (const BVHModel<OBB>&)m1, (const BVHModel<OBB>&)m2))
+      if(!initialize(node, (const BVHModel<OBB>&)m1, SimpleTransform(tf.R, tf.T), (const BVHModel<OBB>&)m2, SimpleTransform(R2, T2)))
         std::cout << "initialize error" << std::endl;
 
       node.enable_statistics = false;
@@ -220,11 +218,9 @@ int main()
     for(unsigned int i = 0; i < transforms.size(); ++i)
     {
       Transform& tf = transforms[i];
-      m1.setTransform(tf.R, tf.T);
-      m2.setTransform(R2, T2);
 
       MeshCollisionTraversalNodeOBB node;
-      if(!initialize(node, (const BVHModel<OBB>&)m1, (const BVHModel<OBB>&)m2))
+      if(!initialize(node, (const BVHModel<OBB>&)m1, SimpleTransform(tf.R, tf.T), (const BVHModel<OBB>&)m2, SimpleTransform(R2, T2)))
         std::cout << "initialize error" << std::endl;
 
       node.enable_statistics = false;
-- 
GitLab