From 59e6ca8f8d0ae0dc15e155e35e0b918fde970d20 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 08:29:02 -0700 Subject: [PATCH] avatar follows HMD using velocity motor --- interface/src/avatar/MyAvatar.cpp | 4 +- libraries/physics/src/CharacterController.cpp | 104 +++++++----------- libraries/physics/src/CharacterController.h | 7 -- 3 files changed, 38 insertions(+), 77 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index cd4ee4784b..c8b88bea32 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1387,9 +1387,7 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { //_bodySensorMatrix = deriveBodyFromHMDSensor(); if (_characterController.isEnabledAndReady()) { - setVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity()); - } else { - setVelocity(getVelocity() + _characterController.getFollowVelocity()); + setVelocity(_characterController.getLinearVelocity()); } _follow.postPhysicsUpdate(*this); diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 1372a6b304..ccd985c1ad 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -75,9 +75,6 @@ CharacterController::CharacterController() { _takeoffToInAirStartTime = 0; _jumpButtonDownStartTime = 0; _jumpButtonDownCount = 0; - _followTime = 0.0f; - _followLinearDisplacement = btVector3(0, 0, 0); - _followAngularDisplacement = btQuaternion::getIdentity(); _hasSupport = false; _pendingFlags = PENDING_FLAG_UPDATE_SHAPE; @@ -203,6 +200,43 @@ const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED; void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; + if (_following) { + // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned + const float NORMAL_WALKING_SPEED = 2.5f; // actual walk speed is 2.5 m/sec + const float FOLLOW_TIMESCALE = 0.8f; + const float FOLLOW_ROTATION_THRESHOLD = PI / 6.0f; + const float FOLLOW_FACTOR = 0.5f; + const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIMESCALE; + + // linear part uses a motor + btTransform bodyTransform = _rigidBody->getWorldTransform(); + btVector3 startPos = bodyTransform.getOrigin(); + btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; + btVector3 vel = deltaPos * (FOLLOW_FACTOR / dt); + btScalar speed = vel.length(); + if (speed > NORMAL_WALKING_SPEED) { + vel *= NORMAL_WALKING_SPEED / speed; + } + const float HORIZONTAL_FOLLOW_TIMESCALE = 0.01f; // a very small timescale here is OK + const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; + glm::quat motorRotation; + addMotor(bulletToGLM(vel), motorRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); + + // angular part uses incremental teleports + btQuaternion startRot = bodyTransform.getRotation(); + glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot)); + 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 = 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; + _rigidBody->setWorldTransform(btTransform(endRot, startPos)); + } computeNewVelocity(dt, velocity); if (_moveKinematically) { @@ -222,50 +256,6 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { // This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate(). _rigidBody->setLinearVelocity(velocity + _parentVelocity); - if (_following) { - // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned - const float NORMAL_WALKING_SPEED = 1.5f; // actual walk speed is 2.5 m/sec - const float FOLLOW_TIME = 0.8f; - const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); - const float FOLLOW_FACTOR = 0.5f; - - 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 * (FOLLOW_FACTOR / 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(); - glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot)); - 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 = 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; - - // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account. - btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset); - btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset); - - _followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement; - _followAngularDisplacement = angularDisplacement * _followAngularDisplacement; - - _rigidBody->setWorldTransform(btTransform(endRot, endPos)); - } - _followTime += dt; _ghost.setWorldTransform(_rigidBody->getWorldTransform()); } } @@ -422,22 +412,6 @@ void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyM _following = true; } -glm::vec3 CharacterController::getFollowLinearDisplacement() const { - return bulletToGLM(_followLinearDisplacement); -} - -glm::quat CharacterController::getFollowAngularDisplacement() const { - return bulletToGLM(_followAngularDisplacement); -} - -glm::vec3 CharacterController::getFollowVelocity() const { - if (_followTime > 0.0f) { - return bulletToGLM(_followLinearDisplacement) / _followTime; - } else { - return glm::vec3(); - } -} - glm::vec3 CharacterController::getLinearVelocity() const { glm::vec3 velocity(0.0f); if (_rigidBody) { @@ -700,10 +674,6 @@ void CharacterController::preSimulation() { _previousFlags = _pendingFlags; _pendingFlags &= ~PENDING_FLAG_JUMP; - - _followTime = 0.0f; - _followLinearDisplacement = btVector3(0.0f, 0.0f, 0.0f); - _followAngularDisplacement = btQuaternion::getIdentity(); } void CharacterController::postSimulation() { diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 7f112ca385..a9ce30c4d9 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -88,10 +88,6 @@ public: void setParentVelocity(const glm::vec3& parentVelocity); void setFollowParameters(const glm::mat4& desiredWorldBodyMatrix); void disableFollow() { _following = false; } - float getFollowTime() const { return _followTime; } - glm::vec3 getFollowLinearDisplacement() const; - glm::quat getFollowAngularDisplacement() const; - glm::vec3 getFollowVelocity() const; glm::vec3 getLinearVelocity() const; glm::vec3 getVelocityChange() const; @@ -177,9 +173,6 @@ protected: btScalar _gravity; btScalar _jumpSpeed; - btScalar _followTime; - btVector3 _followLinearDisplacement; - btQuaternion _followAngularDisplacement; btVector3 _linearAcceleration; bool _following { false };