Merge pull request #78 from jmirabel/python

Enable Convex / Convex queries + Add Python bindings.

CMakeLists.txt

@@ -37,6 +37,7 @@ set(CXX_DISABLE_WERROR TRUE)
 include(cmake/base.cmake)
 include(cmake/eigen.cmake)
 include(cmake/boost.cmake)
+include(cmake/python.cmake)
 include(cmake/hpp.cmake)
 
 set(PROJECT_NAME hpp-fcl)
@@ -57,6 +58,8 @@ IF(APPLE)
   endif("${isSystemDir}" STREQUAL "-1")
 ENDIF(APPLE)
 
+OPTION(BUILD_PYTHON_INTERFACE OFF)
+
 setup_hpp_project()
 
 add_required_dependency("eigen3 >= 3.0.0")
@@ -66,10 +69,13 @@ include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS})
 set (RUN_TESTS TRUE CACHE BOOL "compile and run unit tests")
 
 # Required dependencies
+set(BOOST_COMPONENTS thread date_time system)
 if (RUN_TESTS)
-  set(BOOST_COMPONENTS thread date_time filesystem system unit_test_framework)
-else ()
-  set(BOOST_COMPONENTS thread date_time system)
+  set(BOOST_COMPONENTS ${BOOST_COMPONENTS} filesystem unit_test_framework)
+endif ()
+if (BUILD_PYTHON_INTERFACE)
+  FINDPYTHON()
+  set(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
 endif ()
 
 search_for_boost()
@@ -153,6 +159,9 @@ SET(${PROJECT_NAME}_HEADERS
   )
 
 add_subdirectory(src)
+if (BUILD_PYTHON_INTERFACE)
+  add_subdirectory(python)
+endif ()
 if (RUN_TESTS)
   add_subdirectory(test)
 endif ()

doc/gjk.py

@@ -95,9 +95,14 @@ implications = [
         [ [- 8,  9,  12,], [- 10] ],
         [ [  8,- 9,- 12,], [  10] ],
 
-        [ [- 4,  9,- 10,], [- 11,- 12] ],
-        [ [- 6,  5,- 11,], [- 12,- 10] ],
-        [ [- 8,  7,- 12,], [- 10,- 11] ],
+        [ [ 10, 3, 9, -12, 4, -5], [1] ],
+        [ [ 10, -3, 1, -4], [9] ],
+        [ [ 10, -3, -1, 2, -6, 11], [5] ],
+        [ [ -10, 11, 2, -12, -5, -1], [6] ],
+        [ [ -10,11,-2,1,5], [-6] ],
+        [ [-10,-11,12,1,-7,-2,4],[-5]],
+        [ [-10,-11,12,-3,2,7],[-8]],
+        [ [-10,-11,12,-3,-2],[-1]],
         ]
 
 def set_test_values (current_tests, test_values, itest, value):
@@ -128,6 +133,11 @@ def set_test_values (current_tests, test_values, itest, value):
                             raise ValueError ("Absurd case")
     return remaining_tests, next_test_values
 
+def set_tests_values (current_tests, test_values, itests, values):
+    for itest,value in zip(itests,values):
+        current_tests, test_values = set_test_values (current_tests, test_values, itest, value)
+    return current_tests, test_values
+
 def apply_test_values (cases, test_values):
     def canSatisfy (values, indices):
         for k in indices:
@@ -229,12 +239,12 @@ def printComments (order, indent, file):
         for comment in order['comments']:
             print (indent + "// " + comment, file=file)
 
-def printOrder (order, indent = "", start=True,file=sys.stdout):
+def printOrder (order, indent = "", start=True,file=sys.stdout,curTests=[]):
     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)
+        print (indent+"const vertex_id_t 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)
@@ -301,20 +311,22 @@ def printOrder (order, indent = "", start=True,file=sys.stdout):
         check    = checks[order['test']]
         check_hr = checks_hr[order['test']]
         printComments (order, indent, file)
+        nextTests_t=curTests+["a"+str(order['test']+1),]
+        nextTests_f=curTests+["!a"+str(order['test']+1),]
         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)
+                print (indent + "assert(!({} <= 0)); // Not {} / {}".format(check, check_hr, ".".join(nextTests_f)), file=file)
+                printOrder (order['false'], indent=indent, start=False, file=file, curTests=nextTests_f)
         elif order['false'] is None:
-            print (indent + "assert({} <= 0); // {}".format(check, check_hr), file=file)
-            printOrder (order['true'], indent=indent, start=False, file=file)
+            print (indent + "assert({} <= 0); // {} / {}".format(check, check_hr, ".".join(nextTests_t)), file=file)
+            printOrder (order['true'], indent=indent, start=False, file=file, curTests=nextTests_t)
         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 + "if ({} <= 0) {{ // if {} / {}".format(check, check_hr, ".".join(nextTests_t)), file=file)
+            printOrder (order['true'], indent=indent+"  ", start=False, file=file, curTests=nextTests_t)
+            print (indent + "}} else {{ // not {} / {}".format(check_hr, ".".join(nextTests_f)), file=file)
+            printOrder (order['false'], indent=indent+"  ", start=False, file=file, curTests=nextTests_f)
             print (indent + "}} // end of {}".format(check_hr), file=file)
 
     if start:

include/hpp/fcl/BVH/BVH_model.h

@@ -52,11 +52,11 @@ namespace hpp
 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
-{
+class ConvexBase;
 
+/// @brief A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh)
+class BVHModelBase : public CollisionGeometry
+{
 public:
   /// @brief Geometry point data
   Vec3f* vertices;
@@ -76,11 +76,8 @@ public:
   /// @brief The state of BVH building process
   BVHBuildState build_state;
 
-  /// @brief Split rule to split one BV node into two children
-  boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
-
-  /// @brief Fitting rule to fit a BV node to a set of geometry primitives
-  boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
+  /// @brief Convex<Triangle> representation of this object
+  boost::shared_ptr< ConvexBase > convex;
   
   /// @brief Model type described by the instance
   BVHModelType getModelType() const
@@ -94,66 +91,32 @@ public:
   }
 
   /// @brief Constructing an empty BVH
-  BVHModel() : vertices(NULL),
-               tri_indices(NULL),
-               prev_vertices(NULL),
-               num_tris(0),
-               num_vertices(0),
-               build_state(BVH_BUILD_STATE_EMPTY),
-               bv_splitter(new BVSplitter<BV>(SPLIT_METHOD_MEAN)),
-               bv_fitter(new BVFitter<BV>()),
-               num_tris_allocated(0),
-               num_vertices_allocated(0),
-               num_bvs_allocated(0),
-               num_vertex_updated(0),
-               primitive_indices(NULL),
-               bvs(NULL),
-               num_bvs(0)
+  BVHModelBase() : vertices(NULL),
+                   tri_indices(NULL),
+                   prev_vertices(NULL),
+                   num_tris(0),
+                   num_vertices(0),
+                   build_state(BVH_BUILD_STATE_EMPTY),
+                   num_tris_allocated(0),
+                   num_vertices_allocated(0),
+                   num_vertex_updated(0)
   {
   }
 
   /// @brief copy from another BVH
-  BVHModel(const BVHModel& other);
+  BVHModelBase(const BVHModelBase& other);
 
   /// @brief deconstruction, delete mesh data related.
-  ~BVHModel()
+  virtual ~BVHModelBase ()
   {
     delete [] vertices;
     delete [] tri_indices;
-    delete [] bvs;
-
     delete [] prev_vertices;
-    delete [] primitive_indices;
-  }
-
-  /// @brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some flexibility here
-  
-  /// @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];
-  }
-
-  /// @brief Get the number of bv in the BVH
-  int getNumBVs() const
-  {
-    return num_bvs;
   }
 
   /// @brief Get the object type: it is a BVH
   OBJECT_TYPE getObjectType() const { return OT_BVH; }
 
-  /// @brief Get the BV type: default is unknown
-  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
-
   /// @brief Compute the AABB for the BVH, used for broad-phase collision
   void computeLocalAABB();
 
@@ -206,16 +169,16 @@ public:
   /// @brief End BVH model update, will also refit or rebuild the bounding volume hierarchy
   int endUpdateModel(bool refit = true, bool bottomup = true);
 
-  /// @brief Check the number of memory used
-  int memUsage(int msg) const;
+  /// @brief Build this Convex<Triangle> representation of this model.
+  /// \note this only takes the points of this model. It does not check that the
+  ///       object is convex. It does not compute a convex hull.
+  void buildConvexRepresentation(bool share_memory);
+
+  virtual int memUsage(int msg) const = 0;
 
-/// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent 
+  /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent 
   /// BV node. When traversing the BVH, this can save one matrix transformation.
-  void makeParentRelative()
-  {
-    Matrix3f I (Matrix3f::Identity());
-    makeParentRelativeRecurse(0, I, Vec3f());
-  }
+  virtual void makeParentRelative() = 0;
 
   Vec3f computeCOM() const
   {
@@ -267,14 +230,96 @@ public:
     return C.trace() * Matrix3f::Identity() - C;
   }
 
-private:
+protected:
+  virtual void deleteBVs() = 0;
+  virtual bool allocateBVs() = 0;
+
+  /// @brief Build the bounding volume hierarchy
+  virtual int buildTree() = 0;
+
+  /// @brief Refit the bounding volume hierarchy
+  virtual int refitTree(bool bottomup) = 0;
 
   int num_tris_allocated;
   int num_vertices_allocated;
-  int num_bvs_allocated;
   int num_vertex_updated; /// for ccd vertex update
-  unsigned int* primitive_indices;
+};
+
+/// @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 BVHModelBase
+{
 
+public:
+  /// @brief Split rule to split one BV node into two children
+  boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
+
+  /// @brief Fitting rule to fit a BV node to a set of geometry primitives
+  boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
+
+  /// @brief Constructing an empty BVH
+  BVHModel() : BVHModelBase (),
+               bv_splitter(new BVSplitter<BV>(SPLIT_METHOD_MEAN)),
+               bv_fitter(new BVFitter<BV>()),
+               num_bvs_allocated(0),
+               primitive_indices(NULL),
+               bvs(NULL),
+               num_bvs(0)
+  {
+  }
+
+  /// @brief copy from another BVH
+  BVHModel(const BVHModel& other);
+
+  /// @brief deconstruction, delete mesh data related.
+  ~BVHModel()
+  {
+    delete [] bvs;
+    delete [] primitive_indices;
+  }
+
+  /// @brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some flexibility here
+  
+  /// @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];
+  }
+
+  /// @brief Get the number of bv in the BVH
+  int getNumBVs() const
+  {
+    return num_bvs;
+  }
+
+  /// @brief Get the BV type: default is unknown
+  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
+
+  /// @brief Check the number of memory used
+  int memUsage(int msg) const;
+
+  /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent 
+  /// BV node. When traversing the BVH, this can save one matrix transformation.
+  void makeParentRelative()
+  {
+    Matrix3f I (Matrix3f::Identity());
+    makeParentRelativeRecurse(0, I, Vec3f());
+  }
+
+private:
+  void deleteBVs();
+  bool allocateBVs();
+
+  int num_bvs_allocated;
+  unsigned int* primitive_indices;
 
   /// @brief Bounding volume hierarchy
   BVNode<BV>* bvs;

include/hpp/fcl/collision_object.h

@@ -203,7 +203,7 @@ public:
   /// @brief compute the AABB in world space
   inline void computeAABB()
   {
-    if(isQuatIdentity(t.getQuatRotation()))
+    if(t.getRotation().isIdentity())
     {
       aabb = translate(cgeom->aabb_local, t.getTranslation());
     }
@@ -240,8 +240,6 @@ public:
     return t.getRotation();
   }
 
-
-
   /// @brief get object's transform
   inline const Transform3f& getTransform() const
   {

include/hpp/fcl/data_types.h

@@ -55,29 +55,35 @@ typedef boost::int32_t FCL_INT32;
 /// @brief Triangle with 3 indices for points
 class Triangle
 {
-  /// @brief indices for each vertex of triangle
-  std::size_t vids[3];
-
 public:
+  typedef std::size_t index_type;
+  typedef int size_type;
+
   /// @brief Default constructor
   Triangle() {}
 
   /// @brief Create a triangle with given vertex indices
-  Triangle(std::size_t p1, std::size_t p2, std::size_t p3)
+  Triangle(index_type p1, index_type p2, index_type p3)
   {
     set(p1, p2, p3);
   }
 
   /// @brief Set the vertex indices of the triangle
-  inline void set(std::size_t p1, std::size_t p2, std::size_t p3)
+  inline void set(index_type p1, index_type p2, index_type p3)
   {
     vids[0] = p1; vids[1] = p2; vids[2] = p3;
   }
 
   /// @access the triangle index
-  inline std::size_t operator[](int i) const { return vids[i]; }
+  inline index_type operator[](int i) const { return vids[i]; }
 
-  inline std::size_t& operator[](int i) { return vids[i]; }
+  inline index_type& operator[](int i) { return vids[i]; }
+
+  static inline size_type size() { return 3; }
+
+private:
+  /// @brief indices for each vertex of triangle
+  index_type vids[3];
 };
 
 }

include/hpp/fcl/fwd.hh

@@ -51,6 +51,9 @@ namespace fcl {
   class Transform3f;
 
   class AABB;
+
+  class BVHModelBase;
+  typedef boost::shared_ptr<BVHModelBase> BVHModelPtr_t;
 }
 } // namespace hpp
 

include/hpp/fcl/math/transform.h

@@ -69,18 +69,11 @@ inline bool areQuatEquals (const Quaternion3f& q1, const Quaternion3f& q2)
 /// @brief Simple transform class used locally by InterpMotion
 class Transform3f
 {
-  /// @brief Whether matrix cache is set
-  mutable bool matrix_set;
   /// @brief Matrix cache
-  mutable Matrix3f R;
+  Matrix3f R;
 
   /// @brief Tranlation vector
   Vec3f T;
-
-  /// @brief Rotation
-  Quaternion3f q;
-
-  const Matrix3f& getRotationInternal() const;
 public:
 
   /// @brief Default transform is no movement
@@ -90,49 +83,40 @@ public:
   }
 
   /// @brief Construct transform from rotation and translation
-  Transform3f(const Matrix3f& R_, const Vec3f& T_) : matrix_set(true),
-                                                     R(R_),
-                                                     T(T_)
-  {
-    q = Quaternion3f(R_);
-  }
+  template <typename Matrixx3Like, typename Vector3Like>
+  Transform3f(const Eigen::MatrixBase<Matrixx3Like>& R_,
+              const Eigen::MatrixBase<Vector3Like>& T_) :
+    R(R_),
+    T(T_)
+  {}
 
   /// @brief Construct transform from rotation and translation
-  Transform3f(const Quaternion3f& q_, const Vec3f& T_) : matrix_set(false),
-                                                         T(T_),
-                                                         q(q_)
-  {
-  }
+  template <typename Vector3Like>
+  Transform3f(const Quaternion3f& q_,
+              const Eigen::MatrixBase<Vector3Like>& T_) :
+    R(q_.toRotationMatrix()),
+    T(T_)
+  {}
 
   /// @brief Construct transform from rotation
-  Transform3f(const Matrix3f& R_) : matrix_set(true),
-                                    R(R_),
+  Transform3f(const Matrix3f& R_) : R(R_),
                                     T(Vec3f::Zero())
-  {
-    q = Quaternion3f(R_);
-  }
+  {}
 
   /// @brief Construct transform from rotation
-  Transform3f(const Quaternion3f& q_) : matrix_set(false),
-                                        T(Vec3f::Zero()),
-                                        q(q_)
-  {
-  }
+  Transform3f(const Quaternion3f& q_) : R(q_),
+                                        T(Vec3f::Zero())
+  {}
 
   /// @brief Construct transform from translation
-  Transform3f(const Vec3f& T_) : matrix_set(true), 
-                                 T(T_),
-                                 q(Quaternion3f::Identity())
-  {
-    R.setIdentity();
-  }
+  Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()),
+                                 T(T_)
+  {}
 
   /// @brief operator = 
   Transform3f& operator = (const Transform3f& tf)
   {
-    matrix_set = tf.matrix_set;
     R = tf.R;
-    q = tf.q;
     T = tf.T;
     return *this;
   }
@@ -146,31 +130,28 @@ public:
   /// @brief get rotation
   inline const Matrix3f& getRotation() const
   {
-    if(matrix_set) return R;
-    return getRotationInternal();
+    return R;
   }
 
   /// @brief get quaternion
-  inline const Quaternion3f& getQuatRotation() const
+  inline Quaternion3f getQuatRotation() const
   {
-    return q;
+    return Quaternion3f (R);
   }
 
   /// @brief set transform from rotation and translation
-  inline void setTransform(const Matrix3f& R_, const Vec3f& T_)
+  template <typename Matrixx3Like, typename Vector3Like>
+  inline void setTransform(const Eigen::MatrixBase<Matrixx3Like>& R_, const Eigen::MatrixBase<Vector3Like>& T_)
   {
     R.noalias() = R_;
     T.noalias() = T_;
-    q = Quaternion3f(R_);
-    matrix_set = true;
   }
 
   /// @brief set transform from rotation and translation
   inline void setTransform(const Quaternion3f& q_, const Vec3f& T_)
   {
-    matrix_set = false;
-    q = q_;
-    T.noalias() = T_;
+    R = q_.toRotationMatrix();
+    T = T_;
   }
 
   /// @brief set transform from rotation
@@ -178,8 +159,6 @@ public:
   inline void setRotation(const Eigen::MatrixBase<Derived>& R_)
   {
     R.noalias() = R_;
-    matrix_set = true;
-    q = Quaternion3f(R);
   }
 
   /// @brief set transform from translation
@@ -192,53 +171,54 @@ public:
   /// @brief set transform from rotation
   inline void setQuatRotation(const Quaternion3f& q_)
   {
-    matrix_set = false;
-    q = q_;
+    R = q_.toRotationMatrix();
   }
 
   /// @brief transform a given vector by the transform
   template <typename Derived>
   inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const
   {
-    return q * v + T;
+    return R * v + T;
   }
 
   /// @brief inverse transform
-  inline Transform3f& inverse()
+  inline Transform3f& inverseInPlace()
   {
-    matrix_set = false;
-    q = q.conjugate();
-    T = q * (-T);
+    R.transposeInPlace();
+    T = - R * T;
     return *this;
   }
 
+  /// @brief inverse transform
+  inline Transform3f inverse()
+  {
+    return Transform3f (R.transpose(), - R.transpose() * T);
+  }
+
   /// @brief inverse the transform and multiply with another
   inline Transform3f inverseTimes(const Transform3f& other) const
   {
-    const Quaternion3f& q_inv = q.conjugate();
-    return Transform3f(q_inv * other.q, q_inv * (other.T - T));
+    return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T));
   }
 
   /// @brief multiply with another transform
   inline const Transform3f& operator *= (const Transform3f& other)
   {
-    matrix_set = false;
-    T += q * other.T;