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

include/fcl/BVH/BVH_model.h

@@ -216,6 +216,7 @@ public:
   /// @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,6 +46,64 @@
 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:
@@ -53,6 +111,19 @@ public:
   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 @@ public:
   }
 
   /// @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 @@ public:
 
   /// @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 @@ public:
 
 
   /// @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 @@ public:
   }
 
   /// @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 @@ public:
   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 @@ public:
   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 @@ public:
   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 @@ public:
   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);
 }

include/fcl/collision_data.h

@@ -48,6 +48,9 @@
 namespace fcl
 {
 
+/// @brief Type of narrow phase GJK solver
+enum GJKSolverType {GST_LIBCCD, GST_INDEP};
+
 /// @brief Contact information returned by collision
 struct Contact
 {
@@ -167,7 +170,7 @@ struct CollisionResult;
 
 /// @brief request to the collision algorithm
 struct CollisionRequest
-{
+{  
   /// @brief The maximum number of contacts will return
   size_t num_max_contacts;
 
@@ -183,16 +186,29 @@ struct CollisionRequest
   /// @brief whether the cost computation is approximated
   bool use_approximate_cost;
 
+  /// @brief narrow phase solver
+  GJKSolverType gjk_solver_type;
+
+  /// @brief whether enable gjk intial guess
+  bool enable_cached_gjk_guess;
+  
+  /// @brief the gjk intial guess set by user
+  Vec3f cached_gjk_guess;
+
   CollisionRequest(size_t num_max_contacts_ = 1,
                    bool enable_contact_ = false,
                    size_t num_max_cost_sources_ = 1,
                    bool enable_cost_ = false,
-                   bool use_approximate_cost_ = true) : num_max_contacts(num_max_contacts_),
-                                                        enable_contact(enable_contact_),
-                                                        num_max_cost_sources(num_max_cost_sources_),
-                                                        enable_cost(enable_cost_),
-                                                        use_approximate_cost(use_approximate_cost_)
+                   bool use_approximate_cost_ = true,
+                   GJKSolverType gjk_solver_type_ = GST_LIBCCD) : num_max_contacts(num_max_contacts_),
+                                                                  enable_contact(enable_contact_),
+                                                                  num_max_cost_sources(num_max_cost_sources_),
+                                                                  enable_cost(enable_cost_),
+                                                                  use_approximate_cost(use_approximate_cost_),
+                                                                  gjk_solver_type(gjk_solver_type_)
   {
+    enable_cached_gjk_guess = false;
+    cached_gjk_guess = Vec3f(1, 0, 0);
   }
 
   bool isSatisfied(const CollisionResult& result) const;
@@ -208,6 +224,9 @@ private:
   /// @brief cost sources
   std::set<CostSource> cost_sources;
 
+public:
+  Vec3f cached_gjk_guess;
+
 public:
   CollisionResult()
   {
@@ -277,6 +296,7 @@ public:
   }
 };
 
+
 struct DistanceResult;
 
 /// @brief request to the distance computation
@@ -285,13 +305,29 @@ struct DistanceRequest
   /// @brief whether to return the nearest points
   bool enable_nearest_points;
 
-  DistanceRequest(bool enable_nearest_points_ = false) : enable_nearest_points(enable_nearest_points_)
+  /// @brief error threshold for approximate distance
+  FCL_REAL rel_err; // relative error, between 0 and 1
+  FCL_REAL abs_err; // absoluate error
+
+  /// @brief narrow phase solver type
+  GJKSolverType gjk_solver_type;
+
+
+
+  DistanceRequest(bool enable_nearest_points_ = false,
+                  FCL_REAL rel_err_ = 0.0,
+                  FCL_REAL abs_err_ = 0.0,
+                  GJKSolverType gjk_solver_type_ = GST_LIBCCD) : enable_nearest_points(enable_nearest_points_),
+                                                                rel_err(rel_err_),
+                                                                abs_err(abs_err_),
+                                                                gjk_solver_type(gjk_solver_type_)
   {
   }
 
   bool isSatisfied(const DistanceResult& result) const;
 };
 
+
 /// @brief distance result
 struct DistanceResult
 {
@@ -389,6 +425,94 @@ public:
 };
 
 
+enum CCDMotionType {CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE};
+enum CCDSolverType {CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER};
+
+
+struct ContinuousCollisionRequest
+{
+  /// @brief maximum num of iterations
+  std::size_t num_max_iterations;
+
+  /// @brief error in first contact time
+  FCL_REAL toc_err;
+
+  /// @brief ccd motion type
+  CCDMotionType ccd_motion_type;
+
+  /// @brief gjk solver type
+  GJKSolverType gjk_solver_type;
+
+  /// @brief ccd solver type
+  CCDSolverType ccd_solver_type;
+  
+  ContinuousCollisionRequest(std::size_t num_max_iterations_ = 10,
+                             FCL_REAL toc_err_ = 0,
+                             CCDMotionType ccd_motion_type_ = CCDM_TRANS,
+                             GJKSolverType gjk_solver_type_ = GST_LIBCCD,
+                             CCDSolverType ccd_solver_type_ = CCDC_NAIVE) : num_max_iterations(num_max_iterations_),
+                                                                            toc_err(toc_err_),
+                                                                            ccd_motion_type(ccd_motion_type_),
+                                                                            gjk_solver_type(gjk_solver_type_),
+                                                                            ccd_solver_type(ccd_solver_type_)
+  {
+  }
+  
+};
+/// @brief continuous collision result
+struct ContinuousCollisionResult
+{
+  /// @brief collision or not
+  bool is_collide;
+  
+  /// @brief time of contact in [0, 1]
+  FCL_REAL time_of_contact;
+  
+  ContinuousCollisionResult() : is_collide(false), time_of_contact(1.0)
+  {
+  }
+};
+
+
+enum PenetrationDepthType {PDT_LOCAL, PDT_AL};
+
+struct PenetrationDepthMetricBase
+{
+  virtual FCL_REAL operator() (const Transform3f& tf1, const Transform3f& tf2) const = 0;
+};
+
+struct WeightEuclideanPDMetric : public PenetrationDepthMetricBase
+{
+  
+};
+
+struct PenetrationDepthRequest
+{
+  /// @brief PD algorithm type
+  PenetrationDepthType pd_type;
+
+  /// @brief gjk solver type
+  GJKSolverType gjk_solver_type;
+
+  PenetrationDepthRequest(PenetrationDepthType pd_type_ = PDT_LOCAL,
+                          GJKSolverType gjk_solver_type_ = GST_LIBCCD) : pd_type(pd_type_),
+                                                                         gjk_solver_type(gjk_solver_type_)
+  {
+  }
+};
+
+struct PenetrationDepthResult
+{
+  /// @brief penetration depth value
+  FCL_REAL pd_value;
+
+  /// @brief the transform where the collision is resolved
+  Transform3f resolve_trans;
+
+  
+};
+
+
 
 
 }

include/fcl/collision_object.h

@@ -115,6 +115,7 @@ public:
 
   /// @brief threshold for free (<= is free)
   FCL_REAL threshold_free;
+
 };
 
 /// @brief the object for collision or distance computation, contains the geometry and the transform information

include/fcl/continuous_collision.h

+#ifndef FCL_CONTINUOUS_COLLISION_H
+#define FCL_CONTINUOUS_COLLISION_H
+
+#include "fcl/math/vec_3f.h"
+#include "fcl/collision_object.h"
+#include "fcl/collision_data.h"
+
+namespace fcl
+{
+
+/// @brief continous collision checking between two objects
+FCL_REAL continuous_collide(const CollisionGeometry* o1, const Transform3f& tf1_beg, const Transform3f& tf1_end,
+                            const CollisionGeometry* o2, const Transform3f& tf2_beg, const Transform3f& tf2_end,
+                            const ContinuousCollisionRequest& request,
+                            ContinuousCollisionResult& result);
+
+FCL_REAL continuous_collide(const CollisionObject* o1, const Transform3f& tf1_end,
+                            const CollisionObject* o2, const Transform3f& tf2_end,
+                            const ContinuousCollisionRequest& request,
+                            ContinuousCollisionResult& result);
+
+FCL_REAL collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2,
+                 const ContinuousCollisionRequest& request,
+                 ContinuousCollisionResult& result);
+          
+}
+
+#endif

include/fcl/distance.h

@@ -46,16 +46,6 @@ namespace fcl
 /// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them. 
 /// Return value is the minimum distance generated between the two objects.
 
-template<typename NarrowPhaseSolver>
-FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver,
-                  const DistanceRequest& request, DistanceResult& result);
-
-template<typename NarrowPhaseSolver>
-FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
-                  const CollisionGeometry* o2, const Transform3f& tf2,
-                  const NarrowPhaseSolver* nsolver,
-                  const DistanceRequest& request, DistanceResult& result);
-
 FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
                   const DistanceRequest& request, DistanceResult& result);
 

include/fcl/intersect.h

@@ -91,13 +91,13 @@ public:
 
   /// @brief CCD intersect between one vertex and one face, using additional filter 
   static bool intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
-                           const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
-                           FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
+                                    const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
+                                    FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
 
   /// @brief CCD intersect between two edges, using additional filter 
   static bool intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
-                           const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
-                           FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
+                                    const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
+                                    FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);