From e484a904a2da25dba7506411c0759480a2c4134f Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 13 Oct 2015 17:36:00 -0700 Subject: [PATCH] Rotate the avatar to align with the HMD while moving MyAvatar: refactored updateFromHMDSensorMatrix() a bit by splitting it into several methods, because it was getting quite large and becoming hard to follow. * beginStraighteningLean() - can be called when we would like to trigger a re-centering action. * shouldBeginStraighteningLean() - contains some of the logic to decide if we should begin a re-centering action. for now it encapulates the capsule check. * processStraighteningLean() - performs the actual re-centering calculation. New code was added to MyAvatar::updateFromHMDSensorMatrix() to trigger re-centering when the avatar speed rises over a threshold. Secondly the Rig::computeMotionAnimationState() state machine for animGraph added a state change hysteresis of 100ms. This hysteresis should help smooth over two issues. 1) When the delta position is 0, because the physics timestep was not evaluated. 2) During re-centering due to desired motion, the avatar velocity can fluctuate causing undesired animation state fluctuation. --- interface/src/avatar/MyAvatar.cpp | 53 +++++++- interface/src/avatar/MyAvatar.h | 6 + libraries/animation/src/AnimStateMachine.cpp | 2 +- libraries/animation/src/Rig.cpp | 133 +++++++++++++------ libraries/animation/src/Rig.h | 2 + 5 files changed, 145 insertions(+), 51 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 605a81e9d9..bb16ac6e21 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -320,6 +320,7 @@ static bool capsuleCheck(const glm::vec3& pos, float capsuleLen, float capsuleRa // as it moves through the world. void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { + // calc deltaTime auto now = usecTimestampNow(); auto deltaUsecs = now - _lastUpdateFromHMDTime; _lastUpdateFromHMDTime = now; @@ -334,21 +335,61 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { bool hmdIsAtRest = _hmdAtRestDetector.update(deltaTime, _hmdSensorPosition, _hmdSensorOrientation); - const float STRAIGHTENING_LEAN_DURATION = 0.5f; // seconds + // 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; + } else { + isMoving = glm::length(workingVelocity) > MOVE_ENTER_SPEED_THRESHOLD; + } + + bool justStartedMoving = (_lastIsMoving != isMoving) && isMoving; + _lastIsMoving = isMoving; + + if (shouldBeginStraighteningLean() || hmdIsAtRest || justStartedMoving) { + beginStraighteningLean(); + } + + processStraighteningLean(deltaTime); +} + +void MyAvatar::beginStraighteningLean() { + // begin homing toward derived body position. + if (!_straighteningLean) { + _straighteningLean = true; + _straighteningLeanAlpha = 0.0f; + } +} + +bool MyAvatar::shouldBeginStraighteningLean() const { // define a vertical capsule const float STRAIGHTENING_LEAN_CAPSULE_RADIUS = 0.2f; // meters const float STRAIGHTENING_LEAN_CAPSULE_LENGTH = 0.05f; // length of the cylinder part of the capsule in meters. + // detect if the derived body position is outside of a capsule around the _bodySensorMatrix auto newBodySensorMatrix = deriveBodyFromHMDSensor(); glm::vec3 diff = extractTranslation(newBodySensorMatrix) - extractTranslation(_bodySensorMatrix); - if (!_straighteningLean && (capsuleCheck(diff, STRAIGHTENING_LEAN_CAPSULE_LENGTH, STRAIGHTENING_LEAN_CAPSULE_RADIUS) || hmdIsAtRest)) { + bool isBodyPosOutsideCapsule = capsuleCheck(diff, STRAIGHTENING_LEAN_CAPSULE_LENGTH, STRAIGHTENING_LEAN_CAPSULE_RADIUS); - // begin homing toward derived body position. - _straighteningLean = true; - _straighteningLeanAlpha = 0.0f; + if (isBodyPosOutsideCapsule) { + return true; + } else { + return false; + } +} - } else if (_straighteningLean) { +void MyAvatar::processStraighteningLean(float deltaTime) { + if (_straighteningLean) { + + const float STRAIGHTENING_LEAN_DURATION = 0.5f; // seconds auto newBodySensorMatrix = deriveBodyFromHMDSensor(); auto worldBodyMatrix = _sensorToWorldMatrix * newBodySensorMatrix; diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index cca0c4152f..3b913eb509 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -271,6 +271,10 @@ private: const RecorderPointer getRecorder() const { return _recorder; } const PlayerPointer getPlayer() const { return _player; } + void beginStraighteningLean(); + bool shouldBeginStraighteningLean() const; + void processStraighteningLean(float deltaTime); + bool cameraInsideHead() const; // These are made private for MyAvatar so that you will use the "use" methods instead @@ -366,6 +370,8 @@ private: quint64 _lastUpdateFromHMDTime = usecTimestampNow(); AtRestDetector _hmdAtRestDetector; + glm::vec3 _lastPosition; + bool _lastIsMoving = false; }; QScriptValue audioListenModeToScriptValue(QScriptEngine* engine, const AudioListenerMode& audioListenerMode); diff --git a/libraries/animation/src/AnimStateMachine.cpp b/libraries/animation/src/AnimStateMachine.cpp index 5304cefe46..c0bb66c2a0 100644 --- a/libraries/animation/src/AnimStateMachine.cpp +++ b/libraries/animation/src/AnimStateMachine.cpp @@ -93,7 +93,7 @@ void AnimStateMachine::switchState(const AnimVariantMap& animVars, State::Pointe const float dt = 0.0f; Triggers triggers; _nextPoses = nextStateNode->evaluate(animVars, dt, triggers); -#if WANT_DEBUGa +#if WANT_DEBUG qCDebug(animation) << "AnimStateMachine::switchState:" << _currentState->getID() << "->" << desiredState->getID() << "duration =" << duration << "targetFrame =" << desiredState->_interpTarget; #endif _currentState = desiredState; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 340c09060a..25f28a3f64 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -437,16 +437,6 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos static float t = 0.0f; _animVars.set("sine", static_cast(0.5 * sin(t) + 0.5)); - // default anim vars to notMoving and notTurning - _animVars.set("isMovingForward", false); - _animVars.set("isMovingBackward", false); - _animVars.set("isMovingLeft", false); - _animVars.set("isMovingRight", false); - _animVars.set("isNotMoving", true); - _animVars.set("isTurningLeft", false); - _animVars.set("isTurningRight", false); - _animVars.set("isNotTurning", true); - const float ANIM_WALK_SPEED = 1.4f; // m/s _animVars.set("walkTimeScale", glm::clamp(0.5f, 2.0f, glm::length(localVel) / ANIM_WALK_SPEED)); @@ -470,47 +460,102 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos } if (glm::length(localVel) > moveThresh) { - if (fabsf(forwardSpeed) > 0.5f * fabsf(lateralSpeed)) { - if (forwardSpeed > 0.0f) { - // forward - _animVars.set("isMovingForward", true); - _animVars.set("isNotMoving", false); - - } else { - // backward - _animVars.set("isMovingBackward", true); - _animVars.set("isNotMoving", false); - } - } else { - if (lateralSpeed > 0.0f) { - // right - _animVars.set("isMovingRight", true); - _animVars.set("isNotMoving", false); - } else { - // left - _animVars.set("isMovingLeft", true); - _animVars.set("isNotMoving", false); - } + if (_desiredState != RigRole::Move) { + _desiredStateAge = 0.0f; } - _state = RigRole::Move; + _desiredState = RigRole::Move; } else { if (fabsf(turningSpeed) > turnThresh) { - if (turningSpeed > 0.0f) { - // turning right - _animVars.set("isTurningRight", true); - _animVars.set("isNotTurning", false); - } else { - // turning left - _animVars.set("isTurningLeft", true); - _animVars.set("isNotTurning", false); + if (_desiredState != RigRole::Turn) { + _desiredStateAge = 0.0f; } - _state = RigRole::Turn; - } else { - // idle - _state = RigRole::Idle; + _desiredState = RigRole::Turn; + } else { // idle + if (_desiredState != RigRole::Idle) { + _desiredStateAge = 0.0f; + } + _desiredState = RigRole::Idle; } } + const float STATE_CHANGE_HYSTERESIS_TIMER = 0.1f; + + if ((_desiredStateAge >= STATE_CHANGE_HYSTERESIS_TIMER) && _desiredState != _state) { + _state = _desiredState; + _desiredStateAge = 0.0f; + } + + _desiredStateAge += deltaTime; + + if (_state == RigRole::Move) { + if (glm::length(localVel) > MOVE_ENTER_SPEED_THRESHOLD) { + if (fabsf(forwardSpeed) > 0.5f * fabsf(lateralSpeed)) { + if (forwardSpeed > 0.0f) { + // forward + _animVars.set("isMovingForward", true); + _animVars.set("isMovingBackward", false); + _animVars.set("isMovingRight", false); + _animVars.set("isMovingLeft", false); + _animVars.set("isNotMoving", false); + + } else { + // backward + _animVars.set("isMovingBackward", true); + _animVars.set("isMovingForward", false); + _animVars.set("isMovingRight", false); + _animVars.set("isMovingLeft", false); + _animVars.set("isNotMoving", false); + } + } else { + if (lateralSpeed > 0.0f) { + // right + _animVars.set("isMovingRight", true); + _animVars.set("isMovingLeft", false); + _animVars.set("isMovingForward", false); + _animVars.set("isMovingBackward", false); + _animVars.set("isNotMoving", false); + } else { + // left + _animVars.set("isMovingLeft", true); + _animVars.set("isMovingRight", false); + _animVars.set("isMovingForward", false); + _animVars.set("isMovingBackward", false); + _animVars.set("isNotMoving", false); + } + } + _animVars.set("isTurningLeft", false); + _animVars.set("isTurningRight", false); + _animVars.set("isNotTurning", true); + } + } else if (_state == RigRole::Turn) { + if (turningSpeed > 0.0f) { + // turning right + _animVars.set("isTurningRight", true); + _animVars.set("isTurningLeft", false); + _animVars.set("isNotTurning", false); + } else { + // turning left + _animVars.set("isTurningLeft", true); + _animVars.set("isTurningRight", false); + _animVars.set("isNotTurning", false); + } + _animVars.set("isMovingForward", false); + _animVars.set("isMovingBackward", false); + _animVars.set("isMovingRight", false); + _animVars.set("isMovingLeft", false); + _animVars.set("isNotMoving", true); + } else { + // default anim vars to notMoving and notTurning + _animVars.set("isMovingForward", false); + _animVars.set("isMovingBackward", false); + _animVars.set("isMovingLeft", false); + _animVars.set("isMovingRight", false); + _animVars.set("isNotMoving", true); + _animVars.set("isTurningLeft", false); + _animVars.set("isTurningRight", false); + _animVars.set("isNotTurning", true); + } + t += deltaTime; } diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 6d9f7b4688..71c27e7213 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -236,6 +236,8 @@ public: Move }; RigRole _state = RigRole::Idle; + RigRole _desiredState = RigRole::Idle; + float _desiredStateAge = 0.0f; float _leftHandOverlayAlpha = 0.0f; float _rightHandOverlayAlpha = 0.0f; };