diff --git a/include/fcl/ccd/conservative_advancement.h b/include/fcl/ccd/conservative_advancement.h
index 7f4290ce13c7a4357ba75e5723c558b6c6b57220..21a63438b414c6f0eddc7998f858477d41861187 100644
--- a/include/fcl/ccd/conservative_advancement.h
+++ b/include/fcl/ccd/conservative_advancement.h
@@ -46,11 +46,11 @@
namespace fcl
{
-template<typename BV>
+template<typename BV, typename ConservativeAdvancementNode, typename CollisionNode>
int conservativeAdvancement(const CollisionGeometry* o1,
- MotionBase<BV>* motion1,
+ MotionBase* motion1,
const CollisionGeometry* o2,
- MotionBase<BV>* motion2,
+ MotionBase* motion2,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc);
diff --git a/include/fcl/ccd/motion.h b/include/fcl/ccd/motion.h
index a54b9c16212d561d001d4d33655fd5932ed51646..ae34b60019946ec71692169d8c4213d91d15f65f 100644
--- a/include/fcl/ccd/motion.h
+++ b/include/fcl/ccd/motion.h
@@ -46,95 +46,31 @@
namespace fcl
{
-
-template<typename BV>
-class SplineMotion : public MotionBase<BV>
+class SplineMotion : public MotionBase
{
public:
- /** \brief Construct motion from 4 deBoor points */
+ /// @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)
- {
- Td[0] = Td0;
- Td[1] = Td1;
- Td[2] = Td2;
- Td[3] = Td3;
-
- Rd[0] = Rd0;
- Rd[1] = Rd1;
- Rd[2] = Rd2;
- Rd[3] = Rd3;
-
- Rd0Rd0 = Rd[0].dot(Rd[0]);
- Rd0Rd1 = Rd[0].dot(Rd[1]);
- Rd0Rd2 = Rd[0].dot(Rd[2]);
- Rd0Rd3 = Rd[0].dot(Rd[3]);
- Rd1Rd1 = Rd[1].dot(Rd[1]);
- Rd1Rd2 = Rd[1].dot(Rd[2]);
- Rd1Rd3 = Rd[1].dot(Rd[3]);
- Rd2Rd2 = Rd[2].dot(Rd[2]);
- Rd2Rd3 = Rd[2].dot(Rd[3]);
- Rd3Rd3 = Rd[3].dot(Rd[3]);
-
- TA = Td[1] * 3 - Td[2] * 3 + Td[3] - Td[0];
- TB = (Td[0] - Td[1] * 2 + Td[2]) * 3;
- TC = (Td[2] - Td[0]) * 3;
-
- RA = Rd[1] * 3 - Rd[2] * 3 + Rd[3] - Rd[0];
- RB = (Rd[0] - Rd[1] * 2 + Rd[2]) * 3;
- RC = (Rd[2] - Rd[0]) * 3;
-
- integrate(0.0);
- }
-
- /** \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)
- {
- if(dt > 1) dt = 1;
-
- Vec3f cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt);
- Vec3f cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt);
- FCL_REAL cur_angle = cur_w.length();
- cur_w.normalize();
+ const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3);
- Quaternion3f cur_q;
- cur_q.fromAxisAngle(cur_w, cur_angle);
+
+ /// @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);
- tf.setTransform(cur_q, cur_T);
-
- tf_t = dt;
-
- return true;
+ /// @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
+ {
+ return mb_visitor.visit(*this);
}
- /** \brief Compute the motion bound for a bounding volume along a given direction n
- * For general BV, not implemented so return trivial 0
- */
- FCL_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
-
- FCL_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
+ /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor
+ FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
{
- FCL_REAL T_bound = computeTBound(n);
-
- FCL_REAL R_bound = fabs(a.dot(n)) + a.length() + (a.cross(n)).length();
- FCL_REAL R_bound_tmp = fabs(b.dot(n)) + b.length() + (b.cross(n)).length();
- if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
- R_bound_tmp = fabs(c.dot(n)) + c.length() + (c.cross(n)).length();
- if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
-
- FCL_REAL dWdW_max = computeDWMax();
- FCL_REAL ratio = std::min(1 - tf_t, dWdW_max);
-
- R_bound *= 2 * ratio;
-
- // std::cout << R_bound << " " << T_bound << std::endl;
-
- return R_bound + T_bound;
+ return mb_visitor.visit(*this);
}
- /** \brief Get the rotation and translation in current step */
+ /// @brief Get the rotation and translation in current step
void getCurrentTransform(Matrix3f& R, Vec3f& T) const
{
R = tf.getRotation();
@@ -159,145 +95,13 @@ public:
protected:
void computeSplineParameter()
{
-
- }
-
- FCL_REAL getWeight0(FCL_REAL t) const
- {
- return (1 - 3 * t + 3 * t * t - t * t * t) / 6.0;
- }
-
- FCL_REAL getWeight1(FCL_REAL t) const
- {
- return (4 - 6 * t * t + 3 * t * t * t) / 6.0;
- }
-
- FCL_REAL getWeight2(FCL_REAL t) const
- {
- return (1 + 3 * t + 3 * t * t - 3 * t * t * t) / 6.0;
- }
-
- FCL_REAL getWeight3(FCL_REAL t) const
- {
- return t * t * t / 6.0;
- }
-
- FCL_REAL computeTBound(const Vec3f& n) const
- {
- FCL_REAL Ta = TA.dot(n);
- FCL_REAL Tb = TB.dot(n);
- FCL_REAL Tc = TC.dot(n);
-
- std::vector<FCL_REAL> T_potential;
- T_potential.push_back(tf_t);
- T_potential.push_back(1);
- if(Tb * Tb - 3 * Ta * Tc >= 0)
- {
- if(Ta == 0)
- {
- if(Tb != 0)
- {
- FCL_REAL tmp = -Tc / (2 * Tb);
- if(tmp < 1 && tmp > tf_t)
- T_potential.push_back(tmp);
- }
- }
- else
- {
- FCL_REAL tmp_delta = sqrt(Tb * Tb - 3 * Ta * Tc);
- FCL_REAL tmp1 = (-Tb + tmp_delta) / (3 * Ta);
- FCL_REAL tmp2 = (-Tb - tmp_delta) / (3 * Ta);
- if(tmp1 < 1 && tmp1 > tf_t)
- T_potential.push_back(tmp1);
- if(tmp2 < 1 && tmp2 > tf_t)
- T_potential.push_back(tmp2);
- }
- }
-
- FCL_REAL T_bound = Ta * T_potential[0] * T_potential[0] * T_potential[0] + Tb * T_potential[0] * T_potential[0] + Tc * T_potential[0];
- for(unsigned int i = 1; i < T_potential.size(); ++i)
- {
- FCL_REAL T_bound_tmp = Ta * T_potential[i] * T_potential[i] * T_potential[i] + Tb * T_potential[i] * T_potential[i] + Tc * T_potential[i];
- if(T_bound_tmp > T_bound) T_bound = T_bound_tmp;
- }
-
-
- FCL_REAL cur_delta = Ta * tf_t * tf_t * tf_t + Tb * tf_t * tf_t + Tc * tf_t;
-
- T_bound -= cur_delta;
- T_bound /= 6.0;
-
- return T_bound;
- }
-
- FCL_REAL computeDWMax() const
- {
- // first compute ||w'||
- int a00[5] = {1,-4,6,-4,1};
- int a01[5] = {-3,10,-11,4,0};
- int a02[5] = {3,-8,6,0,-1};
- int a03[5] = {-1,2,-1,0,0};
- int a11[5] = {9,-24,16,0,0};
- int a12[5] = {-9,18,-5,-4,0};
- int a13[5] = {3,-4,0,0,0};
- int a22[5] = {9,-12,-2,4,1};
- int a23[5] = {-3,2,1,0,0};
- int a33[5] = {1,0,0,0,0};
-
- FCL_REAL a[5];
-
- for(int i = 0; i < 5; ++i)
- {
- a[i] = Rd0Rd0 * a00[i] + Rd0Rd1 * a01[i] + Rd0Rd2 * a02[i] + Rd0Rd3 * a03[i]
- + Rd0Rd1 * a01[i] + Rd1Rd1 * a11[i] + Rd1Rd2 * a12[i] + Rd1Rd3 * a13[i]
- + Rd0Rd2 * a02[i] + Rd1Rd2 * a12[i] + Rd2Rd2 * a22[i] + Rd2Rd3 * a23[i]
- + Rd0Rd3 * a03[i] + Rd1Rd3 * a13[i] + Rd2Rd3 * a23[i] + Rd3Rd3 * a33[i];
- a[i] /= 4.0;
- }
-
- // compute polynomial for ||w'||'
- int da00[4] = {4,-12,12,-4};
- int da01[4] = {-12,30,-22,4};
- int da02[4] = {12,-24,12,0};
- int da03[4] = {-4,6,-2,0};
- int da11[4] = {36,-72,32,0};
- int da12[4] = {-36,54,-10,-4};
- int da13[4] = {12,-12,0,0};
- int da22[4] = {36,-36,-4,4};
- int da23[4] = {-12,6,2,0};
- int da33[4] = {4,0,0,0};
-
- FCL_REAL da[4];
- for(int i = 0; i < 4; ++i)
- {
- da[i] = Rd0Rd0 * da00[i] + Rd0Rd1 * da01[i] + Rd0Rd2 * da02[i] + Rd0Rd3 * da03[i]
- + Rd0Rd1 * da01[i] + Rd1Rd1 * da11[i] + Rd1Rd2 * da12[i] + Rd1Rd3 * da13[i]
- + Rd0Rd2 * da02[i] + Rd1Rd2 * da12[i] + Rd2Rd2 * da22[i] + Rd2Rd3 * da23[i]
- + Rd0Rd3 * da03[i] + Rd1Rd3 * da13[i] + Rd2Rd3 * da23[i] + Rd3Rd3 * da33[i];
- da[i] /= 4.0;
- }
-
- FCL_REAL roots[3];
-
- int root_num = PolySolver::solveCubic(da, roots);
-
- FCL_REAL dWdW_max = a[0] * tf_t * tf_t * tf_t + a[1] * tf_t * tf_t * tf_t + a[2] * tf_t * tf_t + a[3] * tf_t + a[4];
- FCL_REAL dWdW_1 = a[0] + a[1] + a[2] + a[3] + a[4];
- if(dWdW_max < dWdW_1) dWdW_max = dWdW_1;
- for(int i = 0; i < root_num; ++i)
- {
- FCL_REAL v = roots[i];
-
- if(v >= tf_t && v <= 1)
- {
- FCL_REAL value = a[0] * v * v * v * v + a[1] * v * v * v + a[2] * v * v + a[3] * v + a[4];
- if(value > dWdW_max) dWdW_max = value;
- }
- }
-
- return sqrt(dWdW_max);
}
+ FCL_REAL getWeight0(FCL_REAL t) const;
+ FCL_REAL getWeight1(FCL_REAL t) const;
+ FCL_REAL getWeight2(FCL_REAL t) const;
+ FCL_REAL getWeight3(FCL_REAL t) const;
+
Vec3f Td[4];
Vec3f Rd[4];
@@ -305,31 +109,41 @@ protected:
Vec3f RA, RB, RC;
FCL_REAL Rd0Rd0, Rd0Rd1, Rd0Rd2, Rd0Rd3, Rd1Rd1, Rd1Rd2, Rd1Rd3, Rd2Rd2, Rd2Rd3, Rd3Rd3;
- /** \brief The transformation at current time t */
+ //// @brief The transformation at current time t
Transform3f tf;
- /** \brief The time related with tf */
+ /// @brief The time related with tf
FCL_REAL tf_t;
+
+public:
+ FCL_REAL computeTBound(const Vec3f& n) const;
+
+ FCL_REAL computeDWMax() const;
+
+ FCL_REAL getCurrentTime() const
+ {
+ return tf_t;
+ }
+
};
-template<typename BV>
-class ScrewMotion : public MotionBase<BV>
+class ScrewMotion : public MotionBase
{
public:
- /** Default transformations are all identities */
+ /// @brief Default transformations are all identities
ScrewMotion()
{
- /** Default angular velocity is zero */
+ // Default angular velocity is zero
axis.setValue(1, 0, 0);
angular_vel = 0;
- /** Default reference point is local zero point */
+ // Default reference point is local zero point
- /** Default linear velocity is zero */
+ // Default linear velocity is zero
linear_vel = 0;
}
- /** \brief Construct motion from the initial rotation/translation and goal rotation/translation */
+ /// @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),
tf2(R2, T2),
@@ -338,7 +152,7 @@ public:
computeScrewParameter();
}
- /** \brief Construct motion from the initial transform and goal transform */
+ /// @brief Construct motion from the initial transform and goal transform
ScrewMotion(const Transform3f& tf1_,
const Transform3f& tf2_) : tf1(tf1_),
tf2(tf2_),
@@ -347,9 +161,8 @@ public:
computeScrewParameter();
}
- /** \brief Integrate the motion from 0 to dt
- * We compute the current transformation from zero point instead of from last integrate time, for precision.
- */
+ /// @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)
{
if(dt > 1) dt = 1;
@@ -362,30 +175,20 @@ public:
return true;
}
- /** \brief Compute the motion bound for a bounding volume along a given direction n
- * For general BV, not implemented so return trivial 0
- */
- FCL_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
-
- FCL_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) 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
{
- FCL_REAL proj_max = ((tf.getQuatRotation().transform(a) + tf.getTranslation() - p).cross(axis)).sqrLength();
- FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(b) + tf.getTranslation() - p).cross(axis)).sqrLength();
- if(tmp > proj_max) proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(c) + tf.getTranslation() - p).cross(axis)).sqrLength();
- if(tmp > proj_max) proj_max = tmp;
-
- proj_max = sqrt(proj_max);
-
- FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
- FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
- FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
+ return mb_visitor.visit(*this);
+ }
- return mu;
+ /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor
+ FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
+ {
+ return mb_visitor.visit(*this);
}
- /** \brief Get the rotation and translation in current step */
+
+ /// @brief Get the rotation and translation in current step
void getCurrentTransform(Matrix3f& R, Vec3f& T) const
{
R = tf.getRotation();
@@ -446,143 +249,94 @@ protected:
return delta_t * tf1.getQuatRotation();
}
- /** \brief The transformation at time 0 */
+ /// @brief The transformation at time 0
Transform3f tf1;
- /** \brief The transformation at time 1 */
+ /// @brief The transformation at time 1
Transform3f tf2;
- /** \brief The transformation at current time t */
+ /// @brief The transformation at current time t
Transform3f tf;
- /** \brief screw axis */
+ /// @brief screw axis
Vec3f axis;
- /** \brief A point on the axis S */
+ /// @brief A point on the axis S
Vec3f p;
- /** \brief linear velocity along the axis */
+ /// @brief linear velocity along the axis
FCL_REAL linear_vel;
- /** \brief angular velocity */
+ /// @brief angular velocity
FCL_REAL angular_vel;
-};
-
-
-/** \brief Compute the motion bound for a bounding volume along a given direction n
- * according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized)
- * and ci are the endpoints of the generator primitives of RSS.
- * Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
- */
-template<>
-FCL_REAL ScrewMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const;
-
-/** \brief Linear interpolation motion
- * Each Motion is assumed to have constant linear velocity and angular velocity
- * The motion is R(t)(p - p_ref) + p_ref + T(t)
- * Therefore, R(0) = R0, R(1) = R1
- * T(0) = T0 + R0 p_ref - p_ref
- * T(1) = T1 + R1 p_ref - p_ref
- */
-template<typename BV>
-class InterpMotion : public MotionBase<BV>
-{
public:
- /** \brief Default transformations are all identities */
- InterpMotion()
- {
- /** Default angular velocity is zero */
- angular_axis.setValue(1, 0, 0);
- angular_vel = 0;
-
- /** Default reference point is local zero point */
- /** Default linear velocity is zero */
- }
-
- /** \brief Construct motion from the initial rotation/translation and goal rotation/translation */
- InterpMotion(const Matrix3f& R1, const Vec3f& T1,
- const Matrix3f& R2, const Vec3f& T2) : tf1(R1, T1),
- tf2(R2, T2),
- tf(tf1)
+ inline FCL_REAL getLinearVelocity() const
{
- /** Compute the velocities for the motion */
- computeVelocity();
+ return linear_vel;
}
-
- InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_) : tf1(tf1_),
- tf2(tf2_),
- tf(tf1)
+ inline FCL_REAL getAngularVelocity() const
{
- /** Compute the velocities for the motion */
- computeVelocity();
+ return angular_vel;
}
- /** \brief Construct motion from the initial rotation/translation and goal rotation/translation related to some rotation center
- */
- InterpMotion(const Matrix3f& R1, const Vec3f& T1,
- const Matrix3f& R2, const Vec3f& T2,
- const Vec3f& O) : tf1(R1, T1),
- tf2(R2, T2),
- tf(tf1),
- reference_p(O)
+ inline const Vec3f& getAxis() const
{
- /** Compute the velocities for the motion */
- computeVelocity();
+ return axis;
}
- InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_, const Vec3f& O) : tf1(tf1_),
- tf2(tf2_),
- tf(tf1),
- reference_p(O)
+ inline const Vec3f& getAxisOrigin() const
{
+ return p;
}
+};
- /** \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)
- {
- if(dt > 1) dt = 1;
- tf.setQuatRotation(absoluteRotation(dt));
- tf.setTranslation(linear_vel * dt + tf1.transform(reference_p) - tf.getQuatRotation().transform(reference_p));
+/// @brief Linear interpolation motion
+/// Each Motion is assumed to have constant linear velocity and angular velocity
+/// The motion is R(t)(p - p_ref) + p_ref + T(t)
+/// Therefore, R(0) = R0, R(1) = R1
+/// T(0) = T0 + R0 p_ref - p_ref
+/// T(1) = T1 + R1 p_ref - p_ref
+class InterpMotion : public MotionBase
+{
+public:
+ /// @brief Default transformations are all identities
+ InterpMotion();
- return true;
- }
+ /// @brief Construct motion from the initial rotation/translation and goal rotation/translation
+ InterpMotion(const Matrix3f& R1, const Vec3f& T1,
+ const Matrix3f& R2, const Vec3f& T2);
- /** \brief Compute the motion bound for a bounding volume along a given direction n
- * For general BV, not implemented so return trivial 0
- */
- FCL_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
+ InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_);
- /** \brief Compute the motion bound for a triangle along a given direction n
- * according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / \|w\|. w is the angular velocity
- * and ci are the triangle vertex coordinates.
- * Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
- */
- FCL_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
- {
- FCL_REAL proj_max = ((tf.getQuatRotation().transform(a - reference_p)).cross(angular_axis)).sqrLength();
- FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(b - reference_p)).cross(angular_axis)).sqrLength();
- if(tmp > proj_max) proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(c - reference_p)).cross(angular_axis)).sqrLength();
- if(tmp > proj_max) proj_max = tmp;
+ /// @brief Construct motion from the initial rotation/translation and goal rotation/translation related to some rotation center
+ InterpMotion(const Matrix3f& R1, const Vec3f& T1,
+ const Matrix3f& R2, const Vec3f& T2,
+ const Vec3f& O);
+
+ InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_, const Vec3f& O);
- proj_max = sqrt(proj_max);
+ /// @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);
- FCL_REAL v_dot_n = linear_vel.dot(n);
- FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
- FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
+ /// @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
+ {
+ return mb_visitor.visit(*this);
+ }
- return mu;
+ /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor
+ FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
+ {
+ return mb_visitor.visit(*this);
}
- /** \brief Get the rotation and translation in current step */
+ /// @brief Get the rotation and translation in current step
void getCurrentTransform(Matrix3f& R, Vec3f& T) const
{
R = tf.getRotation();
@@ -606,62 +360,55 @@ public:
protected:
- void computeVelocity()
- {
- linear_vel = tf2.transform(reference_p) - tf1.transform(reference_p);
- Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
- deltaq.toAxisAngle(angular_axis, angular_vel);
- if(angular_vel < 0)
- {
- angular_vel = -angular_vel;
- angular_axis = -angular_axis;
- }
- }
-
-
- Quaternion3f deltaRotation(FCL_REAL dt) const
- {
- Quaternion3f res;
- res.fromAxisAngle(angular_axis, (FCL_REAL)(dt * angular_vel));
- return res;
- }
-
- Quaternion3f absoluteRotation(FCL_REAL dt) const
- {
- Quaternion3f delta_t = deltaRotation(dt);
- return delta_t * tf1.getQuatRotation();
- }
+ void computeVelocity();
- /** \brief The transformation at time 0 */
+ Quaternion3f deltaRotation(FCL_REAL dt) const;
+
+ Quaternion3f absoluteRotation(FCL_REAL dt) const;
+
+ /// @brief The transformation at time 0
Transform3f tf1;
- /** \brief The transformation at time 1 */
+ /// @brief The transformation at time 1
Transform3f tf2;
- /** \brief The transformation at current time t */
+ /// @brief The transformation at current time t
Transform3f tf;
- /** \brief Linear velocity */
+ /// @brief Linear velocity
Vec3f linear_vel;
- /** \brief Angular speed */
+ /// @brief Angular speed
FCL_REAL angular_vel;
- /** \brief Angular velocity axis */
+ /// @brief Angular velocity axis
Vec3f angular_axis;
- /** \brief Reference point for the motion (in the object's local frame) */
+ /// @brief Reference point for the motion (in the object's local frame)
Vec3f reference_p;
-};
+public:
+ const Vec3f& getReferencePoint() const
+ {
+ return reference_p;
+ }
+
+ const Vec3f& getAngularAxis() const
+ {
+ return angular_axis;
+ }
+
+ FCL_REAL getAngularVelocity() const
+ {
+ return angular_vel;
+ }
+
+ const Vec3f& getLinearVelocity() const
+ {
+ return linear_vel;
+ }
+};
-/** \brief Compute the motion bound for a bounding volume along a given direction n
- * according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized)
- * and ci are the endpoints of the generator primitives of RSS.
- * Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
- */
-template<>
-FCL_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const;
}
diff --git a/include/fcl/ccd/motion_base.h b/include/fcl/ccd/motion_base.h
index 50f641ae0f2386ad4a24412d1765573a0fc34c53..35f5c92b67f15412ae6c6e57f1a276fb204dbe71 100644
--- a/include/fcl/ccd/motion_base.h
+++ b/include/fcl/ccd/motion_base.h
@@ -45,7 +45,66 @@
namespace fcl
{
+class MotionBase;
+
+class SplineMotion;
+class ScrewMotion;
+class InterpMotion;
+
+/// @brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects
+class BVMotionBoundVisitor
+{
+public:
+ virtual FCL_REAL visit(const MotionBase& motion) const = 0;
+ 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;
+};
+
template<typename BV>
+class TBVMotionBoundVisitor : public BVMotionBoundVisitor
+{
+public:
+ TBVMotionBoundVisitor(const BV& bv_, const Vec3f& n_) : bv(bv_), n(n_) {}
+
+ virtual FCL_REAL visit(const MotionBase& motion) const { return 0; }
+ 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; }
+
+protected:
+ BV bv;
+ Vec3f n;
+};
+
+template<>
+FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const SplineMotion& motion) const;
+
+template<>
+FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const ScrewMotion& motion) const;
+
+template<>
+FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const InterpMotion& motion) const;
+
+
+
+class TriangleMotionBoundVisitor
+{
+public:
+ TriangleMotionBoundVisitor(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_, const Vec3f& n_) :
+ a(a_), b(b_), c(c_), n(n_) {}
+
+ virtual FCL_REAL visit(const MotionBase& motion) const { return 0; }
+ virtual FCL_REAL visit(const SplineMotion& motion) const;
+ virtual FCL_REAL visit(const ScrewMotion& motion) const;
+ virtual FCL_REAL visit(const InterpMotion& motion) const;
+
+protected:
+ Vec3f a, b, c, n;
+};
+
+
+
class MotionBase
{
public:
@@ -55,10 +114,10 @@ public:
virtual bool integrate(double dt) = 0;
/** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects */
- virtual FCL_REAL computeMotionBound(const BV& bv, const Vec3f& n) 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 Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const = 0;
+ 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;
diff --git a/include/fcl/traversal/traversal_node_bvhs.h b/include/fcl/traversal/traversal_node_bvhs.h
index 2a943fa2855324f65e95dbd4ffef04542ffdcd46..c7fb633aec147a5d352866f59df5210f9d6c07e6 100644
--- a/include/fcl/traversal/traversal_node_bvhs.h
+++ b/include/fcl/traversal/traversal_node_bvhs.h
@@ -137,7 +137,6 @@ public:
mutable int num_bv_tests;
mutable int num_leaf_tests;
mutable FCL_REAL query_time_seconds;
-
};
@@ -744,8 +743,9 @@ public:
// n is the local frame of object 1
Vec3f n = P2 - P1;
// here n is already in global frame as we assume the body is in original configuration (I, 0) for general BVH
- FCL_REAL bound1 = motion1->computeMotionBound(p1, p2, p3, n);
- FCL_REAL bound2 = motion2->computeMotionBound(q1, q2, q3, n);
+ TriangleMotionBoundVisitor mb_visitor1(p1, p2, p3, n), mb_visitor2(q1, q2, q3, n);
+ FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
+ FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
FCL_REAL bound = bound1 + bound2;
@@ -785,8 +785,9 @@ public:
assert(c == d);
- FCL_REAL bound1 = motion1->computeMotionBound((this->tree1 + c1)->bv, n);
- FCL_REAL bound2 = motion2->computeMotionBound((this->tree2 + c2)->bv, n);
+ TBVMotionBoundVisitor<BV> mb_visitor1((this->tree1 + c1)->bv, n), mb_visitor2((this->tree2 + c2)->bv, n);
+ FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
+ FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
FCL_REAL bound = bound1 + bound2;
@@ -833,8 +834,8 @@ public:
mutable FCL_REAL delta_t;
/// @brief Motions for the two objects in query
- MotionBase<BV>* motion1;
- MotionBase<BV>* motion2;
+ MotionBase* motion1;
+ MotionBase* motion2;
mutable std::vector<ConservativeAdvancementStackData> stack;
};
diff --git a/src/ccd/conservative_advancement.cpp b/src/ccd/conservative_advancement.cpp
index 8899a256c89e76d678c01dcaa064a4d230064697..95ebf27bfa628129a6ad3b3398fa0556fdaa1f09 100644
--- a/src/ccd/conservative_advancement.cpp
+++ b/src/ccd/conservative_advancement.cpp
@@ -47,11 +47,11 @@ namespace fcl
{
-template<typename BV>
+template<typename BV, typename ConservativeAdvancementNode, typename CollisionNode>
int conservativeAdvancement(const CollisionGeometry* o1,
- MotionBase<BV>* motion1,
+ MotionBase* motion1,
const CollisionGeometry* o2,
- MotionBase<BV>* motion2,
+ MotionBase* motion2,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc)
@@ -75,15 +75,15 @@ int conservativeAdvancement(const CollisionGeometry* o1,
return 0;
- const BVHModel<RSS>* model1 = (const BVHModel<RSS>*)o1;
- const BVHModel<RSS>* model2 = (const BVHModel<RSS>*)o2;
+ const BVHModel<BV>* model1 = (const BVHModel<BV>*)o1;
+ const BVHModel<BV>* model2 = (const BVHModel<BV>*)o2;
Transform3f tf1, tf2;
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
// whether the first start configuration is in collision
- MeshCollisionTraversalNodeRSS cnode;
+ CollisionNode cnode;
if(!initialize(cnode, *model1, tf1, *model2, tf2, request, result))
std::cout << "initialize error" << std::endl;
@@ -102,16 +102,14 @@ int conservativeAdvancement(const CollisionGeometry* o1,
// std::cout << "zero collide" << std::endl;
return b;
}
-
-
- MeshConservativeAdvancementTraversalNodeRSS node;
+
+ ConservativeAdvancementNode node;
initialize(node, *model1, tf1, *model2, tf2);
node.motion1 = motion1;
node.motion2 = motion2;
-
do
{
Matrix3f R1_t, R2_t;
@@ -155,13 +153,21 @@ int conservativeAdvancement(const CollisionGeometry* o1,
}
-template int conservativeAdvancement<RSS>(const CollisionGeometry* o1,
- MotionBase<RSS>* motion1,
- const CollisionGeometry* o2,
- MotionBase<RSS>* motion2,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc);
-
+template
+int conservativeAdvancement<RSS, MeshConservativeAdvancementTraversalNodeRSS, MeshCollisionTraversalNodeRSS>(const CollisionGeometry* o1,
+ MotionBase* motion1,
+ const CollisionGeometry* o2,
+ MotionBase* motion2,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc);
+
+template int conservativeAdvancement<OBBRSS, MeshConservativeAdvancementTraversalNodeOBBRSS, MeshCollisionTraversalNodeOBBRSS>(const CollisionGeometry* o1,
+ MotionBase* motion1,
+ const CollisionGeometry* o2,
+ MotionBase* motion2,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc);
}
diff --git a/src/ccd/motion.cpp b/src/ccd/motion.cpp
index ad84a1deaf6d46779fd050ae9ebe3c2046ba014e..2665fb66af5785ac69319be230531c83be78411e 100644
--- a/src/ccd/motion.cpp
+++ b/src/ccd/motion.cpp
@@ -41,53 +41,10 @@ namespace fcl
{
template<>
-FCL_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
+FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const SplineMotion& motion) const
{
- FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).sqrLength();
- FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] - reference_p)).cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
-
- c_proj_max = sqrt(c_proj_max);
-
- FCL_REAL v_dot_n = linear_vel.dot(n);
- FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
- FCL_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max);
-
- return mu;
-}
-
-template<>
-FCL_REAL ScrewMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
-{
- FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr)).cross(axis)).sqrLength();
- FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0])).cross(axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
-
- c_proj_max = sqrt(c_proj_max);
-
- FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
- FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
- FCL_REAL origin_proj = ((tf.getTranslation() - p).cross(axis)).length();
-
- FCL_REAL mu = v_dot_n + w_cross_n * (c_proj_max + bv.r + origin_proj);
-
- return mu;
-}
-
-template<>
-FCL_REAL SplineMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
-{
- FCL_REAL T_bound = computeTBound(n);
+ FCL_REAL T_bound = motion.computeTBound(n);
+ FCL_REAL tf_t = motion.getCurrentTime();
Vec3f c1 = bv.Tr;
Vec3f c2 = bv.Tr + bv.axis[0] * bv.l[0];
@@ -96,12 +53,12 @@ FCL_REAL SplineMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) co
FCL_REAL tmp;
// max_i |c_i * n|
- FCL_REAL cn_max = fabs(c1.dot(n));
- tmp = fabs(c2.dot(n));
+ FCL_REAL cn_max = std::fabs(c1.dot(n));
+ tmp = std::fabs(c2.dot(n));
if(tmp > cn_max) cn_max = tmp;
- tmp = fabs(c3.dot(n));
+ tmp = std::fabs(c3.dot(n));
if(tmp > cn_max) cn_max = tmp;
- tmp = fabs(c4.dot(n));
+ tmp = std::fabs(c4.dot(n));
if(tmp > cn_max) cn_max = tmp;
// max_i ||c_i||
@@ -124,15 +81,449 @@ FCL_REAL SplineMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) co
if(tmp > cxn_max) cxn_max = tmp;
cxn_max = sqrt(cxn_max);
- FCL_REAL dWdW_max = computeDWMax();
+ FCL_REAL dWdW_max = motion.computeDWMax();
FCL_REAL ratio = std::min(1 - tf_t, dWdW_max);
FCL_REAL R_bound = 2 * (cn_max + cmax + cxn_max + 3 * bv.r) * ratio;
+ // std::cout << R_bound << " " << T_bound << std::endl;
+
+ return R_bound + T_bound;
+}
+
+
+FCL_REAL TriangleMotionBoundVisitor::visit(const SplineMotion& motion) const
+{
+ FCL_REAL T_bound = motion.computeTBound(n);
+ FCL_REAL tf_t = motion.getCurrentTime();
+
+ FCL_REAL R_bound = std::fabs(a.dot(n)) + a.length() + (a.cross(n)).length();
+ FCL_REAL R_bound_tmp = std::fabs(b.dot(n)) + b.length() + (b.cross(n)).length();
+ if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
+ R_bound_tmp = std::fabs(c.dot(n)) + c.length() + (c.cross(n)).length();
+ if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
+
+ FCL_REAL dWdW_max = motion.computeDWMax();
+ FCL_REAL ratio = std::min(1 - tf_t, dWdW_max);
+
+ R_bound *= 2 * ratio;
+
// std::cout << R_bound << " " << T_bound << std::endl;
return R_bound + T_bound;
}
+SplineMotion::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)
+{
+ Td[0] = Td0;
+ Td[1] = Td1;
+ Td[2] = Td2;
+ Td[3] = Td3;
+
+ Rd[0] = Rd0;
+ Rd[1] = Rd1;
+ Rd[2] = Rd2;
+ Rd[3] = Rd3;
+
+ Rd0Rd0 = Rd[0].dot(Rd[0]);
+ Rd0Rd1 = Rd[0].dot(Rd[1]);
+ Rd0Rd2 = Rd[0].dot(Rd[2]);
+ Rd0Rd3 = Rd[0].dot(Rd[3]);
+ Rd1Rd1 = Rd[1].dot(Rd[1]);
+ Rd1Rd2 = Rd[1].dot(Rd[2]);
+ Rd1Rd3 = Rd[1].dot(Rd[3]);
+ Rd2Rd2 = Rd[2].dot(Rd[2]);
+ Rd2Rd3 = Rd[2].dot(Rd[3]);
+ Rd3Rd3 = Rd[3].dot(Rd[3]);
+
+ TA = Td[1] * 3 - Td[2] * 3 + Td[3] - Td[0];
+ TB = (Td[0] - Td[1] * 2 + Td[2]) * 3;
+ TC = (Td[2] - Td[0]) * 3;
+
+ RA = Rd[1] * 3 - Rd[2] * 3 + Rd[3] - Rd[0];
+ RB = (Rd[0] - Rd[1] * 2 + Rd[2]) * 3;
+ RC = (Rd[2] - Rd[0]) * 3;
+
+ integrate(0.0);
+}
+
+bool SplineMotion::integrate(double dt)
+{
+ if(dt > 1) dt = 1;
+
+ Vec3f cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt);
+ Vec3f cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt);
+ FCL_REAL cur_angle = cur_w.length();
+ cur_w.normalize();
+
+ Quaternion3f cur_q;
+ cur_q.fromAxisAngle(cur_w, cur_angle);
+
+ tf.setTransform(cur_q, cur_T);
+
+ tf_t = dt;
+
+ return true;
+}
+
+
+FCL_REAL SplineMotion::computeTBound(const Vec3f& n) const
+{
+ FCL_REAL Ta = TA.dot(n);
+ FCL_REAL Tb = TB.dot(n);
+ FCL_REAL Tc = TC.dot(n);
+
+ std::vector<FCL_REAL> T_potential;
+ T_potential.push_back(tf_t);
+ T_potential.push_back(1);
+ if(Tb * Tb - 3 * Ta * Tc >= 0)
+ {
+ if(Ta == 0)
+ {
+ if(Tb != 0)
+ {
+ FCL_REAL tmp = -Tc / (2 * Tb);
+ if(tmp < 1 && tmp > tf_t)
+ T_potential.push_back(tmp);
+ }
+ }
+ else
+ {
+ FCL_REAL tmp_delta = sqrt(Tb * Tb - 3 * Ta * Tc);
+ FCL_REAL tmp1 = (-Tb + tmp_delta) / (3 * Ta);
+ FCL_REAL tmp2 = (-Tb - tmp_delta) / (3 * Ta);
+ if(tmp1 < 1 && tmp1 > tf_t)
+ T_potential.push_back(tmp1);
+ if(tmp2 < 1 && tmp2 > tf_t)
+ T_potential.push_back(tmp2);
+ }
+ }
+
+ FCL_REAL T_bound = Ta * T_potential[0] * T_potential[0] * T_potential[0] + Tb * T_potential[0] * T_potential[0] + Tc * T_potential[0];
+ for(unsigned int i = 1; i < T_potential.size(); ++i)
+ {
+ FCL_REAL T_bound_tmp = Ta * T_potential[i] * T_potential[i] * T_potential[i] + Tb * T_potential[i] * T_potential[i] + Tc * T_potential[i];
+ if(T_bound_tmp > T_bound) T_bound = T_bound_tmp;
+ }
+
+
+ FCL_REAL cur_delta = Ta * tf_t * tf_t * tf_t + Tb * tf_t * tf_t + Tc * tf_t;
+
+ T_bound -= cur_delta;
+ T_bound /= 6.0;
+
+ return T_bound;
+}
+
+
+FCL_REAL SplineMotion::computeDWMax() const
+{
+ // first compute ||w'||
+ int a00[5] = {1,-4,6,-4,1};
+ int a01[5] = {-3,10,-11,4,0};
+ int a02[5] = {3,-8,6,0,-1};
+ int a03[5] = {-1,2,-1,0,0};
+ int a11[5] = {9,-24,16,0,0};
+ int a12[5] = {-9,18,-5,-4,0};
+ int a13[5] = {3,-4,0,0,0};
+ int a22[5] = {9,-12,-2,4,1};
+ int a23[5] = {-3,2,1,0,0};
+ int a33[5] = {1,0,0,0,0};
+
+ FCL_REAL a[5];
+
+ for(int i = 0; i < 5; ++i)
+ {
+ a[i] = Rd0Rd0 * a00[i] + Rd0Rd1 * a01[i] + Rd0Rd2 * a02[i] + Rd0Rd3 * a03[i]
+ + Rd0Rd1 * a01[i] + Rd1Rd1 * a11[i] + Rd1Rd2 * a12[i] + Rd1Rd3 * a13[i]
+ + Rd0Rd2 * a02[i] + Rd1Rd2 * a12[i] + Rd2Rd2 * a22[i] + Rd2Rd3 * a23[i]
+ + Rd0Rd3 * a03[i] + Rd1Rd3 * a13[i] + Rd2Rd3 * a23[i] + Rd3Rd3 * a33[i];
+ a[i] /= 4.0;
+ }
+
+ // compute polynomial for ||w'||'
+ int da00[4] = {4,-12,12,-4};
+ int da01[4] = {-12,30,-22,4};
+ int da02[4] = {12,-24,12,0};
+ int da03[4] = {-4,6,-2,0};
+ int da11[4] = {36,-72,32,0};
+ int da12[4] = {-36,54,-10,-4};
+ int da13[4] = {12,-12,0,0};
+ int da22[4] = {36,-36,-4,4};
+ int da23[4] = {-12,6,2,0};
+ int da33[4] = {4,0,0,0};
+
+ FCL_REAL da[4];
+ for(int i = 0; i < 4; ++i)
+ {
+ da[i] = Rd0Rd0 * da00[i] + Rd0Rd1 * da01[i] + Rd0Rd2 * da02[i] + Rd0Rd3 * da03[i]
+ + Rd0Rd1 * da01[i] + Rd1Rd1 * da11[i] + Rd1Rd2 * da12[i] + Rd1Rd3 * da13[i]
+ + Rd0Rd2 * da02[i] + Rd1Rd2 * da12[i] + Rd2Rd2 * da22[i] + Rd2Rd3 * da23[i]
+ + Rd0Rd3 * da03[i] + Rd1Rd3 * da13[i] + Rd2Rd3 * da23[i] + Rd3Rd3 * da33[i];
+ da[i] /= 4.0;
+ }
+
+ FCL_REAL roots[3];
+
+ int root_num = PolySolver::solveCubic(da, roots);
+
+ FCL_REAL dWdW_max = a[0] * tf_t * tf_t * tf_t + a[1] * tf_t * tf_t * tf_t + a[2] * tf_t * tf_t + a[3] * tf_t + a[4];
+ FCL_REAL dWdW_1 = a[0] + a[1] + a[2] + a[3] + a[4];
+ if(dWdW_max < dWdW_1) dWdW_max = dWdW_1;
+ for(int i = 0; i < root_num; ++i)
+ {
+ FCL_REAL v = roots[i];
+
+ if(v >= tf_t && v <= 1)
+ {
+ FCL_REAL value = a[0] * v * v * v * v + a[1] * v * v * v + a[2] * v * v + a[3] * v + a[4];
+ if(value > dWdW_max) dWdW_max = value;
+ }
+ }
+
+ return sqrt(dWdW_max);
+}
+
+FCL_REAL SplineMotion::getWeight0(FCL_REAL t) const
+{
+ return (1 - 3 * t + 3 * t * t - t * t * t) / 6.0;
+}
+
+FCL_REAL SplineMotion::getWeight1(FCL_REAL t) const
+{
+ return (4 - 6 * t * t + 3 * t * t * t) / 6.0;
+}
+
+FCL_REAL SplineMotion::getWeight2(FCL_REAL t) const
+{
+ return (1 + 3 * t + 3 * t * t - 3 * t * t * t) / 6.0;
+}
+
+FCL_REAL SplineMotion::getWeight3(FCL_REAL t) const
+{
+ return t * t * t / 6.0;
+}
+
+
+
+
+/// @brief Compute the motion bound for a bounding volume along a given direction n
+/// according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized)
+/// and ci are the endpoints of the generator primitives of RSS.
+/// Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
+template<>
+FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const ScrewMotion& motion) const
+{
+ Transform3f tf;
+ motion.getCurrentTransform(tf);
+
+ const Vec3f& axis = motion.getAxis();
+ FCL_REAL linear_vel = motion.getLinearVelocity();
+ FCL_REAL angular_vel = motion.getAngularVelocity();
+ const Vec3f& p = motion.getAxisOrigin();
+
+ FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr)).cross(axis)).sqrLength();
+ FCL_REAL tmp;
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0])).cross(axis)).sqrLength();
+ if(tmp > c_proj_max) c_proj_max = tmp;
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
+ if(tmp > c_proj_max) c_proj_max = tmp;
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
+ if(tmp > c_proj_max) c_proj_max = tmp;
+
+ c_proj_max = sqrt(c_proj_max);
+
+ FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
+ FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
+ FCL_REAL origin_proj = ((tf.getTranslation() - p).cross(axis)).length();
+
+ FCL_REAL mu = v_dot_n + w_cross_n * (c_proj_max + bv.r + origin_proj);
+
+ return mu;
+}
+
+FCL_REAL TriangleMotionBoundVisitor::visit(const ScrewMotion& motion) const
+{
+ Transform3f tf;
+ motion.getCurrentTransform(tf);
+
+ const Vec3f& axis = motion.getAxis();
+ FCL_REAL linear_vel = motion.getLinearVelocity();
+ FCL_REAL angular_vel = motion.getAngularVelocity();
+ const Vec3f& p = motion.getAxisOrigin();
+
+ FCL_REAL proj_max = ((tf.getQuatRotation().transform(a) + tf.getTranslation() - p).cross(axis)).sqrLength();
+ FCL_REAL tmp;
+ tmp = ((tf.getQuatRotation().transform(b) + tf.getTranslation() - p).cross(axis)).sqrLength();
+ if(tmp > proj_max) proj_max = tmp;
+ tmp = ((tf.getQuatRotation().transform(c) + tf.getTranslation() - p).cross(axis)).sqrLength();
+ if(tmp > proj_max) proj_max = tmp;
+
+ proj_max = std::sqrt(proj_max);
+
+ FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
+ FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
+ FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
+
+ return mu;
+}
+
+
+
+
+
+/// @brief Compute the motion bound for a bounding volume along a given direction n
+/// according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized)
+/// and ci are the endpoints of the generator primitives of RSS.
+/// Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
+template<>
+FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const InterpMotion& motion) const
+{
+ Transform3f tf;
+ motion.getCurrentTransform(tf);
+
+ const Vec3f& reference_p = motion.getReferencePoint();
+ const Vec3f& angular_axis = motion.getAngularAxis();
+ FCL_REAL angular_vel = motion.getAngularVelocity();
+ const Vec3f& linear_vel = motion.getLinearVelocity();
+
+ FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).sqrLength();
+ FCL_REAL tmp;
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] - reference_p)).cross(angular_axis)).sqrLength();
+ if(tmp > c_proj_max) c_proj_max = tmp;
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
+ if(tmp > c_proj_max) c_proj_max = tmp;
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
+ if(tmp > c_proj_max) c_proj_max = tmp;
+
+ c_proj_max = std::sqrt(c_proj_max);
+
+ FCL_REAL v_dot_n = linear_vel.dot(n);
+ FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
+ FCL_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max);
+
+ return mu;
+}
+
+/// @brief Compute the motion bound for a triangle along a given direction n
+/// according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / \|w\|. w is the angular velocity
+/// and ci are the triangle vertex coordinates.
+/// Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
+FCL_REAL TriangleMotionBoundVisitor::visit(const InterpMotion& motion) const
+{
+ Transform3f tf;
+ motion.getCurrentTransform(tf);
+
+ const Vec3f& reference_p = motion.getReferencePoint();
+ const Vec3f& angular_axis = motion.getAngularAxis();
+ FCL_REAL angular_vel = motion.getAngularVelocity();
+ const Vec3f& linear_vel = motion.getLinearVelocity();
+
+ FCL_REAL proj_max = ((tf.getQuatRotation().transform(a - reference_p)).cross(angular_axis)).sqrLength();
+ FCL_REAL tmp;
+ tmp = ((tf.getQuatRotation().transform(b - reference_p)).cross(angular_axis)).sqrLength();
+ if(tmp > proj_max) proj_max = tmp;
+ tmp = ((tf.getQuatRotation().transform(c - reference_p)).cross(angular_axis)).sqrLength();
+ if(tmp > proj_max) proj_max = tmp;
+
+ proj_max = std::sqrt(proj_max);
+
+ FCL_REAL v_dot_n = linear_vel.dot(n);
+ FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
+ FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
+
+ return mu;
+}
+
+InterpMotion::InterpMotion()
+{
+ // Default angular velocity is zero
+ angular_axis.setValue(1, 0, 0);
+ angular_vel = 0;
+
+ // Default reference point is local zero point
+
+ // Default linear velocity is zero
+}
+
+InterpMotion::InterpMotion(const Matrix3f& R1, const Vec3f& T1,
+ const Matrix3f& R2, const Vec3f& T2) :
+ tf1(R1, T1),
+ tf2(R2, T2),
+ tf(tf1)
+{
+ // Compute the velocities for the motion
+ computeVelocity();
+}
+
+
+InterpMotion::InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_) :
+ tf1(tf1_),
+ tf2(tf2_),
+ tf(tf1)
+{
+ // Compute the velocities for the motion
+ computeVelocity();
+}
+
+InterpMotion::InterpMotion(const Matrix3f& R1, const Vec3f& T1,
+ const Matrix3f& R2, const Vec3f& T2,
+ const Vec3f& O) :
+ tf1(R1, T1),
+ tf2(R2, T2),
+ tf(tf1),
+ reference_p(O)
+{
+ // Compute the velocities for the motion
+ computeVelocity();
+}
+
+InterpMotion::InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_, const Vec3f& O) :
+ tf1(tf1_),
+ tf2(tf2_),
+ tf(tf1),
+ reference_p(O)
+{
+}
+
+bool InterpMotion::integrate(double dt)
+{
+ if(dt > 1) dt = 1;
+
+ tf.setQuatRotation(absoluteRotation(dt));
+ tf.setTranslation(linear_vel * dt + tf1.transform(reference_p) - tf.getQuatRotation().transform(reference_p));
+
+ return true;
+}
+
+
+void InterpMotion::computeVelocity()
+{
+ linear_vel = tf2.transform(reference_p) - tf1.transform(reference_p);
+ Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
+ deltaq.toAxisAngle(angular_axis, angular_vel);
+ if(angular_vel < 0)
+ {
+ angular_vel = -angular_vel;
+ angular_axis = -angular_axis;
+ }
+}
+
+
+Quaternion3f InterpMotion::deltaRotation(FCL_REAL dt) const
+{
+ Quaternion3f res;
+ res.fromAxisAngle(angular_axis, (FCL_REAL)(dt * angular_vel));
+ return res;
+}
+
+Quaternion3f InterpMotion::absoluteRotation(FCL_REAL dt) const
+{
+ Quaternion3f delta_t = deltaRotation(dt);
+ return delta_t * tf1.getQuatRotation();
+}
+
+
}
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index 2c1eb5ea235dc87b49c571579db3c462b671196a..fd91349f06336211a7c8987ff2b2e5f29a649705 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -450,7 +450,7 @@ bool meshConservativeAdvancementTraversalNodeCanStop(FCL_REAL c,
FCL_REAL min_distance,
FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w,
const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- const MotionBase<BV>* motion1, const MotionBase<BV>* motion2,
+ const MotionBase* motion1, const MotionBase* motion2,
std::vector<ConservativeAdvancementStackData>& stack,
FCL_REAL& delta_t)
{
@@ -484,8 +484,9 @@ bool meshConservativeAdvancementTraversalNodeCanStop(FCL_REAL c,
getBVAxis(model1->getBV(c1).bv, 1) * n[1] +
getBVAxis(model1->getBV(c1).bv, 2) * n[2];
- FCL_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
- FCL_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, n_transformed);
+ TBVMotionBoundVisitor<BV> mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, n_transformed);
+ FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
+ FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
FCL_REAL bound = bound1 + bound2;
@@ -556,7 +557,7 @@ bool meshConservativeAdvancementOrientedNodeCanStop(FCL_REAL c,
FCL_REAL min_distance,
FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w,
const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- const MotionBase<BV>* motion1, const MotionBase<BV>* motion2,
+ const MotionBase* motion1, const MotionBase* motion2,
std::vector<ConservativeAdvancementStackData>& stack,
FCL_REAL& delta_t)
{
@@ -595,8 +596,9 @@ bool meshConservativeAdvancementOrientedNodeCanStop(FCL_REAL c,
n_transformed = R0 * n_transformed;
n_transformed.normalize();
- FCL_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
- FCL_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, -n_transformed);
+ TBVMotionBoundVisitor<BV> mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, -n_transformed);
+ FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
+ FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
FCL_REAL bound = bound1 + bound2;
@@ -631,7 +633,7 @@ void meshConservativeAdvancementOrientedNodeLeafTesting(int b1, int b2,
const Triangle* tri_indices1, const Triangle* tri_indices2,
const Vec3f* vertices1, const Vec3f* vertices2,
const Matrix3f& R, const Vec3f& T,
- const MotionBase<BV>* motion1, const MotionBase<BV>* motion2,
+ const MotionBase* motion1, const MotionBase* motion2,
bool enable_statistics,
FCL_REAL& min_distance,
Vec3f& p1, Vec3f& p2,
@@ -684,8 +686,10 @@ void meshConservativeAdvancementOrientedNodeLeafTesting(int b1, int b2,
motion1->getCurrentRotation(R0);
Vec3f n_transformed = R0 * n;
n_transformed.normalize();
- FCL_REAL bound1 = motion1->computeMotionBound(t11, t12, t13, n_transformed);
- FCL_REAL bound2 = motion2->computeMotionBound(t21, t22, t23, -n_transformed);
+
+ TriangleMotionBoundVisitor mb_visitor1(t11, t12, t13, n_transformed), mb_visitor2(t21, t22, t23, -n_transformed);
+ FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
+ FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
FCL_REAL bound = bound1 + bound2;