Merge pull request #75 from jmirabel/devel

Improve GJK implementation + OBB overlap test + bug fixes

CMakeLists.txt

@@ -97,10 +97,8 @@ ADD_REQUIRED_DEPENDENCY("assimp >= 2.0")
 if(ASSIMP_FOUND)
   if (NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.1150")
     add_definitions(-DHPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES)
-    SET(HPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES TRUE)
     message(STATUS "Assimp version has unified headers")
   else()
-    SET(HPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES FALSE)
     message(STATUS "Assimp version does not have unified headers")
   endif()
 endif()
@@ -123,7 +121,6 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/shape/geometric_shapes.h
   include/hpp/fcl/distance_func_matrix.h
   include/hpp/fcl/collision.h
-  include/hpp/fcl/collision_node.h
   include/hpp/fcl/collision_func_matrix.h
   include/hpp/fcl/distance.h
   include/hpp/fcl/math/matrix_3f.h
@@ -162,10 +159,6 @@ endif ()
 
 pkg_config_append_libs("hpp-fcl")
 PKG_CONFIG_APPEND_BOOST_LIBS(thread date_time filesystem system)
-IF(HPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES)
-  # FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES kept for backard compatibility reasons.
-  PKG_CONFIG_APPEND_CFLAGS("-DFCL_USE_ASSIMP_UNIFIED_HEADER_NAMES -DHPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES")
-ENDIF(HPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES)
 IF(HPP_FCL_HAVE_OCTOMAP)
   # FCL_HAVE_OCTOMAP kept for backward compatibility reasons.
   PKG_CONFIG_APPEND_CFLAGS(

doc/gjk.py

+#!/usr/bin/env python3
+import pdb
+import sys
+
+# ABC = AB^AC
+# (ABC^AJ).a = (j.c - j.b) a.a + (j.a - j.c) b.a + (j.b - j.a) c.a, for j = b or c
+
+segment_fmt = "{j}a_aa"
+plane_fmt = ""
+edge_fmt = "{j}a * {b}a_{c}a + {j}{b} * {c}a_aa - {j}{c} * {b}a_aa"
+
+# These checks must be negative and not positive, as in the cheat sheet.
+# They are the same as in the cheat sheet, except that we consider (...).dot(A) instead of (...).dot(-A)
+plane_tests = [ "C.dot (a_cross_b)", "D.dot(a_cross_c)", "-D.dot(a_cross_b)" ]
+checks =  plane_tests \
+        + [ edge_fmt.format (**{'j':j,'b':"b",'c':"c"}) for j in [ "b", "c" ] ] \
+        + [ edge_fmt.format (**{'j':j,'b':"c",'c':"d"}) for j in [ "c", "d" ] ] \
+        + [ edge_fmt.format (**{'j':j,'b':"d",'c':"b"}) for j in [ "d", "b" ] ] \
+        + [ segment_fmt.format(**{'j':j}) for j in [ "b", "c", "d" ] ]
+checks_hr = [ "ABC.AO >= 0", "ACD.AO >= 0", "ADB.AO >= 0" ] \
+        + [ "(ABC ^ {}).AO >= 0".format(n) for n in [ "AB", "AC" ] ] \
+        + [ "(ACD ^ {}).AO >= 0".format(n) for n in [ "AC", "AD" ] ] \
+        + [ "(ADB ^ {}).AO >= 0".format(n) for n in [ "AD", "AB" ] ] \
+        + [ "AB.AO >= 0", "AC.AO >= 0", "AD.AO >= 0" ]
+
+# weights of the checks.
+weights = [ 2, ] * 3 + [ 3, ] * 6 + [ 1, ] * 3
+
+# Segment tests first, because they have lower weight.
+#tests = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, ]
+tests = [9, 10, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, ]
+assert len(tests) == len(checks)
+assert sorted(tests) == list(range(len(tests)))
+
+regions = [ "ABC", "ACD", "ADB", "AB", "AC", "AD", "A", "Inside", ]
+cases = list(range(len(regions)))
+
+# The following 3 lists refer to table doc/GJK_tetrahedra_boolean_table.ods
+
+# A check ID is (+/- (index+1)) where a minus sign encodes a NOT operation
+# and index refers to an index in list checks.
+
+# definitions is a list of list of check IDs to be ANDed.
+# For instance, a0.a3.!a4 -> [ 1, 4, -5]
+definitions = [
+        [  1,  4,- 5 ],
+        [  2,  6,- 7 ],
+        [  3,  8,- 9 ],
+        [- 4,  9, 10 ],
+        [- 6,  5, 11 ],
+        [- 8,  7, 12 ],
+        [-10,-11,-12 ],
+        [- 1,- 2,- 3 ],
+        ]
+# conditions is a list of (list of (list of check IDs to be ANDed) to be ORed).
+conditions = [
+        [],
+        [],
+        [],
+        [],
+        [],
+        [],
+        [],
+        [ ], #[ [10, 11, 12], ], # I don't think this is always true...
+        ]
+# rejections is a list of (list of (list of check IDs to be ANDed) to be ORed).
+rejections = [
+        [ [  2,  6,  7], [  3,-  8,-  9], ],
+        [ [  3,  8,  9], [  1,-  4,-  5], ],
+        [ [  1,  4,  5], [  2,-  6,-  7], ],
+        [ [- 1,- 3], ],
+        [ [- 2,- 1], ],
+        [ [- 3,- 2], ],
+        [ [  4,- 5], [  6,- 7], [  8,- 9], ],
+        [],
+        ]
+
+implications = [
+        [ [  4,  5, 10, ], [ 11],],
+        [ [  6,  7, 11, ], [ 12],],
+        [ [  8,  9, 12, ], [ 10],],
+
+        [ [- 4,- 5, 11, ], [ 10],],
+        [ [- 6,- 7, 12, ], [ 11],],
+        [ [- 8,- 9, 10, ], [ 12],],
+
+        [ [  1,  4,  5,  6], [- 7] ],
+        [ [  2,  6,  9,  8], [- 9] ],
+        [ [  3,  8,  9,  4], [- 5] ],
+
+        [ [- 4,  5,  10,], [- 11] ],
+        [ [  4,- 5,- 10,], [  11] ],
+        [ [- 6,  7,  11,], [- 12] ],
+        [ [  6,- 7,- 11,], [  12] ],
+        [ [- 8,  9,  12,], [- 10] ],
+        [ [  8,- 9,- 12,], [  10] ],
+
+        [ [- 4,  9,- 10,], [- 11,- 12] ],
+        [ [- 6,  5,- 11,], [- 12,- 10] ],
+        [ [- 8,  7,- 12,], [- 10,- 11] ],
+        ]
+
+def set_test_values (current_tests, test_values, itest, value):
+    def satisfies (values, indices):
+        for k in indices:
+            if k > 0 and values[ k-1] != True : return False
+            if k < 0 and values[-k-1] != False: return False
+        return True
+
+    remaining_tests = list(current_tests)
+    next_test_values = list(test_values)
+
+    remaining_tests.remove (itest)
+    next_test_values[itest] = value
+    rerun = True
+    while rerun:
+        rerun = False
+        for impl in implications:
+            if satisfies(next_test_values, impl[0]):
+                for id in impl[1]:
+                    k = (id - 1) if id > 0 else (-id-1)
+                    if k in remaining_tests:
+                        next_test_values[k] = (id > 0)
+                        remaining_tests.remove(k)
+                        rerun = True
+                    else:
+                        if next_test_values[k] != (id > 0):
+                            raise ValueError ("Absurd case")
+    return remaining_tests, next_test_values
+
+def apply_test_values (cases, test_values):
+    def canSatisfy (values, indices):
+        for k in indices:
+            if k > 0 and values[ k-1] == False: return False
+            if k < 0 and values[-k-1] == True : return False
+        return True
+    def satisfies (values, indices):
+        for k in indices:
+            if k > 0 and values[ k-1] != True : return False
+            if k < 0 and values[-k-1] != False: return False
+        return True
+
+    # Check all cases.
+    left_cases = []
+    for case in cases:
+        defi = definitions[case]
+        conds = conditions[case]
+        rejs = rejections[case]
+        if satisfies (test_values, defi):
+            # A definition is True, stop recursion
+            return [ case ]
+        if not canSatisfy (test_values, defi):
+            continue
+        for cond in conds:
+            if satisfies (test_values, cond):
+                # A condition is True, stop recursion
+                return [ case ]
+        append = True
+        for rej in rejs:
+            if satisfies (test_values, rej):
+                # A rejection is True, discard this case
+                append = False
+                break
+        if append: left_cases.append (case)
+    return left_cases
+
+def max_number_of_tests (current_tests, cases, test_values = [None,]*len(tests), prevBestScore = float('inf'), prevScore = 0):
+    for test in current_tests:
+        assert test_values[test] == None, "Test " +str(test)+ " already performed"
+
+    left_cases = apply_test_values (cases, test_values)
+
+    if len(left_cases) == 1:
+        return prevScore, { 'case': left_cases[0], }
+    elif len(left_cases) == 0:
+        return prevScore, { 'case': None, 'comments': [ "applied " + str(test_values), "to " + ", ".join([regions[c] for c in cases ]) ] }
+
+    assert len(current_tests) > 0, "No more test but " + str(left_cases) + " remains"
+
+    currentBestScore = prevBestScore
+    bestScore = float('inf')
+    bestOrder = [None, None]
+    for i, test in enumerate(current_tests):
+        assert bestScore >= currentBestScore
+
+        currentScore = prevScore + len(left_cases) * weights[test]
+        #currentScore = prevScore + weights[test]
+        if currentScore > currentBestScore: # Cannot do better -> stop
+            continue
+
+        try:
+            remaining_tests, next_test_values = set_test_values (current_tests, test_values, test, True)
+        except ValueError:
+            remaining_tests = None
+
+        if remaining_tests is not None:
+            # Do not put this in try catch as I do not want other ValueError to be understood as an infeasible branch.
+            score_if_t, order_if_t = max_number_of_tests (remaining_tests, left_cases, next_test_values, currentBestScore, currentScore)
+            if score_if_t >= currentBestScore: # True didn't do better -> stop
+                continue
+        else:
+            score_if_t, order_if_t = prevScore, None
+
+        try:
+            remaining_tests, next_test_values = set_test_values (current_tests, test_values, test, False)
+        except ValueError:
+            remaining_tests = None
+
+        if remaining_tests is not None:
+            # Do not put this in try catch as I do not want other ValueError to be understood as an infeasible branch.
+            score_if_f, order_if_f = max_number_of_tests (remaining_tests, left_cases, next_test_values, currentBestScore, currentScore)
+        else:
+            score_if_f, order_if_f = prevScore, None
+
+        currentScore = max(score_if_t, score_if_f)
+        if currentScore < bestScore:
+            if currentScore < currentBestScore:
+                bestScore = currentScore
+                bestOrder = { 'test': test, 'true': order_if_t, 'false': order_if_f }
+                #pdb.set_trace()
+                currentBestScore = currentScore
+                if len(tests) == len(current_tests):
+                    print ("New best score: {}".format(currentBestScore))
+
+    return bestScore, bestOrder
+
+def printComments (order, indent, file):
+    if 'comments' in order:
+        for comment in order['comments']:
+            print (indent + "// " + comment, file=file)
+
+def printOrder (order, indent = "", start=True,file=sys.stdout):
+    if start:
+        print ("bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next)", file=file)
+        print ("{", file=file)
+        print (indent+"// The code of this function was generated using doc/gjk.py", file=file)
+        print (indent+"const int a = 3, b = 2, c = 1, d = 0;", file=file)
+        for l in "abcd":
+            print (indent+"const Vec3f& {} (current.vertex[{}]->w);".format(l.upper(),l), file=file)
+        print (indent+"const FCL_REAL aa = A.squaredNorm();".format(l), file=file)
+        for l in "dcb":
+            for m in "abcd":
+                if m <= l:
+                    print (indent+"const FCL_REAL {0}{1}    = {2}.dot({3});".format(l,m,l.upper(),m.upper()), file=file)
+                else:
+                    print (indent+"const FCL_REAL& {0}{1}    = {1}{0};".format(l,m), file=file)
+            print (indent+"const FCL_REAL {0}a_aa = {0}a - aa;".format(l), file=file)
+        for l0,l1 in zip("bcd","cdb"):
+            print (indent+"const FCL_REAL {0}a_{1}a = {0}a - {1}a;".format(l0,l1), file=file)
+        for l in "bc":
+            print (indent+"const Vec3f a_cross_{0} = A.cross({1});".format(l,l.upper()), file=file)
+        print("", file=file)
+        print(       "#define REGION_INSIDE()               "+indent+"\\", file=file)
+        print(indent+"  ray.setZero();                      \\", file=file)
+        print(indent+"  next.vertex[0] = current.vertex[d]; \\", file=file)
+        print(indent+"  next.vertex[1] = current.vertex[c]; \\", file=file)
+        print(indent+"  next.vertex[2] = current.vertex[b]; \\", file=file)
+        print(indent+"  next.vertex[3] = current.vertex[a]; \\", file=file)
+        print(indent+"  next.rank=4;                        \\", file=file)
+        print(indent+"  return true;", file=file)
+        print("", file=file)
+
+    if 'case' in order:
+        case = order['case']
+        if case is None:
+            print (indent + "// There are no case corresponding to this set of tests.", file=file)
+            printComments (order, indent, file)
+            print (indent + "assert(false);", file=file)
+            return
+        region = regions[case]
+        print (indent + "// Region " + region, file=file)
+        printComments (order, indent, file)
+        toFree = ['b', 'c', 'd']
+        if region == "Inside":
+            print (indent + "REGION_INSIDE()", file=file)
+            toFree = []
+        elif region == 'A':
+            print (indent + "originToPoint (current, a, A, next, ray);", file=file)
+        elif len(region)==2:
+            a = region[0]
+            B = region[1]
+            print (indent + "originToSegment (current, a, {b}, A, {B}, {B}-A, -{b}a_aa, next, ray);".format(
+                **{ 'b':B.lower(), 'B':B,} ), file=file)
+            toFree.remove(B.lower())
+        elif len(region)==3:
+            B = region[1]
+            C = region[2]
+            test = plane_tests[['ABC','ACD','ADB'].index(region)]
+            if test.startswith('-'): test = test[1:]
+            else:                    test = '-'+test
+            print (indent + "originToTriangle (current, a, {b}, {c}, ({B}-A).cross({C}-A), {t}, next, ray);".format(
+                **{'b':B.lower(), 'c':C.lower(), 'B':B, 'C':C, 't':test}), file=file)
+            toFree.remove(B.lower())
+            toFree.remove(C.lower())
+        else:
+            assert False, "Unknown region " + region
+        for pt in toFree:
+            print (indent + "free_v[nfree++] = current.vertex[{}];".format(pt), file=file)
+    else:
+        assert "test" in order and 'true' in order and 'false' in order
+        check    = checks[order['test']]
+        check_hr = checks_hr[order['test']]
+        printComments (order, indent, file)
+        if order['true'] is None:
+            if order['false'] is None:
+                print (indent + """assert(false && "Case {} should never happen.");""".format(check_hr))
+            else:
+                print (indent + "assert(!({} <= 0)); // Not {}".format(check, check_hr), file=file)
+                printOrder (order['false'], indent=indent, start=False, file=file)
+        elif order['false'] is None:
+            print (indent + "assert({} <= 0); // {}".format(check, check_hr), file=file)
+            printOrder (order['true'], indent=indent, start=False, file=file)
+        else:
+            print (indent + "if ({} <= 0) {{ // if {}".format(check, check_hr), file=file)
+            printOrder (order['true'], indent=indent+"  ", start=False, file=file)
+            print (indent + "}} else {{ // not {}".format(check_hr), file=file)
+            printOrder (order['false'], indent=indent+"  ", start=False, file=file)
+            print (indent + "}} // end of {}".format(check_hr), file=file)
+
+    if start:
+        print ("",file=file)
+        print ("#undef REGION_INSIDE", file=file)
+        print (indent+"return false;", file=file)
+        print ("}", file=file)
+
+def unit_tests ():
+    # a4, a5, a10, a11, a12
+    cases = list(range(len(regions)))
+    pdb.set_trace()
+    left_cases = apply_test_values (cases,
+            test_values=[None,None,None,True,True,None,None,None,None,True,True,True])
+    assert len(left_cases) > 1
+
+#unit_tests()
+
+score, order = max_number_of_tests (tests, cases)
+
+print(score)
+printOrder(order, indent="  ")
+
+# TODO add weights such that:
+# - it is preferred to have all the use of one check in one branch.
+#   idea: ponderate by the number of remaining tests.

include/hpp/fcl/BV/AABB.h

@@ -96,12 +96,8 @@ public:
   }    
 
   /// Not implemented
-  inline bool overlap(const AABB& other, const CollisionRequest&,
-                      FCL_REAL& sqrDistLowerBound) const
-  {
-    sqrDistLowerBound = sqrt (-1);
-    return overlap (other);
-  }
+  bool overlap(const AABB& other, const CollisionRequest& request,
+               FCL_REAL& sqrDistLowerBound) const;
 
    /// @brief Check whether the AABB contains another AABB
   inline bool contain(const AABB& other) const

include/hpp/fcl/BV/OBBRSS.h

@@ -148,18 +148,27 @@ public:
 };
 
 /// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2);
+inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2)
+{
+  return overlap(R0, T0, b1.obb, b2.obb);
+}
 
 /// Check collision between two OBBRSS
 /// \param  b1 first OBBRSS in configuration (R0, T0)
 /// \param  b2 second OBBRSS in identity position
 /// \retval squared lower bound on the distance if OBBRSS do not overlap.
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
-             const OBBRSS& b2, const CollisionRequest& request,
-	     FCL_REAL& sqrDistLowerBound);
+inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
+                    const OBBRSS& b2, const CollisionRequest& request,
+	            FCL_REAL& sqrDistLowerBound)
+{
+  return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound);
+}
 
 /// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points
-FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
+inline FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL)
+{
+  return distance(R0, T0, b1.rss, b2.rss, P, Q);
+}
 
 }
 

include/hpp/fcl/BVH/BVH_model.h

@@ -54,8 +54,7 @@ namespace fcl
 
 /// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh)
 template<typename BV>
-class BVHModel : public CollisionGeometry,
-                 private boost::noncopyable
+class BVHModel : public CollisionGeometry
 {
 
 public:
@@ -132,12 +131,14 @@ public:
   /// @brief Access the bv giving the its index
   const BVNode<BV>& getBV(int id) const
   {
+    assert (id < num_bvs);
     return bvs[id];
   }
 
   /// @brief Access the bv giving the its index
   BVNode<BV>& getBV(int id)
   {
+    assert (id < num_bvs);
     return bvs[id];
   }
 

include/hpp/fcl/BVH/BV_fitter.h

@@ -41,6 +41,7 @@
 #include <hpp/fcl/BVH/BVH_internal.h>
 #include <hpp/fcl/BV/kIOS.h>
 #include <hpp/fcl/BV/OBBRSS.h>
+#include <hpp/fcl/BV/AABB.h>
 #include <iostream>
 
 namespace hpp
@@ -70,6 +71,8 @@ void fit<kIOS>(Vec3f* ps, int n, kIOS& bv);
 template<>
 void fit<OBBRSS>(Vec3f* ps, int n, OBBRSS& bv);
 
+template<>
+void fit<AABB>(Vec3f* ps, int n, AABB& bv);
 
 /// @brief Interface for fitting a bv given the triangles or points inside it.
 template<typename BV>
@@ -91,11 +94,11 @@ public:
 
 /// @brief The class for the default algorithm fitting a bounding volume to a set of points
 template<typename BV>
-class BVFitter : public BVFitterBase<BV>
+class BVFitterTpl : public BVFitterBase<BV>
 {
 public:
   /// @brief default deconstructor
-  virtual ~BVFitter() {}
+  virtual ~BVFitterTpl() {}
 
   /// @brief Prepare the geometry primitive data for fitting
   void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
@@ -115,6 +118,28 @@ public:
     type = type_;
   }
 
+  /// @brief Clear the geometry primitive data
+  void clear()
+  {
+    vertices = NULL;
+    prev_vertices = NULL;
+    tri_indices = NULL;
+    type = BVH_MODEL_UNKNOWN;
+  }
+
+protected:
+
+  Vec3f* vertices;
+  Vec3f* prev_vertices;
+  Triangle* tri_indices;
+  BVHModelType type;
+};
+
+/// @brief The class for the default algorithm fitting a bounding volume to a set of points
+template<typename BV>
+class BVFitter : public BVFitterTpl<BV>
+{
+public:
   /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
   /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data
   BV fit(unsigned int* primitive_indices, int num_primitives)
@@ -154,200 +179,61 @@ public:
     return bv;
   }
 
-  /// @brief Clear the geometry primitive data
-  void clear()
-  {
-    vertices = NULL;
-    prev_vertices = NULL;
-    tri_indices = NULL;
-    type = BVH_MODEL_UNKNOWN;
-  }
-
-private:
-
-  Vec3f* vertices;
-  Vec3f* prev_vertices;
-  Triangle* tri_indices;
-  BVHModelType type;
+protected:
+  using BVFitterTpl<BV>::vertices;
+  using BVFitterTpl<BV>::prev_vertices;
+  using BVFitterTpl<BV>::tri_indices;
+  using BVFitterTpl<BV>::type;
 };
 
-
 /// @brief Specification of BVFitter for OBB bounding volume
 template<>
-class BVFitter<OBB> : public BVFitterBase<OBB>
+class BVFitter<OBB> : public BVFitterTpl<OBB>
 {
 public:
-  /// @brief Prepare the geometry primitive data for fitting
-  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
-    vertices = vertices_;
-    prev_vertices = NULL;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Prepare the geometry primitive data for fitting, for deformable mesh
-  void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
-    vertices = vertices_;
-    prev_vertices = prev_vertices_;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
   /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
   /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
   OBB fit(unsigned int* primitive_indices, int num_primitives);
-
-  /// brief Clear the geometry primitive data
-  void clear()
-  {
-    vertices = NULL;
-    prev_vertices = NULL;
-    tri_indices = NULL;
-    type = BVH_MODEL_UNKNOWN;
-  }
-
-private:
-
-  Vec3f* vertices;
-  Vec3f* prev_vertices;
-  Triangle* tri_indices;
-  BVHModelType type;
 };
 
-
 /// @brief Specification of BVFitter for RSS bounding volume
 template<>
-class BVFitter<RSS> : public BVFitterBase<RSS>
+class BVFitter<RSS> : public BVFitterTpl<RSS>
 {
 public:
-  /// brief Prepare the geometry primitive data for fitting
-  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
-    vertices = vertices_;
-    prev_vertices = NULL;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Prepare the geometry primitive data for fitting, for deformable mesh
-  void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
-    vertices = vertices_;
-    prev_vertices = prev_vertices_;
-    tri_indices = tri_indices_;
-    type = type_;