Added CriticallyDampedSpringPoseHelper to help smooth out hips.

interface/src/avatar/MySkeletonModel.cpp

@@ -36,6 +36,7 @@ Rig::CharacterControllerState convertCharacterControllerState(CharacterControlle
 static AnimPose computeHipsInSensorFrame(MyAvatar* myAvatar, bool isFlying) {
     glm::mat4 worldToSensorMat = glm::inverse(myAvatar->getSensorToWorldMatrix());
 
+
     // check for pinned hips.
     auto hipsIndex = myAvatar->getJointIndex("Hips");
     if (myAvatar->isJointPinned(hipsIndex)) {
@@ -199,49 +200,38 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
     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);
 
-        if (!_prevHipsValid) {
-            AnimPose hips = computeHipsInSensorFrame(myAvatar, isFlying);
-            _prevHips = hips;
-        }
-
-        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);
+            _smoothHipsHelper.setHorizontalTranslationTimescale(TRANS_HORIZ_TIMESCALE);
+            _smoothHipsHelper.setVerticalTranslationTimescale(TRANS_VERT_TIMESCALE);
+            _smoothHipsHelper.setRotationTimescale(ROT_TIMESCALE);
         } 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);
+            _smoothHipsHelper.setHorizontalTranslationTimescale(FLY_IDLE_TRANSITION_TIMESCALE);
+            _smoothHipsHelper.setVerticalTranslationTimescale(FLY_IDLE_TRANSITION_TIMESCALE);
+            _smoothHipsHelper.setRotationTimescale(FLY_IDLE_TRANSITION_TIMESCALE);
         }
 
-        // smootly lerp hips, in sensorframe, with different coeff for horiz and vertical translation.
-        float hipsY = hips.trans().y;
-        hips.trans() = lerp(_prevHips.trans(), hips.trans(), transHorizAlpha);
-        hips.trans().y = lerp(_prevHips.trans().y, hipsY, transVertAlpha);
-        hips.rot() = safeLerp(_prevHips.rot(), hips.rot(), rotAlpha);
-
-        _prevHips = hips;
-        _prevHipsValid = true;
+        AnimPose sensorHips = computeHipsInSensorFrame(myAvatar, isFlying);
+        if (!_prevIsEstimatingHips) {
+            _smoothHipsHelper.teleport(sensorHips);
+        }
+        sensorHips = _smoothHipsHelper.update(sensorHips, deltaTime);
 
         glm::mat4 invRigMat = glm::inverse(myAvatar->getTransform().getMatrix() * Matrices::Y_180);
         AnimPose sensorToRigPose(invRigMat * myAvatar->getSensorToWorldMatrix());
 
-        params.primaryControllerPoses[Rig::PrimaryControllerType_Hips] = sensorToRigPose * hips;
+        params.primaryControllerPoses[Rig::PrimaryControllerType_Hips] = sensorToRigPose * sensorHips;
         params.primaryControllerFlags[Rig::PrimaryControllerType_Hips] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated;
 
         // set spine2 if we have hand controllers
         if (myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid() &&
-                myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() &&
-                !(params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] & (uint8_t)Rig::ControllerFlags::Enabled)) {
+            myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() &&
+            !(params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] & (uint8_t)Rig::ControllerFlags::Enabled)) {
 
             AnimPose currentSpine2Pose;
             AnimPose currentHeadPose;
@@ -267,8 +257,9 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
             }
         }
 
+        _prevIsEstimatingHips = true;
     } else {
-        _prevHipsValid = false;
+        _prevIsEstimatingHips = false;
     }
 
     params.isTalking = head->getTimeWithoutTalking() <= 1.5f;

interface/src/avatar/MySkeletonModel.h

@@ -10,6 +10,7 @@
 #define hifi_MySkeletonModel_h
 
 #include <avatars-renderer/SkeletonModel.h>
+#include <AnimUtil.h>
 #include "MyAvatar.h"
 
 /// A skeleton loaded from a model.
@@ -26,11 +27,12 @@ public:
 private:
     void updateFingers();
 
-    AnimPose _prevHips;  // sensor frame
-    bool _prevHipsValid { false };
+    CriticallyDampedSpringPoseHelper _smoothHipsHelper;  // sensor frame
     bool _prevIsFlying { false };
     float _flyIdleTimer { 0.0f };
 
+    float _prevIsEstimatingHips { false };
+
     std::map<int, int> _jointRotationFrameOffsetMap;
 };
 

libraries/animation/src/AnimUtil.h

@@ -38,4 +38,57 @@ AnimPose boneLookAt(const glm::vec3& target, const AnimPose& bone);
 // and returns a bodyRot that is also z-forward and y-up
 glm::quat computeBodyFacingFromHead(const glm::quat& headRot, const glm::vec3& up);
 
+
+// Uses a approximation of a critically damped spring to smooth full AnimPoses.
+// It provides seperate timescales for horizontal, vertical and rotation components.
+// The timescale is roughly how much time it will take the spring will reach halfway toward it's target.
+class CriticallyDampedSpringPoseHelper {
+public:
+    CriticallyDampedSpringPoseHelper() : _prevPoseValid(false) {}
+
+    void setHorizontalTranslationTimescale(float timescale) {
+        _horizontalTranslationTimescale = timescale;
+    }
+    void setVerticalTranslationTimescale(float timescale) {
+        _verticalTranslationTimescale = timescale;
+    }
+    void setRotationTimescale(float timescale) {
+        _rotationTimescale = timescale;
+    }
+
+    AnimPose update(const AnimPose& pose, float deltaTime) {
+        if (!_prevPoseValid) {
+            _prevPose = pose;
+            _prevPoseValid = true;
+        }
+
+        const float horizontalTranslationAlpha = glm::min(deltaTime / _horizontalTranslationTimescale, 1.0f);
+        const float verticalTranslationAlpha = glm::min(deltaTime / _verticalTranslationTimescale, 1.0f);
+        const float rotationAlpha = glm::min(deltaTime / _rotationTimescale, 1.0f);
+
+        const float poseY = pose.trans().y;
+        AnimPose newPose = _prevPose;
+        newPose.trans() = lerp(_prevPose.trans(), pose.trans(), horizontalTranslationAlpha);
+        newPose.trans().y = lerp(_prevPose.trans().y, poseY, verticalTranslationAlpha);
+        newPose.rot() = safeLerp(_prevPose.rot(), pose.rot(), rotationAlpha);
+
+        _prevPose = newPose;
+        _prevPoseValid = true;
+
+        return newPose;
+    }
+
+    void teleport(const AnimPose& pose) {
+        _prevPoseValid = true;
+        _prevPose = pose;
+    }
+
+protected:
+    AnimPose _prevPose;
+    float _horizontalTranslationTimescale { 0.15f };
+    float _verticalTranslationTimescale { 0.15f };
+    float _rotationTimescale { 0.15f };
+    bool _prevPoseValid;
+};
+
 #endif

libraries/animation/src/Rig.cpp

@@ -1609,6 +1609,7 @@ glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, int kneeIndex, int up
 
 void Rig::updateFromControllerParameters(const ControllerParameters& params, float dt) {
     if (!_animSkeleton || !_animNode) {
+        _previousControllerParameters = params;
         return;
     }
 
@@ -1700,6 +1701,8 @@ void Rig::updateFromControllerParameters(const ControllerParameters& params, flo
             }
         }
     }
+
+    _previousControllerParameters = params;
 }
 
 void Rig::initAnimGraph(const QUrl& url) {

libraries/animation/src/Rig.h

@@ -394,6 +394,8 @@ protected:
 
     AnimContext _lastContext;
     AnimVariantMap _lastAnimVars;
+
+    ControllerParameters _previousControllerParameters;
 };
 
 #endif /* defined(__hifi__Rig__) */