add missing template instantiation for conservativeAdvancement

Now partial template instantiation are available for RSS/OBBRSS
which resemble the situation of 0.5.0 where

MeshConservativeAdvancementTraversalNodeRSS/OBBRSS are used and not
MeshConservativeAdvancementTraversalNode

The later refitting the whole tree, while the former two only work with
transformations locally stored in the node.

Refactors code to avoid duplication

resolves flexible-collision-library#297

include/fcl/narrowphase/detail/conservative_advancement_func_matrix-inl.h

@@ -70,17 +70,16 @@ namespace fcl
 namespace detail
 {
 
-//==============================================================================
-template<typename BV>
-bool conservativeAdvancement(const BVHModel<BV>& o1,
- const MotionBase<typename BV::S>* motion1,
- const BVHModel<BV>& o2,
- const MotionBase<typename BV::S>* motion2,
- const CollisionRequest<typename BV::S>& request,
- CollisionResult<typename BV::S>& result,
- typename BV::S& toc)
-{
- using S = typename BV::S;
+
+template<typename S, typename TraversalNode>
+bool conservativeAdvancementExec(TraversalNode& node,
+ const BVHModel<typename TraversalNode::BV>& o1,
+ const MotionBase<S>* motion1,
+ const BVHModel<typename TraversalNode::BV>& o2,
+ const MotionBase<S>* motion2,
+ const CollisionRequest<S>& request,
+ CollisionResult<S>& result,
+ S& toc) {
 
  Transform3<S> tf1;
  Transform3<S> tf2;
@@ -94,13 +93,11 @@ bool conservativeAdvancement(const BVHModel<BV>& o1,
  return true;
  }
 
+ typedef typename TraversalNode::BV BV;
 
  BVHModel<BV>* o1_tmp = new BVHModel<BV>(o1);
  BVHModel<BV>* o2_tmp = new BVHModel<BV>(o2);
 
-
- MeshConservativeAdvancementTraversalNode<BV> node;
-
  node.motion1 = motion1;
  node.motion2 = motion2;
 
@@ -146,6 +143,56 @@ bool conservativeAdvancement(const BVHModel<BV>& o1,
  return false;
 }
 
+
+
+//==============================================================================
+template<typename BV>
+bool conservativeAdvancement(const BVHModel<BV>& o1,
+ const MotionBase<typename BV::S>* motion1,
+ const BVHModel<BV>& o2,
+ const MotionBase<typename BV::S>* motion2,
+ const CollisionRequest<typename BV::S>& request,
+ CollisionResult<typename BV::S>& result,
+ typename BV::S& toc)
+{
+
+ MeshConservativeAdvancementTraversalNode<BV> node;
+
+ return conservativeAdvancementExec(node, o1, motion1, o2, motion2, request, result, toc);
+
+}
+
+
+template<typename S>
+bool conservativeAdvancement(const BVHModel<RSS<S>>& o1,
+ const MotionBase<S>* motion1,
+ const BVHModel<RSS<S>>& o2,
+ const MotionBase<S>* motion2,
+ const CollisionRequest<S>& request,
+ CollisionResult<S>& result,
+ S& toc)
+{
+
+ MeshConservativeAdvancementTraversalNodeRSS<S> node;
+
+ return conservativeAdvancementExec(node, o1, motion1, o2, motion2, request, result, toc);
+}
+
+template<typename S>
+bool conservativeAdvancement(const BVHModel<OBBRSS<S>>& o1,
+ const MotionBase<S>* motion1,
+ const BVHModel<OBBRSS<S>>& o2,
+ const MotionBase<S>* motion2,
+ const CollisionRequest<S>& request,
+ CollisionResult<S>& result,
+ S& toc)
+{
+
+ MeshConservativeAdvancementTraversalNodeOBBRSS<S> node;
+
+ return conservativeAdvancementExec(node, o1, motion1, o2, motion2, request, result, toc);
+}
+
 template<typename BV, typename ConservativeAdvancementOrientedNode>
 bool conservativeAdvancementMeshOriented(const BVHModel<BV>& o1,
  const MotionBase<typename BV::S>* motion1,

include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h

@@ -57,9 +57,9 @@ class FCL_EXPORT MeshConservativeAdvancementTraversalNodeRSS<double>;
 extern template
 bool initialize(
  MeshConservativeAdvancementTraversalNodeRSS<double>& node,
- const BVHModel<RSS<double>>& model1,
+ BVHModel<RSS<double>>& model1,
  const Transform3<double>& tf1,
- const BVHModel<RSS<double>>& model2,
+ BVHModel<RSS<double>>& model2,
  const Transform3<double>& tf2,
  double w);
 
@@ -71,9 +71,9 @@ class FCL_EXPORT MeshConservativeAdvancementTraversalNodeOBBRSS<double>;
 extern template
 bool initialize(
  MeshConservativeAdvancementTraversalNodeOBBRSS<double>& node,
- const BVHModel<OBBRSS<double>>& model1,
+ BVHModel<OBBRSS<double>>& model1,
  const Transform3<double>& tf1,
- const BVHModel<OBBRSS<double>>& model2,
+ BVHModel<OBBRSS<double>>& model2,
  const Transform3<double>& tf2,
  double w);
 
@@ -776,9 +776,9 @@ bool setupMeshConservativeAdvancementOrientedDistanceNode(
 template <typename S>
 bool initialize(
  MeshConservativeAdvancementTraversalNodeRSS<S>& node,
- const BVHModel<RSS<S>>& model1,
+ BVHModel<RSS<S>>& model1,
  const Transform3<S>& tf1,
- const BVHModel<RSS<S>>& model2,
+ BVHModel<RSS<S>>& model2,
  const Transform3<S>& tf2,
  S w)
 {
@@ -790,9 +790,9 @@ bool initialize(
 template <typename S>
 bool initialize(
  MeshConservativeAdvancementTraversalNodeOBBRSS<S>& node,
- const BVHModel<OBBRSS<S>>& model1,
+ BVHModel<OBBRSS<S>>& model1,
  const Transform3<S>& tf1,
- const BVHModel<OBBRSS<S>>& model2,
+ BVHModel<OBBRSS<S>>& model2,
  const Transform3<S>& tf2,
  S w)
 {

include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h

@@ -49,13 +49,14 @@ namespace detail
 {
 
 /// @brief continuous collision node using conservative advancement. when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration
-template <typename BV>
+template <typename BV_>
 class FCL_EXPORT MeshConservativeAdvancementTraversalNode
- : public MeshDistanceTraversalNode<BV>
+ : public MeshDistanceTraversalNode<BV_>
 {
 public:
 
- using S = typename BV::S;
+ typedef BV_ BV;
+ typedef typename BV::S S;
 
  MeshConservativeAdvancementTraversalNode(S w_ = 1);
 
@@ -153,9 +154,9 @@ template <typename S>
 FCL_EXPORT
 bool initialize(
  MeshConservativeAdvancementTraversalNodeRSS<S>& node,
- const BVHModel<RSS<S>>& model1,
+ BVHModel<RSS<S>>& model1,
  const Transform3<S>& tf1,
- const BVHModel<RSS<S>>& model2,
+ BVHModel<RSS<S>>& model2,
  const Transform3<S>& tf2,
  S w = 1);
 
@@ -202,9 +203,9 @@ template <typename S>
 FCL_EXPORT
 bool initialize(
  MeshConservativeAdvancementTraversalNodeOBBRSS<S>& node,
- const BVHModel<OBBRSS<S>>& model1,
+ BVHModel<OBBRSS<S>>& model1,
  const Transform3<S>& tf1,
- const BVHModel<OBBRSS<S>>& model2,
+ BVHModel<OBBRSS<S>>& model2,
  const Transform3<S>& tf2,
  S w = 1);
 

src/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.cpp

@@ -51,9 +51,9 @@ class MeshConservativeAdvancementTraversalNodeRSS<double>;
 template
 bool initialize(
  MeshConservativeAdvancementTraversalNodeRSS<double>& node,
- const BVHModel<RSS<double>>& model1,
+ BVHModel<RSS<double>>& model1,
  const Transform3<double>& tf1,
- const BVHModel<RSS<double>>& model2,
+ BVHModel<RSS<double>>& model2,
  const Transform3<double>& tf2,
  double w);
 
@@ -65,9 +65,9 @@ class MeshConservativeAdvancementTraversalNodeOBBRSS<double>;
 template
 bool initialize(
  MeshConservativeAdvancementTraversalNodeOBBRSS<double>& node,
- const BVHModel<OBBRSS<double>>& model1,
+ BVHModel<OBBRSS<double>>& model1,
  const Transform3<double>& tf1,
- const BVHModel<OBBRSS<double>>& model2,
+ BVHModel<OBBRSS<double>>& model2,
  const Transform3<double>& tf2,
  double w);