Commit e93c27fc authored by Nghia Truong's avatar Nghia Truong

BUG: Fix narrow phase CD for point-sphere when the point coincides with sphere center

parent ef55f5d5
......@@ -320,12 +320,11 @@ pointToSphere(const Vec3r& point, uint32_t pointIdx, Sphere* const sphere,
const auto pc = sphereCenter - point;
const auto distSqr = pc.squaredNorm();
if (distSqr < sphereRadiusSqr && distSqr > Real(1e-12))
if (distSqr < sphereRadiusSqr)
{
const auto direction = pc / std::sqrt(distSqr);
const auto pointOnSphere = sphereCenter - sphereRadius * direction;
const auto penetrationDir = point - pointOnSphere;
const Vec3r direction = distSqr > Real(1e-12) ? pc / std::sqrt(distSqr) : Vec3r(0, 0, 0);
const Vec3r pointOnSphere = sphereCenter - sphereRadius * direction;
const Vec3r penetrationDir = point - pointOnSphere;
colData->MAColData.safeAppend({ pointIdx, penetrationDir });
}
}
......
Markdown is supported
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