From c5599623f7913905c11f5d9d1f2804dff84a4954 Mon Sep 17 00:00:00 2001
From: amantley <amantley@googlemail.com>
Date: Wed, 25 Jul 2018 10:29:29 -0700
Subject: [PATCH] added a reset when the hips go below 1/3 of the default
 avatar height of the hips also reset the vertical this prevents real life
 sitting

---
 interface/src/avatar/MyAvatar.cpp | 18 +++++++++++++++---
 interface/src/avatar/MyAvatar.h   |  4 ++--
 2 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index bf7276d5d5..1dc4e82acc 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -3042,7 +3042,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;
@@ -3061,6 +3061,7 @@ glm::vec3 MyAvatar::computeCounterBalance() const {
     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 tposeRightFoot = glm::vec3(0.0f, -1.0f, 0.0f);
 
     if (_skeletonModel->getRig().indexOfJoint(cgHeadMass.name) != -1) {
         cgHeadMass.position = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(cgHeadMass.name));
@@ -3079,6 +3080,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);
@@ -3102,6 +3106,8 @@ glm::vec3 MyAvatar::computeCounterBalance() const {
     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 = glm::length(tposeHips - tposeRightFoot);
+    float sitSquatThreshold = tposeHips.y - (0.333f)*hipFootDefault;
     float hipHeight = 0.0f;
     if (headHipDefault > headMinusHipXz) {
         hipHeight = sqrtf((headHipDefault * headHipDefault) - (headMinusHipXz * headMinusHipXz));
@@ -3113,6 +3119,12 @@ 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);
+        qCDebug(interfaceapp) << "doing a reset for sitting";
+
     }
     return counterBalancedCg;
 }
@@ -3162,7 +3174,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);
@@ -3180,7 +3192,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);
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index add00962da..da16759a5e 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -1017,12 +1017,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