From bc56df0be18c85e6735d96623800742100ac8aa5 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 8 Sep 2016 14:31:31 -0700 Subject: [PATCH] out-of-body with reduced recovery speeds --- interface/src/avatar/MyAvatar.cpp | 164 ++++++++---------- interface/src/avatar/MyAvatar.h | 13 +- libraries/physics/src/CharacterController.cpp | 34 ++-- libraries/physics/src/CharacterController.h | 5 +- 4 files changed, 98 insertions(+), 118 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 24dbc62318..a3371e6caa 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/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++) { - deactivate((FollowType)i); - } + _activeBits = 0; } 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. - _timeRemaining[(int)type] = FOLLOW_TIME; + _activeBits |= (uint8_t)(0x01 << (int)type); } 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++) { - if (isActive((FollowType)i)) { - return true; - } - } - return false; + return (bool)_activeBits; } -float MyAvatar::FollowHelper::getMaxTimeRemaining() const { - 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 { +void MyAvatar::FollowHelper::updateRotationActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) { 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) { - return true; - } else if (forwardLeanAmount < 0 && forwardLeanAmount < -MAX_BACKWARD_LEAN) { - return true; + if (isActive(Horizontal)) { + if (fabsf(forwardLeanAmount) < MIN_LEAN && lateralLeanAmount < MIN_LEAN) { + deactivate(Horizontal); + } + } 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)) { - activate(Rotation); + updateRotationActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix); + 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)) { - activate(Horizontal); + if (isActive(Horizontal)) { + glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix); + followWorldPose.trans.x = desiredTranslation.x; + followWorldPose.trans.z = desiredTranslation.z; } - if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) { - activate(Vertical); + if (isActive(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(); } } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index c4ffc08cbc..9614bf9bf8 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/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 decrementTimeRemaining(float dt); - bool shouldActivateRotation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const; - bool shouldActivateVertical(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const; - 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); + void updateRotationActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); + void updateHorizontalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); + void updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); + void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix); }; FollowHelper _follow; diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 19207f13ed..af983deea9 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/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; - const float MAX_DISPLACEMENT = 0.5f * _radius; - _followTimeRemaining -= dt; - if (_followTimeRemaining >= MINIMUM_TIME_REMAINING) { + if (_following) { + // HACK these copied form elsewhere + const float NORMAL_WALKING_SPEED = 0.5f; + 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 linearDisplacement = clampLength(vel * dt, MAX_DISPLACEMENT); // clamp displacement to prevent tunneling. + btVector3 vel = deltaPos * (0.5f / dt); + 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) { - _followTimeRemaining = timeRemaining; +void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyMatrix) { _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(); } diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 586ea175e6..26ad481c46 100644 --- a/libraries/physics/src/CharacterController.h +++ b/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;