add the interface for continuous collision

include/fcl/broadphase/broadphase.h

@@ -146,6 +146,86 @@ protected:
 };
 
 
+/// @brief Callback for continuous collision between two objects. Return value is whether can stop now.
+typedef bool (*ContinuousCollisionCallBack)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata);
+
+/// @brief Callback for continuous distance between two objects, Return value is whether can stop now, also return the minimum distance till now.
+typedef bool (*ContinuousDistanceCallBack)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata, FCL_REAL& dist);
+
+
+/// @brief Base class for broad phase continuous collision. It helps to accelerate the continuous collision/distance between N objects. Also support self collision, self distance and collision/distance with another M objects.
+class BroadPhaseContinuousCollisionManager
+{
+public:
+  BroadPhaseContinuousCollisionManager()
+  {
+  }
+
+  virtual ~BroadPhaseContinuousCollisionManager() {}
+
+  /// @brief add objects to the manager
+  virtual void registerObjects(const std::vector<ContinuousCollisionObject*>& other_objs)
+  {
+    for(size_t i = 0; i < other_objs.size(); ++i)
+      registerObject(other_objs[i]);
+  }
+
+  /// @brief add one object to the manager
+  virtual void registerObject(ContinuousCollisionObject* obj) = 0;
+
+  /// @brief remove one object from the manager
+  virtual void unregisterObject(ContinuousCollisionObject* obj) = 0;
+
+  /// @brief initialize the manager, related with the specific type of manager
+  virtual void setup() = 0;
+
+  /// @brief update the condition of manager
+  virtual void update() = 0;
+
+  /// @brief update the manager by explicitly given the object updated
+  virtual void update(ContinuousCollisionObject* updated_obj)
+  {
+    update();
+  }
+
+  /// @brief update the manager by explicitly given the set of objects update
+  virtual void update(const std::vector<ContinuousCollisionObject*>& updated_objs)
+  {
+    update();
+  }
+
+  /// @brief clear the manager
+  virtual void clear() = 0;
+
+  /// @brief return the objects managed by the manager
+  virtual void getObjects(std::vector<ContinuousCollisionObject*>& objs) const = 0;
+
+  /// @brief perform collision test between one object and all the objects belonging to the manager
+  virtual void collide(ContinuousCollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0;
+
+  /// @brief perform distance computation between one object and all the objects belonging to the manager
+  virtual void distance(ContinuousCollisionObject* obj, void* cdata, DistanceCallBack callback) const = 0;
+
+  /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+  virtual void collide(void* cdata, CollisionCallBack callback) const = 0;
+
+  /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+  virtual void distance(void* cdata, DistanceCallBack callback) const = 0;
+
+  /// @brief perform collision test with objects belonging to another manager
+  virtual void collide(BroadPhaseContinuousCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0;
+
+  /// @brief perform distance test with objects belonging to another manager
+  virtual void distance(BroadPhaseContinuousCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const = 0;
+
+  /// @brief whether the manager is empty
+  virtual bool empty() const = 0;
+  
+  /// @brief the number of objects managed by the manager
+  virtual size_t size() const = 0;
+};
+
+
 }
 
 

include/fcl/ccd/conservative_advancement.h

@@ -48,9 +48,9 @@ namespace fcl
 
 template<typename BV, typename ConservativeAdvancementNode, typename CollisionNode>
 int conservativeAdvancement(const CollisionGeometry* o1,
-                            MotionBase* motion1,
+                            const MotionBase* motion1,
                             const CollisionGeometry* o2,
-                            MotionBase* motion2,
+                            const MotionBase* motion2,
                             const CollisionRequest& request,
                             CollisionResult& result,
                             FCL_REAL& toc);

include/fcl/ccd/motion.h

@@ -56,7 +56,7 @@ public:
   
   /// @brief Integrate the motion from 0 to dt
   /// We compute the current transformation from zero point instead of from last integrate time, for precision.
-  bool integrate(double dt);
+  bool integrate(double dt) const;
 
   /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
   FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
@@ -92,6 +92,84 @@ public:
     tf_ = tf;
   }
 
+  void getTaylorModel(TMatrix3& tm, TVector3& tv) const
+  {
+    // set tv
+    Vec3f c[4];
+    c[0] = (Td[0] + Td[1] * 4 + Td[2] + Td[3]) * (1/6.0);
+    c[1] = (-Td[0] + Td[2]) * (1/2.0);
+    c[2] = (Td[0] - Td[1] * 2 + Td[2]) * (1/2.0);
+    c[3] = (-Td[0] + Td[1] * 3 - Td[2] * 3 + Td[3]) * (1/6.0);
+    tv.setTimeInterval(getTimeInterval());
+    for(std::size_t i = 0; i < 3; ++i)
+    {
+      for(std::size_t j = 0; j < 4; ++j)
+      {
+        tv[i].coeff(j) = c[j][i];
+      }
+    }
+
+    // set tm
+    Matrix3f I(1, 0, 0, 0, 1, 0, 0, 0, 1);
+    // R(t) = R(t0) + R'(t0) (t-t0) + 1/2 R''(t0)(t-t0)^2 + 1 / 6 R'''(t0) (t-t0)^3 + 1 / 24 R''''(l)(t-t0)^4; t0 = 0.5
+    /// 1. compute M(1/2)
+    Vec3f Rt0 = (Rd[0] + Rd[1] * 23 + Rd[2] * 23 + Rd[3]) * (1 / 48.0);
+    FCL_REAL Rt0_len = Rt0.length();
+    FCL_REAL inv_Rt0_len = 1.0 / Rt0_len;
+    FCL_REAL inv_Rt0_len_3 = inv_Rt0_len * inv_Rt0_len * inv_Rt0_len;
+    FCL_REAL inv_Rt0_len_5 = inv_Rt0_len_3 * inv_Rt0_len * inv_Rt0_len;
+    FCL_REAL theta0 = Rt0_len;
+    FCL_REAL costheta0 = cos(theta0);
+    FCL_REAL sintheta0 = sin(theta0);
+    
+    Vec3f Wt0 = Rt0 * inv_Rt0_len;
+    Matrix3f hatWt0;
+    hat(hatWt0, Wt0);
+    Matrix3f hatWt0_sqr = hatWt0 * hatWt0;
+    Matrix3f Mt0 = I + hatWt0 * sintheta0 + hatWt0_sqr * (1 - costheta0);
+
+    /// 2. compute M'(1/2)
+    Vec3f dRt0 = (-Rd[0] - Rd[1] * 5 + Rd[2] * 5 + Rd[3]) * (1 / 8.0);
+    FCL_REAL Rt0_dot_dRt0 = Rt0.dot(dRt0);
+    FCL_REAL dtheta0 = Rt0_dot_dRt0 * inv_Rt0_len;
+    Vec3f dWt0 = dRt0 * inv_Rt0_len - Rt0 * (Rt0_dot_dRt0 * inv_Rt0_len_3);
+    Matrix3f hatdWt0;
+    hat(hatdWt0, dWt0);
+    Matrix3f dMt0 = hatdWt0 * sintheta0 + hatWt0 * (costheta0 * dtheta0) + hatWt0_sqr * (sintheta0 * dtheta0) + (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (1 - costheta0);
+
+    /// 3.1. compute M''(1/2)
+    Vec3f ddRt0 = (Rd[0] - Rd[1] - Rd[2] + Rd[3]) * 0.5;
+    FCL_REAL Rt0_dot_ddRt0 = Rt0.dot(ddRt0);
+    FCL_REAL dRt0_dot_dRt0 = dRt0.sqrLength();
+    FCL_REAL ddtheta0 = (Rt0_dot_ddRt0 + dRt0_dot_dRt0) * inv_Rt0_len - Rt0_dot_dRt0 * Rt0_dot_dRt0 * inv_Rt0_len_3;
+    Vec3f ddWt0 = ddRt0 * inv_Rt0_len - (dRt0 * (2 * Rt0_dot_dRt0) + Rt0 * (Rt0_dot_ddRt0 + dRt0_dot_dRt0)) * inv_Rt0_len_3 + (Rt0 * (3 * Rt0_dot_dRt0 * Rt0_dot_dRt0)) * inv_Rt0_len_5;
+    Matrix3f hatddWt0;
+    hat(hatddWt0, ddWt0);
+    Matrix3f ddMt0 =
+      hatddWt0 * sintheta0 +
+      hatWt0 * (costheta0 * dtheta0 - sintheta0 * dtheta0 * dtheta0 + costheta0 * ddtheta0) +
+      hatdWt0 * (costheta0 * dtheta0) +
+      (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (sintheta0 * dtheta0 * 2) +
+      hatdWt0 * hatdWt0 * (2 * (1 - costheta0)) +
+      hatWt0 * hatWt0 * (costheta0 * dtheta0 * dtheta0 + sintheta0 * ddtheta0) +
+      (hatWt0 * hatddWt0 + hatddWt0 + hatWt0) * (1 - costheta0);
+
+
+    tm.setTimeInterval(getTimeInterval());
+    for(std::size_t i = 0; i < 3; ++i)
+    {
+      for(std::size_t j = 0; j < 3; ++j)
+      {
+        tm(i, j).coeff(0) = Mt0(i, j) - dMt0(i, j) * 0.5 + ddMt0(i, j) * 0.25 * 0.5;
+        tm(i, j).coeff(1) = dMt0(i, j) - ddMt0(i, j) * 0.5;
+        tm(i, j).coeff(2) = ddMt0(i, j) * 0.5;
+        tm(i, j).coeff(3) = 0;
+
+        tm(i, j).remainder() = Interval(-1/48.0, 1/48.0); /// not correct, should fix
+      }
+    } 
+  }
+
 protected:
   void computeSplineParameter()
   {
@@ -110,10 +188,10 @@ protected:
 
   FCL_REAL Rd0Rd0, Rd0Rd1, Rd0Rd2, Rd0Rd3, Rd1Rd1, Rd1Rd2, Rd1Rd3, Rd2Rd2, Rd2Rd3, Rd3Rd3;
   //// @brief The transformation at current time t
-  Transform3f tf;
+  mutable Transform3f tf;
 
   /// @brief The time related with tf
-  FCL_REAL tf_t;
+  mutable FCL_REAL tf_t;
 
 public:
   FCL_REAL computeTBound(const Vec3f& n) const;
@@ -163,12 +241,12 @@ public:
 
   /// @brief Integrate the motion from 0 to dt
   /// We compute the current transformation from zero point instead of from last integrate time, for precision.
-  bool integrate(double dt)
+  bool integrate(double dt) const
   {
     if(dt > 1) dt = 1;
-
+    
     tf.setQuatRotation(absoluteRotation(dt));
-
+    
     Quaternion3f delta_rot = deltaRotation(dt);
     tf.setTranslation(p + axis * (dt * linear_vel) + delta_rot.transform(tf1.getTranslation() - p));
 
@@ -210,6 +288,29 @@ public:
     tf_ = tf;
   }
 
+  void getTaylorModel(TMatrix3& tm, TVector3& tv) const
+  {
+    Matrix3f hat_axis;
+    hat(hat_axis, axis);
+
+    TaylorModel cos_model(getTimeInterval());
+    generateTaylorModelForCosFunc(cos_model, angular_vel, 0);
+    
+    TaylorModel sin_model(getTimeInterval());
+    generateTaylorModelForSinFunc(sin_model, angular_vel, 0);
+
+    TMatrix3 delta_R = hat_axis * sin_model - hat_axis * hat_axis * (cos_model - 1) + Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
+
+    TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval());
+    generateTaylorModelForLinearFunc(a, 0, linear_vel * axis[0]);
+    generateTaylorModelForLinearFunc(b, 0, linear_vel * axis[1]);
+    generateTaylorModelForLinearFunc(c, 0, linear_vel * axis[2]);
+    TVector3 delta_T = p - delta_R * p + TVector3(a, b, c);
+
+    tm = delta_R * tf1.getRotation();
+    tv = delta_R * tf1.getTranslation() + delta_T;
+  }
+
 protected:
   void computeScrewParameter()
   {
@@ -256,7 +357,7 @@ protected:
   Transform3f tf2;
 
   /// @brief The transformation at current time t
-  Transform3f tf;
+  mutable Transform3f tf;
 
   /// @brief screw axis
   Vec3f axis;
@@ -322,7 +423,7 @@ public:
 
   /// @brief Integrate the motion from 0 to dt
   /// We compute the current transformation from zero point instead of from last integrate time, for precision.
-  bool integrate(double dt);
+  bool integrate(double dt) const;
 
   /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
   FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
@@ -358,6 +459,28 @@ public:
     tf_ = tf;
   }
 
+  void getTaylorModel(TMatrix3& tm, TVector3& tv) const
+  {
+    Matrix3f hat_angular_axis;
+    hat(hat_angular_axis, angular_axis);
+
+    TaylorModel cos_model(getTimeInterval());
+    generateTaylorModelForCosFunc(cos_model, angular_vel, 0);
+    TaylorModel sin_model(getTimeInterval());
+    generateTaylorModelForSinFunc(sin_model, angular_vel, 0);
+
+    TMatrix3 delta_R = hat_angular_axis * sin_model - hat_angular_axis * hat_angular_axis * (cos_model - 1) + Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
+
+    TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval());
+    generateTaylorModelForLinearFunc(a, 0, linear_vel[0]);
+    generateTaylorModelForLinearFunc(b, 0, linear_vel[1]);
+    generateTaylorModelForLinearFunc(c, 0, linear_vel[2]);
+    TVector3 delta_T(a, b, c);
+    
+    tm = delta_R * tf1.getRotation();
+    tv = tf1.transform(reference_p) + delta_T - delta_R * tf1.getQuatRotation().transform(reference_p);
+  }
+
 protected:
 
   void computeVelocity();
@@ -373,7 +496,7 @@ protected:
   Transform3f tf2;
 
   /// @brief The transformation at current time t
-  Transform3f tf;
+  mutable Transform3f tf;
 
   /// @brief Linear velocity
   Vec3f linear_vel;

include/fcl/ccd/motion_base.h

@@ -40,6 +40,8 @@
 
 
 #include "fcl/math/transform.h"
+#include "fcl/ccd/taylor_matrix.h"
+#include "fcl/ccd/taylor_vector.h"
 #include "fcl/BV/RSS.h"
 
 namespace fcl
@@ -108,10 +110,14 @@ protected:
 class MotionBase
 {
 public:
+  MotionBase() : time_interval_(boost::shared_ptr<TimeInterval>(new TimeInterval(0, 1)))
+  {
+  }
+  
   virtual ~MotionBase() {}
 
   /** \brief Integrate the motion from 0 to dt */
-  virtual bool integrate(double dt) = 0;
+  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;
@@ -127,6 +133,17 @@ public:
   virtual void getCurrentTranslation(Vec3f& T) const = 0;
 
   virtual void getCurrentTransform(Transform3f& tf) const = 0;
+
+  virtual void getTaylorModel(TMatrix3& tm, TVector3& tv) const = 0;
+
+  const boost::shared_ptr<TimeInterval>& getTimeInterval() const
+  {
+    return time_interval_;
+  }
+protected:
+
+  boost::shared_ptr<TimeInterval> time_interval_;
+  
 };
 
 

include/fcl/ccd/taylor_matrix.h

@@ -45,10 +45,11 @@
 namespace fcl
 {
 
-struct TMatrix3
+class TMatrix3
 {
   TVector3 v_[3];
-
+  
+public:
   TMatrix3();
   TMatrix3(const boost::shared_ptr<TimeInterval>& time_interval);
   TMatrix3(TaylorModel m[3][3]);
@@ -75,23 +76,45 @@ struct TMatrix3
 
   TMatrix3 operator + (const TMatrix3& m) const;
   TMatrix3& operator += (const TMatrix3& m);
+  TMatrix3 operator + (const Matrix3f& m) const;
+  TMatrix3& operator += (const Matrix3f& m);
 
   TMatrix3 operator - (const TMatrix3& m) const;
   TMatrix3& operator -= (const TMatrix3& m);
+  TMatrix3 operator - (const Matrix3f& m) const;
+  TMatrix3& operator -= (const Matrix3f& m);
+  TMatrix3 operator - () const;
 
   IMatrix3 getBound() const;
+  IMatrix3 getBound(FCL_REAL l, FCL_REAL r) const;
+  IMatrix3 getBound(FCL_REAL t) const;
+
+  IMatrix3 getTightBound() const;
+  IMatrix3 getTightBound(FCL_REAL l, FCL_REAL r) const;
+
+
   void print() const;
   void setIdentity();
   void setZero();
   FCL_REAL diameter() const;
 
   void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval);
+  void setTimeInterval(FCL_REAL l, FCL_REAL r);
+
+  const boost::shared_ptr<TimeInterval>& getTimeInterval() const;
 
   TMatrix3& rotationConstrain();
 };
 
 TMatrix3 rotationConstrain(const TMatrix3& m);
 
+TMatrix3 operator * (const Matrix3f& m, const TaylorModel& a);
+TMatrix3 operator * (const TaylorModel& a, const Matrix3f& m);
+TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m);
+TMatrix3 operator * (FCL_REAL d, const TMatrix3& m);
+TMatrix3 operator + (const Matrix3f& m1, const TMatrix3& m2);
+TMatrix3 operator - (const Matrix3f& m1, const TMatrix3& m2);
+
 }
 
 #endif

include/fcl/ccd/taylor_model.h

@@ -52,12 +52,28 @@ struct TimeInterval
   Interval t4_; // [t1, t2]^4
   Interval t5_; // [t1, t2]^5
   Interval t6_; // [t1, t2]^6
+
+  TimeInterval() {}
+  TimeInterval(FCL_REAL l, FCL_REAL r)
+  {
+    setValue(l, r);
+  }
+
+  void setValue(FCL_REAL l, FCL_REAL r)
+  {
+    t_.setValue(l, r);
+    t2_.setValue(l * t_[0], r * t_[1]);
+    t3_.setValue(l * t2_[0], r * t2_[1]);
+    t4_.setValue(l * t3_[0], r * t3_[1]);
+    t5_.setValue(l * t4_[0], r * t4_[1]);
+    t6_.setValue(l * t5_[0], r * t5_[1]);    
+  }
 };
 
 /// @brief TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function
 /// over a time interval, with an interval remainder.
 /// All the operations on two Taylor models assume their time intervals are the same.
-struct TaylorModel
+class TaylorModel
 {
   /// @brief time interval
   boost::shared_ptr<TimeInterval> time_interval_;
@@ -68,12 +84,29 @@ struct TaylorModel
   /// @brief interval remainder
   Interval r_;
 
-  void setTimeInterval(FCL_REAL l, FCL_REAL r);
-  void setTimeInterval(const boost::shared_ptr<TimeInterval> time_interval)
+public:
+
+  void setTimeInterval(FCL_REAL l, FCL_REAL r)
+  {
+    time_interval_->setValue(l, r);
+  }
+  
+  void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval)
   {
     time_interval_ = time_interval;
   }
 
+  const boost::shared_ptr<TimeInterval>& getTimeInterval() const
+  {
+    return time_interval_;
+  }
+
+  FCL_REAL coeff(std::size_t i) const { return coeffs_[i]; }
+  FCL_REAL& coeff(std::size_t i) { return coeffs_[i]; }
+  const Interval& remainder() const { return r_; }
+  Interval& remainder() { return r_; }
+  
+
   TaylorModel();
   TaylorModel(const boost::shared_ptr<TimeInterval>& time_interval);
   TaylorModel(FCL_REAL coeff, const boost::shared_ptr<TimeInterval>& time_interval);
@@ -89,6 +122,9 @@ struct TaylorModel
   TaylorModel operator + (FCL_REAL d) const;
   TaylorModel& operator += (FCL_REAL d);
 
+  TaylorModel operator - (FCL_REAL d) const;
+  TaylorModel& operator -= (FCL_REAL d);
+
   TaylorModel operator * (const TaylorModel& other) const;
   TaylorModel operator * (FCL_REAL d) const;
   TaylorModel& operator *= (const TaylorModel& other);
@@ -109,8 +145,17 @@ struct TaylorModel
   void setZero();
 };
 
+TaylorModel operator * (FCL_REAL d, const TaylorModel& a);
+TaylorModel operator + (FCL_REAL d, const TaylorModel& a);
+TaylorModel operator - (FCL_REAL d, const TaylorModel& a);
+
+/// @brief Generate Taylor model for cos(w t + q0)
 void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0);
+
+/// @brief Generate Taylor model for sin(w t + q0)
 void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0);
+
+/// @brief Generate Taylor model for p + v t
 void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v);
 
 }

include/fcl/ccd/taylor_vector.h

@@ -43,25 +43,32 @@
 namespace fcl
 {
 
-struct TVector3
+class TVector3
 {
   TaylorModel i_[3];
 
+public:
+  
   TVector3();
   TVector3(const boost::shared_ptr<TimeInterval>& time_interval);
   TVector3(TaylorModel v[3]);
   TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2);
   TVector3(const Vec3f& v, const boost::shared_ptr<TimeInterval>& time_interval);
-
+  
   TVector3 operator + (const TVector3& other) const;
   TVector3& operator += (const TVector3& other);
 
-  TVector3 operator + (FCL_REAL d) const;
-  TVector3& operator += (FCL_REAL d);
+  TVector3 operator + (const Vec3f& other) const;
+  TVector3& operator += (const Vec3f& other);
 
   TVector3 operator - (const TVector3& other) const;
   TVector3& operator -= (const TVector3& other);
 
+  TVector3 operator - (const Vec3f& other) const;
+  TVector3& operator -= (const Vec3f& other);
+
+  TVector3 operator - () const;
+
   TVector3 operator * (const TaylorModel& d) const;
   TVector3& operator *= (const TaylorModel& d);
   TVector3 operator * (FCL_REAL d) const;
@@ -76,8 +83,12 @@ struct TVector3
   TVector3 cross(const Vec3f& other) const;
 
   IVector3 getBound() const;
+  IVector3 getBound(FCL_REAL l, FCL_REAL r) const;
   IVector3 getBound(FCL_REAL t) const;
 
+  IVector3 getTightBound() const;
+  IVector3 getTightBound(FCL_REAL l, FCL_REAL r) const;
+
   void print() const;
   FCL_REAL volumn() const;
   void setZero();
@@ -85,11 +96,18 @@ struct TVector3
   TaylorModel squareLength() const;
 
   void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval);
+  void setTimeInterval(FCL_REAL l, FCL_REAL r);
+
+  const boost::shared_ptr<TimeInterval>& getTimeInterval() const;
 };
 
 void generateTVector3ForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity);
 
 
+TVector3 operator * (const Vec3f& v, const TaylorModel& a);
+TVector3 operator + (const Vec3f& v1, const TVector3& v2);
+TVector3 operator - (const Vec3f& v1, const TVector3& v2);
+
 }
 
 #endif

include/fcl/collision.h

@@ -71,6 +71,15 @@ 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);
 }
 
 #endif

include/fcl/collision_object.h

@@ -40,6 +40,7 @@
 
 #include "fcl/BV/AABB.h"
 #include "fcl/math/transform.h"
+#include "fcl/ccd/motion_base.h"
 #include <boost/shared_ptr.hpp>
 
 namespace fcl
@@ -314,6 +315,118 @@ protected:
   void *user_data;
 };
 
+
+/// @brief the object for continuous collision or distance computation, contains the geometry and the motion information
+class ContinuousCollisionObject
+{
+public:
+  ContinuousCollisionObject(const boost::shared_ptr<CollisionGeometry>& cgeom_) : cgeom(cgeom_)
+  {
+  }
+