Merge branch 'master' of github.com:highfidelity/hifi into infinite-scroll

interface/src/Menu.cpp

@@ -588,6 +588,10 @@ Menu::Menu() {
         });
 
     addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::FixGaze, 0, false);
+    addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::ToggleHipsFollowing, 0, false,
+        avatar.get(), SLOT(setToggleHips(bool)));
+    addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawBaseOfSupport, 0, false,
+        avatar.get(), SLOT(setEnableDebugDrawBaseOfSupport(bool)));
     addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawDefaultPose, 0, false,
         avatar.get(), SLOT(setEnableDebugDrawDefaultPose(bool)));
     addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawAnimPose, 0, false,

interface/src/Menu.h

@@ -30,6 +30,7 @@ namespace MenuOption {
     const QString AddressBar = "Show Address Bar";
     const QString Animations = "Animations...";
     const QString AnimDebugDrawAnimPose = "Debug Draw Animation";
+    const QString AnimDebugDrawBaseOfSupport = "Debug Draw Base of Support";
     const QString AnimDebugDrawDefaultPose = "Debug Draw Default Pose";
     const QString AnimDebugDrawPosition= "Debug Draw Position";
     const QString AskToResetSettings = "Ask To Reset Settings on Start";
@@ -202,6 +203,7 @@ namespace MenuOption {
     const QString ThirdPerson = "Third Person";
     const QString ThreePointCalibration = "3 Point Calibration";
     const QString ThrottleFPSIfNotFocus = "Throttle FPS If Not Focus"; // FIXME - this value duplicated in Basic2DWindowOpenGLDisplayPlugin.cpp
+    const QString ToggleHipsFollowing = "Toggle Hips Following";
     const QString ToolWindow = "Tool Window";
     const QString TransmitterDrive = "Transmitter Drive";
     const QString TurnWithHead = "Turn using Head";

interface/src/avatar/MyAvatar.cpp

@@ -52,6 +52,7 @@
 
 #include "MyHead.h"
 #include "MySkeletonModel.h"
+#include "AnimUtil.h"
 #include "Application.h"
 #include "AvatarManager.h"
 #include "AvatarActionHold.h"
@@ -422,12 +423,12 @@ void MyAvatar::update(float deltaTime) {
     }
 
 #ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE
-    glm::vec3 p = transformPoint(getSensorToWorldMatrix(), getControllerPoseInAvatarFrame(controller::Pose::HEAD) *
+    auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
-                                 glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y));
+    glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation());
-    DebugDraw::getInstance().addMarker("facing-avg", getOrientation(), p, glm::vec4(1.0f));
+    glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y));
-    p = transformPoint(getSensorToWorldMatrix(), getHMDSensorPosition() +
+    glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y));
-                       glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y));
+    DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f));
-    DebugDraw::getInstance().addMarker("facing", getOrientation(), p, glm::vec4(1.0f));
+    DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f));
 #endif
 
     if (_goToPending) {
@@ -712,7 +713,8 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
     _hmdSensorOrientation = glmExtractRotation(hmdSensorMatrix);
     auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
     if (headPose.isValid()) {
-        _headControllerFacing = getFacingDir2D(headPose.rotation);
+        glm::quat bodyOrientation = computeBodyFacingFromHead(headPose.rotation, Vectors::UNIT_Y);
+        _headControllerFacing = getFacingDir2D(bodyOrientation);
     } else {
         _headControllerFacing = glm::vec2(1.0f, 0.0f);
     }
@@ -1079,6 +1081,22 @@ float loadSetting(Settings& settings, const QString& name, float defaultValue) {
     return value;
 }
 
+void MyAvatar::setToggleHips(bool followHead) {
+    _follow.setToggleHipsFollowing(followHead);
+}
+
+void MyAvatar::FollowHelper::setToggleHipsFollowing(bool followHead) {
+    _toggleHipsFollowing = followHead;
+}
+
+bool MyAvatar::FollowHelper::getToggleHipsFollowing() const {
+    return _toggleHipsFollowing;
+}
+
+void MyAvatar::setEnableDebugDrawBaseOfSupport(bool isEnabled) {
+    _enableDebugDrawBaseOfSupport = isEnabled;
+}
+
 void MyAvatar::setEnableDebugDrawDefaultPose(bool isEnabled) {
     _enableDebugDrawDefaultPose = isEnabled;
 
@@ -1210,6 +1228,8 @@ void MyAvatar::loadData() {
     settings.endGroup();
 
     setEnableMeshVisible(Menu::getInstance()->isOptionChecked(MenuOption::MeshVisible));
+    _follow.setToggleHipsFollowing (Menu::getInstance()->isOptionChecked(MenuOption::ToggleHipsFollowing));
+    setEnableDebugDrawBaseOfSupport(Menu::getInstance()->isOptionChecked(MenuOption::AnimDebugDrawBaseOfSupport));
     setEnableDebugDrawDefaultPose(Menu::getInstance()->isOptionChecked(MenuOption::AnimDebugDrawDefaultPose));
     setEnableDebugDrawAnimPose(Menu::getInstance()->isOptionChecked(MenuOption::AnimDebugDrawAnimPose));
     setEnableDebugDrawPosition(Menu::getInstance()->isOptionChecked(MenuOption::AnimDebugDrawPosition));
@@ -2819,6 +2839,7 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
     auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
     if (headPose.isValid()) {
         headPosition = headPose.translation;
+        // AJT: TODO: can remove this Y_180
         headOrientation = headPose.rotation * Quaternions::Y_180;
     }
     const glm::quat headOrientationYawOnly = cancelOutRollAndPitch(headOrientation);
@@ -2841,6 +2862,8 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
     // eyeToNeck offset is relative full HMD orientation.
     // while neckToRoot offset is only relative to HMDs yaw.
     // Y_180 is necessary because rig is z forward and hmdOrientation is -z forward
+
+    // AJT: TODO: can remove this Y_180, if we remove the higher level one.
     glm::vec3 headToNeck = headOrientation * Quaternions::Y_180 * (localNeck - localHead);
     glm::vec3 neckToRoot = headOrientationYawOnly  * Quaternions::Y_180 * -localNeck;
 
@@ -2850,6 +2873,202 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
     return createMatFromQuatAndPos(headOrientationYawOnly, bodyPos);
 }
 
+// ease in function for dampening cg movement
+static float slope(float num) {
+    const float CURVE_CONSTANT = 1.0f;
+    float ret = 1.0f;
+    if (num > 0.0f) {
+        ret = 1.0f - (1.0f / (1.0f + CURVE_CONSTANT * num));
+    }
+    return ret;
+}
+
+// This function gives a soft clamp at the edge of the base of support
+// dampenCgMovement returns the damped cg value in Avatar space.
+// cgUnderHeadHandsAvatarSpace is also in Avatar space
+// baseOfSupportScale is based on the height of the user
+static glm::vec3 dampenCgMovement(glm::vec3 cgUnderHeadHandsAvatarSpace, float baseOfSupportScale) {
+    float distanceFromCenterZ = cgUnderHeadHandsAvatarSpace.z;
+    float distanceFromCenterX = cgUnderHeadHandsAvatarSpace.x;
+
+    // In the forward direction we need a different scale because forward is in
+    // the direction of the hip extensor joint, which means bending usually happens
+    // well before reaching the edge of the base of support.
+    const float clampFront = DEFAULT_AVATAR_SUPPORT_BASE_FRONT * DEFAULT_AVATAR_FORWARD_DAMPENING_FACTOR * baseOfSupportScale;
+    float clampBack = DEFAULT_AVATAR_SUPPORT_BASE_BACK * DEFAULT_AVATAR_LATERAL_DAMPENING_FACTOR * baseOfSupportScale;
+    float clampLeft = DEFAULT_AVATAR_SUPPORT_BASE_LEFT * DEFAULT_AVATAR_LATERAL_DAMPENING_FACTOR * baseOfSupportScale;
+    float clampRight = DEFAULT_AVATAR_SUPPORT_BASE_RIGHT * DEFAULT_AVATAR_LATERAL_DAMPENING_FACTOR * baseOfSupportScale;
+    glm::vec3 dampedCg(0.0f, 0.0f, 0.0f);
+
+    // find the damped z coord of the cg
+    if (cgUnderHeadHandsAvatarSpace.z < 0.0f) {
+        // forward displacement
+        dampedCg.z = slope(fabs(distanceFromCenterZ / clampFront)) * clampFront;
+    } else {
+        // backwards displacement
+        dampedCg.z = slope(fabs(distanceFromCenterZ / clampBack)) * clampBack;
+    }
+
+    // find the damped x coord of the cg
+    if (cgUnderHeadHandsAvatarSpace.x > 0.0f) {
+        // right of center
+        dampedCg.x = slope(fabs(distanceFromCenterX / clampRight)) * clampRight;
+    } else {
+        // left of center
+        dampedCg.x = slope(fabs(distanceFromCenterX / clampLeft)) * clampLeft;
+    }
+    return dampedCg;
+}
+
+// computeCounterBalance returns the center of gravity in Avatar space
+glm::vec3 MyAvatar::computeCounterBalance() const {
+    struct JointMass {
+        QString name;
+        float weight;
+        glm::vec3 position;
+        JointMass() {};
+        JointMass(QString n, float w, glm::vec3 p) {
+            name = n;
+            weight = w;
+            position = p;
+        }
+    };
+
+    // init the body part weights
+    JointMass cgHeadMass(QString("Head"), DEFAULT_AVATAR_HEAD_MASS, glm::vec3(0.0f, 0.0f, 0.0f));
+    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);
+
+    if (_skeletonModel->getRig().indexOfJoint(cgHeadMass.name) != -1) {
+        cgHeadMass.position = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(cgHeadMass.name));
+        tposeHead = getAbsoluteDefaultJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(cgHeadMass.name));
+    }
+    if (_skeletonModel->getRig().indexOfJoint(cgLeftHandMass.name) != -1) {
+        cgLeftHandMass.position = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(cgLeftHandMass.name));
+    } else {
+        cgLeftHandMass.position = DEFAULT_AVATAR_LEFTHAND_POS;
+    }
+    if (_skeletonModel->getRig().indexOfJoint(cgRightHandMass.name) != -1) {
+        cgRightHandMass.position = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint(cgRightHandMass.name));
+    } else {
+        cgRightHandMass.position = DEFAULT_AVATAR_RIGHTHAND_POS;
+    }
+    if (_skeletonModel->getRig().indexOfJoint("Hips") != -1) {
+        tposeHips = getAbsoluteDefaultJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("Hips"));
+    }
+
+    // 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);
+    float totalMass = cgHeadMass.weight + cgLeftHandMass.weight + cgRightHandMass.weight;
+
+    glm::vec3 currentCg = (1.0f / totalMass) * sumOfMoments;
+    currentCg.y = 0.0f;
+    // dampening the center of gravity, in effect, limits the value to the perimeter of the base of support
+    float baseScale = 1.0f;
+    if (getUserEyeHeight() > 0.0f) {
+        baseScale = getUserEyeHeight() / DEFAULT_AVATAR_EYE_HEIGHT;
+    }
+    glm::vec3 desiredCg = dampenCgMovement(currentCg, baseScale);
+
+    // compute hips position to maintain desiredCg
+    glm::vec3 counterBalancedForHead = (totalMass + DEFAULT_AVATAR_HIPS_MASS) * desiredCg;
+    counterBalancedForHead -= sumOfMoments;
+    glm::vec3 counterBalancedCg = (1.0f / DEFAULT_AVATAR_HIPS_MASS) * counterBalancedForHead;
+
+    // find the height of the hips
+    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 hipHeight = 0.0f;
+    if (headHipDefault > headMinusHipXz) {
+        hipHeight = sqrtf((headHipDefault * headHipDefault) - (headMinusHipXz * headMinusHipXz));
+    }
+    counterBalancedCg.y = (cgHeadMass.position.y - hipHeight);
+
+    // this is to be sure that the feet don't lift off the floor.
+    // add 5 centimeters to allow for going up on the toes.
+    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;
+    }
+    return counterBalancedCg;
+}
+
+// this function matches the hips rotation to the new cghips-head axis
+// headOrientation, headPosition and hipsPosition are in avatar space
+// returns the matrix of the hips in Avatar space
+static glm::mat4 computeNewHipsMatrix(glm::quat headOrientation, glm::vec3 headPosition, glm::vec3 hipsPosition) {
+
+    glm::quat bodyOrientation = computeBodyFacingFromHead(headOrientation, Vectors::UNIT_Y);
+
+    const float MIX_RATIO = 0.3f;
+    glm::quat hipsRot = safeLerp(Quaternions::IDENTITY, bodyOrientation, MIX_RATIO);
+    glm::vec3 hipsFacing = hipsRot * Vectors::UNIT_Z;
+
+    glm::vec3 spineVec = headPosition - hipsPosition;
+    glm::vec3 u, v, w;
+    generateBasisVectors(glm::normalize(spineVec), hipsFacing, u, v, w);
+    return glm::mat4(glm::vec4(w, 0.0f),
+        glm::vec4(u, 0.0f),
+        glm::vec4(v, 0.0f),
+        glm::vec4(hipsPosition, 1.0f));
+}
+
+static void drawBaseOfSupport(float baseOfSupportScale, float footLocal, glm::mat4 avatarToWorld) {
+    // scale the base of support based on user height
+    float clampFront = DEFAULT_AVATAR_SUPPORT_BASE_FRONT * baseOfSupportScale;
+    float clampBack = DEFAULT_AVATAR_SUPPORT_BASE_BACK * baseOfSupportScale;
+    float clampLeft = DEFAULT_AVATAR_SUPPORT_BASE_LEFT * baseOfSupportScale;
+    float clampRight = DEFAULT_AVATAR_SUPPORT_BASE_RIGHT * baseOfSupportScale;
+    float floor = footLocal + 0.05f;
+
+    // transform the base of support corners to world space
+    glm::vec3 frontRight = transformPoint(avatarToWorld, { clampRight, floor, clampFront });
+    glm::vec3 frontLeft = transformPoint(avatarToWorld, { clampLeft, floor, clampFront });
+    glm::vec3 backRight = transformPoint(avatarToWorld, { clampRight, floor, clampBack });
+    glm::vec3 backLeft = transformPoint(avatarToWorld, { clampLeft, floor, clampBack });
+
+    // draw the borders
+    const glm::vec4 rayColor = { 1.0f, 0.0f, 0.0f, 1.0f };
+    DebugDraw::getInstance().drawRay(backLeft, frontLeft, rayColor);
+    DebugDraw::getInstance().drawRay(backLeft, backRight, rayColor);
+    DebugDraw::getInstance().drawRay(backRight, frontRight, rayColor);
+    DebugDraw::getInstance().drawRay(frontLeft, frontRight, rayColor);
+}
+
+// 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 sensorToWorldMat = getSensorToWorldMatrix();
+    glm::mat4 worldToSensorMat = glm::inverse(sensorToWorldMat);
+    auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
+
+    glm::mat4 sensorHeadMat = createMatFromQuatAndPos(headPose.rotation * Quaternions::Y_180, headPose.translation);
+
+    // convert into avatar space
+    glm::mat4 avatarToWorldMat = getTransform().getMatrix();
+    glm::mat4 avatarHeadMat = glm::inverse(avatarToWorldMat) * sensorToWorldMat * sensorHeadMat;
+
+    if (_enableDebugDrawBaseOfSupport) {
+        float scaleBaseOfSupport = getUserEyeHeight() / DEFAULT_AVATAR_EYE_HEIGHT;
+        glm::vec3 rightFootPositionLocal = getAbsoluteJointTranslationInObjectFrame(_skeletonModel->getRig().indexOfJoint("RightFoot"));
+        drawBaseOfSupport(scaleBaseOfSupport, rightFootPositionLocal.y, avatarToWorldMat);
+    }
+
+    // get the new center of gravity
+    const 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);
+
+    // convert hips from avatar to sensor space
+    // The Y_180 is to convert from z forward to -z forward.
+    return worldToSensorMat * avatarToWorldMat * avatarHipsMat;
+}
+
 float MyAvatar::getUserHeight() const {
     return _userHeight.get();
 }
@@ -3014,9 +3233,7 @@ void MyAvatar::FollowHelper::decrementTimeRemaining(float dt) {
 bool MyAvatar::FollowHelper::shouldActivateRotation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const {
     const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); // 30 degrees
     glm::vec2 bodyFacing = getFacingDir2D(currentBodyMatrix);
-    
     return glm::dot(-myAvatar.getHeadControllerFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD;
-    
 }
 
 bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const {
@@ -3087,11 +3304,19 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat
 
     AnimPose followWorldPose(currentWorldMatrix);
 
+    glm::quat currentHipsLocal = myAvatar.getAbsoluteJointRotationInObjectFrame(myAvatar.getJointIndex("Hips"));
+    const glm::quat hipsinWorldSpace = followWorldPose.rot() * (Quaternions::Y_180 * (currentHipsLocal));
+    const glm::vec3 avatarUpWorld = glm::normalize(followWorldPose.rot()*(Vectors::UP));
+    glm::quat resultingSwingInWorld;
+    glm::quat resultingTwistInWorld;
+    swingTwistDecomposition(hipsinWorldSpace, avatarUpWorld, resultingSwingInWorld, resultingTwistInWorld);
+
     // remove scale present from sensorToWorldMatrix
     followWorldPose.scale() = glm::vec3(1.0f);
 
     if (isActive(Rotation)) {
-        followWorldPose.rot() = glmExtractRotation(desiredWorldMatrix);
+            //use the hmd reading for the hips follow
+            followWorldPose.rot() = glmExtractRotation(desiredWorldMatrix);
     }
     if (isActive(Horizontal)) {
         glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix);
@@ -3487,6 +3712,10 @@ void MyAvatar::updateHoldActions(const AnimPose& prePhysicsPose, const AnimPose&
     }
 }
 
+bool MyAvatar::isRecenteringHorizontally() const {
+    return _follow.isActive(FollowHelper::Horizontal);
+}
+
 const MyHead* MyAvatar::getMyHead() const {
     return static_cast<const MyHead*>(getHead());
 }

interface/src/avatar/MyAvatar.h

@@ -105,6 +105,9 @@ class MyAvatar : public Avatar {
      *     by 30cm. <em>Read-only.</em>
      * @property {Pose} rightHandTipPose - The pose of the right hand as determined by the hand controllers, with the position
      *     by 30cm. <em>Read-only.</em>
+     * @property {boolean} centerOfGravityModelEnabled=true - If <code>true</code> then the avatar hips are placed according to the center of
+     *     gravity model that balance the center of gravity over the base of support of the feet.  Setting the value <code>false</code> 
+     *     will result in the default behaviour where the hips are placed under the head.
      * @property {boolean} hmdLeanRecenterEnabled=true - If <code>true</code> then the avatar is re-centered to be under the 
      *     head's position. In room-scale VR, this behavior is what causes your avatar to follow your HMD as you walk around 
      *     the room. Setting the value <code>false</code> is useful if you want to pin the avatar to a fixed position.
@@ -199,6 +202,7 @@ class MyAvatar : public Avatar {
     Q_PROPERTY(float energy READ getEnergy WRITE setEnergy)
     Q_PROPERTY(bool isAway READ getIsAway WRITE setAway)
 
+    Q_PROPERTY(bool centerOfGravityModelEnabled READ getCenterOfGravityModelEnabled WRITE setCenterOfGravityModelEnabled)
     Q_PROPERTY(bool hmdLeanRecenterEnabled READ getHMDLeanRecenterEnabled WRITE setHMDLeanRecenterEnabled)
     Q_PROPERTY(bool collisionsEnabled READ getCollisionsEnabled WRITE setCollisionsEnabled)
     Q_PROPERTY(bool characterControllerEnabled READ getCharacterControllerEnabled WRITE setCharacterControllerEnabled)
@@ -480,7 +484,16 @@ public:
      */
     Q_INVOKABLE QString getDominantHand() const { return _dominantHand; }
 
-
+    /**jsdoc
+    * @function MyAvatar.setCenterOfGravityModelEnabled
+    * @param {boolean} enabled
+    */
+    Q_INVOKABLE void setCenterOfGravityModelEnabled(bool value) { _centerOfGravityModelEnabled = value; }
+    /**jsdoc
+    * @function MyAvatar.getCenterOfGravityModelEnabled
+    * @returns {boolean}
+    */
+    Q_INVOKABLE bool getCenterOfGravityModelEnabled() const { return _centerOfGravityModelEnabled; }
     /**jsdoc
      * @function MyAvatar.setHMDLeanRecenterEnabled
      * @param {boolean} enabled
@@ -564,6 +577,13 @@ public:
      */
     Q_INVOKABLE void triggerRotationRecenter();
 
+    /**jsdoc
+    *The isRecenteringHorizontally function returns true if MyAvatar
+    *is translating the root of the Avatar to keep the center of gravity under the head.
+    *isActive(Horizontal) is returned.
+    *@function MyAvatar.isRecenteringHorizontally
+    */
+    Q_INVOKABLE bool isRecenteringHorizontally() const;
 
     eyeContactTarget getEyeContactTarget();
 
@@ -956,10 +976,18 @@ public:
     void removeHoldAction(AvatarActionHold* holdAction);  // thread-safe
     void updateHoldActions(const AnimPose& prePhysicsPose, const AnimPose& postUpdatePose);
 
+
     // derive avatar body position and orientation from the current HMD Sensor location.
-    // results are in HMD frame
+    // results are in sensor frame (-z forward)
     glm::mat4 deriveBodyFromHMDSensor() const;
 
+    glm::vec3 computeCounterBalance() const;
+
+    // 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;
+
     /**jsdoc
      * @function MyAvatar.isUp
      * @param {Vec3} direction
@@ -1107,7 +1135,16 @@ public slots:
      */
     Q_INVOKABLE void updateMotionBehaviorFromMenu();
 
-
+    /**jsdoc
+    * @function MyAvatar.setToggleHips
+    * @param {boolean} enabled
+    */
+    void setToggleHips(bool followHead);
+    /**jsdoc
+    * @function MyAvatar.setEnableDebugDrawBaseOfSupport
+    * @param {boolean} enabled
+    */
+    void setEnableDebugDrawBaseOfSupport(bool isEnabled);
     /**jsdoc
      * @function MyAvatar.setEnableDebugDrawDefaultPose
      * @param {boolean} enabled
@@ -1458,8 +1495,8 @@ private:
     glm::quat _hmdSensorOrientation;
     glm::vec3 _hmdSensorPosition;
     // cache head controller pose in sensor space
-    glm::vec2 _headControllerFacing;  // facing vector in xz plane
+    glm::vec2 _headControllerFacing;  // facing vector in xz plane (sensor space)
-    glm::vec2 _headControllerFacingMovingAverage { 0, 0 };   // facing vector in xz plane
+    glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f };   // facing vector in xz plane (sensor space)
 
     // cache of the current body position and orientation of the avatar's body,
     // in sensor space.
@@ -1495,9 +1532,12 @@ private:
         void setForceActivateVertical(bool val);
         bool getForceActivateHorizontal() const;
         void setForceActivateHorizontal(bool val);
-        std::atomic<bool> _forceActivateRotation{ false };
+        bool getToggleHipsFollowing() const;
-        std::atomic<bool> _forceActivateVertical{ false };
+        void setToggleHipsFollowing(bool followHead);
-        std::atomic<bool> _forceActivateHorizontal{ false };
+        std::atomic<bool> _forceActivateRotation { false };
+        std::atomic<bool> _forceActivateVertical { false };
+        std::atomic<bool> _forceActivateHorizontal { false };
+        std::atomic<bool> _toggleHipsFollowing { true };
     };
     FollowHelper _follow;
 
@@ -1510,6 +1550,7 @@ private:
     bool _prevShouldDrawHead;
     bool _rigEnabled { true };
 
+    bool _enableDebugDrawBaseOfSupport { false };
     bool _enableDebugDrawDefaultPose { false };
     bool _enableDebugDrawAnimPose { false };
     bool _enableDebugDrawHandControllers { false };
@@ -1532,6 +1573,7 @@ private:
     std::map<controller::Action, controller::Pose> _controllerPoseMap;
     mutable std::mutex _controllerPoseMapMutex;
 
+    bool _centerOfGravityModelEnabled { true };
     bool _hmdLeanRecenterEnabled { true };
     bool _sprint { false };
 

interface/src/avatar/MySkeletonModel.cpp

@@ -45,7 +45,14 @@ static AnimPose computeHipsInSensorFrame(MyAvatar* myAvatar, bool isFlying) {
         return result;
     }
 
-    glm::mat4 hipsMat = myAvatar->deriveBodyFromHMDSensor();
+    glm::mat4 hipsMat;
+    if (myAvatar->getCenterOfGravityModelEnabled()) {
+        // then we use center of gravity model
+        hipsMat = myAvatar->deriveBodyUsingCgModel();
+    } else {
+        // otherwise use the default of putting the hips under the head
+        hipsMat = myAvatar->deriveBodyFromHMDSensor();
+    }
     glm::vec3 hipsPos = extractTranslation(hipsMat);
     glm::quat hipsRot = glmExtractRotation(hipsMat);
 
@@ -53,8 +60,11 @@ static AnimPose computeHipsInSensorFrame(MyAvatar* myAvatar, bool isFlying) {
     glm::mat4 avatarToSensorMat = worldToSensorMat * avatarToWorldMat;
 
     // dampen hips rotation, by mixing it with the avatar orientation in sensor space
-    const float MIX_RATIO = 0.5f;
+    // turning this off for center of gravity model because it is already mixed in there
-    hipsRot = safeLerp(glmExtractRotation(avatarToSensorMat), hipsRot, MIX_RATIO);
+    if (!(myAvatar->getCenterOfGravityModelEnabled())) {
+        const float MIX_RATIO = 0.5f;
+        hipsRot = safeLerp(glmExtractRotation(avatarToSensorMat), hipsRot, MIX_RATIO);
+    }
 
     if (isFlying) {
         // rotate the hips back to match the flying animation.
@@ -73,6 +83,7 @@ static AnimPose computeHipsInSensorFrame(MyAvatar* myAvatar, bool isFlying) {
         hipsPos = headPos + tiltRot * (hipsPos - headPos);
     }
 
+    // AJT: TODO can we remove this?
     return AnimPose(hipsRot * Quaternions::Y_180, hipsPos);
 }
 
@@ -170,6 +181,15 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
         }
     }
 
+    bool isFlying = (myAvatar->getCharacterController()->getState() == CharacterController::State::Hover || myAvatar->getCharacterController()->computeCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS);
+    if (isFlying != _prevIsFlying) {
+        const float FLY_TO_IDLE_HIPS_TRANSITION_TIME = 0.5f;
+        _flyIdleTimer = FLY_TO_IDLE_HIPS_TRANSITION_TIME;
+    } else {
+        _flyIdleTimer -= deltaTime;
+    }
+    _prevIsFlying = isFlying;
+
     // if hips are not under direct control, estimate the hips position.
     if (avatarHeadPose.isValid() && !(params.primaryControllerFlags[Rig::PrimaryControllerType_Hips] & (uint8_t)Rig::ControllerFlags::Enabled)) {
         bool isFlying = (myAvatar->getCharacterController()->getState() == CharacterController::State::Hover || myAvatar->getCharacterController()->computeCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS);
@@ -181,14 +201,28 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
 
         AnimPose hips = computeHipsInSensorFrame(myAvatar, isFlying);
 
+        // timescale in seconds
+        const float TRANS_HORIZ_TIMESCALE = 0.15f;
+        const float TRANS_VERT_TIMESCALE = 0.01f; // We want the vertical component of the hips to follow quickly to prevent spine squash/stretch.
+        const float ROT_TIMESCALE = 0.15f;
+        const float FLY_IDLE_TRANSITION_TIMESCALE = 0.25f;
+
+        float transHorizAlpha, transVertAlpha, rotAlpha;
+        if (_flyIdleTimer < 0.0f) {
+            transHorizAlpha = glm::min(deltaTime / TRANS_HORIZ_TIMESCALE, 1.0f);
+            transVertAlpha = glm::min(deltaTime / TRANS_VERT_TIMESCALE, 1.0f);
+            rotAlpha = glm::min(deltaTime / ROT_TIMESCALE, 1.0f);
+        } else {
+            transHorizAlpha = glm::min(deltaTime / FLY_IDLE_TRANSITION_TIMESCALE, 1.0f);
+            transVertAlpha = glm::min(deltaTime / FLY_IDLE_TRANSITION_TIMESCALE, 1.0f);
+            rotAlpha = glm::min(deltaTime / FLY_IDLE_TRANSITION_TIMESCALE, 1.0f);
+        }
+
         // smootly lerp hips, in sensorframe, with different coeff for horiz and vertical translation.
-        const float ROT_ALPHA = 0.9f;
-        const float TRANS_HORIZ_ALPHA = 0.9f;
-        const float TRANS_VERT_ALPHA = 0.1f;
         float hipsY = hips.trans().y;
-        hips.trans() = lerp(hips.trans(), _prevHips.trans(), TRANS_HORIZ_ALPHA);
+        hips.trans() = lerp(_prevHips.trans(), hips.trans(), transHorizAlpha);
-        hips.trans().y = lerp(hipsY, _prevHips.trans().y, TRANS_VERT_ALPHA);
+        hips.trans().y = lerp(_prevHips.trans().y, hipsY, transVertAlpha);
-        hips.rot() = safeLerp(hips.rot(), _prevHips.rot(), ROT_ALPHA);
+        hips.rot() = safeLerp(_prevHips.rot(), hips.rot(), rotAlpha);
 
         _prevHips = hips;
         _prevHipsValid = true;

interface/src/avatar/MySkeletonModel.h

@@ -28,6 +28,8 @@ private:
 
     AnimPose _prevHips;  // sensor frame
     bool _prevHipsValid { false };
+    bool _prevIsFlying { false };
+    float _flyIdleTimer { 0.0f };
 
     std::map<int, int> _jointRotationFrameOffsetMap;
 };

libraries/animation/src/AnimUtil.cpp

@@ -9,7 +9,9 @@
 //
 
 #include "AnimUtil.h"
-#include "GLMHelpers.h"
+#include <GLMHelpers.h>
+#include <NumericalConstants.h>
+#include <DebugDraw.h>
 
 // TODO: use restrict keyword
 // TODO: excellent candidate for simd vectorization.
@@ -107,3 +109,44 @@ AnimPose boneLookAt(const glm::vec3& target, const AnimPose& bone) {
                      glm::vec4(bone.trans(), 1.0f));
     return AnimPose(lookAt);
 }
+
+// This will attempt to determine the proper body facing of a characters body
+// assumes headRot is z-forward and y-up.
+// and returns a bodyRot that is also z-forward and y-up
+glm::quat computeBodyFacingFromHead(const glm::quat& headRot, const glm::vec3& up) {
+
+    glm::vec3 bodyUp = glm::normalize(up);
+
+    // initially take the body facing from the head.
+    glm::vec3 headUp = headRot * Vectors::UNIT_Y;
+    glm::vec3 headForward = headRot * Vectors::UNIT_Z;
+    glm::vec3 headLeft = headRot * Vectors::UNIT_X;
+    const float NOD_THRESHOLD = cosf(glm::radians(45.0f));
+    const float TILT_THRESHOLD = cosf(glm::radians(30.0f));
+
+    glm::vec3 bodyForward = headForward;
+
+    float nodDot = glm::dot(headForward, bodyUp);
+    float tiltDot = glm::dot(headLeft, bodyUp);
+
+    if (fabsf(tiltDot) < TILT_THRESHOLD) { // if we are not tilting too much
+        if (nodDot < -NOD_THRESHOLD) { // head is looking downward
+            // the body should face in the same direction as the top the head.
+            bodyForward = headUp;
+        } else if (nodDot > NOD_THRESHOLD) {  // head is looking upward
+            // the body should face away from the top of the head.
+            bodyForward = -headUp;
+        }
+    }
+
+    // cancel out upward component
+    bodyForward = glm::normalize(bodyForward - nodDot * bodyUp);
+
+    glm::vec3 u, v, w;
+    generateBasisVectors(bodyForward, bodyUp, u, v, w);
+
+    // create matrix from orthogonal basis vectors
+    glm::mat4 bodyMat(glm::vec4(w, 0.0f), glm::vec4(v, 0.0f), glm::vec4(u, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 1.0f));
+
+    return glmExtractRotation(bodyMat);
+}

libraries/animation/src/AnimUtil.h

@@ -33,4 +33,9 @@ inline glm::quat safeLerp(const glm::quat& a, const glm::quat& b, float alpha) {
 
 AnimPose boneLookAt(const glm::vec3& target, const AnimPose& bone);
 
+// This will attempt to determine the proper body facing of a characters body
+// assumes headRot is z-forward and y-up.
+// and returns a bodyRot that is also z-forward and y-up
+glm::quat computeBodyFacingFromHead(const glm::quat& headRot, const glm::vec3& up);
+
 #endif

libraries/shared/src/AvatarConstants.h

@@ -20,6 +20,16 @@ const float DEFAULT_AVATAR_EYE_TO_TOP_OF_HEAD = 0.11f; // meters
 const float DEFAULT_AVATAR_NECK_TO_TOP_OF_HEAD = 0.185f; // meters
 const float DEFAULT_AVATAR_NECK_HEIGHT = DEFAULT_AVATAR_HEIGHT - DEFAULT_AVATAR_NECK_TO_TOP_OF_HEAD;
 const float DEFAULT_AVATAR_EYE_HEIGHT = DEFAULT_AVATAR_HEIGHT - DEFAULT_AVATAR_EYE_TO_TOP_OF_HEAD;
+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_FORWARD_DAMPENING_FACTOR = 0.5f;
+const float DEFAULT_AVATAR_LATERAL_DAMPENING_FACTOR = 2.0f;
+const float DEFAULT_AVATAR_HIPS_MASS = 40.0f;
+const float DEFAULT_AVATAR_HEAD_MASS = 20.0f;
+const float DEFAULT_AVATAR_LEFTHAND_MASS = 2.0f;
+const float DEFAULT_AVATAR_RIGHTHAND_MASS = 2.0f;
 
 // Used when avatar is missing joints... (avatar space)
 const glm::quat DEFAULT_AVATAR_MIDDLE_EYE_ROT { Quaternions::Y_180 };

libraries/shared/src/GLMHelpers.cpp

@@ -574,8 +574,9 @@ void generateBasisVectors(const glm::vec3& primaryAxis, const glm::vec3& seconda
     vAxisOut = glm::cross(wAxisOut, uAxisOut);
 }
 
+// assumes z-forward and y-up
 glm::vec2 getFacingDir2D(const glm::quat& rot) {
-    glm::vec3 facing3D = rot * Vectors::UNIT_NEG_Z;
+    glm::vec3 facing3D = rot * Vectors::UNIT_Z;
     glm::vec2 facing2D(facing3D.x, facing3D.z);
     const float ALMOST_ZERO = 0.0001f;
     if (glm::length(facing2D) < ALMOST_ZERO) {
@@ -585,8 +586,9 @@ glm::vec2 getFacingDir2D(const glm::quat& rot) {
     }
 }
 
+// assumes z-forward and y-up
 glm::vec2 getFacingDir2D(const glm::mat4& m) {
-    glm::vec3 facing3D = transformVectorFast(m, Vectors::UNIT_NEG_Z);
+    glm::vec3 facing3D = transformVectorFast(m, Vectors::UNIT_Z);
     glm::vec2 facing2D(facing3D.x, facing3D.z);
     const float ALMOST_ZERO = 0.0001f;
     if (glm::length(facing2D) < ALMOST_ZERO) {

libraries/shared/src/GLMHelpers.h

@@ -250,7 +250,10 @@ glm::vec3 transformVectorFull(const glm::mat4& m, const glm::vec3& v);
 void generateBasisVectors(const glm::vec3& primaryAxis, const glm::vec3& secondaryAxis,
                           glm::vec3& uAxisOut, glm::vec3& vAxisOut, glm::vec3& wAxisOut);
 
+// assumes z-forward and y-up
 glm::vec2 getFacingDir2D(const glm::quat& rot);
+
+// assumes z-forward and y-up
 glm::vec2 getFacingDir2D(const glm::mat4& m);
 
 inline bool isNaN(const glm::vec3& value) { return isNaN(value.x) || isNaN(value.y) || isNaN(value.z); }

scripts/defaultScripts.js

@@ -33,7 +33,7 @@ var DEFAULT_SCRIPTS_COMBINED = [
     "system/emote.js"
 ];
 var DEFAULT_SCRIPTS_SEPARATE = [
-    "system/controllers/controllerScripts.js"
+    "system/controllers/controllerScripts.js",
     //"system/chat.js"
 ];