Verified Commit edbf758d authored by Justin Carpentier's avatar Justin Carpentier
Browse files

core/collision: improve efficiency and remove useless computations

parent e9431c9d
......@@ -2019,7 +2019,7 @@ namespace fcl {
// FCL_REAL* penetration_depth, Vec3f* normal)
{
static const FCL_REAL eps (sqrt (std::numeric_limits<FCL_REAL>::epsilon()));
Plane new_s2 = transform(s2, tf2);
const Plane new_s2 = transform(s2, tf2);
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
......@@ -2027,7 +2027,7 @@ namespace fcl {
const Vec3f Q (R.transpose() * new_s2.n);
const Vec3f A (Q.cwiseProduct(s1.halfSide));
FCL_REAL signed_dist = new_s2.signedDistance(T);
const FCL_REAL signed_dist = new_s2.signedDistance(T);
distance = std::abs(signed_dist) - A.lpNorm<1>();
if(distance > 0) {
// Is the box above or below the plane
......@@ -2048,11 +2048,6 @@ namespace fcl {
return false;
}
Vec3f axis[3];
axis[0] = R.col(0);
axis[1] = R.col(1);
axis[2] = R.col(2);
// find the deepest point
Vec3f p = T;
......@@ -2065,29 +2060,29 @@ namespace fcl {
std::abs(Q[0] + 1) < planeIntersectTolerance<FCL_REAL>())
{
int sign2 = (A[0] > 0) ? -sign : sign;
p += R.col(0) * (s1.halfSide[0] * sign2);
p.noalias() += R.col(0) * (s1.halfSide[0] * sign2);
}
else if(std::abs(Q[1] - 1) < planeIntersectTolerance<FCL_REAL>() ||
std::abs(Q[1] + 1) < planeIntersectTolerance<FCL_REAL>())
{
int sign2 = (A[1] > 0) ? -sign : sign;
p += R.col(1) * (s1.halfSide[1] * sign2);
p.noalias() += R.col(1) * (s1.halfSide[1] * sign2);
}
else if(std::abs(Q[2] - 1) < planeIntersectTolerance<FCL_REAL>() ||
std::abs(Q[2] + 1) < planeIntersectTolerance<FCL_REAL>())
{
int sign2 = (A[2] > 0) ? -sign : sign;
p += R.col(2) * (s1.halfSide[2] * sign2);
p.noalias() += R.col(2) * (s1.halfSide[2] * sign2);
}
else
{
Vec3f tmp(sign * R * s1.halfSide);
p += (A.array() > 0).select (- tmp, tmp);
p.noalias() += (A.array() > 0).select (- tmp, tmp);
}
// compute the contact point by project the deepest point onto the plane
if (signed_dist > 0) normal = -new_s2.n; else normal = new_s2.n;
p1 = p2 = p - new_s2.n * new_s2.signedDistance(p);
p1 = p2.noalias() = p - new_s2.n * new_s2.signedDistance(p);
return true;
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment