Merge pull request #3398 from AndrewMeadows/inertia

fix velocity drift of MyAvatar

interface/src/avatar/Avatar.cpp

@@ -53,8 +53,8 @@ Avatar::Avatar() :
     AvatarData(),
     _skeletonModel(this),
     _bodyYawDelta(0.0f),
-    _lastPosition(_position),
     _velocity(0.0f),
+    _positionDeltaAccumulator(0.0f),
     _lastVelocity(0.0f),
     _acceleration(0.0f),
     _angularVelocity(0.0f),
@@ -208,16 +208,30 @@ void Avatar::simulate(float deltaTime) {
     }
 
     // NOTE: we shouldn't extrapolate an Avatar instance forward in time... 
-    // until velocity is in AvatarData update message.
+    // until velocity is included in AvatarData update message.
     //_position += _velocity * deltaTime;
     measureMotionDerivatives(deltaTime);
 }
 
+void Avatar::slamPosition(const glm::vec3& newPosition) { 
+    AvatarData::setPosition(newPosition);
+    _positionDeltaAccumulator = glm::vec3(0.0f);
+    _velocity = glm::vec3(0.0f);
+    _lastVelocity = glm::vec3(0.0f); 
+}
+
+void Avatar::applyPositionDelta(const glm::vec3& delta) {
+    _position += delta;
+    _positionDeltaAccumulator += delta;
+}
+
 void Avatar::measureMotionDerivatives(float deltaTime) {
     // linear
     float invDeltaTime = 1.0f / deltaTime;
-    _velocity = (_position - _lastPosition) * invDeltaTime;
-    _lastPosition = _position;
+    // Floating point error prevents us from computing velocity in a naive way
+    // (e.g. vel = (pos - oldPos) / dt) so instead we use _positionOffsetAccumulator.
+    _velocity = _positionDeltaAccumulator * invDeltaTime;
+    _positionDeltaAccumulator = glm::vec3(0.0f);
     _acceleration = (_velocity - _lastVelocity) * invDeltaTime;
     _lastVelocity = _velocity;
     // angular

interface/src/avatar/Avatar.h

@@ -161,6 +161,13 @@ public:
     /// \param vector position to be scaled. Will store the result
     void scaleVectorRelativeToPosition(glm::vec3 &positionToScale) const;
 
+    void slamPosition(const glm::vec3& position);
+
+    // Call this when updating Avatar position with a delta.  This will allow us to 
+    // _accurately_ measure position changes and compute the resulting velocity 
+    // (otherwise floating point error will cause problems at large positions).
+    void applyPositionDelta(const glm::vec3& delta);
+
 public slots:
     void updateCollisionGroups();
     
@@ -173,12 +180,15 @@ protected:
     QVector<Model*> _attachmentModels;
     float _bodyYawDelta;
 
+    glm::vec3 _velocity;
+
     // These position histories and derivatives are in the world-frame.
     // The derivatives are the MEASURED results of all external and internal forces
     // and are therefor READ-ONLY --> motion control of the Avatar is NOT obtained 
     // by setting these values.
-    glm::vec3 _lastPosition;
-    glm::vec3 _velocity;
+    // Floating point error prevents us from accurately measuring velocity using a naive approach 
+    // (e.g. vel = (pos - lastPos)/dt) so instead we use _positionDeltaAccumulator.
+    glm::vec3 _positionDeltaAccumulator;
     glm::vec3 _lastVelocity;
     glm::vec3 _acceleration;
     glm::vec3 _angularVelocity;

interface/src/avatar/MyAvatar.cpp

@@ -227,7 +227,7 @@ void MyAvatar::simulate(float deltaTime) {
             const float MAX_RAGDOLL_DISPLACEMENT_2 = 1.0f;
             float length2 = glm::length2(ragdollDisplacement);
             if (length2 > EPSILON && length2 < MAX_RAGDOLL_DISPLACEMENT_2) {
-                setPosition(getPosition() + ragdollDisplacement);
+                applyPositionDelta(ragdollDisplacement);
             }
         } else {
             _skeletonModel.moveShapesTowardJoints(1.0f);
@@ -609,13 +609,6 @@ void MyAvatar::setGravity(const glm::vec3& gravity) {
     // so it continues to point opposite to the previous gravity setting.
 }
 
-void MyAvatar::slamPosition(const glm::vec3& newPosition) {
-    AvatarData::setPosition(newPosition);
-    _lastPosition = _position;
-    _velocity = glm::vec3(0.0f);
-    _lastVelocity = glm::vec3(0.0f); 
-}
-
 AnimationHandlePointer MyAvatar::addAnimationHandle() {
     AnimationHandlePointer handle = _skeletonModel.createAnimationHandle();
     _animationHandles.append(handle);
@@ -1275,15 +1268,11 @@ void MyAvatar::updatePosition(float deltaTime) {
         if (_motionBehaviors & AVATAR_MOTION_MOTOR_ENABLED) {
             glm::vec3 targetVelocity = _motorVelocity;
             if (_motionBehaviors & AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME) {
-                // rotate _motorVelocity into world frame
+                // rotate targetVelocity into world frame
                 glm::quat rotation = getHead()->getCameraOrientation();
                 targetVelocity = rotation * _motorVelocity;
             }
     
-            glm::vec3 targetDirection(0.0f);
-            if (glm::length2(targetVelocity) > EPSILON) {
-                targetDirection = glm::normalize(targetVelocity);
-            }
             glm::vec3 deltaVelocity = targetVelocity - velocity;
         
             if (_motionBehaviors & AVATAR_MOTION_MOTOR_COLLISION_SURFACE_ONLY && glm::length2(_gravity) > EPSILON) {
@@ -1315,7 +1304,7 @@ void MyAvatar::updatePosition(float deltaTime) {
         // update position
         const float MIN_AVATAR_SPEED = 0.075f;
         if (speed > MIN_AVATAR_SPEED) {
-            _position += velocity * deltaTime;
+            applyPositionDelta(deltaTime * velocity);
         }
     }
 

interface/src/avatar/MyAvatar.h

@@ -57,7 +57,6 @@ public:
     void setLeanScale(float scale) { _leanScale = scale; }
     void setLocalGravity(glm::vec3 gravity);
     void setShouldRenderLocally(bool shouldRender) { _shouldRender = shouldRender; }
-    void slamPosition(const glm::vec3& position);
 
     // getters
     float getLeanScale() const { return _leanScale; }