Blend between non-estimated and estimated hips in Rig

libraries/animation/src/AnimUtil.h

@@ -91,4 +91,41 @@ protected:
     bool _prevPoseValid;
 };
 
+class SnapshotBlendPoseHelper {
+public:
+    SnapshotBlendPoseHelper() : _snapshotValid(false) {}
+
+    void setBlendDuration(float duration) {
+        _duration = duration;
+    }
+
+    void setSnapshot(const AnimPose& pose) {
+        _snapshotValid = true;
+        _snapshotPose = pose;
+        _timer = _duration;
+    }
+
+    AnimPose update(const AnimPose& targetPose, float deltaTime) {
+        _timer -= deltaTime;
+        if (_timer > 0.0f) {
+            float alpha = (_duration - _timer) / _duration;
+
+            // ease in expo
+            alpha = 1.0f - powf(2.0f, -10.0f * alpha);
+
+            AnimPose newPose = targetPose;
+            newPose.blend(_snapshotPose, alpha);
+            return newPose;
+        } else {
+            return targetPose;
+        }
+    }
+
+protected:
+    AnimPose _snapshotPose;
+    float _duration { 1.0f };
+    float _timer { 0.0f };
+    bool _snapshotValid { false };
+};
+
 #endif

libraries/animation/src/Rig.cpp

@@ -1620,7 +1620,9 @@ void Rig::updateFromControllerParameters(const ControllerParameters& params, flo
     bool leftHandEnabled = params.primaryControllerFlags[PrimaryControllerType_LeftHand] & (uint8_t)ControllerFlags::Enabled;
     bool rightHandEnabled = params.primaryControllerFlags[PrimaryControllerType_RightHand] & (uint8_t)ControllerFlags::Enabled;
     bool hipsEnabled = params.primaryControllerFlags[PrimaryControllerType_Hips] & (uint8_t)ControllerFlags::Enabled;
+    bool prevHipsEnabled = _previousControllerParameters.primaryControllerFlags[PrimaryControllerType_Hips] & (uint8_t)ControllerFlags::Enabled;
     bool hipsEstimated = params.primaryControllerFlags[PrimaryControllerType_Hips] & (uint8_t)ControllerFlags::Estimated;
+    bool prevHipsEstimated = _previousControllerParameters.primaryControllerFlags[PrimaryControllerType_Hips] & (uint8_t)ControllerFlags::Estimated;
     bool leftFootEnabled = params.primaryControllerFlags[PrimaryControllerType_LeftFoot] & (uint8_t)ControllerFlags::Enabled;
     bool rightFootEnabled = params.primaryControllerFlags[PrimaryControllerType_RightFoot] & (uint8_t)ControllerFlags::Enabled;
     bool spine2Enabled = params.primaryControllerFlags[PrimaryControllerType_Spine2] & (uint8_t)ControllerFlags::Enabled;
@@ -1659,9 +1661,26 @@ void Rig::updateFromControllerParameters(const ControllerParameters& params, flo
     }
 
     if (hipsEnabled) {
+
+        // Apply a bit of smoothing when the hips toggle between estimated and non-estimated poses.
+        // This should help smooth out problems with the vive tracker when the sensor is occluded.
+        if (prevHipsEnabled && hipsEstimated != prevHipsEstimated) {
+            // blend from a snapshot of the previous hips.
+            const float HIPS_BLEND_DURATION = 0.3f;
+            _hipsBlendHelper.setBlendDuration(HIPS_BLEND_DURATION);
+            _hipsBlendHelper.setSnapshot(_previousControllerParameters.primaryControllerPoses[PrimaryControllerType_Hips]);
+        } else if (!prevHipsEnabled) {
+            // we have no sensible value to blend from.
+            const float HIPS_BLEND_DURATION = 0.0f;
+            _hipsBlendHelper.setBlendDuration(HIPS_BLEND_DURATION);
+            _hipsBlendHelper.setSnapshot(params.primaryControllerPoses[PrimaryControllerType_Hips]);
+        }
+
+        AnimPose hips = _hipsBlendHelper.update(params.primaryControllerPoses[PrimaryControllerType_Hips], dt);
+
         _animVars.set("hipsType", (int)IKTarget::Type::RotationAndPosition);
-        _animVars.set("hipsPosition", params.primaryControllerPoses[PrimaryControllerType_Hips].trans());
+        _animVars.set("hipsPosition", hips.trans());
-        _animVars.set("hipsRotation", params.primaryControllerPoses[PrimaryControllerType_Hips].rot());
+        _animVars.set("hipsRotation", hips.rot());
     } else {
         _animVars.set("hipsType", (int)IKTarget::Type::Unknown);
     }

libraries/animation/src/Rig.h

@@ -24,6 +24,7 @@
 #include "AnimNode.h"
 #include "AnimNodeLoader.h"
 #include "SimpleMovingAverage.h"
+#include "AnimUtil.h"
 
 class Rig;
 class AnimInverseKinematics;
@@ -395,6 +396,7 @@ protected:
     AnimContext _lastContext;
     AnimVariantMap _lastAnimVars;
 
+    SnapshotBlendPoseHelper _hipsBlendHelper;
     ControllerParameters _previousControllerParameters;
 };