Moved outOfBody detection into MyAvatar::FollowHelper

in preparation for different follow behavior for in-body and out-of-body.

interface/src/avatar/MyAvatar.cpp

@@ -82,8 +82,14 @@ const float MyAvatar::ZOOM_MIN = 0.5f;
 const float MyAvatar::ZOOM_MAX = 25.0f;
 const float MyAvatar::ZOOM_DEFAULT = 1.5f;
 
+// OUTOFBODY_HACK defined in Rig.cpp
 extern bool OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS;
 
+// OUTOFBODY_HACK defined in SkeletonModel.cpp
+extern const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION;
+extern const float TRUNCATE_IK_CAPSULE_LENGTH;
+extern const float TRUNCATE_IK_CAPSULE_RADIUS;
+
 MyAvatar::MyAvatar(RigPointer rig) :
     Avatar(rig),
     _wasPushing(false),
@@ -1338,6 +1344,8 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
     } else {
         setVelocity(getVelocity() + _characterController.getFollowVelocity());
     }
+
+    _follow.postPhysicsUpdate(*this);
 }
 
 QString MyAvatar::getScriptedMotorFrame() const {
@@ -2183,6 +2191,19 @@ 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);
+}
+
 float MyAvatar::getAccelerationEnergy() {
     glm::vec3 velocity = getVelocity();
     int changeInVelocity = abs(velocity.length() - priorVelocity.length());

interface/src/avatar/MyAvatar.h

@@ -315,6 +315,8 @@ public slots:
     glm::vec3 getPositionForAudio();
     glm::quat getOrientationForAudio();
 
+    bool isOutOfBody() const { return _follow._isOutOfBody; }
+
 signals:
     void audioListenerModeChanged();
     void transformChanged();
@@ -445,7 +447,8 @@ private:
             NumFollowTypes
         };
         glm::mat4 _desiredBodyMatrix;
-        uint8_t _activeBits;
+        uint8_t _activeBits { 0 };
+        bool _isOutOfBody { false };
 
         void deactivate();
         void deactivate(FollowType type);
@@ -457,6 +460,7 @@ private:
         void updateHorizontalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix);
         void updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix);
         void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix);
+        void postPhysicsUpdate(MyAvatar& myAvatar);
     };
     FollowHelper _follow;
 

interface/src/avatar/SkeletonModel.cpp

@@ -23,6 +23,10 @@
 #include "InterfaceLogging.h"
 #include "AnimDebugDraw.h"
 
+const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f);
+const float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f;
+const float TRUNCATE_IK_CAPSULE_RADIUS = 0.5f;
+
 SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent, RigPointer rig) :
     Model(rig, parent),
     _owningAvatar(owningAvatar),
@@ -86,7 +90,6 @@ Rig::CharacterControllerState convertCharacterControllerState(CharacterControlle
     };
 }
 
-
 // Called within Model::simulate call, below.
 void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
     const FBXGeometry& geometry = getFBXGeometry();
@@ -107,6 +110,9 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
 
         Rig::HeadParameters headParams;
 
+        glm::vec3 hmdPositionInRigSpace;
+        glm::vec3 truncatedHMDPositionInRigSpace;
+
         if (qApp->isHMDMode()) {
             headParams.isInHMD = true;
 
@@ -116,9 +122,20 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
             glm::mat4 worldToRig = glm::inverse(rigToWorld);
             glm::mat4 rigHMDMat = worldToRig * worldHMDMat;
 
-            headParams.rigHeadPosition = extractTranslation(rigHMDMat);
+            hmdPositionInRigSpace = extractTranslation(rigHMDMat);
+
+            // 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);
+            } else {
+                truncatedHMDPositionInRigSpace = hmdPositionInRigSpace;
+            }
+
+            headParams.rigHeadPosition = truncatedHMDPositionInRigSpace;
             headParams.rigHeadOrientation = extractRotation(rigHMDMat);
             headParams.worldHeadOrientation = extractRotation(worldHMDMat);
+
         } else {
             headParams.isInHMD = false;
 
@@ -139,6 +156,12 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
             handParams.isLeftEnabled = true;
             handParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation();
             handParams.leftOrientation = Quaternions::Y_180 * leftPose.getRotation();
+
+            // truncate hand target
+            if (myAvatar->isOutOfBody() && qApp->isHMDMode()) {
+                glm::vec3 offset = handParams.leftPosition - hmdPositionInRigSpace;
+                handParams.leftPosition = truncatedHMDPositionInRigSpace + offset;
+            }
         } else {
             handParams.isLeftEnabled = false;
         }
@@ -148,6 +171,12 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
             handParams.isRightEnabled = true;
             handParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation();
             handParams.rightOrientation = Quaternions::Y_180 * rightPose.getRotation();
+
+            // truncate hand target
+            if (myAvatar->isOutOfBody() && qApp->isHMDMode()) {
+                glm::vec3 offset = handParams.rightPosition - hmdPositionInRigSpace;
+                handParams.rightPosition = truncatedHMDPositionInRigSpace + offset;
+            }
         } else {
             handParams.isRightEnabled = false;
         }

libraries/animation/src/Rig.cpp

@@ -47,10 +47,6 @@ const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 0.9f, 0.0f);
 const glm::vec3 DEFAULT_HEAD_POS(0.0f, 0.75f, 0.0f);
 const glm::vec3 DEFAULT_NECK_POS(0.0f, 0.70f, 0.0f);
 
-const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f);
-float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0;
-float TRUNCATE_IK_CAPSULE_RADIUS = 0.5;
-
 extern Rig* OUTOFBODY_HACK_RIG_POINTER;
 
 void Rig::overrideAnimation(const QString& url, float fps, bool loop, float firstFrame, float lastFrame) {
@@ -1003,37 +999,6 @@ void Rig::computeHeadNeckAnimVars(const AnimPose& hmdPose, glm::vec3& headPositi
     neckOrientationOut = safeMix(hmdOrientation, _animSkeleton->getRelativeDefaultPose(neckIndex).rot, 0.5f);
 }
 
-static 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;
-    } else {
-        return glm::length(glm::vec2(point.x, point.z) - glm::vec2(capsulePosition.x, capsulePosition.z)) < capsuleRadius;
-    }
-}
-
-static 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;
-    } 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);
-    }
-}
-
 void Rig::updateNeckJoint(int index, const HeadParameters& params) {
     if (_animSkeleton && index >= 0 && index < _animSkeleton->getNumJoints()) {
         glm::quat yFlip180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f));
@@ -1044,20 +1009,6 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) {
             AnimPose hmdPose(glm::vec3(1.0f), params.rigHeadOrientation * yFlip180, params.rigHeadPosition);
             computeHeadNeckAnimVars(hmdPose, headPos, headRot, neckPos, neckRot);
 
-            // decide if we SHOULD truncate IK targets
-            if (!pointIsInsideCapsule(params.rigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS)) {
-                _desiredRigHeadPosition = headPos;
-                _truncateIKTargets = true;
-            } else {
-                _truncateIKTargets = false;
-            }
-
-            // truncate head IK target.
-            if (_truncateIKTargets) {
-                headPos = projectPointOntoCapsule(_desiredRigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS);
-                neckPos = (neckPos - _desiredRigHeadPosition) + headPos;
-            }
-
             _animVars.set("headPosition", headPos);
             _animVars.set("headRotation", headRot);
             _animVars.set("headType", (int)IKTarget::Type::HmdHead);
@@ -1132,13 +1083,6 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
 
             glm::vec3 handPosition = params.leftPosition;
 
-            // truncate hand IK target
-            if (_truncateIKTargets) {
-                glm::vec3 offset = handPosition - _desiredRigHeadPosition;
-                glm::vec3 headPos = projectPointOntoCapsule(_desiredRigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS);
-                handPosition = headPos + offset;
-            }
-
             // prevent the hand IK targets from intersecting the body capsule
             glm::vec3 displacement(glm::vec3::_null);
             if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
@@ -1158,13 +1102,6 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
 
             glm::vec3 handPosition = params.rightPosition;
 
-            // truncate hand IK target
-            if (_truncateIKTargets) {
-                glm::vec3 offset = handPosition - _desiredRigHeadPosition;
-                glm::vec3 headPos = projectPointOntoCapsule(_desiredRigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS);
-                handPosition = headPos + offset;
-            }
-
             // prevent the hand IK targets from intersecting the body capsule
             glm::vec3 displacement(glm::vec3::_null);
             if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {

libraries/shared/src/GeometryUtil.cpp

@@ -578,3 +578,34 @@ 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;
+    } else {
+        return glm::length(glm::vec2(point.x, point.z) - glm::vec2(capsulePosition.x, capsulePosition.z)) < capsuleRadius;
+    }
+}
+
+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;
+    } 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);
+    }
+}

libraries/shared/src/GeometryUtil.h

@@ -160,5 +160,8 @@ 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);
 
 #endif // hifi_GeometryUtil_h