fix some bug in gjk; add continuous collision, still need more test

include/fcl/BVH/BVH_model.h

@@ -216,6 +216,7 @@ class BVHModel : public CollisionGeometry,
   /// @brief Fitting rule to fit a BV node to a set of geometry primitives
   boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
 
+
 private:
 
   int num_tris_allocated;

include/fcl/broadphase/hierarchy_tree.hxx

@@ -1390,7 +1390,7 @@ void HierarchyTree<BV>::bottomup(size_t* lbeg, size_t* lend)
   size_t* lcur_end = lend;
   while(lbeg < lcur_end - 1)
   {
-    size_t* min_it1, *min_it2;
+    size_t* min_it1 = NULL, *min_it2 = NULL;
     FCL_REAL min_size = std::numeric_limits<FCL_REAL>::max();
     for(size_t* it1 = lbeg; it1 < lcur_end; ++it1)
     {

include/fcl/ccd/conservative_advancement.h

@@ -46,15 +46,22 @@
 namespace fcl
 {
 
-template<typename BV, typename ConservativeAdvancementNode, typename CollisionNode>
-int conservativeAdvancement(const CollisionGeometry* o1,
-                            const MotionBase* motion1,
-                            const CollisionGeometry* o2,
-                            const MotionBase* motion2,
-                            const CollisionRequest& request,
-                            CollisionResult& result,
-                            FCL_REAL& toc);
+template<typename NarrowPhaseSolver>
+struct ConservativeAdvancementFunctionMatrix
+{
+  typedef FCL_REAL (*ConservativeAdvancementFunc)(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result);
+
+  ConservativeAdvancementFunc conservative_advancement_matrix[NODE_COUNT][NODE_COUNT];
+
+  ConservativeAdvancementFunctionMatrix();
+};
+
+
 
 }
 
 #endif
+
+
+
+

include/fcl/ccd/motion.h

@@ -46,13 +46,84 @@
 namespace fcl
 {
 
+class TranslationMotion : public MotionBase
+{
+public:
+  /// @brief Construct motion from intial and goal transform
+  TranslationMotion(const Transform3f& tf1,
+                    const Transform3f& tf2) : MotionBase(),
+                                              rot(tf1.getQuatRotation()),
+                                              trans_start(tf1.getTranslation()),
+                                              trans_range(tf2.getTranslation() - tf1.getTranslation())
+  {
+  }
+
+  TranslationMotion(const Matrix3f& R, const Vec3f& T1, const Vec3f& T2) : MotionBase()
+  {
+    rot.fromRotation(R);
+    trans_start = T1;
+    trans_range = T2 - T1;
+  }
+
+  bool integrate(FCL_REAL dt) const
+  {
+    if(dt > 1) dt = 1;
+    tf = Transform3f(rot, trans_start + trans_range * dt);
+    return true;
+  }
+
+  FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
+  {
+    return mb_visitor.visit(*this);
+  }
+
+  FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
+  {
+    return mb_visitor.visit(*this);
+  }
+
+  void getCurrentTransform(Transform3f& tf_) const
+  {
+    tf_ = tf;
+  }
+
+  void getTaylorModel(TMatrix3& tm, TVector3& tv) const
+  {
+  }
+
+  Vec3f getVelocity() const
+  {
+    return trans_range;
+  }
+
+ private:
+  /// @brief initial and goal transforms
+  Quaternion3f rot;
+  Vec3f trans_start, trans_range;
+
+  mutable Transform3f tf;
+};
+
 class SplineMotion : public MotionBase
 {
 public:
   /// @brief Construct motion from 4 deBoor points
   SplineMotion(const Vec3f& Td0, const Vec3f& Td1, const Vec3f& Td2, const Vec3f& Td3,
                const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3);
 
+  // @brief Construct motion from initial and goal transform
+  SplineMotion(const Matrix3f& R1, const Vec3f& T1,
+               const Matrix3f& R2, const Vec3f& T2) : MotionBase()
+  {
+    // TODO
+  }
+
+  SplineMotion(const Transform3f& tf1,
+               const Transform3f& tf2) : MotionBase()
+  {
+    // TODO
+  }
+
 
   /// @brief Integrate the motion from 0 to dt
   /// We compute the current transformation from zero point instead of from last integrate time, for precision.
@@ -71,22 +142,6 @@ class SplineMotion : public MotionBase
   }
 
   /// @brief Get the rotation and translation in current step
-  void getCurrentTransform(Matrix3f& R, Vec3f& T) const
-  {
-    R = tf.getRotation();
-    T = tf.getTranslation();
-  }
-
-  void getCurrentRotation(Matrix3f& R) const
-  {
-    R = tf.getRotation();
-  }
-
-  void getCurrentTranslation(Vec3f& T) const
-  {
-    T = tf.getTranslation();
-  }
-
   void getCurrentTransform(Transform3f& tf_) const
   {
     tf_ = tf;
@@ -209,7 +264,7 @@ class ScrewMotion : public MotionBase
 {
 public:
   /// @brief Default transformations are all identities
-  ScrewMotion()
+  ScrewMotion() : MotionBase()
   {
     // Default angular velocity is zero
     axis.setValue(1, 0, 0);
@@ -223,7 +278,8 @@ class ScrewMotion : public MotionBase
 
   /// @brief Construct motion from the initial rotation/translation and goal rotation/translation
   ScrewMotion(const Matrix3f& R1, const Vec3f& T1,
-              const Matrix3f& R2, const Vec3f& T2) : tf1(R1, T1),
+              const Matrix3f& R2, const Vec3f& T2) : MotionBase(),
+                                                     tf1(R1, T1),
                                                      tf2(R2, T2),
                                                      tf(tf1)
   {
@@ -267,22 +323,6 @@ class ScrewMotion : public MotionBase
 
 
   /// @brief Get the rotation and translation in current step
-  void getCurrentTransform(Matrix3f& R, Vec3f& T) const
-  {
-    R = tf.getRotation();
-    T = tf.getTranslation();
-  }
-
-  void getCurrentRotation(Matrix3f& R) const
-  {
-    R = tf.getRotation();
-  }
-
-  void getCurrentTranslation(Vec3f& T) const
-  {
-    T = tf.getTranslation();
-  }
-
   void getCurrentTransform(Transform3f& tf_) const
   {
     tf_ = tf;
@@ -438,22 +478,6 @@ class InterpMotion : public MotionBase
   }
 
   /// @brief Get the rotation and translation in current step
-  void getCurrentTransform(Matrix3f& R, Vec3f& T) const
-  {
-    R = tf.getRotation();
-    T = tf.getTranslation();
-  }
-
-  void getCurrentRotation(Matrix3f& R) const
-  {
-    R = tf.getRotation();
-  }
-
-  void getCurrentTranslation(Vec3f& T) const
-  {
-    T = tf.getTranslation();
-  }
-
   void getCurrentTransform(Transform3f& tf_) const
   {
     tf_ = tf;

include/fcl/ccd/motion_base.h

@@ -52,6 +52,7 @@ class MotionBase;
 class SplineMotion;
 class ScrewMotion;
 class InterpMotion;
+class TranslationMotion;
 
 /// @brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects
 class BVMotionBoundVisitor
@@ -61,6 +62,7 @@ class BVMotionBoundVisitor
   virtual FCL_REAL visit(const SplineMotion& motion) const = 0;
   virtual FCL_REAL visit(const ScrewMotion& motion) const = 0;
   virtual FCL_REAL visit(const InterpMotion& motion) const = 0;
+  virtual FCL_REAL visit(const TranslationMotion& motion) const = 0;
 };
 
 template<typename BV>
@@ -73,6 +75,7 @@ class TBVMotionBoundVisitor : public BVMotionBoundVisitor
   virtual FCL_REAL visit(const SplineMotion& motion) const { return 0; }
   virtual FCL_REAL visit(const ScrewMotion& motion) const { return 0; }
   virtual FCL_REAL visit(const InterpMotion& motion) const { return 0; }
+  virtual FCL_REAL visit(const TranslationMotion& motion) const { return 0; }
 
 protected:
   BV bv;
@@ -88,6 +91,8 @@ FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const ScrewMotion& motion) const;
 template<>
 FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const InterpMotion& motion) const;
 
+template<>
+FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const TranslationMotion& motion) const;
 
 
 class TriangleMotionBoundVisitor
@@ -100,6 +105,7 @@ class TriangleMotionBoundVisitor
   virtual FCL_REAL visit(const SplineMotion& motion) const;
   virtual FCL_REAL visit(const ScrewMotion& motion) const;
   virtual FCL_REAL visit(const InterpMotion& motion) const;
+  virtual FCL_REAL visit(const TranslationMotion& motion) const;
 
 protected:
   Vec3f a, b, c, n;
@@ -120,17 +126,48 @@ class MotionBase
   virtual bool integrate(double dt) const = 0;
 
   /** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects */
-  virtual FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const= 0;
+  virtual FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const = 0;
 
   /** \brief Compute the motion bound for a triangle, given the closest direction n between two query objects */
   virtual FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const = 0;
 
   /** \brief Get the rotation and translation in current step */
-  virtual void getCurrentTransform(Matrix3f& R, Vec3f& T) const = 0;
+  void getCurrentTransform(Matrix3f& R, Vec3f& T) const
+  {
+    Transform3f tf;
+    getCurrentTransform(tf);
+    R = tf.getRotation();
+    T = tf.getTranslation();
+  }
 
-  virtual void getCurrentRotation(Matrix3f& R) const = 0;
+  void getCurrentTransform(Quaternion3f& Q, Vec3f& T) const
+  {
+    Transform3f tf;
+    getCurrentTransform(tf);
+    Q = tf.getQuatRotation();
+    T = tf.getTranslation();
+  }
 
-  virtual void getCurrentTranslation(Vec3f& T) const = 0;
+  void getCurrentRotation(Matrix3f& R) const
+  {
+    Transform3f tf;
+    getCurrentTransform(tf);
+    R = tf.getRotation();
+  }
+
+  void getCurrentRotation(Quaternion3f& Q) const
+  {
+    Transform3f tf;
+    getCurrentTransform(tf);
+    Q = tf.getQuatRotation();
+  }
+
+  void getCurrentTranslation(Vec3f& T) const
+  {
+    Transform3f tf;
+    getCurrentTransform(tf);
+    T = tf.getTranslation();
+  }
 
   virtual void getCurrentTransform(Transform3f& tf) const = 0;
 

include/fcl/collision.h

@@ -50,34 +50,12 @@ namespace fcl
 /// performs the collision between them. 
 /// Return value is the number of contacts generated between the two objects.
 
-template<typename NarrowPhaseSolver>
 std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
-                    const NarrowPhaseSolver* nsolver,
                     const CollisionRequest& request,
                     CollisionResult& result);
 
-template<typename NarrowPhaseSolver>
 std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
                     const CollisionGeometry* o2, const Transform3f& tf2,
-                    const NarrowPhaseSolver* nsolver,
-                    const CollisionRequest& request,
-                    CollisionResult& result);
-
-std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
-                    const CollisionRequest& request,
-                    CollisionResult& result);
-
-std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
-                    const CollisionGeometry* o2, const Transform3f& tf2,
-                    const CollisionRequest& request,
-                    CollisionResult& result);
-
-std::size_t collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2,
-                    const CollisionRequest& request,
-                    CollisionResult& result);
-
-std::size_t collide(const CollisionGeometry* o1, const MotionBase* motion1,
-                    const CollisionGeometry* o2, const MotionBase* motion2,
                     const CollisionRequest& request,
                     CollisionResult& result);
 }