Commit c845c9a6 authored by Jia Pan's avatar Jia Pan
Browse files

add the interface for continuous collision

parent 0f0a2d18
......@@ -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;
};
}
......
......@@ -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);
......
......@@ -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;
......
......@@ -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_;
};
......
......@@ -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
......@@ -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);
}
......
......@@ -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
......@@ -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
......@@ -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_)
{
}