Merge pull request #70 from jmirabel/cleaning

[Cleaning]  Remove classes MeshCollisionTraversalNode[bvtype]

CMakeLists.txt

@@ -130,6 +130,7 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/math/vec_3f.h
   include/hpp/fcl/math/tools.h
   include/hpp/fcl/math/transform.h
+  include/hpp/fcl/traversal/details/traversal.h
   include/hpp/fcl/traversal/traversal_node_shapes.h
   include/hpp/fcl/traversal/traversal_node_setup.h
   include/hpp/fcl/traversal/traversal_recurse.h

include/hpp/fcl/BV/AABB.h

@@ -262,6 +262,14 @@ static inline AABB rotate(const AABB& aabb, const Matrix3f& t)
   return res;
 }
 
+/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2);
+
+/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+	     const AABB& b2, const CollisionRequest& request,
+             FCL_REAL& sqrDistLowerBound);
+
 }
 
 } // namespace hpp

include/hpp/fcl/BV/RSS.h

@@ -155,6 +155,10 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
 /// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
 bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2);
 
+/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
+             const CollisionRequest& request,
+             FCL_REAL& sqrDistLowerBound);
 
 }
 

include/hpp/fcl/BV/kDOP.h

@@ -39,7 +39,7 @@
 #define HPP_FCL_KDOP_H
 
 #include <stdexcept>
-#include <hpp/fcl/math/vec_3f.h>
+#include <hpp/fcl/math/matrix_3f.h>
 
 namespace hpp
 {
@@ -176,6 +176,20 @@ public:
 
 };
 
+template<size_t N>
+bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
+             const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/)
+{
+  throw std::logic_error ("not implemented");
+}
+
+template<size_t N>
+bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
+             const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/,
+             const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/)
+{
+  throw std::logic_error ("not implemented");
+}
 
 /// @brief translate the KDOP BV
 template<size_t N>

include/hpp/fcl/BV/kIOS.h

@@ -157,6 +157,12 @@ kIOS translate(const kIOS& bv, const Vec3f& t);
 /// @todo Not efficient
 bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
 
+/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.
+/// @todo Not efficient
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2,
+             const CollisionRequest& request,
+             FCL_REAL& sqrDistLowerBound);
+
 /// @brief Approximate distance between two kIOS bounding volumes
 /// @todo P and Q is not returned, need implementation
 FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);

include/hpp/fcl/narrowphase/narrowphase.h

@@ -101,6 +101,7 @@ namespace fcl
     }
 
     //// @brief intersection checking between one shape and a triangle with transformation
+    /// \return true if the shape are colliding.
     template<typename S>
     bool shapeTriangleInteraction
     (const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,

include/hpp/fcl/traversal/details/traversal.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, LAAS CNRS
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Joseph Mirabel */
+
+
+#ifndef HPP_FCL_TRAVERSAL_DETAILS_TRAVERSAL_H
+#define HPP_FCL_TRAVERSAL_DETAILS_TRAVERSAL_H
+
+namespace hpp
+{
+namespace fcl
+{
+
+enum {
+  RelativeTransformationIsIdentity = 1
+};
+
+namespace details
+{
+  template <bool enabled>
+  struct RelativeTransformation
+  {
+    RelativeTransformation () : R (Matrix3f::Identity()) {}
+
+    const Matrix3f& _R () const { return R; }
+    const Vec3f   & _T () const { return T; }
+
+    Matrix3f R;
+    Vec3f T;
+  };
+
+  template <>
+  struct RelativeTransformation <false>
+  {
+    static const Matrix3f& _R () { throw std::logic_error ("should never reach this point"); }
+    static const Vec3f   & _T () { throw std::logic_error ("should never reach this point"); }
+  };
+} // namespace details
+
+}
+
+} // namespace hpp
+
+#endif

include/hpp/fcl/traversal/traversal_node_base.h

@@ -137,6 +137,8 @@ public:
   virtual ~DistanceTraversalNodeBase();
 
   /// @brief BV test between b1 and b2
+  /// \return a lower bound of the distance between the two BV.
+  /// \note except for OBB, this method returns the distance.
   virtual FCL_REAL BVTesting(int b1, int b2) const;
 
   /// @brief Leaf test between node b1 and b2, if they are both leafs

include/hpp/fcl/traversal/traversal_node_bvhs.h

@@ -47,6 +47,7 @@
 #include <hpp/fcl/intersect.h>
 #include <hpp/fcl/shape/geometric_shapes.h>
 #include <hpp/fcl/narrowphase/narrowphase.h>
+#include <hpp/fcl/traversal/details/traversal.h>
 
 #include <boost/shared_array.hpp>
 #include <boost/shared_ptr.hpp>
@@ -124,24 +125,6 @@ public:
   {
     return model2->getBV(b).rightChild();
   }
-
-  /// @brief BV culling test in one BVTT node
-  bool BVTesting(int b1, int b2) const
-  {
-    if(enable_statistics) num_bv_tests++;
-    return !model1->getBV(b1).overlap(model2->getBV(b2));
-  }
-  
-  /// BV test between b1 and b2
-  /// \param b1, b2 Bounding volumes to test,
-  /// \retval sqrDistLowerBound square of a lower bound of the minimal
-  ///         distance between bounding volumes.
-  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
-  {
-    if(enable_statistics) num_bv_tests++;
-    return !model1->getBV(b1).overlap(model2->getBV(b2), request,
-                                      sqrDistLowerBound);
-  }
   
   /// @brief The first BVH model
   const BVHModel<BV>* model1;
@@ -154,12 +137,16 @@ public:
   mutable FCL_REAL query_time_seconds;
 };
 
-
 /// @brief Traversal node for collision between two meshes
-template<typename BV>
+template<typename BV, int _Options = RelativeTransformationIsIdentity>
 class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
 {
 public:
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
   MeshCollisionTraversalNode(const CollisionRequest& request) :
   BVHCollisionTraversalNode<BV> (request)
   {
@@ -169,6 +156,36 @@ public:
     tri_indices2 = NULL;
   }
 
+  /// @brief BV culling test in one BVTT node
+  bool BVTesting(int b1, int b2) const
+  {
+    if(this->enable_statistics) this->num_bv_tests++;
+    if (RTIsIdentity)
+      return !this->model1->getBV(b1).overlap(this->model2->getBV(b2));
+    else
+      return !overlap(RT._R(), RT._T(),
+          this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
+  }
+  
+  /// BV test between b1 and b2
+  /// \param b1, b2 Bounding volumes to test,
+  /// \retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+  {
+    if(this->enable_statistics) this->num_bv_tests++;
+    if (RTIsIdentity)
+      return !this->model1->getBV(b1).overlap(this->model2->getBV(b2),
+          this->request, sqrDistLowerBound);
+    else {
+      bool res = !overlap(RT._R(), RT._T(),
+          this->model1->getBV(b1).bv, this->model2->getBV(b2).bv,
+          this->request, sqrDistLowerBound);
+      assert (!res || sqrDistLowerBound > 0);
+      return res;
+    }
+  }
+
   /// Intersection testing between leaves (two triangles)
   ///
   /// \param b1, b2 id of primitive in bounding volume hierarchy
@@ -242,61 +259,41 @@ public:
 
   Triangle* tri_indices1;
   Triangle* tri_indices2;
-};
-
-
-/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
-class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode<OBB>
-{
-public:
-  MeshCollisionTraversalNodeOBB (const CollisionRequest& request);
-
-  bool BVTesting(int b1, int b2) const;
-
-  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
-
-  Matrix3f R;
-  Vec3f T;
-};
 
-class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode<RSS>
-{
-public:
-  MeshCollisionTraversalNodeRSS (const CollisionRequest& request);
-
-  bool BVTesting(int b1, int b2) const;
-
-  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
-
-  Matrix3f R;
-  Vec3f T;
+  details::RelativeTransformation<!bool(RTIsIdentity)> RT;
 };
 
-class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS>
-{
-public:
-  MeshCollisionTraversalNodekIOS (const CollisionRequest& request);
- 
-  bool BVTesting(int b1, int b2) const;
-
-  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
-
-  Matrix3f R;
-  Vec3f T;
-};
+/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
+typedef MeshCollisionTraversalNode<OBB   , 0> MeshCollisionTraversalNodeOBB   ;
+typedef MeshCollisionTraversalNode<RSS   , 0> MeshCollisionTraversalNodeRSS   ;
+typedef MeshCollisionTraversalNode<kIOS  , 0> MeshCollisionTraversalNodekIOS  ;
+typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
 
-class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode<OBBRSS>
+namespace details
 {
-public:
-  MeshCollisionTraversalNodeOBBRSS (const CollisionRequest& request);
- 
-  bool BVTesting(int b1, int b2) const;
-
-  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
+  template<typename BV> struct DistanceTraversalBVTesting_impl
+  {
+    static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2)
+    {
+      return b1.distance(b2);
+    }
+  };
 
-  Matrix3f R;
-  Vec3f T;
-};
+  template<> struct DistanceTraversalBVTesting_impl<OBB>
+  {
+    static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2)
+    {
+      FCL_REAL sqrDistLowerBound;
+      CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
+      // request.break_distance = ?
+      if (b1.overlap(b2, request, sqrDistLowerBound)) {
+        // TODO A penetration upper bound should be computed.
+        return -1;
+      }
+      return sqrt (sqrDistLowerBound);
+    }
+  };
+} // namespace details
 
 /// @brief Traversal node for distance computation between BVH models
 template<typename BV>
@@ -367,7 +364,8 @@ public:
   FCL_REAL BVTesting(int b1, int b2) const
   {
     if(enable_statistics) num_bv_tests++;
-    return model1->getBV(b1).distance(model2->getBV(b2));
+    return details::DistanceTraversalBVTesting_impl<BV>
+      ::run (model1->getBV(b1), model2->getBV(b2));
   }
 
   /// @brief The first BVH model

src/BV/AABB.cpp

@@ -128,6 +128,20 @@ FCL_REAL AABB::distance(const AABB& other) const
   return std::sqrt(result);
 }
 
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2)
+{
+  AABB bb1 (translate (rotate (b1, R0), T0));
+  return bb1.overlap (b2);
+}
+
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+	     const AABB& b2, const CollisionRequest& request,
+             FCL_REAL& sqrDistLowerBound)
+{
+  AABB bb1 (translate (rotate (b1, R0), T0));
+  return bb1.overlap (b2, request, sqrDistLowerBound);
+}
+
 }
 
 } // namespace hpp

src/BV/RSS.cpp

@@ -862,6 +862,25 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
   return (dist <= (b1.r + b2.r));
 }
 
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
+             const CollisionRequest& /*request*/,
+             FCL_REAL& sqrDistLowerBound)
+{
+  // ROb2 = R0 . b2
+  // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
+
+  // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
+  // R = b2^T RO^T b1
+  Vec3f Ttemp (R0 * b2.Tr + T0 - b1.Tr);
+  Vec3f T(b1.axes.transpose() * Ttemp);
+  Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
+
+  FCL_REAL dist = rectDistance(R, T, b1.l, b2.l) - b1.r - b2.r;
+  if (dist <= 0) return true;
+  sqrDistLowerBound = dist * dist;
+  return false;
+}
+
 bool RSS::contain(const Vec3f& p) const
 {
   Vec3f local_p = p - Tr;

src/BV/kIOS.cpp

@@ -61,17 +61,27 @@ bool kIOS::overlap(const kIOS& other) const
   }
 
   return obb.overlap(other.obb);
-
-  return true;
 }
 
-  bool kIOS::overlap(const kIOS& other, const CollisionRequest&,
-                     FCL_REAL& sqrDistLowerBound) const
+bool kIOS::overlap(const kIOS& other, const CollisionRequest& request,
+                   FCL_REAL& sqrDistLowerBound) const
+{
+  for(unsigned int i = 0; i < num_spheres; ++i)
   {
-    sqrDistLowerBound = sqrt (-1);
-    return overlap (other);
+    for(unsigned int j = 0; j < other.num_spheres; ++j)
+    {
+      FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
+      FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
+      if(o_dist > sum_r * sum_r) {
+        o_dist = sqrt(o_dist) - sum_r;
+        sqrDistLowerBound = o_dist*o_dist;
+        return false;
+      }
+    }
   }
 
+  return obb.overlap(other.obb, request, sqrDistLowerBound);
+}
 
 bool kIOS::contain(const Vec3f& p) const
 {
@@ -192,6 +202,23 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2
   return b1.overlap(b2_temp);
 }
 
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2,
+             const CollisionRequest& request,
+             FCL_REAL& sqrDistLowerBound)
+{
+  kIOS b2_temp = b2;
+  for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
+  {
+    b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
+  }
+
+  
+  b2_temp.obb.To = R0 * b2_temp.obb.To + T0;
+  b2_temp.obb.axes.applyOnTheLeft(R0);
+
+  return b1.overlap(b2_temp, request, sqrDistLowerBound);
+}
+
 FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P, Vec3f* Q)
 {
   kIOS b2_temp = b2;

src/distance_func_matrix.cpp

@@ -379,6 +379,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB, Convex, NarrowPhaseSolver>::distance;
   distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB, Plane, NarrowPhaseSolver>::distance;
   distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB, Halfspace, NarrowPhaseSolver>::distance;
+  */
 
   distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere, NarrowPhaseSolver>::distance;
@@ -388,7 +389,6 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB, Convex, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB, Plane, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer<OBB, Halfspace, NarrowPhaseSolver>::distance;
-  */
 
   distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS, Sphere, NarrowPhaseSolver>::distance;
@@ -448,6 +448,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer<OBBRSS, Halfspace, NarrowPhaseSolver>::distance;
 
   distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB, NarrowPhaseSolver>;
+  distance_matrix[BV_OBB][BV_OBB] = &BVHDistance<OBB, NarrowPhaseSolver>;
   distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS, NarrowPhaseSolver>;
   distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS, NarrowPhaseSolver>;
   distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS, NarrowPhaseSolver>;

src/traversal/traversal_node_bvhs.cpp

@@ -86,92 +86,6 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
 }
 } // namespace details
 
-MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB
-(const CollisionRequest& request) :
-  MeshCollisionTraversalNode<OBB> (request)
-{
-  R.setIdentity();
-}
-
-bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
-{
-  if(enable_statistics) num_bv_tests++;
-  return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-bool MeshCollisionTraversalNodeOBB::BVTesting
-(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
-{
-  if(enable_statistics) num_bv_tests++;
-  return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv,
-                  request, sqrDistLowerBound);
-}
-
-MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS
-(const CollisionRequest& request) :
-  MeshCollisionTraversalNode<RSS> (request)
-{
-  R.setIdentity();
-}
-
-bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
-{
-  if(enable_statistics) num_bv_tests++;
-  return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2,
-                                              FCL_REAL& sqrDistLowerBound) const
-{
-  if(enable_statistics) num_bv_tests++;
-  sqrDistLowerBound = 0;
-  return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS
-(const CollisionRequest& request) :
-  MeshCollisionTraversalNode<kIOS>(request)
-{
-  R.setIdentity();
-}
-
-bool MeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const
-{
-  if(enable_statistics) num_bv_tests++;
-  return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-bool MeshCollisionTraversalNodekIOS::BVTesting
-(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
-{
-  if(enable_statistics) num_bv_tests++;
-  sqrDistLowerBound = 0;
-  return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-MeshCollisionTraversalNodeOBBRSS::MeshCollisionTraversalNodeOBBRSS
-(const CollisionRequest& request) :
-  MeshCollisionTraversalNode<OBBRSS> (request)
-{
-  R.setIdentity();
-}
-
-bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
-{
-  if(enable_statistics) num_bv_tests++;
-  return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-  bool MeshCollisionTraversalNodeOBBRSS::BVTesting
-  (int b1, int b2, FCL_REAL& sqrDistLowerBound) const
-  {
-    if(enable_statistics) num_bv_tests++;
-    bool res (!overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv,
-                       request, sqrDistLowerBound));
-    assert (!res || sqrDistLowerBound > 0);
-    return res;
-  }
-
 namespace details
 {
 

src/traversal/traversal_node_setup.cpp

@@ -68,7 +68,7 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
 
   node.result = &result;
 
-  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
+  relativeTransform(tf1.getRotation(), tf1