diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 5e14c66ff1..9666e21ce0 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -177,9 +177,8 @@ void MyAvatar::reset(bool andReload) { // Reset dynamic state. _wasPushing = _isPushing = _isBraking = _billboardValid = false; - _isFollowingHMD = false; - _hmdFollowVelocity = Vectors::ZERO; - _hmdFollowSpeed = 0.0f; + _followVelocity = Vectors::ZERO; + _followSpeed = 0.0f; _skeletonModel.reset(); getHead()->reset(); _targetVelocity = glm::vec3(0.0f); @@ -352,52 +351,39 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { void MyAvatar::updateHMDFollowVelocity() { // compute offset to body's target position (in sensor-frame) auto sensorBodyMatrix = deriveBodyFromHMDSensor(); - _hmdFollowOffset = extractTranslation(sensorBodyMatrix) - extractTranslation(_bodySensorMatrix); - glm::vec3 truncatedOffset = _hmdFollowOffset; - if (truncatedOffset.y < 0.0f) { - // don't pull the body DOWN to match the target (allow animation system to squat) - truncatedOffset.y = 0.0f; - } - float truncatedOffsetDistance = glm::length(truncatedOffset); + glm::vec3 offset = extractTranslation(sensorBodyMatrix) - extractTranslation(_bodySensorMatrix); + _followOffsetDistance = glm::length(offset); + + const float FOLLOW_TIMESCALE = 0.5f; + const float FOLLOW_THRESHOLD_SPEED = 0.2f; + const float FOLLOW_MIN_DISTANCE = 0.02f; + const float FOLLOW_THRESHOLD_DISTANCE = 0.2f; - bool isMoving; - if (_lastIsMoving) { - const float MOVE_EXIT_SPEED_THRESHOLD = 0.07f; // m/sec - isMoving = glm::length(_velocity) >= MOVE_EXIT_SPEED_THRESHOLD; - } else { - 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; bool hmdIsAtRest = _hmdAtRestDetector.update(_hmdSensorPosition, _hmdSensorOrientation); - const float MIN_HMD_HIP_SHIFT = 0.05f; - if (justStartedMoving || (hmdIsAtRest && truncatedOffsetDistance > MIN_HMD_HIP_SHIFT)) { - _isFollowingHMD = true; - } + bool avatarIsMoving = glm::length(_velocity - _followVelocity) > FOLLOW_THRESHOLD_SPEED; + bool shouldFollow = hmdIsAtRest || avatarIsMoving; - bool needNewFollowSpeed = (_isFollowingHMD && _hmdFollowSpeed == 0.0f); - if (!needNewFollowSpeed) { - // check to see if offset has exceeded its threshold - const float MAX_HMD_HIP_SHIFT = 0.2f; - if (truncatedOffsetDistance > MAX_HMD_HIP_SHIFT) { - _isFollowingHMD = true; - needNewFollowSpeed = true; + // linear part + _followOffsetDistance = glm::length(offset); + if (_followOffsetDistance < FOLLOW_MIN_DISTANCE) { + // close enough + _followOffsetDistance = 0.0f; + } else { + glm::vec3 truncatedOffset = offset; + if (truncatedOffset.y < 0.0f) { + truncatedOffset.y = 0.0f; + } + float truncatedDistance = glm::length(truncatedOffset); + bool needsNewSpeed = truncatedDistance > FOLLOW_THRESHOLD_DISTANCE; + if (needsNewSpeed || (shouldFollow && _followSpeed == 0.0f)) { + // compute new speed + _followSpeed = _followOffsetDistance / FOLLOW_TIMESCALE; + } + if (_followSpeed > 0.0f) { + // to compute new velocity we must rotate offset into the world-frame + glm::quat sensorToWorldRotation = extractRotation(_sensorToWorldMatrix); + _followVelocity = _followSpeed * glm::normalize(sensorToWorldRotation * offset); } - } - if (_isFollowingHMD) { - // only bother to rotate into world frame if we're following - glm::quat sensorToWorldRotation = extractRotation(_sensorToWorldMatrix); - _hmdFollowOffset = sensorToWorldRotation * _hmdFollowOffset; - } - 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); } } @@ -1300,11 +1286,11 @@ void MyAvatar::prepareForPhysicsSimulation() { _characterController.setAvatarPositionAndOrientation(getPosition(), getOrientation()); if (qApp->isHMDMode()) { updateHMDFollowVelocity(); - } else if (_isFollowingHMD) { - _isFollowingHMD = false; - _hmdFollowVelocity = Vectors::ZERO; + } else if (_followSpeed > 0.0f) { + _followVelocity = Vectors::ZERO; + _followSpeed = 0.0f; } - _characterController.setHMDVelocity(_hmdFollowVelocity); + _characterController.setFollowVelocity(_followVelocity); } void MyAvatar::harvestResultsFromPhysicsSimulation() { @@ -1312,35 +1298,27 @@ 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); + if (_followSpeed > 0.0f) { + adjustSensorTransform(); + setVelocity(_characterController.getLinearVelocity() + _followVelocity); } else { setVelocity(_characterController.getLinearVelocity()); } } -void MyAvatar::adjustSensorTransform(glm::vec3 hmdShift) { +void MyAvatar::adjustSensorTransform() { // 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; - } - } + float linearDistance = _characterController.getFollowTime() * _followSpeed; + float blendFactor = linearDistance < _followOffsetDistance ? linearDistance / _followOffsetDistance : 1.0f; 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; - _hmdFollowVelocity = Vectors::ZERO; + _followVelocity = Vectors::ZERO; + _followSpeed = 0.0f; // and slam the body's transform anyway to eliminate any slight errors glm::vec3 finalBodyPosition = extractTranslation(worldBodyMatrix); nextAttitude(finalBodyPosition, finalBodyRotation); @@ -1520,6 +1498,9 @@ void MyAvatar::initAnimGraph() { QUrl::fromLocalFile(PathUtils::resourcesPath() + "meshes/defaultAvatar_full/avatar-animation.json") : _animGraphUrl); _rig->initAnimGraph(graphUrl, _skeletonModel.getGeometry()->getFBXGeometry()); + + _bodySensorMatrix = deriveBodyFromHMDSensor(); // Based on current cached HMD position/rotation.. + updateSensorToWorldMatrix(); // Uses updated position/orientation and _bodySensorMatrix changes } void MyAvatar::destroyAnimGraph() { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 52f1ffce3f..e1c733c625 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -208,7 +208,7 @@ public: void prepareForPhysicsSimulation(); void harvestResultsFromPhysicsSimulation(); - void adjustSensorTransform(glm::vec3 hmdShift); + void adjustSensorTransform(); const QString& getCollisionSoundURL() { return _collisionSoundURL; } void setCollisionSoundURL(const QString& url); @@ -394,9 +394,10 @@ 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 }; + + glm::vec3 _followVelocity { Vectors::ZERO }; + float _followSpeed { 0.0f }; + float _followOffsetDistance { 0.0f }; bool _goToPending; glm::vec3 _goToPosition; @@ -414,9 +415,6 @@ private: glm::vec3 _customListenPosition; glm::quat _customListenOrientation; - bool _isFollowingHMD { false }; - float _followHMDAlpha { 0.0f }; - AtRestDetector _hmdAtRestDetector; bool _lastIsMoving { false }; }; diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index ad2ca32b05..e8f686da6f 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -60,7 +60,7 @@ MyCharacterController::MyCharacterController(MyAvatar* avatar) { _floorDistance = MAX_FALL_HEIGHT; _walkVelocity.setValue(0.0f, 0.0f, 0.0f); - _hmdVelocity.setValue(0.0f, 0.0f, 0.0f); + _followVelocity.setValue(0.0f, 0.0f, 0.0f); _jumpSpeed = JUMP_SPEED; _isOnGround = false; _isJumping = false; @@ -68,7 +68,7 @@ MyCharacterController::MyCharacterController(MyAvatar* avatar) { _isHovering = true; _isPushingUp = false; _jumpToHoverStart = 0; - _lastStepDuration = 0.0f; + _followTime = 0.0f; _pendingFlags = PENDING_FLAG_UPDATE_SHAPE; updateShapeIfNecessary(); @@ -161,16 +161,14 @@ void MyCharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) } } - // Rather than add _hmdVelocity to the velocity of the RigidBody, we explicitly teleport + // Rather than add _followVelocity to the velocity of the RigidBody, we explicitly teleport // the RigidBody forward according to the formula: distance = rate * time - if (_hmdVelocity.length2() > 0.0f) { + if (_followVelocity.length2() > 0.0f) { btTransform bodyTransform = _rigidBody->getWorldTransform(); - bodyTransform.setOrigin(bodyTransform.getOrigin() + dt * _hmdVelocity); + bodyTransform.setOrigin(bodyTransform.getOrigin() + dt * _followVelocity); _rigidBody->setWorldTransform(bodyTransform); } - // MyAvatar will ask us how far we stepped for HMD motion, which will depend on how - // much time has accumulated in _lastStepDuration. - _lastStepDuration += dt; + _followTime += dt; } void MyCharacterController::jump() { @@ -346,8 +344,8 @@ void MyCharacterController::setTargetVelocity(const glm::vec3& velocity) { _walkVelocity = glmToBullet(velocity); } -void MyCharacterController::setHMDVelocity(const glm::vec3& velocity) { - _hmdVelocity = glmToBullet(velocity); +void MyCharacterController::setFollowVelocity(const glm::vec3& velocity) { + _followVelocity = glmToBullet(velocity); } glm::vec3 MyCharacterController::getLinearVelocity() const { @@ -400,7 +398,7 @@ void MyCharacterController::preSimulation() { } } } - _lastStepDuration = 0.0f; + _followTime = 0.0f; } void MyCharacterController::postSimulation() { diff --git a/interface/src/avatar/MyCharacterController.h b/interface/src/avatar/MyCharacterController.h index de711c84f4..82aa958309 100644 --- a/interface/src/avatar/MyCharacterController.h +++ b/interface/src/avatar/MyCharacterController.h @@ -64,8 +64,8 @@ public: void getAvatarPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const; void setTargetVelocity(const glm::vec3& velocity); - void setHMDVelocity(const glm::vec3& velocity); - glm::vec3 getHMDShift() const { return _lastStepDuration * bulletToGLM(_hmdVelocity); } + void setFollowVelocity(const glm::vec3& velocity); + float getFollowTime() const { return _followTime; } glm::vec3 getLinearVelocity() const; @@ -75,7 +75,7 @@ protected: protected: btVector3 _currentUp; btVector3 _walkVelocity; - btVector3 _hmdVelocity; + btVector3 _followVelocity; btTransform _avatarBodyTransform; glm::vec3 _shapeLocalOffset; @@ -93,7 +93,7 @@ protected: btScalar _gravity; btScalar _jumpSpeed; - btScalar _lastStepDuration; + btScalar _followTime; bool _enabled; bool _isOnGround;