diff --git a/trunk/fcl/include/fcl/geometric_shapes_utility.h b/trunk/fcl/include/fcl/geometric_shapes_utility.h
index 166e5fc4f9f37d81af1b2a95822fc2ab6714a156..8fe6a290ab3a60189625d11a9a09d96d0232ba69 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_utility.h
+++ b/trunk/fcl/include/fcl/geometric_shapes_utility.h
@@ -109,6 +109,8 @@ namespace fcl
 
   template<>
   void computeBV<OBB>(const Plane& s, OBB& bv);
+
+  // TODO: implement computeBV for RSS and KDOP
 }
 
 #endif
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index 7d2bac6f531362a272e47be7d8e3010d356aa716..f08bf14bd5db392485c7325726013234e9378589 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -59,7 +59,7 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, con
 
 /** \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, S& model2,
+bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& model1, const S& model2,
                 int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
                 bool use_refit = false, bool refit_bottomup = false)
 {
@@ -71,16 +71,16 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& mode
 
   if(!model1.isIdentityTransform())
   {
-    std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
+    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 = matMulVec(model1.getRotation(), p) + model1.getTranslation();
-      vertices_transformed1[i] = new_v;
+      vertices_transformed[i] = new_v;
     }
 
     model1.beginReplaceModel();
-    model1.replaceSubModel(vertices_transformed1);
+    model1.replaceSubModel(vertices_transformed);
     model1.endReplaceModel(use_refit, refit_bottomup);
 
     model1.setIdentityTransform();
@@ -96,6 +96,46 @@ 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,
+                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())
+  {
+    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 = matMulVec(model2.getRotation(), p) + model2.getTranslation();
+      vertices_transformed[i] = new_v;
+    }
+
+    model2.beginReplaceModel();
+    model2.replaceSubModel(vertices_transformed);
+    model2.endReplaceModel(use_refit, refit_bottomup);
+
+    model2.setIdentityTransform();
+  }
+
+  node.vertices = model2.vertices;
+  node.tri_indices = model2.tri_indices;
+  node.num_max_contacts = num_max_contacts;
+  node.exhaustive = exhaustive;
+  node.enable_contact = enable_contact;
+
+  return true;
+}
+
+
+
 /** \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)
@@ -120,6 +160,29 @@ 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)
+{
+  if(model2.getModelType() != BVH_MODEL_TRIANGLES)
+    return false;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.vertices = model2.vertices;
+  node.tri_indices = model2.tri_indices;
+  node.num_max_contacts = num_max_contacts;
+  node.exhaustive = exhaustive;
+  node.enable_contact = enable_contact;
+
+  for(int i = 0; i < 3; ++i)
+    node.R[i] = model2.getRotation()[i];
+  node.T = model2.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,
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
index 01a8f4ba1535e72e117dec3fff8a687b186f6149..ca595bda3936166d77b7dcf80b2597992a870bf2 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -131,7 +131,7 @@ public:
     if(this->enable_statistics) num_bv_tests++;
     BV bv_shape;
     computeBV(*model1, bv_shape);
-    return !model2->getBV(b2).overlap(bv_shape);
+    return !model2->getBV(b2).bv.overlap(bv_shape);
   }
 
   const S* model1;
@@ -296,6 +296,127 @@ public:
 };
 
 
+template<typename S, typename BV>
+class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode<S, BV>
+{
+public:
+  ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode<S, BV>()
+  {
+    vertices = NULL;
+    tri_indices = NULL;
+
+    num_max_contacts = 1;
+    exhaustive = false;
+    enable_contact = false;
+  }
+
+  void leafTesting(int b1, int b2) const
+  {
+    if(this->enable_statistics) this->num_leaf_tests++;
+    const BVNode<BV>& node = this->model2->getBV(b2);
+
+    int primitive_id = node.primitiveId();
+
+    const Triangle& tri_id = tri_indices[primitive_id];
+
+    const Vec3f& p1 = vertices[tri_id[0]];
+    const Vec3f& p2 = vertices[tri_id[1]];
+    const Vec3f& p3 = vertices[tri_id[2]];
+
+    BVH_REAL penetration;
+    Vec3f normal;
+    Vec3f contactp;
+
+    if(!enable_contact) // only interested in collision or not
+    {
+      if(shapeTriangleIntersect(*(this->model1), p1, p2, p3))
+      {
+        pairs.push_back(BVHShapeCollisionPair(primitive_id));
+      }
+    }
+    else
+    {
+      if(shapeTriangleIntersect(*(this->model1), p1, p2, p3, &contactp, &penetration, &normal))
+      {
+        pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
+      }
+    }
+  }
+
+  bool canStop() const
+  {
+    return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
+  }
+
+  Vec3f* vertices;
+  Triangle* tri_indices;
+
+  int num_max_contacts;
+  bool exhaustive;
+  bool enable_contact;
+
+  mutable std::vector<BVHShapeCollisionPair> pairs;
+};
+
+template<typename S>
+class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode<S, OBB>
+{
+public:
+  ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode<S, OBB>()
+  {
+    R[0] = Vec3f(1, 0, 0);
+    R[1] = Vec3f(0, 1, 0);
+    R[2] = Vec3f(0, 0, 1);
+  }
+
+  bool BVTesting(int b1, int b2) const
+  {
+    if(this->enable_statistics) this->num_bv_tests++;
+    OBB bv_shape;
+    computeBV(*this->model1, bv_shape);
+    return !overlap(R, T, bv_shape, this->model2->getBV(b2).bv);
+  }
+
+  void leafTesting(int b1, int b2) const
+  {
+    if(this->enable_statistics) this->num_leaf_tests++;
+    const BVNode<OBB>& node = this->model2->getBV(b2);
+
+    int primitive_id = node.primitiveId();
+
+    const Triangle& tri_id = this->tri_indices[primitive_id];
+
+    const Vec3f& p1 = this->vertices[tri_id[0]];
+    const Vec3f& p2 = this->vertices[tri_id[1]];
+    const Vec3f& p3 = this->vertices[tri_id[2]];
+
+    BVH_REAL penetration;
+    Vec3f normal;
+    Vec3f contactp;
+
+    if(!this->enable_contact) // only interested in collision or not
+    {
+      if(shapeTriangleIntersect(*(this->model1), 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))
+      {
+        this->pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
+      }
+    }
+  }
+
+  // R, T is the transformation of bvh model
+  Vec3f R[3];
+  Vec3f T;
+};
+
+
+
 
 }
 
diff --git a/trunk/fcl/src/collision.cpp b/trunk/fcl/src/collision.cpp
index fc129af26bd7c234228a4f4b271c2a3ac0819d0c..6d942cb7527480448929b1e70c7aefa7d5b71b68 100644
--- a/trunk/fcl/src/collision.cpp
+++ b/trunk/fcl/src/collision.cpp
@@ -61,27 +61,46 @@ int collide(const CollisionObject* o1, const CollisionObject* o2,
   const OBJECT_TYPE object_type2 = o2->getObjectType();
   const NODE_TYPE node_type2 = o2->getNodeType();
 
-  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(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, o2, 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_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);
   }
   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, o2, 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, o2, num_max_contacts, exhaustive, enable_contact, contacts);
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index f592318f5a822b529f521996a41d0fecdb314765..b69fa79ca70a4f704ce24874cb3a67ad3e5b4bf6 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -62,7 +62,7 @@ namespace fcl
 
 
 #define MESHSHAPE_COMMON_CODE() do{ \
-                                    initialize(node, *obj1_tmp, *obj2_tmp, num_max_contacts, exhaustive, enable_contact); \
+                                    initialize(node, *obj1_tmp, *obj2, num_max_contacts, exhaustive, enable_contact); \
                                     collide(&node); \
                                     int num_contacts = node.pairs.size(); \
                                     if(num_contacts > 0) \
@@ -82,12 +82,11 @@ namespace fcl
                                     } \
                                     delete obj1_tmp; \
                                     obj1_tmp = NULL; \
-                                    delete obj2_tmp; \
-                                    obj2_tmp = NULL; \
                                     return num_contacts; \
                                    } while(0)
 
-#define MESHSHAPEOBBRSS_COMMON_CODE() do{ \
+
+#define SHAPEMESH_COMMON_CODE() do{ \
                                     initialize(node, *obj1, *obj2_tmp, num_max_contacts, exhaustive, enable_contact); \
                                     collide(&node); \
                                     int num_contacts = node.pairs.size(); \
@@ -111,7 +110,27 @@ namespace fcl
                                     return num_contacts; \
                                    } while(0)
 
-
+#define MESHSHAPEOBBRSS_COMMON_CODE() do{ \
+                                    initialize(node, *obj1, *obj2, num_max_contacts, exhaustive, enable_contact); \
+                                    collide(&node); \
+                                    int num_contacts = node.pairs.size(); \
+                                    if(num_contacts > 0) \
+                                    { \
+                                      if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts; \
+                                      contacts.resize(num_contacts); \
+                                      if(!enable_contact) \
+                                      { \
+                                        for(int i = 0; i < num_contacts; ++i) \
+                                          contacts[i] = Contact(obj1, obj2, node.pairs[i].id, 0); \
+                                      } \
+                                      else \
+                                      { \
+                                        for(int i = 0; i < num_contacts; ++i) \
+                                          contacts[i] = Contact(obj1, obj2, node.pairs[i].id, 0, node.pairs[i].contact_point, node.pairs[i].normal, node.pairs[i].penetration_depth); \
+                                      } \
+                                    } \
+                                    return num_contacts; \
+                                   } while(0)
 
 #define MESHMESH_COMMON_CODE() do{ \
                                     initialize(node, *obj1_tmp, *obj2_tmp, num_max_contacts, exhaustive, enable_contact); \
@@ -566,7 +585,6 @@ int AABBBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
   const Box* obj2 = (Box*)o2;
-  Box* obj2_tmp = new Box(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -576,7 +594,6 @@ int AABBSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
   const Sphere* obj2 = (Sphere*)o2;
-  Sphere* obj2_tmp = new Sphere(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -586,7 +603,6 @@ int AABBCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
   const Capsule* obj2 = (Capsule*)o2;
-  Capsule* obj2_tmp = new Capsule(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -596,7 +612,6 @@ int AABBConeCollide(const CollisionObject* o1, const CollisionObject* o2, int nu
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
   const Cone* obj2 = (Cone*)o2;
-  Cone* obj2_tmp = new Cone(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -606,7 +621,6 @@ int AABBCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, in
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
   const Cylinder* obj2 = (Cylinder*)o2;
-  Cylinder* obj2_tmp = new Cylinder(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -616,7 +630,6 @@ int AABBConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
   const Convex* obj2 = (Convex*)o2;
-  Convex* obj2_tmp = new Convex(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -626,7 +639,6 @@ int AABBPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
   BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
   const Plane* obj2 = (Plane*)o2;
-  Plane* obj2_tmp = new Plane(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -635,7 +647,6 @@ int OBBBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_
   MeshShapeCollisionTraversalNodeOBB<Box> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
   const Box* obj2 = (Box*)o2;
-  Box* obj2_tmp = new Box(*obj2);
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
@@ -644,7 +655,6 @@ int OBBSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   MeshShapeCollisionTraversalNodeOBB<Sphere> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
   const Sphere* obj2 = (Sphere*)o2;
-  Sphere* obj2_tmp = new Sphere(*obj2);
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
@@ -653,7 +663,6 @@ int OBBCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_
   MeshShapeCollisionTraversalNodeOBB<Capsule> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
   const Capsule* obj2 = (Capsule*)o2;
-  Capsule* obj2_tmp = new Capsule(*obj2);
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
@@ -662,7 +671,6 @@ int OBBConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num
   MeshShapeCollisionTraversalNodeOBB<Cone> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
   const Cone* obj2 = (Cone*)o2;
-  Cone* obj2_tmp = new Cone(*obj2);
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
@@ -671,7 +679,6 @@ int OBBCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int
   MeshShapeCollisionTraversalNodeOBB<Cylinder> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
   const Cylinder* obj2 = (Cylinder*)o2;
-  Cylinder* obj2_tmp = new Cylinder(*obj2);
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
@@ -680,7 +687,6 @@ int OBBConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   MeshShapeCollisionTraversalNodeOBB<Convex> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
   const Convex* obj2 = (Convex*)o2;
-  Convex* obj2_tmp = new Convex(*obj2);
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
@@ -689,7 +695,6 @@ int OBBPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int nu
   MeshShapeCollisionTraversalNodeOBB<Plane> node;
   const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
   const Plane* obj2 = (Plane*)o2;
-  Plane* obj2_tmp = new Plane(*obj2);
   MESHSHAPEOBBRSS_COMMON_CODE();
 }
 
@@ -699,7 +704,6 @@ int RSSBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
   const Box* obj2 = (Box*)o2;
-  Box* obj2_tmp = new Box(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -709,7 +713,6 @@ int RSSSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
   const Sphere* obj2 = (Sphere*)o2;
-  Sphere* obj2_tmp = new Sphere(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -719,7 +722,6 @@ int RSSCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
   const Capsule* obj2 = (Capsule*)o2;
-  Capsule* obj2_tmp = new Capsule(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -729,7 +731,6 @@ int RSSConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
   const Cone* obj2 = (Cone*)o2;
-  Cone* obj2_tmp = new Cone(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -739,7 +740,6 @@ int RSSCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
   const Cylinder* obj2 = (Cylinder*)o2;
-  Cylinder* obj2_tmp = new Cylinder(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -749,7 +749,6 @@ int RSSConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
   const Convex* obj2 = (Convex*)o2;
-  Convex* obj2_tmp = new Convex(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -759,7 +758,6 @@ int RSSPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int nu
   const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
   BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
   const Plane* obj2 = (Plane*)o2;
-  Plane* obj2_tmp = new Plane(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -769,7 +767,6 @@ int KDOP16BoxCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
   const Box* obj2 = (Box*)o2;
-  Box* obj2_tmp = new Box(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -779,7 +776,6 @@ int KDOP16SphereCollide(const CollisionObject* o1, const CollisionObject* o2, in
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
   const Sphere* obj2 = (Sphere*)o2;
-  Sphere* obj2_tmp = new Sphere(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -789,7 +785,6 @@ int KDOP16CapCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
   const Capsule* obj2 = (Capsule*)o2;
-  Capsule* obj2_tmp = new Capsule(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -799,7 +794,6 @@ int KDOP16ConeCollide(const CollisionObject* o1, const CollisionObject* o2, int
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
   const Cone* obj2 = (Cone*)o2;
-  Cone* obj2_tmp = new Cone(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -809,7 +803,6 @@ int KDOP16CylinderCollide(const CollisionObject* o1, const CollisionObject* o2,
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
   const Cylinder* obj2 = (Cylinder*)o2;
-  Cylinder* obj2_tmp = new Cylinder(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -819,7 +812,6 @@ int KDOP16ConvexCollide(const CollisionObject* o1, const CollisionObject* o2, in
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
   const Convex* obj2 = (Convex*)o2;
-  Convex* obj2_tmp = new Convex(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -829,7 +821,6 @@ int KDOP16PlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int
   const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
   BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
   const Plane* obj2 = (Plane*)o2;
-  Plane* obj2_tmp = new Plane(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -839,7 +830,6 @@ int KDOP18BoxCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
   const Box* obj2 = (Box*)o2;
-  Box* obj2_tmp = new Box(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -849,7 +839,6 @@ int KDOP18SphereCollide(const CollisionObject* o1, const CollisionObject* o2, in
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
   const Sphere* obj2 = (Sphere*)o2;
-  Sphere* obj2_tmp = new Sphere(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -859,7 +848,6 @@ int KDOP18CapCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
   const Capsule* obj2 = (Capsule*)o2;
-  Capsule* obj2_tmp = new Capsule(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -869,7 +857,6 @@ int KDOP18ConeCollide(const CollisionObject* o1, const CollisionObject* o2, int
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
   const Cone* obj2 = (Cone*)o2;
-  Cone* obj2_tmp = new Cone(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -879,7 +866,6 @@ int KDOP18CylinderCollide(const CollisionObject* o1, const CollisionObject* o2,
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
   const Cylinder* obj2 = (Cylinder*)o2;
-  Cylinder* obj2_tmp = new Cylinder(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -889,7 +875,6 @@ int KDOP18ConvexCollide(const CollisionObject* o1, const CollisionObject* o2, in
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
   const Convex* obj2 = (Convex*)o2;
-  Convex* obj2_tmp = new Convex(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -899,7 +884,6 @@ int KDOP18PlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int
   const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
   BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
   const Plane* obj2 = (Plane*)o2;
-  Plane* obj2_tmp = new Plane(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -909,7 +893,6 @@ int KDOP24BoxCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
   const Box* obj2 = (Box*)o2;
-  Box* obj2_tmp = new Box(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -919,7 +902,6 @@ int KDOP24SphereCollide(const CollisionObject* o1, const CollisionObject* o2, in
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
   const Sphere* obj2 = (Sphere*)o2;
-  Sphere* obj2_tmp = new Sphere(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -929,7 +911,6 @@ int KDOP24CapCollide(const CollisionObject* o1, const CollisionObject* o2, int n
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
   const Capsule* obj2 = (Capsule*)o2;
-  Capsule* obj2_tmp = new Capsule(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -939,7 +920,6 @@ int KDOP24ConeCollide(const CollisionObject* o1, const CollisionObject* o2, int
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
   const Cone* obj2 = (Cone*)o2;
-  Cone* obj2_tmp = new Cone(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -949,7 +929,6 @@ int KDOP24CylinderCollide(const CollisionObject* o1, const CollisionObject* o2,
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
   const Cylinder* obj2 = (Cylinder*)o2;
-  Cylinder* obj2_tmp = new Cylinder(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -959,7 +938,6 @@ int KDOP24ConvexCollide(const CollisionObject* o1, const CollisionObject* o2, in
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
   const Convex* obj2 = (Convex*)o2;
-  Convex* obj2_tmp = new Convex(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
@@ -969,10 +947,384 @@ int KDOP24PlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int
   const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
   BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
   const Plane* obj2 = (Plane*)o2;
-  Plane* obj2_tmp = new Plane(*obj2);
   MESHSHAPE_COMMON_CODE();
 }
 
+
+/*
+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)
 {
   MeshCollisionTraversalNode<AABB> node;
@@ -1141,7 +1493,55 @@ CollisionFunctionMatrix::CollisionFunctionMatrix()
   collision_matrix[BV_KDOP24][GEOM_CYLINDER] = KDOP24CylinderCollide;
   collision_matrix[BV_KDOP24][GEOM_CONVEX] = KDOP24ConvexCollide;
   collision_matrix[BV_KDOP24][GEOM_PLANE] = KDOP24PlaneCollide;
-
+/*
+  collision_matrix[GEOM_BOX][BV_AABB] = BoxAABBCollide;
+  collision_matrix[GEOM_SPHERE][BV_AABB] = SphereAABBCollide;
+  collision_matrix[GEOM_CAPSULE][BV_AABB] = CapAABBCollide;
+  collision_matrix[GEOM_CONE][BV_AABB] = ConeAABBCollide;
+  collision_matrix[GEOM_CYLINDER][BV_AABB] = CylinderAABBCollide;
+  collision_matrix[GEOM_CONVEX][BV_AABB] = ConvexAABBCollide;
+  collision_matrix[GEOM_PLANE][BV_AABB] = PlaneAABBCollide;
+
+  collision_matrix[GEOM_BOX][BV_OBB] = BoxOBBCollide;
+  collision_matrix[GEOM_SPHERE][BV_OBB] = SphereOBBCollide;
+  collision_matrix[GEOM_CAPSULE][BV_OBB] = CapOBBCollide;
+  collision_matrix[GEOM_CONE][BV_OBB] = ConeOBBCollide;
+  collision_matrix[GEOM_CYLINDER][BV_OBB] = CylinderOBBCollide;
+  collision_matrix[GEOM_CONVEX][BV_OBB] = ConvexOBBCollide;
+  collision_matrix[GEOM_PLANE][BV_OBB] = PlaneOBBCollide;
+
+  collision_matrix[GEOM_BOX][BV_RSS] = BoxRSSCollide;
+  collision_matrix[GEOM_SPHERE][BV_RSS] = SphereRSSCollide;
+  collision_matrix[GEOM_CAPSULE][BV_RSS] = CapRSSCollide;
+  collision_matrix[GEOM_CONE][BV_RSS] = ConeRSSCollide;
+  collision_matrix[GEOM_CYLINDER][BV_RSS] = CylinderRSSCollide;
+  collision_matrix[GEOM_CONVEX][BV_RSS] = ConvexRSSCollide;
+  collision_matrix[GEOM_PLANE][BV_RSS] = PlaneRSSCollide;
+
+  collision_matrix[GEOM_BOX][BV_KDOP16] = BoxKDOP16Collide;
+  collision_matrix[GEOM_SPHERE][BV_KDOP16] = SphereKDOP16Collide;
+  collision_matrix[GEOM_CAPSULE][BV_KDOP16] = CapKDOP16Collide;
+  collision_matrix[GEOM_CONE][BV_KDOP16] = ConeKDOP16Collide;
+  collision_matrix[GEOM_CYLINDER][BV_KDOP16] = CylinderKDOP16Collide;
+  collision_matrix[GEOM_CONVEX][BV_KDOP16] = ConvexKDOP16Collide;
+  collision_matrix[GEOM_PLANE][BV_KDOP16] = PlaneKDOP16Collide;
+
+  collision_matrix[GEOM_BOX][BV_KDOP18] = BoxKDOP18Collide;
+  collision_matrix[GEOM_SPHERE][BV_KDOP18] = SphereKDOP18Collide;
+  collision_matrix[GEOM_CAPSULE][BV_KDOP18] = CapKDOP18Collide;
+  collision_matrix[GEOM_CONE][BV_KDOP18] = ConeKDOP18Collide;
+  collision_matrix[GEOM_CYLINDER][BV_KDOP18] = CylinderKDOP18Collide;
+  collision_matrix[GEOM_CONVEX][BV_KDOP18] = ConvexKDOP18Collide;
+  collision_matrix[GEOM_PLANE][BV_KDOP18] = PlaneKDOP18Collide;
+
+  collision_matrix[GEOM_BOX][BV_KDOP24] = BoxKDOP24Collide;
+  collision_matrix[GEOM_SPHERE][BV_KDOP24] = SphereKDOP24Collide;
+  collision_matrix[GEOM_CAPSULE][BV_KDOP24] = CapKDOP24Collide;
+  collision_matrix[GEOM_CONE][BV_KDOP24] = ConeKDOP24Collide;
+  collision_matrix[GEOM_CYLINDER][BV_KDOP24] = CylinderKDOP24Collide;
+  collision_matrix[GEOM_CONVEX][BV_KDOP24] = ConvexKDOP24Collide;
+  collision_matrix[GEOM_PLANE][BV_KDOP24] = PlaneKDOP24Collide;
+*/
   collision_matrix[BV_AABB][BV_AABB] = AABBAABBCollide;
   collision_matrix[BV_OBB][BV_OBB] = OBBOBBCollide;
   collision_matrix[BV_RSS][BV_RSS] = RSSRSSCollide;
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index e8c4a0a5ff575a13486a6c540839a4e28abde3a1..e1af3defa3c5bade700fbc8681276c41f003d018 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -44,61 +44,83 @@ namespace fcl
 template<>
 void computeBV<AABB>(const Box& s, AABB& bv)
 {
-  BVH_REAL x_range = 0.5 * (fabs(s.getLocalRotation()[0][0] * s.side[0]) + fabs(s.getLocalRotation()[0][1] * s.side[1]) + fabs(s.getLocalRotation()[0][2] * s.side[2]));
-  BVH_REAL y_range = 0.5 * (fabs(s.getLocalRotation()[1][0] * s.side[0]) + fabs(s.getLocalRotation()[1][1] * s.side[1]) + fabs(s.getLocalRotation()[1][2] * s.side[2]));
-  BVH_REAL z_range = 0.5 * (fabs(s.getLocalRotation()[2][0] * s.side[0]) + fabs(s.getLocalRotation()[2][1] * s.side[1]) + fabs(s.getLocalRotation()[2][2] * s.side[2]));
+  Vec3f R[3];
+  matMulMat(s.getRotation(), s.getLocalRotation(), R);
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
 
-  bv.max_ = s.getLocalTranslation() + Vec3f(x_range, y_range, z_range);
-  bv.min_ = s.getLocalTranslation() + Vec3f(-x_range, -y_range, -z_range);
+  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]));
+  BVH_REAL z_range = 0.5 * (fabs(R[2][0] * s.side[0]) + fabs(R[2][1] * s.side[1]) + fabs(R[2][2] * s.side[2]));
+
+  bv.max_ = T + Vec3f(x_range, y_range, z_range);
+  bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
 }
 
 template<>
 void computeBV<AABB>(const Sphere& s, AABB& bv)
 {
-  bv.max_ = s.getLocalTranslation() + Vec3f(s.radius, s.radius, s.radius);
-  bv.min_ = s.getLocalTranslation() + Vec3f(-s.radius, -s.radius, -s.radius);
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.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)
 {
-  BVH_REAL x_range = 0.5 * fabs(s.getLocalRotation()[0][2] * s.lz) + s.radius;
-  BVH_REAL y_range = 0.5 * fabs(s.getLocalRotation()[1][2] * s.lz) + s.radius;
-  BVH_REAL z_range = 0.5 * fabs(s.getLocalRotation()[2][2] * s.lz) + s.radius;
+  Vec3f R[3];
+  matMulMat(s.getRotation(), s.getLocalRotation(), R);
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.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;
+  BVH_REAL z_range = 0.5 * fabs(R[2][2] * s.lz) + s.radius;
 
-  bv.max_ = s.getLocalTranslation() + Vec3f(x_range, y_range, z_range);
-  bv.min_ = s.getLocalTranslation() + Vec3f(-x_range, -y_range, -z_range);
+  bv.max_ = T + Vec3f(x_range, y_range, z_range);
+  bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
 }
 
 template<>
 void computeBV<AABB>(const Cone& s, AABB& bv)
 {
-  BVH_REAL x_range = fabs(s.getLocalRotation()[0][0] * s.radius) + fabs(s.getLocalRotation()[0][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[0][2] * s.lz);
-  BVH_REAL y_range = fabs(s.getLocalRotation()[1][0] * s.radius) + fabs(s.getLocalRotation()[1][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[1][2] * s.lz);
-  BVH_REAL z_range = fabs(s.getLocalRotation()[2][0] * s.radius) + fabs(s.getLocalRotation()[2][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[2][2] * s.lz);
+  Vec3f R[3];
+  matMulMat(s.getRotation(), s.getLocalRotation(), R);
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
 
-  bv.max_ = s.getLocalTranslation() + Vec3f(x_range, y_range, z_range);
-  bv.min_ = s.getLocalTranslation() + Vec3f(-x_range, -y_range, -z_range);
+  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);
+  BVH_REAL z_range = fabs(R[2][0] * s.radius) + fabs(R[2][1] * s.radius) + 0.5 * fabs(R[2][2] * s.lz);
+
+  bv.max_ = T + Vec3f(x_range, y_range, z_range);
+  bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
 }
 
 template<>
 void computeBV<AABB>(const Cylinder& s, AABB& bv)
 {
-  BVH_REAL x_range = fabs(s.getLocalRotation()[0][0] * s.radius) + fabs(s.getLocalRotation()[0][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[0][2] * s.lz);
-  BVH_REAL y_range = fabs(s.getLocalRotation()[1][0] * s.radius) + fabs(s.getLocalRotation()[1][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[1][2] * s.lz);
-  BVH_REAL z_range = fabs(s.getLocalRotation()[2][0] * s.radius) + fabs(s.getLocalRotation()[2][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[2][2] * s.lz);
+  Vec3f R[3];
+  matMulMat(s.getRotation(), s.getLocalRotation(), R);
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.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);
+  BVH_REAL z_range = fabs(R[2][0] * s.radius) + fabs(R[2][1] * s.radius) + 0.5 * fabs(R[2][2] * s.lz);
 
-  bv.max_ = s.getLocalTranslation() + Vec3f(x_range, y_range, z_range);
-  bv.min_ = s.getLocalTranslation() + Vec3f(-x_range, -y_range, -z_range);
+  bv.max_ = T + Vec3f(x_range, y_range, z_range);
+  bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
 }
 
 template<>
 void computeBV<AABB>(const Convex& s, AABB& bv)
 {
+  Vec3f R[3];
+  matMulMat(s.getRotation(), s.getLocalRotation(), R);
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+
   AABB bv_;
   for(int i = 0; i < s.num_points; ++i)
   {
-    Vec3f new_p = matMulVec(s.getLocalRotation(), s.points[i]) + s.getLocalTranslation();
+    Vec3f new_p = matMulVec(R, s.points[i]) + T;
     bv_ += new_p;
   }
 
@@ -108,24 +130,29 @@ void computeBV<AABB>(const Convex& s, AABB& bv)
 template<>
 void computeBV<AABB>(const Plane& s, AABB& bv)
 {
+  Vec3f R[3];
+  matMulMat(s.getRotation(), s.getLocalRotation(), R);
+
+  Vec3f n = matMulVec(R, n);
+
   AABB bv_;
-  if(s.n[1] == (BVH_REAL)0.0 && s.n[2] == (BVH_REAL)0.0)
+  if(n[1] == (BVH_REAL)0.0 && n[2] == (BVH_REAL)0.0)
   {
     // normal aligned with x axis
-    if(s.n[0] < 0) bv_.min_[0] = -s.d;
-    else if(s.n[0] > 0) bv_.max_[0] = s.d;
+    if(n[0] < 0) bv_.min_[0] = -s.d;
+    else if(n[0] > 0) bv_.max_[0] = s.d;
   }
-  else if(s.n[0] == (BVH_REAL)0.0 && s.n[2] == (BVH_REAL)0.0)
+  else if(n[0] == (BVH_REAL)0.0 && n[2] == (BVH_REAL)0.0)
   {
     // normal aligned with y axis
-    if(s.n[1] < 0) bv_.min_[1] = -s.d;
-    else if(s.n[1] > 0) bv_.max_[1] = s.d;
+    if(n[1] < 0) bv_.min_[1] = -s.d;
+    else if(n[1] > 0) bv_.max_[1] = s.d;
   }
-  else if(s.n[0] == (BVH_REAL)0.0 && s.n[1] == (BVH_REAL)0.0)
+  else if(n[0] == (BVH_REAL)0.0 && n[1] == (BVH_REAL)0.0)
   {
     // normal aligned with z axis
-    if(s.n[2] < 0) bv_.min_[2] = -s.d;
-    else if(s.n[2] > 0) bv_.max_[2] = s.d;
+    if(n[2] < 0) bv_.min_[2] = -s.d;
+    else if(n[2] > 0) bv_.max_[2] = s.d;
   }
 
   bv = bv_;
@@ -135,17 +162,23 @@ void computeBV<AABB>(const Plane& s, AABB& bv)
 template<>
 void computeBV<OBB>(const Box& s, OBB& bv)
 {
-  bv.To = s.getLocalTranslation();
-  bv.axis[0] = Vec3f(s.getLocalRotation()[0][0], s.getLocalRotation()[1][0], s.getLocalRotation()[2][0]);
-  bv.axis[1] = Vec3f(s.getLocalRotation()[0][1], s.getLocalRotation()[1][1], s.getLocalRotation()[2][1]);
-  bv.axis[2] = Vec3f(s.getLocalRotation()[0][2], s.getLocalRotation()[1][2], s.getLocalRotation()[2][2]);
+  Vec3f R[3];
+  matMulMat(s.getRotation(), s.getLocalRotation(), R);
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+
+  bv.To = T;
+  bv.axis[0] = Vec3f(R[0][0], R[1][0], R[2][0]);
+  bv.axis[1] = Vec3f(R[0][1], R[1][1], R[2][1]);
+  bv.axis[2] = Vec3f(R[0][2], R[1][2], R[2][2]);
   bv.extent = s.side * (BVH_REAL)0.5;
 }
 
 template<>
 void computeBV<OBB>(const Sphere& s, OBB& bv)
 {
-  bv.To = s.getLocalTranslation();
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+
+  bv.To = T;
   bv.axis[0] = Vec3f(1, 0, 0);
   bv.axis[1] = Vec3f(0, 1, 0);
   bv.axis[2] = Vec3f(0, 0, 1);
@@ -155,54 +188,74 @@ void computeBV<OBB>(const Sphere& s, OBB& bv)
 template<>
 void computeBV<OBB>(const Capsule& s, OBB& bv)
 {
-  bv.To = s.getLocalTranslation();
-  bv.axis[0] = Vec3f(s.getLocalRotation()[0][0], s.getLocalRotation()[1][0], s.getLocalRotation()[2][0]);
-  bv.axis[1] = Vec3f(s.getLocalRotation()[0][1], s.getLocalRotation()[1][1], s.getLocalRotation()[2][1]);
-  bv.axis[2] = Vec3f(s.getLocalRotation()[0][2], s.getLocalRotation()[1][2], s.getLocalRotation()[2][2]);
+  Vec3f R[3];
+  matMulMat(s.getRotation(), s.getLocalRotation(), R);
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+
+  bv.To = T;
+  bv.axis[0] = Vec3f(R[0][0], R[1][0], R[2][0]);
+  bv.axis[1] = Vec3f(R[0][1], R[1][1], R[2][1]);
+  bv.axis[2] = Vec3f(R[0][2], R[1][2], R[2][2]);
   bv.extent = Vec3f(s.radius, s.radius, s.lz / 2 + s.radius);
 }
 
 template<>
 void computeBV<OBB>(const Cone& s, OBB& bv)
 {
-  bv.To = s.getLocalTranslation();
-  bv.axis[0] = Vec3f(s.getLocalRotation()[0][0], s.getLocalRotation()[1][0], s.getLocalRotation()[2][0]);
-  bv.axis[1] = Vec3f(s.getLocalRotation()[0][1], s.getLocalRotation()[1][1], s.getLocalRotation()[2][1]);
-  bv.axis[2] = Vec3f(s.getLocalRotation()[0][2], s.getLocalRotation()[1][2], s.getLocalRotation()[2][2]);
+  Vec3f R[3];
+  matMulMat(s.getRotation(), s.getLocalRotation(), R);
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+
+  bv.To = T;
+  bv.axis[0] = Vec3f(R[0][0], R[1][0], R[2][0]);
+  bv.axis[1] = Vec3f(R[0][1], R[1][1], R[2][1]);
+  bv.axis[2] = Vec3f(R[0][2], R[1][2], R[2][2]);
   bv.extent = Vec3f(s.radius, s.radius, s.lz / 2);
 }
 
 template<>
 void computeBV<OBB>(const Cylinder& s, OBB& bv)
 {
-  bv.To = s.getLocalTranslation();
-  bv.axis[0] = Vec3f(s.getLocalRotation()[0][0], s.getLocalRotation()[1][0], s.getLocalRotation()[2][0]);
-  bv.axis[1] = Vec3f(s.getLocalRotation()[0][1], s.getLocalRotation()[1][1], s.getLocalRotation()[2][1]);
-  bv.axis[2] = Vec3f(s.getLocalRotation()[0][2], s.getLocalRotation()[1][2], s.getLocalRotation()[2][2]);
+  Vec3f R[3];
+  matMulMat(s.getRotation(), s.getLocalRotation(), R);
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+
+  bv.To = T;
+  bv.axis[0] = Vec3f(R[0][0], R[1][0], R[2][0]);
+  bv.axis[1] = Vec3f(R[0][1], R[1][1], R[2][1]);
+  bv.axis[2] = Vec3f(R[0][2], R[1][2], R[2][2]);
   bv.extent = Vec3f(s.radius, s.radius, s.lz / 2);
 }
 
 template<>
 void computeBV<OBB>(const Convex& s, OBB& bv)
 {
+  Vec3f R[3];
+  matMulMat(s.getRotation(), s.getLocalRotation(), R);
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+
   fit(s.points, s.num_points, bv);
 
   Vec3f axis[3];
-  axis[0] = matMulVec(s.getLocalRotation(), bv.axis[0]);
-  axis[1] = matMulVec(s.getLocalRotation(), bv.axis[1]);
-  axis[2] = matMulVec(s.getLocalRotation(), bv.axis[2]);
+  axis[0] = matMulVec(R, bv.axis[0]);
+  axis[1] = matMulVec(R, bv.axis[1]);
+  axis[2] = matMulVec(R, bv.axis[2]);
 
   bv.axis[0] = axis[0];
   bv.axis[1] = axis[1];
   bv.axis[2] = axis[2];
 
-  bv.To = matMulVec(s.getLocalRotation(), bv.To) + s.getLocalTranslation();
+  bv.To = matMulVec(R, bv.To) + T;
 
 }
 
 template<>
 void computeBV<OBB>(const Plane& s, OBB& bv)
 {
+  Vec3f R[3];
+  matMulMat(s.getRotation(), s.getLocalRotation(), R);
+  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+
   // generate other two axes orthonormal to plane normal
   const Vec3f& w = s.n;
   Vec3f u, v;
@@ -235,7 +288,7 @@ void computeBV<OBB>(const Plane& s, OBB& bv)
   bv.extent = Vec3f(0, std::numeric_limits<BVH_REAL>::max(), std::numeric_limits<BVH_REAL>::max());
 
   Vec3f p = s.n * s.d;
-  bv.To = matMulVec(s.getLocalRotation(), p) + s.getLocalTranslation();
+  bv.To = matMulVec(R, p) + T;
 }
 
 void Box::computeLocalAABB()
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 571e4e1042ac18d74096678f4a9eb65e72123344..ce68dc213eed101ba37add5f8e405e071dca2907 100644
--- a/trunk/fcl/test/test_core_collision_shape_mesh_consistency.cpp
+++ b/trunk/fcl/test/test_core_collision_shape_mesh_consistency.cpp
@@ -45,21 +45,56 @@ TEST(consistency_shapemesh, spheresphere)
 {
   Sphere s1(20);
   Sphere s2(10);
+  BVHModel<AABB> s1_aabb;
   BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
   BVHModel<OBB> s2_obb;
-  BVHModel<RSS> s2_rss;
 
+  generateBVHModel(s1_aabb, s1);
   generateBVHModel(s2_aabb, s2);
+  generateBVHModel(s1_obb, s1);
   generateBVHModel(s2_obb, s2);
-  generateBVHModel(s2_rss, s2);
 
   std::vector<Contact> contacts;
   bool res;
 
+  // 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);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(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));
   contacts.clear();
   res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
@@ -69,14 +104,28 @@ TEST(consistency_shapemesh, spheresphere)
   contacts.clear();
   res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
-  //contacts.clear();
-  //res = (collide(&s2_rss, &s1, 1, false, false, contacts) > 0);
-  //ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 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));
   contacts.clear();
   res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
@@ -86,14 +135,28 @@ TEST(consistency_shapemesh, spheresphere)
   contacts.clear();
   res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
   ASSERT_FALSE(res);
-  //contacts.clear();
-  //res = (collide(&s2_rss, &s1, 1, false, false, contacts) > 0);
-  //ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 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
   contacts.clear();
   res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
@@ -103,10 +166,556 @@ TEST(consistency_shapemesh, spheresphere)
   contacts.clear();
   res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
   ASSERT_TRUE(res);
-  //contacts.clear();
-  //res = (collide(&s1, &s2_rss, 1, false, false, contacts) > 0);
-  //ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 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
+  contacts.clear();
+  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 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));
+  contacts.clear();
+  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(consistency_shapemesh, boxbox)
+{
+  Box s1(20, 40, 50);
+  Box s2(10, 10, 10);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+  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);
+
+  std::vector<Contact> contacts;
+  bool res;
+
+  // 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);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+
+
+  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);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 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));
+  contacts.clear();
+  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+}
+
+TEST(consistency_shapemesh, spherebox)
+{
+  Sphere s1(20);
+  Box s2(5, 5, 5);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+  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);
+
+  std::vector<Contact> contacts;
+  bool res;
+
+  // 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);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 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));
+  contacts.clear();
+  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 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));
+  contacts.clear();
+  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(consistency_shapemesh, cylindercylinder)
+{
+  Cylinder s1(5, 10);
+  Cylinder s2(5, 10);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+  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);
+
+  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));
+  contacts.clear();
+  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 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));
+  contacts.clear();
+  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(consistency_shapemesh, conecone)
+{
+  Cone s1(5, 10);
+  Cone s2(5, 10);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+  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);
+
+  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));
+  contacts.clear();
+  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 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));
+  contacts.clear();
+  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 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));
+  contacts.clear();
+  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_TRUE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 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));
+  contacts.clear();
+  res = (collide(&s1, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2_aabb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2_obb, &s1, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1, &s2_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_aabb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s2, &s1_obb, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
+  contacts.clear();
+  res = (collide(&s1_aabb, &s2, 1, false, false, contacts) > 0);
+  ASSERT_FALSE(res);
 }
 
 TEST(consistency, spheresphere)