Basic sphere/capsule-cone penetration test.

interface/src/renderer/Model.cpp

@@ -557,8 +557,8 @@ bool Model::findSpherePenetration(const glm::vec3& penetratorCenter, float penet
             start = extractTranslation(_jointStates[joint.parentIndex].transform);
             startRadius = geometry.joints[joint.parentIndex].boneRadius * radiusScale;
         }
-        if (findSphereCapsulePenetration(relativeCenter, penetratorRadius, start, end,
-                (startRadius + endRadius) / 2.0f, bonePenetration)) {
+        if (findSphereCapsuleConePenetration(relativeCenter, penetratorRadius, start, end,
+                startRadius, endRadius, bonePenetration)) {
             totalPenetration = addPenetrations(totalPenetration, bonePenetration);
             didPenetrate = true; 
         }

libraries/shared/src/GeometryUtil.cpp

@@ -52,6 +52,12 @@ bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetra
         penetratorRadius, penetration);
 }
 
+bool findPointSpherePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeCenter,
+       float penetrateeRadius, glm::vec3& penetration) {
+    return findSpherePenetration(penetrateeCenter - penetratorLocation, glm::vec3(0.0f, -1.0f, 0.0f),
+        penetrateeRadius, penetration);
+}
+
 bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
                                  const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration) {
     return findSpherePointPenetration(penetratorCenter, penetratorRadius + penetrateeRadius, penetrateeCenter, penetration);
@@ -69,6 +75,35 @@ bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penet
         penetrateeStart, penetrateeEnd, penetration);
 }
 
+bool findPointCapsuleConePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeStart,
+        const glm::vec3& penetrateeEnd, float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration) {
+    // compute the projection of the point vector onto the segment vector
+    glm::vec3 segmentVector = penetrateeEnd - penetrateeStart;
+    float lengthSquared = glm::dot(segmentVector, segmentVector);
+    if (lengthSquared < EPSILON) { // start and end the same
+        return findPointSpherePenetration(penetratorLocation, penetrateeStart,
+            glm::max(penetrateeStartRadius, penetrateeEndRadius), penetration);
+    }
+    float proj = glm::dot(penetratorLocation - penetrateeStart, segmentVector) / lengthSquared;
+    if (proj <= 0.0f) { // closest to the start
+        return findPointSpherePenetration(penetratorLocation, penetrateeStart, penetrateeStartRadius, penetration);
+        
+    } else if (proj >= 1.0f) { // closest to the end
+        return findPointSpherePenetration(penetratorLocation, penetrateeEnd, penetrateeEndRadius, penetration);
+    
+    } else { // closest to the middle
+        return findPointSpherePenetration(penetratorLocation, penetrateeStart + segmentVector * proj,
+            glm::mix(penetrateeStartRadius, penetrateeEndRadius, proj), penetration);
+    }
+}
+
+bool findSphereCapsuleConePenetration(const glm::vec3& penetratorCenter,
+        float penetratorRadius, const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd,
+        float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration) {
+    return findPointCapsuleConePenetration(penetratorCenter, penetrateeStart, penetrateeEnd,
+        penetrateeStartRadius + penetratorRadius, penetrateeEndRadius + penetratorRadius, penetration);
+}
+
 bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, 
                                 const glm::vec4& penetrateePlane, glm::vec3& penetration) {
     float distance = glm::dot(penetrateePlane, glm::vec4(penetratorCenter, 1.0f)) - penetratorRadius;

libraries/shared/src/GeometryUtil.h

@@ -19,6 +19,9 @@ bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, const glm::v
 bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
                                 const glm::vec3& penetrateeLocation, glm::vec3& penetration);
 
+bool findPointSpherePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeCenter,
+    float penetrateeRadius, glm::vec3& penetration);
+
 bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
                                  const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration);
                      
@@ -27,6 +30,13 @@ bool findSphereSegmentPenetration(const glm::vec3& penetratorCenter, float penet
 
 bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, const glm::vec3& penetrateeStart,
                                   const glm::vec3& penetrateeEnd, float penetrateeRadius, glm::vec3& penetration);
+
+bool findPointCapsuleConePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeStart,
+    const glm::vec3& penetrateeEnd, float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration);
+    
+bool findSphereCapsuleConePenetration(const glm::vec3& penetratorCenter,
+    float penetratorRadius, const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd,
+    float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration);
                                   
 bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, 
                                 const glm::vec4& penetrateePlane, glm::vec3& penetration);