diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp index ba044c9451..c7d69cff7a 100644 --- a/interface/src/avatar/Avatar.cpp +++ b/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 diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h index b055e83491..cf51ffb8b8 100755 --- a/interface/src/avatar/Avatar.h +++ b/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 _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; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 9a040b7328..282ee512de 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/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); } } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 35af0faee3..f80689249d 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/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; }