Merge pull request #6269 from AndrewMeadows/avatar-velocity

proper avatar velocity for HMD motion

interface/src/avatar/MyAvatar.cpp

@@ -157,6 +157,8 @@ void MyAvatar::reset(bool andReload) {
     // Reset dynamic state.
     _wasPushing = _isPushing = _isBraking = _billboardValid = false;
     _isFollowingHMD = false;
+    _hmdFollowVelocity = Vectors::ZERO;
+    _hmdFollowSpeed = 0.0f;
     _skeletonModel.reset();
     getHead()->reset();
     _targetVelocity = glm::vec3(0.0f);
@@ -332,25 +334,6 @@ glm::mat4 MyAvatar::getSensorToWorldMatrix() const {
     return _sensorToWorldMatrix;
 }
 
-// returns true if pos is OUTSIDE of the vertical capsule
-// where the middle cylinder length is defined by capsuleLen and the radius by capsuleRad.
-static bool pointIsOutsideCapsule(const glm::vec3& pos, float capsuleLen, float capsuleRad) {
-    const float halfCapsuleLen = capsuleLen / 2.0f;
-    if (fabs(pos.y) <= halfCapsuleLen) {
-        // cylinder check for middle capsule
-        glm::vec2 horizPos(pos.x, pos.z);
-        return glm::length(horizPos) > capsuleRad;
-    } else if (pos.y > halfCapsuleLen) {
-        glm::vec3 center(0.0f, halfCapsuleLen, 0.0f);
-        return glm::length(center - pos) > capsuleRad;
-    } else if (pos.y < halfCapsuleLen) {
-        glm::vec3 center(0.0f, -halfCapsuleLen, 0.0f);
-        return glm::length(center - pos) > capsuleRad;
-    } else {
-        return false;
-    }
-}
-
 // Pass a recent sample of the HMD to the avatar.
 // This can also update the avatar's position to follow the HMD
 // as it moves through the world.
@@ -359,101 +342,58 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
     _hmdSensorMatrix = hmdSensorMatrix;
     _hmdSensorPosition = extractTranslation(hmdSensorMatrix);
     _hmdSensorOrientation = glm::quat_cast(hmdSensorMatrix);
+}
 
-    // calc deltaTime
+void MyAvatar::updateHMDFollowVelocity() {
-    auto now = usecTimestampNow();
-    auto deltaUsecs = now - _lastUpdateFromHMDTime;
-    _lastUpdateFromHMDTime = now;
-    double actualDeltaTime = (double)deltaUsecs / (double)USECS_PER_SECOND;
-    const float BIGGEST_DELTA_TIME_SECS = 0.25f;
-    float deltaTime = glm::clamp((float)actualDeltaTime, 0.0f, BIGGEST_DELTA_TIME_SECS);
-
-    bool hmdIsAtRest = _hmdAtRestDetector.update(deltaTime, _hmdSensorPosition, _hmdSensorOrientation);
-
-    // It can be more accurate/smooth to use velocity rather than position,
-    // but some modes (e.g., hmd standing) update position without updating velocity.
-    // So, let's create our own workingVelocity from the worldPosition...
-    glm::vec3 positionDelta = getPosition() - _lastPosition;
-    glm::vec3 workingVelocity = positionDelta / deltaTime;
-    _lastPosition = getPosition();
-
-    const float MOVE_ENTER_SPEED_THRESHOLD = 0.2f; // m/sec
-    const float MOVE_EXIT_SPEED_THRESHOLD = 0.07f;  // m/sec
     bool isMoving;
     if (_lastIsMoving) {
-        isMoving = glm::length(workingVelocity) >= MOVE_EXIT_SPEED_THRESHOLD;
+        const float MOVE_EXIT_SPEED_THRESHOLD = 0.07f;  // m/sec
+        isMoving = glm::length(_velocity) >= MOVE_EXIT_SPEED_THRESHOLD;
     } else {
-        isMoving = glm::length(workingVelocity) > MOVE_ENTER_SPEED_THRESHOLD;
+        const float MOVE_ENTER_SPEED_THRESHOLD = 0.2f; // m/sec
+        isMoving = glm::length(_velocity) > MOVE_ENTER_SPEED_THRESHOLD;
     }
 
     bool justStartedMoving = (_lastIsMoving != isMoving) && isMoving;
     _lastIsMoving = isMoving;
 
-    if (shouldFollowHMD() || hmdIsAtRest || justStartedMoving) {
+    bool hmdIsAtRest = _hmdAtRestDetector.update(_hmdSensorPosition, _hmdSensorOrientation);
-        beginFollowingHMD();
+    if (hmdIsAtRest || justStartedMoving) {
-    }
-
-    followHMD(deltaTime);
-}
-
-glm::vec3 MyAvatar::getHMDCorrectionVelocity() const {
-    // TODO: impelement this
-    return Vectors::ZERO;
-}
-
-void MyAvatar::beginFollowingHMD() {
-    // begin homing toward derived body position.
-    if (!_isFollowingHMD) {
         _isFollowingHMD = true;
-        _followHMDAlpha = 0.0f;
-    }
     }
 
-bool MyAvatar::shouldFollowHMD() const {
+    // compute offset to body's target position (in sensor-frame)
-    if (!_isFollowingHMD) {
+    auto sensorBodyMatrix = deriveBodyFromHMDSensor();
-        // define a vertical capsule
+    _hmdFollowOffset = extractTranslation(sensorBodyMatrix) - extractTranslation(_bodySensorMatrix);
-        const float FOLLOW_HMD_CAPSULE_RADIUS = 0.2f;  // meters
+    glm::vec3 truncatedOffset = _hmdFollowOffset;
-        const float FOLLOW_HMD_CAPSULE_LENGTH = 0.05f;  // length of the cylinder part of the capsule in meters.
+    if (truncatedOffset.y < 0.0f) {
-    
+        // don't pull the body DOWN to match the target (allow animation system to squat)
-        // detect if the derived body position is outside of a capsule around the _bodySensorMatrix
+        truncatedOffset.y = 0.0f;
-        auto newBodySensorMatrix = deriveBodyFromHMDSensor();
-        glm::vec3 localPoint = extractTranslation(newBodySensorMatrix) - extractTranslation(_bodySensorMatrix);
-        return pointIsOutsideCapsule(localPoint, FOLLOW_HMD_CAPSULE_LENGTH, FOLLOW_HMD_CAPSULE_RADIUS);
-    }
-    return false;
     }
 
-void MyAvatar::followHMD(float deltaTime) {
+    bool needNewFollowSpeed = (_isFollowingHMD && _hmdFollowSpeed == 0.0f);
+    if (!needNewFollowSpeed) {
+        // check to see if offset has exceeded its threshold
+        float distance = glm::length(truncatedOffset);
+        const float MAX_HMD_HIP_SHIFT = 0.2f;
+        if (distance > MAX_HMD_HIP_SHIFT) {
+            _isFollowingHMD = true;
+            needNewFollowSpeed = true;
+        }
+    }
     if (_isFollowingHMD) {
-
+        // only bother to rotate into world frame if we're following
-        const float FOLLOW_HMD_DURATION = 0.5f;  // seconds
+        glm::quat sensorToWorldRotation = extractRotation(_sensorToWorldMatrix);
-
+        _hmdFollowOffset = sensorToWorldRotation * _hmdFollowOffset;
-        auto newBodySensorMatrix = deriveBodyFromHMDSensor();
-        auto worldBodyMatrix = _sensorToWorldMatrix * newBodySensorMatrix;
-        glm::vec3 worldBodyPos = extractTranslation(worldBodyMatrix);
-        glm::quat worldBodyRot = glm::normalize(glm::quat_cast(worldBodyMatrix));
-
-        _followHMDAlpha += (1.0f / FOLLOW_HMD_DURATION) * deltaTime;
-
-        if (_followHMDAlpha >= 1.0f) {
-            _isFollowingHMD = false;
-            nextAttitude(worldBodyPos, worldBodyRot);
-            _bodySensorMatrix = newBodySensorMatrix;
-        } else {
-            // interp position toward the desired pos
-            glm::vec3 pos = lerp(getPosition(), worldBodyPos, _followHMDAlpha);
-            glm::quat rot = glm::normalize(safeMix(getOrientation(), worldBodyRot, _followHMDAlpha));
-            nextAttitude(pos, rot);
-
-            // interp sensor matrix toward desired
-            glm::vec3 nextBodyPos = extractTranslation(newBodySensorMatrix);
-            glm::quat nextBodyRot = glm::normalize(glm::quat_cast(newBodySensorMatrix));
-            glm::vec3 prevBodyPos = extractTranslation(_bodySensorMatrix);
-            glm::quat prevBodyRot = glm::normalize(glm::quat_cast(_bodySensorMatrix));
-            pos = lerp(prevBodyPos, nextBodyPos, _followHMDAlpha);
-            rot = glm::normalize(safeMix(prevBodyRot, nextBodyRot, _followHMDAlpha));
-            _bodySensorMatrix = createMatFromQuatAndPos(rot, pos);
     }
+    if (needNewFollowSpeed) {
+        // compute new velocity that will be used to resolve offset of hips from body
+        const float FOLLOW_HMD_DURATION = 0.5f;  // seconds
+        _hmdFollowVelocity = (_hmdFollowOffset / FOLLOW_HMD_DURATION);
+        _hmdFollowSpeed = glm::length(_hmdFollowVelocity);
+    } else if (_isFollowingHMD) {
+        // compute new velocity (but not new speed)
+        _hmdFollowVelocity = _hmdFollowSpeed * glm::normalize(_hmdFollowOffset);
     }
 }
 
@@ -1356,7 +1296,13 @@ void MyAvatar::prepareForPhysicsSimulation() {
     relayDriveKeysToCharacterController();
     _characterController.setTargetVelocity(getTargetVelocity());
     _characterController.setAvatarPositionAndOrientation(getPosition(), getOrientation());
-    _characterController.setHMDVelocity(getHMDCorrectionVelocity());
+    if (qApp->isHMDMode()) {
+        updateHMDFollowVelocity();
+        _characterController.setHMDVelocity(_hmdFollowVelocity);
+    } else {
+        _characterController.setHMDVelocity(Vectors::ZERO);
+        _isFollowingHMD = false;
+    }
 }
 
 void MyAvatar::harvestResultsFromPhysicsSimulation() {
@@ -1364,9 +1310,54 @@ void MyAvatar::harvestResultsFromPhysicsSimulation() {
     glm::quat orientation = getOrientation();
     _characterController.getAvatarPositionAndOrientation(position, orientation);
     nextAttitude(position, orientation);
+    if (_isFollowingHMD) {
+        setVelocity(_characterController.getLinearVelocity() + _hmdFollowVelocity);
+        glm::vec3 hmdShift = _characterController.getHMDShift();
+        adjustSensorTransform(hmdShift);
+    } else {
         setVelocity(_characterController.getLinearVelocity());
-    // TODO: harvest HMD shift here
+    }
-    //glm::vec3 hmdShift = _characterController.getHMDShift();
+}
+
+void MyAvatar::adjustSensorTransform(glm::vec3 hmdShift) {
+    // compute blendFactor of latest hmdShift
+    // which we'll use to blend the rotation part
+    float blendFactor = 1.0f;
+    float shiftLength = glm::length(hmdShift);
+    if (shiftLength > 1.0e-5f) {
+        float offsetLength = glm::length(_hmdFollowOffset);
+        if (offsetLength > shiftLength) {
+            blendFactor = shiftLength / offsetLength;
+        }
+    }
+
+    auto newBodySensorMatrix = deriveBodyFromHMDSensor();
+    auto worldBodyMatrix = _sensorToWorldMatrix * newBodySensorMatrix;
+    glm::quat finalBodyRotation = glm::normalize(glm::quat_cast(worldBodyMatrix));
+    if (blendFactor >= 0.99f) {
+        // the "adjustment" is more or less complete so stop following
+        _isFollowingHMD = false;
+        _hmdFollowSpeed = 0.0f;
+        // and slam the body's transform anyway to eliminate any slight errors
+        glm::vec3 finalBodyPosition = extractTranslation(worldBodyMatrix);
+        nextAttitude(finalBodyPosition, finalBodyRotation);
+        _bodySensorMatrix = newBodySensorMatrix;
+    } else {
+        // physics already did the positional blending for us
+        glm::vec3 newBodyPosition = getPosition();
+        // but the rotational part must be done manually
+        glm::quat newBodyRotation = glm::normalize(safeMix(getOrientation(), finalBodyRotation, blendFactor));
+        nextAttitude(newBodyPosition, newBodyRotation);
+
+        // interp sensor matrix toward the desired
+        glm::vec3 prevPosition = extractTranslation(_bodySensorMatrix);
+        glm::quat prevRotation = glm::normalize(glm::quat_cast(_bodySensorMatrix));
+        glm::vec3 nextPosition = extractTranslation(newBodySensorMatrix);
+        glm::quat nextRotation = glm::normalize(glm::quat_cast(newBodySensorMatrix));
+        _bodySensorMatrix = createMatFromQuatAndPos(
+                glm::normalize(safeMix(prevRotation, nextRotation, blendFactor)),
+                lerp(prevPosition, nextPosition, blendFactor));
+    }
 }
 
 QString MyAvatar::getScriptedMotorFrame() const {

interface/src/avatar/MyAvatar.h

@@ -101,8 +101,6 @@ public:
     // as it moves through the world.
     void updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix);
 
-    glm::vec3 getHMDCorrectionVelocity() const;
-
     // best called at end of main loop, just before rendering.
     // update sensor to world matrix from current body position and hmd sensor.
     // This is so the correct camera can be used for rendering.
@@ -211,6 +209,7 @@ public:
 
     void prepareForPhysicsSimulation();
     void harvestResultsFromPhysicsSimulation();
+    void adjustSensorTransform(glm::vec3 hmdShift);
 
     const QString& getCollisionSoundURL() { return _collisionSoundURL; }
     void setCollisionSoundURL(const QString& url);
@@ -316,9 +315,10 @@ private:
     const RecorderPointer getRecorder() const { return _recorder; }
     const PlayerPointer getPlayer() const { return _player; }
 
-    void beginFollowingHMD();
+    //void beginFollowingHMD();
-    bool shouldFollowHMD() const;
+    //bool shouldFollowHMD() const;
-    void followHMD(float deltaTime);
+    //void followHMD(float deltaTime);
+    void updateHMDFollowVelocity();
 
     bool cameraInsideHead() const;
 
@@ -395,6 +395,9 @@ private:
 
     // used to transform any sensor into world space, including the _hmdSensorMat, or hand controllers.
     glm::mat4 _sensorToWorldMatrix;
+    glm::vec3 _hmdFollowOffset { Vectors::ZERO };
+    glm::vec3 _hmdFollowVelocity { Vectors::ZERO };
+    float _hmdFollowSpeed { 0.0f };
 
     bool _goToPending;
     glm::vec3 _goToPosition;
@@ -415,9 +418,7 @@ private:
     bool _isFollowingHMD { false };
     float _followHMDAlpha { 0.0f };
 
-    quint64 _lastUpdateFromHMDTime { usecTimestampNow() };
     AtRestDetector _hmdAtRestDetector;
-    glm::vec3 _lastPosition;
     bool _lastIsMoving { false };
     quint64 _lastStepPulse { 0 };
     bool _pulseUpdate { false };

libraries/shared/src/AtRestDetector.cpp

@@ -1,5 +1,19 @@
+//
+//  AtRestDetector.cpp
+//  libraries/shared/src
+//
+//  Created by Anthony Thibault on 10/6/2015
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
 #include "AtRestDetector.h"
+
+#include "NumericalConstants.h"
 #include "SharedLogging.h"
+#include "SharedUtil.h"
 
 AtRestDetector::AtRestDetector(const glm::vec3& startPosition, const glm::quat& startRotation) {
     reset(startPosition, startRotation);
@@ -12,9 +26,14 @@ void AtRestDetector::reset(const glm::vec3& startPosition, const glm::quat& star
     glm::quat ql = glm::log(startRotation);
     _quatLogAverage = glm::vec3(ql.x, ql.y, ql.z);
     _quatLogVariance = 0.0f;
+    _lastUpdateTime = usecTimestampNow();
+    _isAtRest = false;
 }
 
-bool AtRestDetector::update(float dt, const glm::vec3& position, const glm::quat& rotation) {
+bool AtRestDetector::update(const glm::vec3& position, const glm::quat& rotation) {
+    uint64_t now = usecTimestampNow();
+    float dt = (float)(_lastUpdateTime - now) / (float)USECS_PER_SECOND;
+    _lastUpdateTime = now;
     const float TAU = 1.0f;
     float delta = glm::min(dt / TAU, 1.0f);
 
@@ -37,5 +56,6 @@ bool AtRestDetector::update(float dt, const glm::vec3& position, const glm::quat
     const float POSITION_VARIANCE_THRESHOLD = 0.001f;
     const float QUAT_LOG_VARIANCE_THRESHOLD = 0.00002f;
 
-    return _positionVariance < POSITION_VARIANCE_THRESHOLD && _quatLogVariance < QUAT_LOG_VARIANCE_THRESHOLD;
+    _isAtRest = _positionVariance < POSITION_VARIANCE_THRESHOLD && _quatLogVariance < QUAT_LOG_VARIANCE_THRESHOLD;
+    return _isAtRest;
 }

libraries/shared/src/AtRestDetector.h

@@ -21,14 +21,17 @@ public:
     void reset(const glm::vec3& startPosition, const glm::quat& startRotation);
 
     // returns true if object is at rest, dt in assumed to be seconds.
-    bool update(float dt, const glm::vec3& position, const glm::quat& startRotation);
+    bool update(const glm::vec3& position, const glm::quat& startRotation);
+
+    bool isAtRest() const { return _isAtRest; }
 
 protected:
     glm::vec3 _positionAverage;
-    float _positionVariance;
-
     glm::vec3 _quatLogAverage;
-    float _quatLogVariance;
+    uint64_t _lastUpdateTime { 0 };
+    float _positionVariance { 0.0f };
+    float _quatLogVariance { 0.0f };
+    bool _isAtRest { false };
 };
 
 #endif