Commit 0f0a2d18 authored by Jia Pan's avatar Jia Pan
Browse files

refactor the ccd motion

parent 75fccd00
......@@ -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);
......
This diff is collapsed.
......@@ -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;
......
......@@ -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;
};
......
......@@ -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);
}
......@@ -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;