Commit fdbffe99 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Fix boxSphereDistance

parent d31b949e
...@@ -2107,30 +2107,39 @@ namespace fcl { ...@@ -2107,30 +2107,39 @@ namespace fcl {
const Vec3f& ob = tfb.getTranslation(); const Vec3f& ob = tfb.getTranslation();
const Matrix3f& Rb = tfb.getRotation(); const Matrix3f& Rb = tfb.getRotation();
const Vec3f d (os - ob);
pb = ob; pb = ob;
bool inside = true; bool outside = false;
const Vec3f os_in_b_frame (Rb.transpose() * (os - ob));
int axis = -1;
FCL_REAL min_d = std::numeric_limits<FCL_REAL>::max();
for (int i = 0; i < 3; ++i) { for (int i = 0; i < 3; ++i) {
bool iinside = false; FCL_REAL facedist;
FCL_REAL dist = Rb.col(i).dot(d), side_2 = b.halfSide(i); if (os_in_b_frame(i) < - b.halfSide(i)) { // outside
if (dist < -side_2) dist = -side_2; // outside pb.noalias() -= b.halfSide(i) * Rb.col(i);
else if (dist > side_2) dist = side_2; // outside outside = true;
else iinside = true; // inside } else if (os_in_b_frame(i) > b.halfSide(i)) { // outside
inside = inside && iinside; pb.noalias() += b.halfSide(i) * Rb.col(i);
outside = true;
pb.noalias() += dist * Rb.col(i); } else {
pb.noalias() += os_in_b_frame(i) * Rb.col(i);
if (!outside && (facedist = b.halfSide(i) - std::fabs(os_in_b_frame(i))) < min_d) {
axis = i;
min_d = facedist;
}
}
} }
normal.noalias() = pb - os; normal.noalias() = pb - os;
FCL_REAL pdist = normal.norm(); FCL_REAL pdist = normal.norm();
if (inside) { if (outside) { // pb is on the box
dist = - pdist - s.radius;
normal /= pdist;
} else {
dist = pdist - s.radius; dist = pdist - s.radius;
normal /= - pdist; normal /= - pdist;
} else { // pb is inside the box
if (os_in_b_frame(axis) >= 0 ) normal = Rb.col(axis);
else normal = -Rb.col(axis);
dist = - min_d - s.radius;
} }
if (inside || dist <= 0) { if (!outside || dist <= 0) {
ps = pb; ps = pb;
return true; return true;
} else { } else {
......
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