From 80b970f2d985da7c872640f284ace633ef6737c0 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 19 Sep 2016 08:58:19 -0700 Subject: [PATCH 01/11] adding MyAvatar::_canonicalScale, but not used yet --- interface/src/avatar/MyAvatar.cpp | 2 ++ interface/src/avatar/MyAvatar.h | 1 + 2 files changed, 3 insertions(+) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index e5ea9fe1af..245eba0098 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1229,6 +1229,8 @@ void MyAvatar::rebuildCollisionShape() { float scale = getUniformScale(); float radius = scale * _skeletonModel->getBoundingCapsuleRadius(); float height = scale * _skeletonModel->getBoundingCapsuleHeight() + 2.0f * radius; + const float CANONICAL_AVATAR_HEIGHT = 2.0f; + _canonicalScale = height / CANONICAL_AVATAR_HEIGHT; glm::vec3 corner(-radius, -0.5f * height, -radius); corner += scale * _skeletonModel->getBoundingCapsuleOffset(); glm::vec3 diagonal(2.0f * radius, height, 2.0f * radius); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 87daba0267..6e00894cb3 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -500,6 +500,7 @@ private: bool _hmdLeanRecenterEnabled = true; bool _moveKinematically { false }; // KINEMATIC_CONTROLLER_HACK + float _canonicalScale { 1.0f }; float AVATAR_MOVEMENT_ENERGY_CONSTANT { 0.001f }; float AUDIO_ENERGY_CONSTANT { 0.000001f }; From 1db1295556638a60f84e0ba72fe0425d0ec195af Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 19 Sep 2016 08:58:51 -0700 Subject: [PATCH 02/11] cleanup around CharacterController::_targetVelocity --- libraries/physics/src/CharacterController.cpp | 14 +++++++++----- libraries/physics/src/CharacterController.h | 1 - 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 97d505577e..7bb8d98ebe 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -135,6 +135,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { _ghost.setMinWallAngle(PI / 4.0f); // HACK _ghost.setUpDirection(_currentUp); _ghost.setGravity(DEFAULT_CHARACTER_GRAVITY); + _ghost.setWorldTransform(_rigidBody->getWorldTransform()); } if (_dynamicsWorld) { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { @@ -209,7 +210,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { btTransform transform = _rigidBody->getWorldTransform(); transform.setOrigin(_ghost.getWorldTransform().getOrigin()); _ghost.setWorldTransform(transform); - _ghost.setMotorVelocity(_simpleMotorVelocity); + _ghost.setMotorVelocity(_targetVelocity); float overshoot = 1.0f * _radius; _ghost.move(dt, overshoot); _rigidBody->setWorldTransform(_ghost.getWorldTransform()); @@ -525,7 +526,7 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel // add components back together and rotate into world-frame velocity = (hVelocity + vVelocity).rotate(axis, angle); - _simpleMotorVelocity += maxTau * (hTargetVelocity + vTargetVelocity).rotate(axis, angle); + _targetVelocity += maxTau * (hTargetVelocity + vTargetVelocity).rotate(axis, angle); // store velocity and weights velocities.push_back(velocity); @@ -543,7 +544,7 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { velocities.reserve(_motors.size()); std::vector weights; weights.reserve(_motors.size()); - _simpleMotorVelocity = btVector3(0.0f, 0.0f, 0.0f); + _targetVelocity = btVector3(0.0f, 0.0f, 0.0f); for (int i = 0; i < (int)_motors.size(); ++i) { applyMotor(i, dt, velocity, velocities, weights); } @@ -559,15 +560,18 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { for (size_t i = 0; i < velocities.size(); ++i) { velocity += (weights[i] / totalWeight) * velocities[i]; } - _simpleMotorVelocity /= totalWeight; + _targetVelocity /= totalWeight; } if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) { velocity = btVector3(0.0f, 0.0f, 0.0f); } // 'thrust' is applied at the very end + _targetVelocity += dt * _linearAcceleration; velocity += dt * _linearAcceleration; - _targetVelocity = velocity; + // Note the differences between these two variables: + // _targetVelocity = ideal final velocity according to input + // velocity = real final velocity after motors are applied to current velocity } void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) { diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 2a3a81b416..85a02cd56a 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -152,7 +152,6 @@ protected: btVector3 _parentVelocity; btVector3 _preSimulationVelocity; btVector3 _velocityChange; - btVector3 _simpleMotorVelocity; // KINEMATIC_CONTROLLER_HACK btTransform _followDesiredBodyTransform; btTransform _characterBodyTransform; From 8dd5c9b92b66c0e0917b72dce2daf679dc60dbcd Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 19 Sep 2016 12:40:16 -0700 Subject: [PATCH 03/11] fix kinematic motion for ground and hover --- interface/src/avatar/MyAvatar.cpp | 3 +- libraries/physics/src/CharacterController.cpp | 26 +++-- libraries/physics/src/CharacterController.h | 4 +- .../physics/src/CharacterGhostObject.cpp | 105 +++++++++++------- libraries/physics/src/CharacterGhostObject.h | 14 ++- 5 files changed, 93 insertions(+), 59 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 245eba0098..cd4ee4784b 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1378,7 +1378,8 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { glm::vec3 position = getPosition(); glm::quat orientation = getOrientation(); if (_characterController.isEnabledAndReady()) { - _characterController.getPositionAndOrientation(position, orientation); + glm::quat bogusOrientation; + _characterController.getPositionAndOrientation(position, bogusOrientation); } nextAttitude(position, orientation); diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 7bb8d98ebe..acd82d73ea 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -130,7 +130,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { // KINEMATIC_CONTROLLER_HACK _ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup)); _ghost.setCollisionWorld(_dynamicsWorld); - _ghost.setDistanceToFeet(_radius + _halfHeight); + _ghost.setRadiusAndHalfHeight(_radius, _halfHeight); _ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); // HACK _ghost.setMinWallAngle(PI / 4.0f); // HACK _ghost.setUpDirection(_currentUp); @@ -177,10 +177,10 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) cons void CharacterController::preStep(btCollisionWorld* collisionWorld) { // trace a ray straight down to see if we're standing on the ground - const btTransform& xform = _rigidBody->getWorldTransform(); + const btTransform& transform = _rigidBody->getWorldTransform(); // rayStart is at center of bottom sphere - btVector3 rayStart = xform.getOrigin() - _halfHeight * _currentUp; + btVector3 rayStart = transform.getOrigin() - _halfHeight * _currentUp; // rayEnd is some short distance outside bottom sphere const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius; @@ -213,7 +213,8 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { _ghost.setMotorVelocity(_targetVelocity); float overshoot = 1.0f * _radius; _ghost.move(dt, overshoot); - _rigidBody->setWorldTransform(_ghost.getWorldTransform()); + transform.setOrigin(_ghost.getWorldTransform().getOrigin()); + _rigidBody->setWorldTransform(transform); _rigidBody->setLinearVelocity(_ghost.getLinearVelocity()); } else { // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform. @@ -400,9 +401,8 @@ void CharacterController::setPositionAndOrientation( // TODO: update gravity if up has changed updateUpAxis(orientation); - btQuaternion bodyOrientation = glmToBullet(orientation); - btVector3 bodyPosition = glmToBullet(position + orientation * _shapeLocalOffset); - _characterBodyTransform = btTransform(bodyOrientation, bodyPosition); + _rotation = glmToBullet(orientation); + _position = glmToBullet(position + orientation * _shapeLocalOffset); } void CharacterController::getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const { @@ -485,10 +485,11 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel if (tau > 1.0f) { tau = 1.0f; } - velocity += (motor.velocity - velocity) * tau; + velocity += tau * (motor.velocity - velocity); // rotate back into world-frame velocity = velocity.rotate(axis, angle); + _targetVelocity += (tau * motor.velocity).rotate(axis, angle); // store the velocity and weight velocities.push_back(velocity); @@ -584,14 +585,14 @@ void CharacterController::preSimulation() { if (_dynamicsWorld) { quint64 now = usecTimestampNow(); - // slam body to where it is supposed to be - _rigidBody->setWorldTransform(_characterBodyTransform); + // slam body transform + _rigidBody->setWorldTransform(btTransform(btTransform(_rotation, _position))); btVector3 velocity = _rigidBody->getLinearVelocity(); _preSimulationVelocity = velocity; // scan for distant floor // rayStart is at center of bottom sphere - btVector3 rayStart = _characterBodyTransform.getOrigin(); + btVector3 rayStart = _position; // rayEnd is straight down MAX_FALL_HEIGHT btScalar rayLength = _radius + MAX_FALL_HEIGHT; @@ -679,6 +680,9 @@ void CharacterController::preSimulation() { } break; } + if (_moveKinematically && _ghost.isHovering()) { + SET_STATE(State::Hover, "kinematic motion"); // HACK + } } else { // OUTOFBODY_HACK -- in collisionless state switch between Ground and Hover states if (rayHasHit) { diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 85a02cd56a..74649d1572 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -153,7 +153,9 @@ protected: btVector3 _preSimulationVelocity; btVector3 _velocityChange; btTransform _followDesiredBodyTransform; - btTransform _characterBodyTransform; + btVector3 _position; + btQuaternion _rotation; + //btTransform _characterBodyTransform; glm::vec3 _shapeLocalOffset; diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index 6529f2c944..a4b6e90266 100755 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -14,6 +14,8 @@ #include #include +#include + #include "CharacterGhostShape.h" #include "CharacterRayResult.h" @@ -38,6 +40,10 @@ void CharacterGhostObject::getCollisionGroupAndMask(int16_t& group, int16_t& mas mask = _collisionFilterMask; } +void CharacterGhostObject::setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight) { + _radius = radius; + _halfHeight = halfHeight; +} void CharacterGhostObject::setUpDirection(const btVector3& up) { btScalar length = up.length(); @@ -99,10 +105,12 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { // TODO: figure out how to untrap character } + btTransform startTransform = getWorldTransform(); + btVector3 startPosition = startTransform.getOrigin(); if (_onFloor) { - // a floor was identified during resolvePenetration() - _hovering = false; - updateTraction(); + // resolvePenetration() pushed the avatar out of a floor so + // we must updateTraction() before using _linearVelocity + updateTraction(startPosition); } btVector3 forwardSweep = dt * _linearVelocity; @@ -110,7 +118,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { btScalar MIN_SWEEP_DISTANCE = 0.0001f; if (stepDistance < MIN_SWEEP_DISTANCE) { // not moving, no need to sweep - updateHoverState(getWorldTransform()); + updateTraction(startPosition); return; } @@ -128,22 +136,19 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { // step forward CharacterSweepResult result(this); - btTransform startTransform = getWorldTransform(); - btTransform transform = startTransform; - btTransform nextTransform = transform; - nextTransform.setOrigin(transform.getOrigin() + forwardSweep); - sweepTest(convexShape, transform, nextTransform, result); // forward + btTransform nextTransform = startTransform; + nextTransform.setOrigin(startPosition + forwardSweep); + sweepTest(convexShape, startTransform, nextTransform, result); // forward if (!result.hasHit()) { - nextTransform.setOrigin(transform.getOrigin() + (stepDistance / longSweepDistance) * forwardSweep); + nextTransform.setOrigin(startPosition + (stepDistance / longSweepDistance) * forwardSweep); setWorldTransform(nextTransform); - updateHoverState(nextTransform); - updateTraction(); + updateTraction(nextTransform.getOrigin()); return; } // check if this hit is obviously unsteppable - btVector3 hitFromBase = result.m_hitPointWorld - (transform.getOrigin() - (_distanceToFeet * _upDirection)); + btVector3 hitFromBase = result.m_hitPointWorld - (startPosition - ((_radius + _halfHeight) * _upDirection)); btScalar hitHeight = hitFromBase.dot(_upDirection); if (hitHeight > _maxStepHeight) { // capsule can't step over the obstacle so move forward as much as possible before we bail @@ -152,8 +157,8 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { if (forwardDistance > stepDistance) { forwardTranslation *= stepDistance / forwardDistance; } - transform.setOrigin(transform.getOrigin() + forwardTranslation); - setWorldTransform(transform); + nextTransform.setOrigin(startPosition + forwardTranslation); + setWorldTransform(nextTransform); return; } // if we get here then we hit something that might be steppable @@ -166,35 +171,37 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { // raise by availableStepHeight before sweeping forward result.resetHitHistory(); - transform.setOrigin(startTransform.getOrigin() + availableStepHeight * _upDirection); - nextTransform.setOrigin(transform.getOrigin() + forwardSweep); - sweepTest(convexShape, transform, nextTransform, result); + startTransform.setOrigin(startPosition + availableStepHeight * _upDirection); + nextTransform.setOrigin(startTransform.getOrigin() + forwardSweep); + sweepTest(convexShape, startTransform, nextTransform, result); if (result.hasHit()) { - transform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * forwardSweep); + startTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * forwardSweep); } else { - transform = nextTransform; + startTransform = nextTransform; } // sweep down in search of future landing spot result.resetHitHistory(); - btVector3 downSweep = (dt * _linearVelocity.dot(_upDirection) - availableStepHeight) * _upDirection; - nextTransform.setOrigin(transform.getOrigin() + downSweep); - sweepTest(convexShape, transform, nextTransform, result); + btVector3 downSweep = (- availableStepHeight) * _upDirection; + nextTransform.setOrigin(startTransform.getOrigin() + downSweep); + sweepTest(convexShape, startTransform, nextTransform, result); if (result.hasHit() && result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) { // can stand on future landing spot, so we interpolate toward it _floorNormal = result.m_hitNormalWorld; + _floorContact = result.m_hitPointWorld; _onFloor = true; _hovering = false; - nextTransform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * downSweep); - btVector3 totalStep = nextTransform.getOrigin() - startTransform.getOrigin(); - transform.setOrigin(startTransform.getOrigin() + (stepDistance / totalStep.length()) * totalStep); + nextTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * downSweep); + btVector3 totalStep = nextTransform.getOrigin() - startPosition; + nextTransform.setOrigin(startPosition + (stepDistance / totalStep.length()) * totalStep); + updateTraction(nextTransform.getOrigin()); } else { // either there is no future landing spot, or there is but we can't stand on it // in any case: we go forward as much as possible - transform.setOrigin(startTransform.getOrigin() + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep); + nextTransform.setOrigin(startPosition + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep); + updateTraction(nextTransform.getOrigin()); } - setWorldTransform(transform); - updateTraction(); + setWorldTransform(nextTransform); } bool CharacterGhostObject::sweepTest( @@ -297,6 +304,11 @@ bool CharacterGhostObject::resolvePenetration(int numTries) { if (normalDotUp > _maxWallNormalUpComponent) { mostFloorPenetration = penetrationDepth; _floorNormal = normal; + if (directionSign > 0.0f) { + _floorContact = pt.m_positionWorldOnA; + } else { + _floorContact = pt.m_positionWorldOnB; + } _onFloor = true; } } @@ -327,17 +339,36 @@ void CharacterGhostObject::refreshOverlappingPairCache() { void CharacterGhostObject::updateVelocity(btScalar dt) { if (_hovering) { - _linearVelocity *= 0.99f; // HACK damping + _linearVelocity *= 0.999f; // HACK damping } else { _linearVelocity += (dt * _gravity) * _upDirection; } } -void CharacterGhostObject::updateTraction() { +void CharacterGhostObject::updateHoverState(const btVector3& position) { + if (_onFloor) { + _hovering = false; + } else { + // cast a ray down looking for floor support + CharacterRayResult rayResult(this); + btScalar distanceToFeet = _radius + _halfHeight; + btScalar slop = 2.0f * getCollisionShape()->getMargin(); // slop to help ray start OUTSIDE the floor object + btVector3 startPos = position - ((distanceToFeet - slop) * _upDirection); + btVector3 endPos = startPos - (2.0f * distanceToFeet) * _upDirection; + rayTest(startPos, endPos, rayResult); + // we're hovering if the ray didn't hit anything or hit unstandable slope + _hovering = !rayResult.hasHit() || rayResult.m_hitNormalWorld.dot(_upDirection) < _maxWallNormalUpComponent; + } +} + +void CharacterGhostObject::updateTraction(const btVector3& position) { + updateHoverState(position); if (_hovering) { _linearVelocity = _motorVelocity; } else if (_onFloor) { - btVector3 pathDirection = _floorNormal.cross(_motorVelocity).cross(_floorNormal); + // compute a velocity that swings the capsule around the _floorContact + btVector3 leverArm = _floorContact - position; + btVector3 pathDirection = leverArm.cross(_motorVelocity.cross(leverArm)); btScalar pathLength = pathDirection.length(); if (pathLength > FLT_EPSILON) { _linearVelocity = (_motorSpeed / pathLength) * pathDirection; @@ -360,13 +391,3 @@ btScalar CharacterGhostObject::measureAvailableStepHeight() const { return result.m_closestHitFraction * _maxStepHeight; } -void CharacterGhostObject::updateHoverState(const btTransform& transform) { - // cast a ray down looking for floor support - CharacterRayResult rayResult(this); - btVector3 startPos = transform.getOrigin() - ((_distanceToFeet - 0.1f) * _upDirection); // 0.1 HACK to make ray hit - btVector3 endPos = startPos - (2.0f * _distanceToFeet) * _upDirection; - rayTest(startPos, endPos, rayResult); - // we're hovering if the ray didn't hit an object we can stand on - _hovering = !(rayResult.hasHit() && rayResult.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent); -} - diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h index dd2f694a59..274057a907 100755 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -31,7 +31,7 @@ public: void setCollisionGroupAndMask(int16_t group, int16_t mask); void getCollisionGroupAndMask(int16_t& group, int16_t& mask) const; - void setDistanceToFeet(btScalar distance) { _distanceToFeet = distance; } + void setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight); void setUpDirection(const btVector3& up); void setMotorVelocity(const btVector3& velocity); void setGravity(btScalar gravity) { _gravity = gravity; } // NOTE: we expect _gravity to be negative (in _upDirection) @@ -50,6 +50,9 @@ public: const btTransform& start, const btTransform& end, CharacterSweepResult& result) const; + + bool isHovering() const { return _hovering; } + protected: void removeFromWorld(); void addToWorld(); @@ -61,17 +64,20 @@ protected: bool resolvePenetration(int numTries); void refreshOverlappingPairCache(); void updateVelocity(btScalar dt); - void updateTraction(); + void updateTraction(const btVector3& position); btScalar measureAvailableStepHeight() const; - void updateHoverState(const btTransform& transform); + void updateHoverState(const btVector3& position); protected: btVector3 _upDirection { 0.0f, 1.0f, 0.0f }; // input, up in world-frame btVector3 _motorVelocity { 0.0f, 0.0f, 0.0f }; // input, velocity character is trying to achieve btVector3 _linearVelocity { 0.0f, 0.0f, 0.0f }; // internal, actual character velocity btVector3 _floorNormal { 0.0f, 0.0f, 0.0f }; // internal, probable floor normal + btVector3 _floorContact { 0.0f, 0.0f, 0.0f }; // internal, last floor contact point btCollisionWorld* _world { nullptr }; // input, pointer to world - btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape + //btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape + btScalar _halfHeight { 0.0f }; + btScalar _radius { 0.0f }; btScalar _motorSpeed { 0.0f }; // internal, cached for speed btScalar _gravity { 0.0f }; // input, amplitude of gravity along _upDirection (should be negative) btScalar _maxWallNormalUpComponent { 0.0f }; // input: max vertical component of wall normal From a8af8d6027e7fc9c1145ecddaeb317cf27c717b7 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 19 Sep 2016 15:03:03 -0700 Subject: [PATCH 04/11] move code into CharacterController::updateState() --- libraries/physics/src/CharacterController.cpp | 197 +++++++++--------- libraries/physics/src/CharacterController.h | 1 + 2 files changed, 100 insertions(+), 98 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index acd82d73ea..1372a6b304 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -581,116 +581,121 @@ void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) { velocity = bulletToGLM(btVelocity); } -void CharacterController::preSimulation() { - if (_dynamicsWorld) { - quint64 now = usecTimestampNow(); +void CharacterController::updateState() { + const btScalar FLY_TO_GROUND_THRESHOLD = 0.1f * _radius; + const btScalar GROUND_TO_FLY_THRESHOLD = 0.8f * _radius + _halfHeight; + const quint64 TAKE_OFF_TO_IN_AIR_PERIOD = 250 * MSECS_PER_SECOND; + const btScalar MIN_HOVER_HEIGHT = 2.5f; + const quint64 JUMP_TO_HOVER_PERIOD = 1100 * MSECS_PER_SECOND; - // slam body transform - _rigidBody->setWorldTransform(btTransform(btTransform(_rotation, _position))); - btVector3 velocity = _rigidBody->getLinearVelocity(); - _preSimulationVelocity = velocity; + // scan for distant floor + // rayStart is at center of bottom sphere + btVector3 rayStart = _position; - // scan for distant floor - // rayStart is at center of bottom sphere - btVector3 rayStart = _position; + // rayEnd is straight down MAX_FALL_HEIGHT + btScalar rayLength = _radius + MAX_FALL_HEIGHT; + btVector3 rayEnd = rayStart - rayLength * _currentUp; - // rayEnd is straight down MAX_FALL_HEIGHT - btScalar rayLength = _radius + MAX_FALL_HEIGHT; - btVector3 rayEnd = rayStart - rayLength * _currentUp; - - const btScalar FLY_TO_GROUND_THRESHOLD = 0.1f * _radius; - const btScalar GROUND_TO_FLY_THRESHOLD = 0.8f * _radius + _halfHeight; - const quint64 TAKE_OFF_TO_IN_AIR_PERIOD = 250 * MSECS_PER_SECOND; - const btScalar MIN_HOVER_HEIGHT = 2.5f; - const quint64 JUMP_TO_HOVER_PERIOD = 1100 * MSECS_PER_SECOND; - const btScalar MAX_WALKING_SPEED = 2.5f; + ClosestNotMe rayCallback(_rigidBody); + rayCallback.m_closestHitFraction = 1.0f; + _dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback); + bool rayHasHit = rayCallback.hasHit(); + quint64 now = usecTimestampNow(); + if (rayHasHit) { + _rayHitStartTime = now; + _floorDistance = rayLength * rayCallback.m_closestHitFraction - (_radius + _halfHeight); + } else { const quint64 RAY_HIT_START_PERIOD = 500 * MSECS_PER_SECOND; - - ClosestNotMe rayCallback(_rigidBody); - rayCallback.m_closestHitFraction = 1.0f; - _dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback); - bool rayHasHit = rayCallback.hasHit(); - if (rayHasHit) { - _rayHitStartTime = now; - _floorDistance = rayLength * rayCallback.m_closestHitFraction - (_radius + _halfHeight); - } else if ((now - _rayHitStartTime) < RAY_HIT_START_PERIOD) { + if ((now - _rayHitStartTime) < RAY_HIT_START_PERIOD) { rayHasHit = true; } else { _floorDistance = FLT_MAX; } + } - // record a time stamp when the jump button was first pressed. - if ((_previousFlags & PENDING_FLAG_JUMP) != (_pendingFlags & PENDING_FLAG_JUMP)) { - if (_pendingFlags & PENDING_FLAG_JUMP) { - _jumpButtonDownStartTime = now; - _jumpButtonDownCount++; - } + // record a time stamp when the jump button was first pressed. + bool jumpButtonHeld = _pendingFlags & PENDING_FLAG_JUMP; + if ((_previousFlags & PENDING_FLAG_JUMP) != (_pendingFlags & PENDING_FLAG_JUMP)) { + if (_pendingFlags & PENDING_FLAG_JUMP) { + _jumpButtonDownStartTime = now; + _jumpButtonDownCount++; } + } - bool jumpButtonHeld = _pendingFlags & PENDING_FLAG_JUMP; + btVector3 velocity = _preSimulationVelocity; - btVector3 actualHorizVelocity = velocity - velocity.dot(_currentUp) * _currentUp; - bool flyingFast = _state == State::Hover && actualHorizVelocity.length() > (MAX_WALKING_SPEED * 0.75f); - - // OUTOFBODY_HACK -- disable normal state transitions while collisionless - if (_collisionGroup == BULLET_COLLISION_GROUP_MY_AVATAR) { - switch (_state) { - case State::Ground: - if (!rayHasHit && !_hasSupport) { - SET_STATE(State::Hover, "no ground detected"); - } else if (_pendingFlags & PENDING_FLAG_JUMP && _jumpButtonDownCount != _takeoffJumpButtonID) { - _takeoffJumpButtonID = _jumpButtonDownCount; - _takeoffToInAirStartTime = now; - SET_STATE(State::Takeoff, "jump pressed"); - } else if (rayHasHit && !_hasSupport && _floorDistance > GROUND_TO_FLY_THRESHOLD) { - SET_STATE(State::InAir, "falling"); - } - break; - case State::Takeoff: - if (!rayHasHit && !_hasSupport) { - SET_STATE(State::Hover, "no ground"); - } else if ((now - _takeoffToInAirStartTime) > TAKE_OFF_TO_IN_AIR_PERIOD) { - SET_STATE(State::InAir, "takeoff done"); - velocity += _jumpSpeed * _currentUp; - _rigidBody->setLinearVelocity(velocity); - } - break; - case State::InAir: { - if ((velocity.dot(_currentUp) <= (JUMP_SPEED / 2.0f)) && ((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport)) { - SET_STATE(State::Ground, "hit ground"); - } else { - btVector3 desiredVelocity = _targetVelocity; - if (desiredVelocity.length2() < MIN_TARGET_SPEED_SQUARED) { - desiredVelocity = btVector3(0.0f, 0.0f, 0.0f); - } - bool vertTargetSpeedIsNonZero = desiredVelocity.dot(_currentUp) > MIN_TARGET_SPEED; - if ((jumpButtonHeld || vertTargetSpeedIsNonZero) && (_takeoffJumpButtonID != _jumpButtonDownCount)) { - SET_STATE(State::Hover, "double jump button"); - } else if ((jumpButtonHeld || vertTargetSpeedIsNonZero) && (now - _jumpButtonDownStartTime) > JUMP_TO_HOVER_PERIOD) { - SET_STATE(State::Hover, "jump button held"); - } - } - break; + // OUTOFBODY_HACK -- disable normal state transitions while collisionless + if (_collisionGroup == BULLET_COLLISION_GROUP_MY_AVATAR) { + switch (_state) { + case State::Ground: + if (!rayHasHit && !_hasSupport) { + SET_STATE(State::Hover, "no ground detected"); + } else if (_pendingFlags & PENDING_FLAG_JUMP && _jumpButtonDownCount != _takeoffJumpButtonID) { + _takeoffJumpButtonID = _jumpButtonDownCount; + _takeoffToInAirStartTime = now; + SET_STATE(State::Takeoff, "jump pressed"); + } else if (rayHasHit && !_hasSupport && _floorDistance > GROUND_TO_FLY_THRESHOLD) { + SET_STATE(State::InAir, "falling"); } - case State::Hover: - if ((_floorDistance < MIN_HOVER_HEIGHT) && !jumpButtonHeld && !flyingFast) { - SET_STATE(State::InAir, "near ground"); - } else if (((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport) && !flyingFast) { - SET_STATE(State::Ground, "touching ground"); - } - break; + break; + case State::Takeoff: + if (!rayHasHit && !_hasSupport) { + SET_STATE(State::Hover, "no ground"); + } else if ((now - _takeoffToInAirStartTime) > TAKE_OFF_TO_IN_AIR_PERIOD) { + SET_STATE(State::InAir, "takeoff done"); + velocity += _jumpSpeed * _currentUp; + _rigidBody->setLinearVelocity(velocity); } - if (_moveKinematically && _ghost.isHovering()) { - SET_STATE(State::Hover, "kinematic motion"); // HACK - } - } else { - // OUTOFBODY_HACK -- in collisionless state switch between Ground and Hover states - if (rayHasHit) { - SET_STATE(State::Ground, "collisionless above ground"); + break; + case State::InAir: { + if ((velocity.dot(_currentUp) <= (JUMP_SPEED / 2.0f)) && ((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport)) { + SET_STATE(State::Ground, "hit ground"); } else { - SET_STATE(State::Hover, "collisionless in air"); + btVector3 desiredVelocity = _targetVelocity; + if (desiredVelocity.length2() < MIN_TARGET_SPEED_SQUARED) { + desiredVelocity = btVector3(0.0f, 0.0f, 0.0f); + } + bool vertTargetSpeedIsNonZero = desiredVelocity.dot(_currentUp) > MIN_TARGET_SPEED; + if ((jumpButtonHeld || vertTargetSpeedIsNonZero) && (_takeoffJumpButtonID != _jumpButtonDownCount)) { + SET_STATE(State::Hover, "double jump button"); + } else if ((jumpButtonHeld || vertTargetSpeedIsNonZero) && (now - _jumpButtonDownStartTime) > JUMP_TO_HOVER_PERIOD) { + SET_STATE(State::Hover, "jump button held"); + } } + break; } + case State::Hover: + btVector3 actualHorizVelocity = velocity - velocity.dot(_currentUp) * _currentUp; + const btScalar MAX_WALKING_SPEED = 2.5f; + bool flyingFast = _state == State::Hover && actualHorizVelocity.length() > (MAX_WALKING_SPEED * 0.75f); + + if ((_floorDistance < MIN_HOVER_HEIGHT) && !jumpButtonHeld && !flyingFast) { + SET_STATE(State::InAir, "near ground"); + } else if (((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport) && !flyingFast) { + SET_STATE(State::Ground, "touching ground"); + } + break; + } + if (_moveKinematically && _ghost.isHovering()) { + SET_STATE(State::Hover, "kinematic motion"); // HACK + } + } else { + // OUTOFBODY_HACK -- in collisionless state switch only between Ground and Hover states + if (rayHasHit) { + SET_STATE(State::Ground, "collisionless above ground"); + } else { + SET_STATE(State::Hover, "collisionless in air"); + } + } +} + +void CharacterController::preSimulation() { + if (_dynamicsWorld) { + // slam body transform and remember velocity + _rigidBody->setWorldTransform(btTransform(btTransform(_rotation, _position))); + _preSimulationVelocity = _rigidBody->getLinearVelocity(); + + updateState(); } _previousFlags = _pendingFlags; @@ -702,13 +707,9 @@ void CharacterController::preSimulation() { } void CharacterController::postSimulation() { - // postSimulation() exists for symmetry and just in case we need to do something here later - - btVector3 velocity = _rigidBody->getLinearVelocity(); - _velocityChange = velocity - _preSimulationVelocity; + _velocityChange = _rigidBody->getLinearVelocity() - _preSimulationVelocity; } - bool CharacterController::getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation) { if (!_rigidBody) { return false; diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 74649d1572..7f112ca385 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -109,6 +109,7 @@ public: }; State getState() const { return _state; } + void updateState(); void setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale); From 59e6ca8f8d0ae0dc15e155e35e0b918fde970d20 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 08:29:02 -0700 Subject: [PATCH 05/11] 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 }; From 7258835f3f481da9f698a03e2fbcf114adee8e3b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 09:14:11 -0700 Subject: [PATCH 06/11] restore orientation from physics to Avatar --- interface/src/avatar/MyAvatar.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index c8b88bea32..a1625bd42e 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1378,8 +1378,7 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { glm::vec3 position = getPosition(); glm::quat orientation = getOrientation(); if (_characterController.isEnabledAndReady()) { - glm::quat bogusOrientation; - _characterController.getPositionAndOrientation(position, bogusOrientation); + _characterController.getPositionAndOrientation(position, orientation); } nextAttitude(position, orientation); From 0ef8ef473404ecc9ac99d094cc651564570b83b2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 09:37:51 -0700 Subject: [PATCH 07/11] tune follow speeds --- libraries/physics/src/CharacterController.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index ccd985c1ad..11917e2f70 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -202,25 +202,31 @@ 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 NORMAL_WALKING_SPEED = 2.0f; // actual walk speed is 2.5 m/sec const float FOLLOW_TIMESCALE = 0.8f; + const float ONE_STEP_AT_NORMAL_WALKING_SPEED = FOLLOW_TIMESCALE * NORMAL_WALKING_SPEED; const float FOLLOW_ROTATION_THRESHOLD = PI / 6.0f; - const float FOLLOW_FACTOR = 0.5f; + const float FOLLOW_FACTOR = 0.25f; const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIMESCALE; + const float MIN_DELTA_DISTANCE = 0.01f; // 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; + btScalar deltaDistance = deltaPos.length(); + if (deltaDistance > MIN_DELTA_DISTANCE) { + btVector3 vel = deltaPos; + if (deltaDistance > ONE_STEP_AT_NORMAL_WALKING_SPEED) { + vel *= NORMAL_WALKING_SPEED / deltaDistance; + } else { + vel *= NORMAL_WALKING_SPEED * (deltaDistance / ONE_STEP_AT_NORMAL_WALKING_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 worldFrameRotation; // identity + addMotor(bulletToGLM(vel), worldFrameRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); } - 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(); From 55e1d058876cf4bbfc9ab60f307b6ceb1d5fa8b4 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 10:11:25 -0700 Subject: [PATCH 08/11] more follow velocity tuning --- libraries/physics/src/CharacterController.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 11917e2f70..0906b43156 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -206,7 +206,6 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { const float FOLLOW_TIMESCALE = 0.8f; const float ONE_STEP_AT_NORMAL_WALKING_SPEED = FOLLOW_TIMESCALE * NORMAL_WALKING_SPEED; const float FOLLOW_ROTATION_THRESHOLD = PI / 6.0f; - const float FOLLOW_FACTOR = 0.25f; const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIMESCALE; const float MIN_DELTA_DISTANCE = 0.01f; @@ -222,7 +221,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { } else { vel *= NORMAL_WALKING_SPEED * (deltaDistance / ONE_STEP_AT_NORMAL_WALKING_SPEED); } - const float HORIZONTAL_FOLLOW_TIMESCALE = 0.01f; // a very small timescale here is OK + const float HORIZONTAL_FOLLOW_TIMESCALE = 0.25f; const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; glm::quat worldFrameRotation; // identity addMotor(bulletToGLM(vel), worldFrameRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); From 3e02bac412d3d1ebe3b2bc3ccee3fbe9fc3da37f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 10:45:13 -0700 Subject: [PATCH 09/11] more velocity tuning --- libraries/physics/src/CharacterController.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 0906b43156..ff868d5485 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -202,7 +202,7 @@ 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.0f; // actual walk speed is 2.5 m/sec + const float NORMAL_WALKING_SPEED = 2.5f; // actual walk speed is 2.5 m/sec const float FOLLOW_TIMESCALE = 0.8f; const float ONE_STEP_AT_NORMAL_WALKING_SPEED = FOLLOW_TIMESCALE * NORMAL_WALKING_SPEED; const float FOLLOW_ROTATION_THRESHOLD = PI / 6.0f; @@ -219,9 +219,9 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { if (deltaDistance > ONE_STEP_AT_NORMAL_WALKING_SPEED) { vel *= NORMAL_WALKING_SPEED / deltaDistance; } else { - vel *= NORMAL_WALKING_SPEED * (deltaDistance / ONE_STEP_AT_NORMAL_WALKING_SPEED); + vel /= FOLLOW_TIMESCALE; } - const float HORIZONTAL_FOLLOW_TIMESCALE = 0.25f; + const float HORIZONTAL_FOLLOW_TIMESCALE = 0.2f; const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; glm::quat worldFrameRotation; // identity addMotor(bulletToGLM(vel), worldFrameRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); From 3c5e13b34b48daf97b60291cbce8eece348d0784 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 13:45:31 -0700 Subject: [PATCH 10/11] final tuning of follow speeds --- libraries/physics/src/CharacterController.cpp | 36 ++++++++++++------- 1 file changed, 23 insertions(+), 13 deletions(-) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index ff868d5485..aa119689f7 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -201,39 +201,49 @@ 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 ONE_STEP_AT_NORMAL_WALKING_SPEED = FOLLOW_TIMESCALE * NORMAL_WALKING_SPEED; - const float FOLLOW_ROTATION_THRESHOLD = PI / 6.0f; - const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIMESCALE; - const float MIN_DELTA_DISTANCE = 0.01f; - // linear part uses a motor + const float MAX_WALKING_SPEED = 2.5f; // TODO: scale this stuff with avatar size + const float MAX_WALKING_SPEED_DISTANCE = 1.0f; + const float NORMAL_WALKING_SPEED = 0.5f * MAX_WALKING_SPEED; + const float NORMAL_WALKING_SPEED_DISTANCE = 0.5f * MAX_WALKING_SPEED_DISTANCE; + const float FEW_SUBSTEPS = 4.0f * dt; btTransform bodyTransform = _rigidBody->getWorldTransform(); btVector3 startPos = bodyTransform.getOrigin(); btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; btScalar deltaDistance = deltaPos.length(); + const float MIN_DELTA_DISTANCE = 0.01f; // TODO: scale by avatar size but cap at (NORMAL_WALKING_SPEED * FEW_SUBSTEPS) if (deltaDistance > MIN_DELTA_DISTANCE) { btVector3 vel = deltaPos; - if (deltaDistance > ONE_STEP_AT_NORMAL_WALKING_SPEED) { - vel *= NORMAL_WALKING_SPEED / deltaDistance; + if (deltaDistance > MAX_WALKING_SPEED_DISTANCE) { + // cap max speed + vel *= MAX_WALKING_SPEED / deltaDistance; + } else if (deltaDistance > NORMAL_WALKING_SPEED_DISTANCE) { + // linearly interpolate to NORMAL_WALKING_SPEED + btScalar alpha = (deltaDistance - NORMAL_WALKING_SPEED_DISTANCE) / (MAX_WALKING_SPEED_DISTANCE - NORMAL_WALKING_SPEED_DISTANCE); + vel *= NORMAL_WALKING_SPEED * (1.0f - alpha) + MAX_WALKING_SPEED * alpha; } else { - vel /= FOLLOW_TIMESCALE; + // use exponential decay but cap at NORMAL_WALKING_SPEED + vel /= FEW_SUBSTEPS; + btScalar speed = vel.length(); + if (speed > NORMAL_WALKING_SPEED) { + vel *= NORMAL_WALKING_SPEED / speed; + } } - const float HORIZONTAL_FOLLOW_TIMESCALE = 0.2f; + const float HORIZONTAL_FOLLOW_TIMESCALE = 0.1f; const float VERTICAL_FOLLOW_TIMESCALE = (_state == State::Hover) ? HORIZONTAL_FOLLOW_TIMESCALE : 20.0f; glm::quat worldFrameRotation; // identity addMotor(bulletToGLM(vel), worldFrameRotation, HORIZONTAL_FOLLOW_TIMESCALE, VERTICAL_FOLLOW_TIMESCALE); } // angular part uses incremental teleports + const float ANGULAR_FOLLOW_TIMESCALE = 0.8f; + const float MAX_ANGULAR_SPEED = (PI / 2.0f) / ANGULAR_FOLLOW_TIMESCALE; 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; + float angularSpeed = deltaAngle / FEW_SUBSTEPS; if (angularSpeed > MAX_ANGULAR_SPEED) { angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed; } From dfe828982b0963ac4260c2ee84cb7e86d680f3e6 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 20 Sep 2016 14:04:22 -0700 Subject: [PATCH 11/11] remove cruft --- libraries/physics/src/CharacterController.h | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index a9ce30c4d9..7d739bfae3 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -152,7 +152,6 @@ protected: btTransform _followDesiredBodyTransform; btVector3 _position; btQuaternion _rotation; - //btTransform _characterBodyTransform; glm::vec3 _shapeLocalOffset;