out-of-body with reduced recovery speeds

interface/src/avatar/MyAvatar.cpp

@@ -416,10 +416,6 @@ void MyAvatar::simulate(float deltaTime) {
         updatePosition(deltaTime);
     }
 
-    // update sensorToWorldMatrix for camera and hand controllers
-    // before we perform rig animations and IK.
-    updateSensorToWorldMatrix();
-
     {
         PerformanceTimer perfTimer("skeleton");
         _skeletonModel->simulate(deltaTime);
@@ -1305,9 +1301,10 @@ void MyAvatar::prepareForPhysicsSimulation() {
 
     _characterController.setPositionAndOrientation(getPosition(), getOrientation());
     if (qApp->isHMDMode()) {
-        _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput());
+        _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix);
     } else {
         _follow.deactivate();
+        getCharacterController()->disableFollow();
     }
 }
 
@@ -1318,7 +1315,10 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
         _characterController.getPositionAndOrientation(position, orientation);
     }
     nextAttitude(position, orientation);
-    _bodySensorMatrix = _follow.postPhysicsUpdate(*this, _bodySensorMatrix);
+
+    // compute new _bodyToSensorMatrix
+    glm::mat4 bodyToWorldMatrix = createMatFromQuatAndPos(orientation, position);
+    _bodySensorMatrix = glm::inverse(_sensorToWorldMatrix) * bodyToWorldMatrix;
 
     if (_characterController.isEnabledAndReady()) {
         setVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity());
@@ -2050,64 +2050,47 @@ MyAvatar::FollowHelper::FollowHelper() {
 }
 
 void MyAvatar::FollowHelper::deactivate() {
-    for (int i = 0; i < NumFollowTypes; i++) {
+    _activeBits = 0;
-        deactivate((FollowType)i);
-    }
 }
 
 void MyAvatar::FollowHelper::deactivate(FollowType type) {
     assert(type >= 0 && type < NumFollowTypes);
-    _timeRemaining[(int)type] = 0.0f;
+    _activeBits &= ~(uint8_t)(0x01 << (int)type);
 }
 
 void MyAvatar::FollowHelper::activate(FollowType type) {
     assert(type >= 0 && type < NumFollowTypes);
-    // TODO: Perhaps, the follow time should be proportional to the displacement.
+    _activeBits |= (uint8_t)(0x01 << (int)type);
-    _timeRemaining[(int)type] = FOLLOW_TIME;
 }
 
 bool MyAvatar::FollowHelper::isActive(FollowType type) const {
     assert(type >= 0 && type < NumFollowTypes);
-    return _timeRemaining[(int)type] > 0.0f;
+    return (bool)(_activeBits & (uint8_t)(0x01 << (int)type));
 }
 
 bool MyAvatar::FollowHelper::isActive() const {
-    for (int i = 0; i < NumFollowTypes; i++) {
+    return (bool)_activeBits;
-        if (isActive((FollowType)i)) {
-            return true;
-        }
-    }
-    return false;
 }
 
-float MyAvatar::FollowHelper::getMaxTimeRemaining() const {
+void MyAvatar::FollowHelper::updateRotationActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) {
-    float max = 0.0f;
-    for (int i = 0; i < NumFollowTypes; i++) {
-        if (_timeRemaining[i] > max) {
-            max = _timeRemaining[i];
-        }
-    }
-    return max;
-}
-
-void MyAvatar::FollowHelper::decrementTimeRemaining(float dt) {
-    for (int i = 0; i < NumFollowTypes; i++) {
-        _timeRemaining[i] -= dt;
-    }
-}
-
-bool MyAvatar::FollowHelper::shouldActivateRotation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const {
     auto cameraMode = qApp->getCamera()->getMode();
     if (cameraMode == CAMERA_MODE_THIRD_PERSON) {
-        return false;
+        deactivate(Rotation);
     } else {
         const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); // 30 degrees
+        const float STOP_FOLLOW_ROTATION_THRESHOLD = 0.99f;
         glm::vec2 bodyFacing = getFacingDir2D(currentBodyMatrix);
-        return glm::dot(myAvatar.getHMDSensorFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD;
+        if (isActive(Rotation)) {
+            if (glm::dot(myAvatar.getHMDSensorFacingMovingAverage(), bodyFacing) > STOP_FOLLOW_ROTATION_THRESHOLD) {
+                deactivate(Rotation);
+            }
+        } else if (glm::dot(myAvatar.getHMDSensorFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD) {
+            activate(Rotation);
+        }
     }
 }
 
-bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const {
+void MyAvatar::FollowHelper::updateHorizontalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) {
 
     // -z axis of currentBodyMatrix in world space.
     glm::vec3 forward = glm::normalize(glm::vec3(-currentBodyMatrix[0][2], -currentBodyMatrix[1][2], -currentBodyMatrix[2][2]));
@@ -2116,85 +2099,74 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar,
     glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix);
 
     float forwardLeanAmount = glm::dot(forward, offset);
-    float lateralLeanAmount = glm::dot(right, offset);
+    float lateralLeanAmount = fabsf(glm::dot(right, offset));
 
     const float MAX_LATERAL_LEAN = 0.3f;
     const float MAX_FORWARD_LEAN = 0.15f;
     const float MAX_BACKWARD_LEAN = 0.1f;
+    const float MIN_LEAN = 0.02f;
 
-    if (forwardLeanAmount > 0 && forwardLeanAmount > MAX_FORWARD_LEAN) {
+    if (isActive(Horizontal)) {
-        return true;
+        if (fabsf(forwardLeanAmount) < MIN_LEAN && lateralLeanAmount < MIN_LEAN) {
-    } else if (forwardLeanAmount < 0 && forwardLeanAmount < -MAX_BACKWARD_LEAN) {
+            deactivate(Horizontal);
-        return true;
+        }
+    } else {
+        if (forwardLeanAmount > MAX_FORWARD_LEAN ||
+                forwardLeanAmount < -MAX_BACKWARD_LEAN ||
+                lateralLeanAmount > MAX_LATERAL_LEAN) {
+            activate(Horizontal);
+        }
     }
-
-    return fabs(lateralLeanAmount) > MAX_LATERAL_LEAN;
 }
 
-bool MyAvatar::FollowHelper::shouldActivateVertical(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const {
+void MyAvatar::FollowHelper::updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) {
-
     const float CYLINDER_TOP = 0.1f;
     const float CYLINDER_BOTTOM = -1.5f;
+    const float MIN_VERTICAL_OFFSET = 0.02f;
 
     glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix);
-    return (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM);
+    if (isActive(Vertical)) {
+        if (fabsf(offset.y) < MIN_VERTICAL_OFFSET) {
+            deactivate(Vertical);
+        }
+    } else if (offset.y > CYLINDER_TOP || offset.y < CYLINDER_BOTTOM) {
+        activate(Vertical);
+    }
 }
 
-void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput) {
+void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) {
     _desiredBodyMatrix = desiredBodyMatrix;
 
     if (myAvatar.getHMDLeanRecenterEnabled()) {
-        if (!isActive(Rotation) && shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix)) {
+        updateRotationActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix);
-            activate(Rotation);
+        updateHorizontalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix);
+        updateVerticalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix);
+
+        glm::mat4 desiredWorldMatrix = myAvatar.getSensorToWorldMatrix() * _desiredBodyMatrix;
+        glm::mat4 currentWorldMatrix = createMatFromQuatAndPos(myAvatar.getOrientation(), myAvatar.getPosition());
+
+        AnimPose followWorldPose(currentWorldMatrix);
+        if (isActive(Rotation)) {
+            followWorldPose.rot = glmExtractRotation(desiredWorldMatrix);
         }
-        if (!isActive(Horizontal) && shouldActivateHorizontal(myAvatar, desiredBodyMatrix, currentBodyMatrix)) {
+        if (isActive(Horizontal)) {
-            activate(Horizontal);
+            glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix);
+            followWorldPose.trans.x = desiredTranslation.x;
+            followWorldPose.trans.z = desiredTranslation.z;
         }
-        if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) {
+        if (isActive(Vertical)) {
-            activate(Vertical);
+            glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix);
+            followWorldPose.trans.y = desiredTranslation.y;
         }
-    }
-
-    glm::mat4 desiredWorldMatrix = myAvatar.getSensorToWorldMatrix() * _desiredBodyMatrix;
-    glm::mat4 currentWorldMatrix = myAvatar.getSensorToWorldMatrix() * currentBodyMatrix;
-
-    AnimPose followWorldPose(currentWorldMatrix);
-    if (isActive(Rotation)) {
-        followWorldPose.rot = glmExtractRotation(desiredWorldMatrix);
-    }
-    if (isActive(Horizontal)) {
-        glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix);
-        followWorldPose.trans.x = desiredTranslation.x;
-        followWorldPose.trans.z = desiredTranslation.z;
-    }
-    if (isActive(Vertical)) {
-        glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix);
-        followWorldPose.trans.y = desiredTranslation.y;
-    }
-
-    myAvatar.getCharacterController()->setFollowParameters(followWorldPose, getMaxTimeRemaining());
-}
-
-glm::mat4 MyAvatar::FollowHelper::postPhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& currentBodyMatrix) {
-    if (isActive()) {
-        float dt = myAvatar.getCharacterController()->getFollowTime();
-        decrementTimeRemaining(dt);
-
-        // apply follow displacement to the body matrix.
-        glm::vec3 worldLinearDisplacement = myAvatar.getCharacterController()->getFollowLinearDisplacement();
-        glm::quat worldAngularDisplacement = myAvatar.getCharacterController()->getFollowAngularDisplacement();
-        glm::quat sensorToWorld = glmExtractRotation(myAvatar.getSensorToWorldMatrix());
-        glm::quat worldToSensor = glm::inverse(sensorToWorld);
-
-        glm::vec3 sensorLinearDisplacement = worldToSensor * worldLinearDisplacement;
-        glm::quat sensorAngularDisplacement = worldToSensor * worldAngularDisplacement * sensorToWorld;
-
-        glm::mat4 newBodyMat = createMatFromQuatAndPos(sensorAngularDisplacement * glmExtractRotation(currentBodyMatrix),
-                                                       sensorLinearDisplacement + extractTranslation(currentBodyMatrix));
-        return newBodyMat;
 
+        if (isActive()) {
+            myAvatar.getCharacterController()->setFollowParameters(followWorldPose);
+        } else {
+            myAvatar.getCharacterController()->disableFollow();
+        }
     } else {
-        return currentBodyMatrix;
+        deactivate();
+        myAvatar.getCharacterController()->disableFollow();
     }
 }
 

interface/src/avatar/MyAvatar.h

@@ -442,7 +442,7 @@ private:
             NumFollowTypes
         };
         glm::mat4 _desiredBodyMatrix;
-        float _timeRemaining[NumFollowTypes];
+        uint8_t _activeBits;
 
         void deactivate();
         void deactivate(FollowType type);
@@ -450,13 +450,10 @@ private:
         void activate(FollowType type);
         bool isActive() const;
         bool isActive(FollowType followType) const;
-        float getMaxTimeRemaining() const;
+        void updateRotationActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix);
-        void decrementTimeRemaining(float dt);
+        void updateHorizontalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix);
-        bool shouldActivateRotation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const;
+        void updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix);
-        bool shouldActivateVertical(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const;
+        void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix);
-        bool shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const;
-        void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput);
-        glm::mat4 postPhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& currentBodyMatrix);
     };
     FollowHelper _follow;
 

libraries/physics/src/CharacterController.cpp

@@ -70,7 +70,6 @@ CharacterController::CharacterController() {
 
     _targetVelocity.setValue(0.0f, 0.0f, 0.0f);
     _followDesiredBodyTransform.setIdentity();
-    _followTimeRemaining = 0.0f;
     _jumpSpeed = JUMP_SPEED;
     _state = State::Hover;
     _isPushingUp = false;
@@ -199,16 +198,24 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
     // Rather than add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal.
     // This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate().
 
-    const float MINIMUM_TIME_REMAINING = 0.005f;
+    if (_following) {
-    const float MAX_DISPLACEMENT = 0.5f * _radius;
+        // HACK these copied form elsewhere
-    _followTimeRemaining -= dt;
+        const float NORMAL_WALKING_SPEED = 0.5f;
-    if (_followTimeRemaining >= MINIMUM_TIME_REMAINING) {
+        const float FOLLOW_TIME = 0.8f;
+        const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f);
+
+        const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIME;
+
         btTransform bodyTransform = _rigidBody->getWorldTransform();
 
         btVector3 startPos = bodyTransform.getOrigin();
         btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos;
-        btVector3 vel = deltaPos / _followTimeRemaining;
+        btVector3 vel = deltaPos * (0.5f / dt);
-        btVector3 linearDisplacement = clampLength(vel * dt, MAX_DISPLACEMENT);  // clamp displacement to prevent tunneling.
+        btScalar speed = vel.length();
+        if (speed > NORMAL_WALKING_SPEED) {
+            vel *= NORMAL_WALKING_SPEED / speed;
+        }
+        btVector3 linearDisplacement = vel * dt;
         btVector3 endPos = startPos + linearDisplacement;
 
         btQuaternion startRot = bodyTransform.getRotation();
@@ -216,7 +223,10 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
         glm::vec2 currentRight(currentFacing.y, -currentFacing.x);
         glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation()));
         float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f));
-        float angularSpeed = deltaAngle / _followTimeRemaining;
+        float angularSpeed = 0.5f * deltaAngle / dt;
+        if (angularSpeed > MAX_ANGULAR_SPEED) {
+            angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed;
+        }
         float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight));
         btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt);
         btQuaternion endRot = angularDisplacement * startRot;
@@ -229,8 +239,8 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
         _followAngularDisplacement = angularDisplacement * _followAngularDisplacement;
 
         _rigidBody->setWorldTransform(btTransform(endRot, endPos));
+        _followTime += dt;
     }
-    _followTime += dt;
 }
 
 void CharacterController::jump() {
@@ -371,9 +381,9 @@ void CharacterController::setParentVelocity(const glm::vec3& velocity) {
     _parentVelocity = glmToBullet(velocity);
 }
 
-void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyMatrix, float timeRemaining) {
+void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyMatrix) {
-    _followTimeRemaining = timeRemaining;
     _followDesiredBodyTransform = glmToBullet(desiredWorldBodyMatrix) * btTransform(btQuaternion::getIdentity(), glmToBullet(_shapeLocalOffset));
+    _following = true;
 }
 
 glm::vec3 CharacterController::getFollowLinearDisplacement() const {
@@ -626,7 +636,7 @@ void CharacterController::preSimulation() {
     _pendingFlags &= ~PENDING_FLAG_JUMP;
 
     _followTime = 0.0f;
-    _followLinearDisplacement = btVector3(0, 0, 0);
+    _followLinearDisplacement = btVector3(0.0f, 0.0f, 0.0f);
     _followAngularDisplacement = btQuaternion::getIdentity();
 }
 

libraries/physics/src/CharacterController.h

@@ -82,7 +82,8 @@ public:
     void getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const;
 
     void setParentVelocity(const glm::vec3& parentVelocity);
-    void setFollowParameters(const glm::mat4& desiredWorldMatrix, float timeRemaining);
+    void setFollowParameters(const glm::mat4& desiredWorldBodyMatrix);
+    void disableFollow() { _following = false; }
     float getFollowTime() const { return _followTime; }
     glm::vec3 getFollowLinearDisplacement() const;
     glm::quat getFollowAngularDisplacement() const;
@@ -142,7 +143,6 @@ protected:
     btVector3 _preSimulationVelocity;
     btVector3 _velocityChange;
     btTransform _followDesiredBodyTransform;
-    btScalar _followTimeRemaining;
     btTransform _characterBodyTransform;
 
     glm::vec3 _shapeLocalOffset;
@@ -168,6 +168,7 @@ protected:
     btVector3 _followLinearDisplacement;
     btQuaternion _followAngularDisplacement;
     btVector3 _linearAcceleration;
+    bool _following { false };
 
     std::atomic_bool _enabled;
     State _state;