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

Fix boxSphereDistance

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