Skip to content

Commit 2d6e2fc

Browse files
committed
Box-Sphere: take swept-sphere radius into account
1 parent eb0b025 commit 2d6e2fc

File tree

1 file changed

+10
-0
lines changed

1 file changed

+10
-0
lines changed

src/narrowphase/details.h

+10
Original file line numberDiff line numberDiff line change
@@ -594,6 +594,16 @@ inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb,
594594
// project point pb onto the box's surface
595595
pb = ps - dist * normal;
596596
}
597+
598+
// Take swept-sphere radius into account
599+
const FCL_REAL ssrb = b.getSweptSphereRadius();
600+
const FCL_REAL ssrs = s.getSweptSphereRadius();
601+
if (ssrb > 0 || ssrs > 0) {
602+
pb += ssrb * normal;
603+
ps -= ssrs * normal;
604+
dist -= (ssrb + ssrs);
605+
}
606+
597607
return dist;
598608
}
599609

0 commit comments

Comments
 (0)