capsule projection and distance tests now work for non-vertical capsules

interface/src/avatar/MyAvatar.cpp

@@ -2300,16 +2300,11 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat
 }
 
 void MyAvatar::FollowHelper::postPhysicsUpdate(MyAvatar& myAvatar) {
-
-    // get HMD position from sensor space into world space, and back into rig space
     glm::mat4 worldHMDMat = myAvatar.getSensorToWorldMatrix() * myAvatar.getHMDSensorMatrix();
-    glm::mat4 rigToWorld = createMatFromQuatAndPos(myAvatar.getRotation() * Quaternions::Y_180, myAvatar.getPosition());
-    glm::mat4 worldToRig = glm::inverse(rigToWorld);
-    glm::mat4 rigHMDMat = worldToRig * worldHMDMat;
-    glm::vec3 rigHMDPosition = extractTranslation(rigHMDMat);
-
-    // detect if the rig head position is too far from the avatar's position.
-    _isOutOfBody = !pointIsInsideCapsule(rigHMDPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS);
+    glm::vec3 worldHMDPosition = extractTranslation(worldHMDMat);
+    glm::vec3 capsuleStart = myAvatar.getPosition() + Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f);
+    glm::vec3 capsuleEnd = myAvatar.getPosition() - Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f);
+    _isOutOfBody = !pointIsInsideCapsule(worldHMDPosition, capsuleStart, capsuleEnd, TRUNCATE_IK_CAPSULE_RADIUS);
 }
 
 float MyAvatar::getAccelerationEnergy() {

interface/src/avatar/SkeletonModel.cpp

@@ -124,10 +124,12 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
 
             hmdPositionInRigSpace = extractTranslation(rigHMDMat);
 
+            glm::vec3 capsuleStart = Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f);
+            glm::vec3 capsuleEnd = -Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f);
+
             // truncate head IK target if it's out of body
             if (myAvatar->isOutOfBody()) {
-                truncatedHMDPositionInRigSpace = projectPointOntoCapsule(hmdPositionInRigSpace, TRUNCATE_IK_CAPSULE_POSITION,
-                                                                         TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS);
+                truncatedHMDPositionInRigSpace = projectPointOntoCapsule(hmdPositionInRigSpace, capsuleStart, capsuleEnd, TRUNCATE_IK_CAPSULE_RADIUS);
             } else {
                 truncatedHMDPositionInRigSpace = hmdPositionInRigSpace;
             }

libraries/shared/src/GeometryUtil.cpp

@@ -579,33 +579,37 @@ float coneSphereAngle(const glm::vec3& coneCenter, const glm::vec3& coneDirectio
     return glm::max(0.0f, theta - phi);
 }
 
-bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) {
-    glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f);
-    glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f);
-    if (point.y > top.y + capsuleRadius) {
-        return false;
-    } else if (point.y > top.y) {
-        return glm::length(point - top) < capsuleRadius;
-    } else if (point.y < bottom.y - capsuleRadius) {
-        return false;
-    } else if (point.y < bottom.y) {
-        return glm::length(point - bottom) < capsuleRadius;
+glm::vec3 nearestPointOnLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd) {
+    glm::vec3 n = segmentEnd - segmentStart;
+    float len = glm::length(n);
+    if (len < EPSILON) {
+        return segmentStart;
     } else {
-        return glm::length(glm::vec2(point.x, point.z) - glm::vec2(capsulePosition.x, capsulePosition.z)) < capsuleRadius;
+        n /= len;
+        float projection = glm::dot(point - segmentStart, n);
+        projection = glm::clamp(projection, 0.0f, len);
+        return segmentStart + n * projection;
     }
 }
 
-glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) {
-    glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f);
-    glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f);
-    if (point.y > top.y) {
-        return capsuleRadius * glm::normalize(point - top) + top;
-    } else if (point.y < bottom.y) {
-        return capsuleRadius * glm::normalize(point - bottom) + bottom;
+float distanceFromLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd) {
+    return glm::length(point - nearestPointOnLineSegment(point, segmentStart, segmentEnd));
+}
+
+bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius) {
+    return distanceFromLineSegment(point, capsuleStart, capsuleEnd) < capsuleRadius;
+}
+
+glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius) {
+    glm::vec3 nearestPoint = nearestPointOnLineSegment(point, capsuleStart, capsuleEnd);
+    glm::vec3 d = point - nearestPoint;
+    float dLen = glm::length(d);
+    if (dLen > EPSILON) {
+        return nearestPoint;  // TODO: maybe we should pick a point actually on the surface...
     } else {
-        glm::vec2 capsulePosition2D(capsulePosition.x, capsulePosition.z);
-        glm::vec2 point2D(point.x, point.z);
-        glm::vec2 projectedPoint2D = capsuleRadius * glm::normalize(point2D - capsulePosition2D) + capsulePosition2D;
-        return glm::vec3(projectedPoint2D.x, point.y, projectedPoint2D.y);
+        return nearestPoint + d * (capsuleRadius / dLen);
     }
 }
+
+
+

libraries/shared/src/GeometryUtil.h

@@ -160,8 +160,9 @@ private:
     static void copyCleanArray(int& lengthA, glm::vec2* vertexArrayA, int& lengthB, glm::vec2* vertexArrayB);
 };
 
-// vertical capsule
-bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius);
-glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius);
+glm::vec3 nearestPointOnLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd);
+float distanceFromLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd);
+bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius);
+glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius);
 
 #endif // hifi_GeometryUtil_h