Merge pull request #13692 from amantley/leanBackSittingFix

Fix For Unnatural Hips Behaviour When Sitting In HMD

interface/src/avatar/MyAvatar.cpp

@@ -3083,7 +3083,7 @@ static glm::vec3 dampenCgMovement(glm::vec3 cgUnderHeadHandsAvatarSpace, float b
 }
 
 // computeCounterBalance returns the center of gravity in Avatar space
-glm::vec3 MyAvatar::computeCounterBalance() const {
+glm::vec3 MyAvatar::computeCounterBalance() {
     struct JointMass {
         QString name;
         float weight;
@@ -3101,7 +3101,8 @@ glm::vec3 MyAvatar::computeCounterBalance() const {
     JointMass cgLeftHandMass(QString("LeftHand"), DEFAULT_AVATAR_LEFTHAND_MASS, glm::vec3(0.0f, 0.0f, 0.0f));
     JointMass cgRightHandMass(QString("RightHand"), DEFAULT_AVATAR_RIGHTHAND_MASS, glm::vec3(0.0f, 0.0f, 0.0f));
     glm::vec3 tposeHead = DEFAULT_AVATAR_HEAD_POS;
-    glm::vec3 tposeHips = glm::vec3(0.0f, 0.0f, 0.0f);
+    glm::vec3 tposeHips = DEFAULT_AVATAR_HIPS_POS;
+    glm::vec3 tposeRightFoot = DEFAULT_AVATAR_RIGHTFOOT_POS;
 
     if (_skeletonModel->getRig().indexOfJoint(cgHeadMass.name) != -1) {
         cgHeadMass.position = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(cgHeadMass.name));
@@ -3120,6 +3121,9 @@ glm::vec3 MyAvatar::computeCounterBalance() const {
     if (_skeletonModel->getRig().indexOfJoint("Hips") != -1) {
         tposeHips = getAbsoluteDefaultJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("Hips"));
     }
+    if (_skeletonModel->getRig().indexOfJoint("RightFoot") != -1) {
+        tposeRightFoot = getAbsoluteDefaultJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("RightFoot"));
+    }
 
     // find the current center of gravity position based on head and hand moments
     glm::vec3 sumOfMoments = (cgHeadMass.weight * cgHeadMass.position) + (cgLeftHandMass.weight * cgLeftHandMass.position) + (cgRightHandMass.weight * cgRightHandMass.position);
@@ -3140,9 +3144,12 @@ glm::vec3 MyAvatar::computeCounterBalance() const {
     glm::vec3 counterBalancedCg = (1.0f / DEFAULT_AVATAR_HIPS_MASS) * counterBalancedForHead;
 
     // find the height of the hips
+    const float UPPER_LEG_FRACTION = 0.3333f;
     glm::vec3 xzDiff((cgHeadMass.position.x - counterBalancedCg.x), 0.0f, (cgHeadMass.position.z - counterBalancedCg.z));
     float headMinusHipXz = glm::length(xzDiff);
     float headHipDefault = glm::length(tposeHead - tposeHips);
+    float hipFootDefault = tposeHips.y - tposeRightFoot.y;
+    float sitSquatThreshold = tposeHips.y - (UPPER_LEG_FRACTION * hipFootDefault);
     float hipHeight = 0.0f;
     if (headHipDefault > headMinusHipXz) {
         hipHeight = sqrtf((headHipDefault * headHipDefault) - (headMinusHipXz * headMinusHipXz));
@@ -3154,6 +3161,10 @@ glm::vec3 MyAvatar::computeCounterBalance() const {
     if (counterBalancedCg.y > (tposeHips.y + 0.05f)) {
         // if the height is higher than default hips, clamp to default hips
         counterBalancedCg.y = tposeHips.y + 0.05f;
+    } else if (counterBalancedCg.y < sitSquatThreshold) {
+        //do a height reset
+        setResetMode(true);
+        _follow.activate(FollowHelper::Vertical);
     }
     return counterBalancedCg;
 }
@@ -3203,7 +3214,7 @@ static void drawBaseOfSupport(float baseOfSupportScale, float footLocal, glm::ma
 // this function finds the hips position using a center of gravity model that
 // balances the head and hands with the hips over the base of support
 // returns the rotation (-z forward) and position of the Avatar in Sensor space
-glm::mat4 MyAvatar::deriveBodyUsingCgModel() const {
+glm::mat4 MyAvatar::deriveBodyUsingCgModel() {
     glm::mat4 sensorToWorldMat = getSensorToWorldMatrix();
     glm::mat4 worldToSensorMat = glm::inverse(sensorToWorldMat);
     auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
@@ -3221,7 +3232,7 @@ glm::mat4 MyAvatar::deriveBodyUsingCgModel() const {
     }
 
     // get the new center of gravity
-    const glm::vec3 cgHipsPosition = computeCounterBalance();
+    glm::vec3 cgHipsPosition = computeCounterBalance();
 
     // find the new hips rotation using the new head-hips axis as the up axis
     glm::mat4 avatarHipsMat = computeNewHipsMatrix(glmExtractRotation(avatarHeadMat), extractTranslation(avatarHeadMat), cgHipsPosition);

interface/src/avatar/MyAvatar.h

@@ -1043,12 +1043,12 @@ public:
     // results are in sensor frame (-z forward)
     glm::mat4 deriveBodyFromHMDSensor() const;
 
-    glm::vec3 computeCounterBalance() const;
+    glm::vec3 computeCounterBalance();
 
     // derive avatar body position and orientation from using the current HMD Sensor location in relation to the previous
     // location of the base of support of the avatar.
     // results are in sensor frame (-z foward)
-    glm::mat4 deriveBodyUsingCgModel() const;
+    glm::mat4 deriveBodyUsingCgModel();
 
     /**jsdoc
      * @function MyAvatar.isUp

libraries/shared/src/AvatarConstants.h

@@ -23,10 +23,10 @@ const float DEFAULT_AVATAR_EYE_HEIGHT = DEFAULT_AVATAR_HEIGHT - DEFAULT_AVATAR_E
 const float DEFAULT_AVATAR_SUPPORT_BASE_LEFT  = -0.25f;
 const float DEFAULT_AVATAR_SUPPORT_BASE_RIGHT =  0.25f;
 const float DEFAULT_AVATAR_SUPPORT_BASE_FRONT = -0.20f;
-const float DEFAULT_AVATAR_SUPPORT_BASE_BACK  =  0.10f;
+const float DEFAULT_AVATAR_SUPPORT_BASE_BACK  =  0.12f;
 const float DEFAULT_AVATAR_LATERAL_STEPPING_THRESHOLD = 0.10f;
 const float DEFAULT_AVATAR_ANTERIOR_STEPPING_THRESHOLD = 0.04f;
-const float DEFAULT_AVATAR_POSTERIOR_STEPPING_THRESHOLD = 0.07f;
+const float DEFAULT_AVATAR_POSTERIOR_STEPPING_THRESHOLD = 0.05f;
 const float DEFAULT_AVATAR_HEAD_ANGULAR_VELOCITY_STEPPING_THRESHOLD = 0.3f;
 const float DEFAULT_AVATAR_MODE_HEIGHT_STEPPING_THRESHOLD = -0.02f;
 const float DEFAULT_HANDS_VELOCITY_DIRECTION_STEPPING_THRESHOLD = 0.4f;