diff --git a/Source/CollisionDetection/CollisionDetection/imstkNarrowPhaseCD.cpp b/Source/CollisionDetection/CollisionDetection/imstkNarrowPhaseCD.cpp
index d6fe6dcea7feaf8d93ba91602ea39a5cc0f0ddd7..7a19771847f7d0a901db57063cbbc9c4d94a5d4a 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkNarrowPhaseCD.cpp
+++ b/Source/CollisionDetection/CollisionDetection/imstkNarrowPhaseCD.cpp
@@ -254,9 +254,9 @@ pointToCapsule(const Vec3r& point, uint32_t pointIdx, Capsule* const capsule,
 
     // Get position of end points of the capsule
     // TODO: Fix this issue of extra computation in future
-    const Vec3d p0     = capsulePos;
-    const Vec3d p1     = p0 + capsule->getOrientationAxis() * length;
-    const Vec3d mid    = 0.5 * (p0 + p1);
+    const Vec3d mid    = capsulePos;
+    const Vec3d p1     = mid + 0.5 * capsule->getOrientationAxis() * length;
+    const Vec3d p0       = 2 * mid - p1;
     const Vec3d p      = p1 - p0;
     const auto  pDotp  = p.dot(p);
     const auto  pDotp0 = p.dot(p0);