From 8c109fd6238cf60e99fbe5e101786013e8bac356 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 8 Dec 2015 17:21:43 -0800 Subject: [PATCH] move raw bullet code from interface to physics lib --- interface/src/avatar/MyAvatar.cpp | 4 +- .../src/avatar/MyCharacterController.cpp | 329 +----------------- interface/src/avatar/MyCharacterController.h | 75 +--- libraries/physics/src/CharacterController.cpp | 329 +++++++++++++++++- libraries/physics/src/CharacterController.h | 85 ++++- 5 files changed, 405 insertions(+), 417 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 6637331b64..34821e6737 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1058,7 +1058,7 @@ void MyAvatar::rebuildSkeletonBody() { void MyAvatar::prepareForPhysicsSimulation() { relayDriveKeysToCharacterController(); _characterController.setTargetVelocity(getTargetVelocity()); - _characterController.setAvatarPositionAndOrientation(getPosition(), getOrientation()); + _characterController.setPositionAndOrientation(getPosition(), getOrientation()); if (qApp->isHMDMode()) { updateHMDFollowVelocity(); } else if (_followSpeed > 0.0f) { @@ -1071,7 +1071,7 @@ void MyAvatar::prepareForPhysicsSimulation() { void MyAvatar::harvestResultsFromPhysicsSimulation() { glm::vec3 position = getPosition(); glm::quat orientation = getOrientation(); - _characterController.getAvatarPositionAndOrientation(position, orientation); + _characterController.getPositionAndOrientation(position, orientation); nextAttitude(position, orientation); if (_followSpeed > 0.0f) { adjustSensorTransform(); diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index e8f686da6f..23d601e58e 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -11,255 +11,25 @@ #include "MyCharacterController.h" -#include -#include -#include -#include - -#include -#include -#include +#include #include "MyAvatar.h" -const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f); -const float DEFAULT_GRAVITY = -5.0f; -const float JUMP_SPEED = 3.5f; - -const float MAX_FALL_HEIGHT = 20.0f; - // TODO: improve walking up steps // TODO: make avatars able to walk up and down steps/slopes // TODO: make avatars stand on steep slope // TODO: make avatars not snag on low ceilings -// helper class for simple ray-traces from character -class ClosestNotMe : public btCollisionWorld::ClosestRayResultCallback { -public: - ClosestNotMe(btRigidBody* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) { - _me = me; - } - virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) { - if (rayResult.m_collisionObject == _me) { - return 1.0f; - } - return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); - } -protected: - btRigidBody* _me; -}; - MyCharacterController::MyCharacterController(MyAvatar* avatar) { - _halfHeight = 1.0f; assert(avatar); _avatar = avatar; - - _enabled = false; - - _floorDistance = MAX_FALL_HEIGHT; - - _walkVelocity.setValue(0.0f, 0.0f, 0.0f); - _followVelocity.setValue(0.0f, 0.0f, 0.0f); - _jumpSpeed = JUMP_SPEED; - _isOnGround = false; - _isJumping = false; - _isFalling = false; - _isHovering = true; - _isPushingUp = false; - _jumpToHoverStart = 0; - _followTime = 0.0f; - - _pendingFlags = PENDING_FLAG_UPDATE_SHAPE; updateShapeIfNecessary(); } MyCharacterController::~MyCharacterController() { } -void MyCharacterController::preStep(btCollisionWorld* collisionWorld) { - // trace a ray straight down to see if we're standing on the ground - const btTransform& xform = _rigidBody->getWorldTransform(); - - // rayStart is at center of bottom sphere - btVector3 rayStart = xform.getOrigin() - _halfHeight * _currentUp; - - // rayEnd is some short distance outside bottom sphere - const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius; - btScalar rayLength = _radius + FLOOR_PROXIMITY_THRESHOLD; - btVector3 rayEnd = rayStart - rayLength * _currentUp; - - // scan down for nearby floor - ClosestNotMe rayCallback(_rigidBody); - rayCallback.m_closestHitFraction = 1.0f; - collisionWorld->rayTest(rayStart, rayEnd, rayCallback); - if (rayCallback.hasHit()) { - _floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius; - } -} - -void MyCharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { - btVector3 actualVelocity = _rigidBody->getLinearVelocity(); - btScalar actualSpeed = actualVelocity.length(); - - btVector3 desiredVelocity = _walkVelocity; - btScalar desiredSpeed = desiredVelocity.length(); - - const btScalar MIN_UP_PUSH = 0.1f; - if (desiredVelocity.dot(_currentUp) < MIN_UP_PUSH) { - _isPushingUp = false; - } - - const btScalar MIN_SPEED = 0.001f; - if (_isHovering) { - if (desiredSpeed < MIN_SPEED) { - if (actualSpeed < MIN_SPEED) { - _rigidBody->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f)); - } else { - const btScalar HOVER_BRAKING_TIMESCALE = 0.1f; - btScalar tau = glm::max(dt / HOVER_BRAKING_TIMESCALE, 1.0f); - _rigidBody->setLinearVelocity((1.0f - tau) * actualVelocity); - } - } else { - const btScalar HOVER_ACCELERATION_TIMESCALE = 0.1f; - btScalar tau = dt / HOVER_ACCELERATION_TIMESCALE; - _rigidBody->setLinearVelocity(actualVelocity - tau * (actualVelocity - desiredVelocity)); - } - } else { - if (onGround()) { - // walking on ground - if (desiredSpeed < MIN_SPEED) { - if (actualSpeed < MIN_SPEED) { - _rigidBody->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f)); - } else { - const btScalar HOVER_BRAKING_TIMESCALE = 0.1f; - btScalar tau = dt / HOVER_BRAKING_TIMESCALE; - _rigidBody->setLinearVelocity((1.0f - tau) * actualVelocity); - } - } else { - // TODO: modify desiredVelocity using floor normal - const btScalar WALK_ACCELERATION_TIMESCALE = 0.1f; - btScalar tau = dt / WALK_ACCELERATION_TIMESCALE; - btVector3 velocityCorrection = tau * (desiredVelocity - actualVelocity); - // subtract vertical component - velocityCorrection -= velocityCorrection.dot(_currentUp) * _currentUp; - _rigidBody->setLinearVelocity(actualVelocity + velocityCorrection); - } - } else { - // transitioning to flying - btVector3 velocityCorrection = desiredVelocity - actualVelocity; - const btScalar FLY_ACCELERATION_TIMESCALE = 0.2f; - btScalar tau = dt / FLY_ACCELERATION_TIMESCALE; - if (!_isPushingUp) { - // actually falling --> compute a different velocity attenuation factor - const btScalar FALL_ACCELERATION_TIMESCALE = 2.0f; - tau = dt / FALL_ACCELERATION_TIMESCALE; - // zero vertical component - velocityCorrection -= velocityCorrection.dot(_currentUp) * _currentUp; - } - _rigidBody->setLinearVelocity(actualVelocity + tau * velocityCorrection); - } - } - - // Rather than add _followVelocity to the velocity of the RigidBody, we explicitly teleport - // the RigidBody forward according to the formula: distance = rate * time - if (_followVelocity.length2() > 0.0f) { - btTransform bodyTransform = _rigidBody->getWorldTransform(); - bodyTransform.setOrigin(bodyTransform.getOrigin() + dt * _followVelocity); - _rigidBody->setWorldTransform(bodyTransform); - } - _followTime += dt; -} - -void MyCharacterController::jump() { - // check for case where user is holding down "jump" key... - // we'll eventually tansition to "hover" - if (!_isJumping) { - if (!_isHovering) { - _jumpToHoverStart = usecTimestampNow(); - _pendingFlags |= PENDING_FLAG_JUMP; - } - } else { - quint64 now = usecTimestampNow(); - const quint64 JUMP_TO_HOVER_PERIOD = 75 * (USECS_PER_SECOND / 100); - if (now - _jumpToHoverStart > JUMP_TO_HOVER_PERIOD) { - _isPushingUp = true; - setHovering(true); - } - } -} - -bool MyCharacterController::onGround() const { - const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius; - return _floorDistance < FLOOR_PROXIMITY_THRESHOLD; -} - -void MyCharacterController::setHovering(bool hover) { - if (hover != _isHovering) { - _isHovering = hover; - _isJumping = false; - - if (_rigidBody) { - if (hover) { - _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); - } else { - _rigidBody->setGravity(DEFAULT_GRAVITY * _currentUp); - } - } - } -} - -void MyCharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale) { - _boxScale = scale; - - float x = _boxScale.x; - float z = _boxScale.z; - float radius = 0.5f * sqrtf(0.5f * (x * x + z * z)); - float halfHeight = 0.5f * _boxScale.y - radius; - float MIN_HALF_HEIGHT = 0.1f; - if (halfHeight < MIN_HALF_HEIGHT) { - halfHeight = MIN_HALF_HEIGHT; - } - - // compare dimensions - float radiusDelta = glm::abs(radius - _radius); - float heightDelta = glm::abs(halfHeight - _halfHeight); - if (radiusDelta < FLT_EPSILON && heightDelta < FLT_EPSILON) { - // shape hasn't changed --> nothing to do - } else { - if (_dynamicsWorld) { - // must REMOVE from world prior to shape update - _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION; - } - _pendingFlags |= PENDING_FLAG_UPDATE_SHAPE; - // only need to ADD back when we happen to be enabled - if (_enabled) { - _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION; - } - } - - // it's ok to change offset immediately -- there are no thread safety issues here - _shapeLocalOffset = corner + 0.5f * _boxScale; -} - -void MyCharacterController::setEnabled(bool enabled) { - if (enabled != _enabled) { - if (enabled) { - // Don't bother clearing REMOVE bit since it might be paired with an UPDATE_SHAPE bit. - // Setting the ADD bit here works for all cases so we don't even bother checking other bits. - _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION; - } else { - if (_dynamicsWorld) { - _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION; - } - _pendingFlags &= ~ PENDING_FLAG_ADD_TO_SIMULATION; - _isOnGround = false; - } - setHovering(true); - _enabled = enabled; - } -} - void MyCharacterController::updateShapeIfNecessary() { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { _pendingFlags &= ~ PENDING_FLAG_UPDATE_SHAPE; @@ -300,7 +70,7 @@ void MyCharacterController::updateShapeIfNecessary() { if (_isHovering) { _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); } else { - _rigidBody->setGravity(DEFAULT_GRAVITY * _currentUp); + _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); } //_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT); } else { @@ -309,98 +79,3 @@ void MyCharacterController::updateShapeIfNecessary() { } } -void MyCharacterController::updateUpAxis(const glm::quat& rotation) { - btVector3 oldUp = _currentUp; - _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS); - if (!_isHovering) { - const btScalar MIN_UP_ERROR = 0.01f; - if (oldUp.distance(_currentUp) > MIN_UP_ERROR) { - _rigidBody->setGravity(DEFAULT_GRAVITY * _currentUp); - } - } -} - -void MyCharacterController::setAvatarPositionAndOrientation( - const glm::vec3& position, - const glm::quat& orientation) { - // TODO: update gravity if up has changed - updateUpAxis(orientation); - - btQuaternion bodyOrientation = glmToBullet(orientation); - btVector3 bodyPosition = glmToBullet(position + orientation * _shapeLocalOffset); - _avatarBodyTransform = btTransform(bodyOrientation, bodyPosition); -} - -void MyCharacterController::getAvatarPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const { - if (_enabled && _rigidBody) { - const btTransform& avatarTransform = _rigidBody->getWorldTransform(); - rotation = bulletToGLM(avatarTransform.getRotation()); - position = bulletToGLM(avatarTransform.getOrigin()) - rotation * _shapeLocalOffset; - } -} - -void MyCharacterController::setTargetVelocity(const glm::vec3& velocity) { - //_walkVelocity = glmToBullet(_avatarData->getTargetVelocity()); - _walkVelocity = glmToBullet(velocity); -} - -void MyCharacterController::setFollowVelocity(const glm::vec3& velocity) { - _followVelocity = glmToBullet(velocity); -} - -glm::vec3 MyCharacterController::getLinearVelocity() const { - glm::vec3 velocity(0.0f); - if (_rigidBody) { - velocity = bulletToGLM(_rigidBody->getLinearVelocity()); - } - return velocity; -} - -void MyCharacterController::preSimulation() { - if (_enabled && _dynamicsWorld) { - // slam body to where it is supposed to be - _rigidBody->setWorldTransform(_avatarBodyTransform); - - // scan for distant floor - // rayStart is at center of bottom sphere - btVector3 rayStart = _avatarBodyTransform.getOrigin() - _halfHeight * _currentUp; - - // rayEnd is straight down MAX_FALL_HEIGHT - btScalar rayLength = _radius + MAX_FALL_HEIGHT; - btVector3 rayEnd = rayStart - rayLength * _currentUp; - - ClosestNotMe rayCallback(_rigidBody); - rayCallback.m_closestHitFraction = 1.0f; - _dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback); - if (rayCallback.hasHit()) { - _floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius; - const btScalar MIN_HOVER_HEIGHT = 3.0f; - if (_isHovering && _floorDistance < MIN_HOVER_HEIGHT && !_isPushingUp) { - setHovering(false); - } - // TODO: use collision events rather than ray-trace test to disable jumping - const btScalar JUMP_PROXIMITY_THRESHOLD = 0.1f * _radius; - if (_floorDistance < JUMP_PROXIMITY_THRESHOLD) { - _isJumping = false; - } - } else { - _floorDistance = FLT_MAX; - setHovering(true); - } - - if (_pendingFlags & PENDING_FLAG_JUMP) { - _pendingFlags &= ~ PENDING_FLAG_JUMP; - if (onGround()) { - _isJumping = true; - btVector3 velocity = _rigidBody->getLinearVelocity(); - velocity += _jumpSpeed * _currentUp; - _rigidBody->setLinearVelocity(velocity); - } - } - } - _followTime = 0.0f; -} - -void MyCharacterController::postSimulation() { - // postSimulation() exists for symmetry and just in case we need to do something here later -} diff --git a/interface/src/avatar/MyCharacterController.h b/interface/src/avatar/MyCharacterController.h index 82aa958309..39f0f99917 100644 --- a/interface/src/avatar/MyCharacterController.h +++ b/interface/src/avatar/MyCharacterController.h @@ -13,12 +13,8 @@ #ifndef hifi_MyCharacterController_h #define hifi_MyCharacterController_h -#include -#include - -#include #include -#include +//#include class btCollisionShape; class MyAvatar; @@ -28,79 +24,10 @@ public: MyCharacterController(MyAvatar* avatar); ~MyCharacterController (); - // TODO: implement these when needed - virtual void setVelocityForTimeInterval(const btVector3 &velocity, btScalar timeInterval) override { assert(false); } - virtual void reset(btCollisionWorld* collisionWorld) override { } - virtual void warp(const btVector3& origin) override { } - virtual void debugDraw(btIDebugDraw* debugDrawer) override { } - virtual void setUpInterpolate(bool value) override { } - - // overrides from btCharacterControllerInterface - virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) override { - preStep(collisionWorld); - playerStep(collisionWorld, deltaTime); - } - virtual void preStep(btCollisionWorld* collisionWorld) override; - virtual void playerStep(btCollisionWorld* collisionWorld, btScalar dt) override; - virtual bool canJump() const override { assert(false); return false; } // never call this - virtual void jump() override; // call this every frame the jump button is pressed - virtual bool onGround() const override; - - // overrides from CharacterController - virtual void preSimulation() override; - virtual void postSimulation() override; - - bool isHovering() const { return _isHovering; } - void setHovering(bool enabled); - - void setEnabled(bool enabled); - bool isEnabled() const { return _enabled && _dynamicsWorld; } - - void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale); - virtual void updateShapeIfNecessary() override; - void setAvatarPositionAndOrientation( const glm::vec3& position, const glm::quat& orientation); - void getAvatarPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const; - - void setTargetVelocity(const glm::vec3& velocity); - void setFollowVelocity(const glm::vec3& velocity); - float getFollowTime() const { return _followTime; } - - glm::vec3 getLinearVelocity() const; - protected: - void updateUpAxis(const glm::quat& rotation); - -protected: - btVector3 _currentUp; - btVector3 _walkVelocity; - btVector3 _followVelocity; - btTransform _avatarBodyTransform; - - glm::vec3 _shapeLocalOffset; - glm::vec3 _boxScale; // used to compute capsule shape - - quint64 _jumpToHoverStart; - MyAvatar* _avatar { nullptr }; - - btScalar _halfHeight; - btScalar _radius; - - btScalar _floorDistance; - - btScalar _gravity; - - btScalar _jumpSpeed; - btScalar _followTime; - - bool _enabled; - bool _isOnGround; - bool _isJumping; - bool _isFalling; - bool _isHovering; - bool _isPushingUp; }; #endif // hifi_MyCharacterController_h diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 8da9541387..ba3a59f69d 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -11,8 +11,55 @@ #include "CharacterController.h" +#include + +#include "BulletUtil.h" #include "PhysicsCollisionGroups.h" +const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f); +const float JUMP_SPEED = 3.5f; +const float MAX_FALL_HEIGHT = 20.0f; + + +// helper class for simple ray-traces from character +class ClosestNotMe : public btCollisionWorld::ClosestRayResultCallback { +public: + ClosestNotMe(btRigidBody* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) { + _me = me; + } + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) { + if (rayResult.m_collisionObject == _me) { + return 1.0f; + } + return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); + } +protected: + btRigidBody* _me; +}; + + +CharacterController::CharacterController() { + _halfHeight = 1.0f; + + _enabled = false; + + _floorDistance = MAX_FALL_HEIGHT; + + _walkVelocity.setValue(0.0f, 0.0f, 0.0f); + _followVelocity.setValue(0.0f, 0.0f, 0.0f); + _jumpSpeed = JUMP_SPEED; + _isOnGround = false; + _isJumping = false; + _isFalling = false; + _isHovering = true; + _isPushingUp = false; + _jumpToHoverStart = 0; + _followTime = 0.0f; + + _pendingFlags = PENDING_FLAG_UPDATE_SHAPE; + +} + bool CharacterController::needsRemoval() const { return ((_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION) == PENDING_FLAG_REMOVE_FROM_SIMULATION); } @@ -23,7 +70,7 @@ bool CharacterController::needsAddition() const { void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { if (_dynamicsWorld != world) { - if (_dynamicsWorld) { + if (_dynamicsWorld) { if (_rigidBody) { _dynamicsWorld->removeRigidBody(_rigidBody); _dynamicsWorld->removeAction(this); @@ -48,5 +95,283 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { } else { _pendingFlags &= ~PENDING_FLAG_REMOVE_FROM_SIMULATION; } -} +} +void CharacterController::preStep(btCollisionWorld* collisionWorld) { + // trace a ray straight down to see if we're standing on the ground + const btTransform& xform = _rigidBody->getWorldTransform(); + + // rayStart is at center of bottom sphere + btVector3 rayStart = xform.getOrigin() - _halfHeight * _currentUp; + + // rayEnd is some short distance outside bottom sphere + const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius; + btScalar rayLength = _radius + FLOOR_PROXIMITY_THRESHOLD; + btVector3 rayEnd = rayStart - rayLength * _currentUp; + + // scan down for nearby floor + ClosestNotMe rayCallback(_rigidBody); + rayCallback.m_closestHitFraction = 1.0f; + collisionWorld->rayTest(rayStart, rayEnd, rayCallback); + if (rayCallback.hasHit()) { + _floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius; + } +} + +void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { + btVector3 actualVelocity = _rigidBody->getLinearVelocity(); + btScalar actualSpeed = actualVelocity.length(); + + btVector3 desiredVelocity = _walkVelocity; + btScalar desiredSpeed = desiredVelocity.length(); + + const btScalar MIN_UP_PUSH = 0.1f; + if (desiredVelocity.dot(_currentUp) < MIN_UP_PUSH) { + _isPushingUp = false; + } + + const btScalar MIN_SPEED = 0.001f; + if (_isHovering) { + if (desiredSpeed < MIN_SPEED) { + if (actualSpeed < MIN_SPEED) { + _rigidBody->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f)); + } else { + const btScalar HOVER_BRAKING_TIMESCALE = 0.1f; + btScalar tau = glm::max(dt / HOVER_BRAKING_TIMESCALE, 1.0f); + _rigidBody->setLinearVelocity((1.0f - tau) * actualVelocity); + } + } else { + const btScalar HOVER_ACCELERATION_TIMESCALE = 0.1f; + btScalar tau = dt / HOVER_ACCELERATION_TIMESCALE; + _rigidBody->setLinearVelocity(actualVelocity - tau * (actualVelocity - desiredVelocity)); + } + } else { + if (onGround()) { + // walking on ground + if (desiredSpeed < MIN_SPEED) { + if (actualSpeed < MIN_SPEED) { + _rigidBody->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f)); + } else { + const btScalar HOVER_BRAKING_TIMESCALE = 0.1f; + btScalar tau = dt / HOVER_BRAKING_TIMESCALE; + _rigidBody->setLinearVelocity((1.0f - tau) * actualVelocity); + } + } else { + // TODO: modify desiredVelocity using floor normal + const btScalar WALK_ACCELERATION_TIMESCALE = 0.1f; + btScalar tau = dt / WALK_ACCELERATION_TIMESCALE; + btVector3 velocityCorrection = tau * (desiredVelocity - actualVelocity); + // subtract vertical component + velocityCorrection -= velocityCorrection.dot(_currentUp) * _currentUp; + _rigidBody->setLinearVelocity(actualVelocity + velocityCorrection); + } + } else { + // transitioning to flying + btVector3 velocityCorrection = desiredVelocity - actualVelocity; + const btScalar FLY_ACCELERATION_TIMESCALE = 0.2f; + btScalar tau = dt / FLY_ACCELERATION_TIMESCALE; + if (!_isPushingUp) { + // actually falling --> compute a different velocity attenuation factor + const btScalar FALL_ACCELERATION_TIMESCALE = 2.0f; + tau = dt / FALL_ACCELERATION_TIMESCALE; + // zero vertical component + velocityCorrection -= velocityCorrection.dot(_currentUp) * _currentUp; + } + _rigidBody->setLinearVelocity(actualVelocity + tau * velocityCorrection); + } + } + + // Rather than add _followVelocity to the velocity of the RigidBody, we explicitly teleport + // the RigidBody forward according to the formula: distance = rate * time + if (_followVelocity.length2() > 0.0f) { + btTransform bodyTransform = _rigidBody->getWorldTransform(); + bodyTransform.setOrigin(bodyTransform.getOrigin() + dt * _followVelocity); + _rigidBody->setWorldTransform(bodyTransform); + } + _followTime += dt; +} + +void CharacterController::jump() { + // check for case where user is holding down "jump" key... + // we'll eventually tansition to "hover" + if (!_isJumping) { + if (!_isHovering) { + _jumpToHoverStart = usecTimestampNow(); + _pendingFlags |= PENDING_FLAG_JUMP; + } + } else { + quint64 now = usecTimestampNow(); + const quint64 JUMP_TO_HOVER_PERIOD = 75 * (USECS_PER_SECOND / 100); + if (now - _jumpToHoverStart > JUMP_TO_HOVER_PERIOD) { + _isPushingUp = true; + setHovering(true); + } + } +} + +bool CharacterController::onGround() const { + const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius; + return _floorDistance < FLOOR_PROXIMITY_THRESHOLD; +} + +void CharacterController::setHovering(bool hover) { + if (hover != _isHovering) { + _isHovering = hover; + _isJumping = false; + + if (_rigidBody) { + if (hover) { + _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); + } else { + _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); + } + } + } +} + +void CharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale) { + _boxScale = scale; + + float x = _boxScale.x; + float z = _boxScale.z; + float radius = 0.5f * sqrtf(0.5f * (x * x + z * z)); + float halfHeight = 0.5f * _boxScale.y - radius; + float MIN_HALF_HEIGHT = 0.1f; + if (halfHeight < MIN_HALF_HEIGHT) { + halfHeight = MIN_HALF_HEIGHT; + } + + // compare dimensions + float radiusDelta = glm::abs(radius - _radius); + float heightDelta = glm::abs(halfHeight - _halfHeight); + if (radiusDelta < FLT_EPSILON && heightDelta < FLT_EPSILON) { + // shape hasn't changed --> nothing to do + } else { + if (_dynamicsWorld) { + // must REMOVE from world prior to shape update + _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION; + } + _pendingFlags |= PENDING_FLAG_UPDATE_SHAPE; + // only need to ADD back when we happen to be enabled + if (_enabled) { + _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION; + } + } + + // it's ok to change offset immediately -- there are no thread safety issues here + _shapeLocalOffset = corner + 0.5f * _boxScale; +} + +void CharacterController::setEnabled(bool enabled) { + if (enabled != _enabled) { + if (enabled) { + // Don't bother clearing REMOVE bit since it might be paired with an UPDATE_SHAPE bit. + // Setting the ADD bit here works for all cases so we don't even bother checking other bits. + _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION; + } else { + if (_dynamicsWorld) { + _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION; + } + _pendingFlags &= ~ PENDING_FLAG_ADD_TO_SIMULATION; + _isOnGround = false; + } + setHovering(true); + _enabled = enabled; + } +} + +void CharacterController::updateUpAxis(const glm::quat& rotation) { + btVector3 oldUp = _currentUp; + _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS); + if (!_isHovering) { + const btScalar MIN_UP_ERROR = 0.01f; + if (oldUp.distance(_currentUp) > MIN_UP_ERROR) { + _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); + } + } +} + +void CharacterController::setPositionAndOrientation( + const glm::vec3& position, + const glm::quat& orientation) { + // TODO: update gravity if up has changed + updateUpAxis(orientation); + + btQuaternion bodyOrientation = glmToBullet(orientation); + btVector3 bodyPosition = glmToBullet(position + orientation * _shapeLocalOffset); + _characterBodyTransform = btTransform(bodyOrientation, bodyPosition); +} + +void CharacterController::getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const { + if (_enabled && _rigidBody) { + const btTransform& avatarTransform = _rigidBody->getWorldTransform(); + rotation = bulletToGLM(avatarTransform.getRotation()); + position = bulletToGLM(avatarTransform.getOrigin()) - rotation * _shapeLocalOffset; + } +} + +void CharacterController::setTargetVelocity(const glm::vec3& velocity) { + //_walkVelocity = glmToBullet(_avatarData->getTargetVelocity()); + _walkVelocity = glmToBullet(velocity); +} + +void CharacterController::setFollowVelocity(const glm::vec3& velocity) { + _followVelocity = glmToBullet(velocity); +} + +glm::vec3 CharacterController::getLinearVelocity() const { + glm::vec3 velocity(0.0f); + if (_rigidBody) { + velocity = bulletToGLM(_rigidBody->getLinearVelocity()); + } + return velocity; +} + +void CharacterController::preSimulation() { + if (_enabled && _dynamicsWorld) { + // slam body to where it is supposed to be + _rigidBody->setWorldTransform(_characterBodyTransform); + + // scan for distant floor + // rayStart is at center of bottom sphere + btVector3 rayStart = _characterBodyTransform.getOrigin() - _halfHeight * _currentUp; + + // rayEnd is straight down MAX_FALL_HEIGHT + btScalar rayLength = _radius + MAX_FALL_HEIGHT; + btVector3 rayEnd = rayStart - rayLength * _currentUp; + + ClosestNotMe rayCallback(_rigidBody); + rayCallback.m_closestHitFraction = 1.0f; + _dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback); + if (rayCallback.hasHit()) { + _floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius; + const btScalar MIN_HOVER_HEIGHT = 3.0f; + if (_isHovering && _floorDistance < MIN_HOVER_HEIGHT && !_isPushingUp) { + setHovering(false); + } + // TODO: use collision events rather than ray-trace test to disable jumping + const btScalar JUMP_PROXIMITY_THRESHOLD = 0.1f * _radius; + if (_floorDistance < JUMP_PROXIMITY_THRESHOLD) { + _isJumping = false; + } + } else { + _floorDistance = FLT_MAX; + setHovering(true); + } + + if (_pendingFlags & PENDING_FLAG_JUMP) { + _pendingFlags &= ~ PENDING_FLAG_JUMP; + if (onGround()) { + _isJumping = true; + btVector3 velocity = _rigidBody->getLinearVelocity(); + velocity += _jumpSpeed * _currentUp; + _rigidBody->setLinearVelocity(velocity); + } + } + } + _followTime = 0.0f; +} + +void CharacterController::postSimulation() { + // postSimulation() exists for symmetry and just in case we need to do something here later +} diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index e9e6f1328e..6edbabdab8 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -17,11 +17,14 @@ #include #include +#include + const uint32_t PENDING_FLAG_ADD_TO_SIMULATION = 1U << 0; const uint32_t PENDING_FLAG_REMOVE_FROM_SIMULATION = 1U << 1; const uint32_t PENDING_FLAG_UPDATE_SHAPE = 1U << 2; const uint32_t PENDING_FLAG_JUMP = 1U << 3; +const float DEFAULT_CHARACTER_GRAVITY = -5.0f; class btRigidBody; class btCollisionWorld; @@ -29,31 +32,89 @@ class btDynamicsWorld; class CharacterController : public btCharacterControllerInterface { public: + CharacterController(); + virtual ~CharacterController() {} + bool needsRemoval() const; bool needsAddition() const; void setDynamicsWorld(btDynamicsWorld* world); btCollisionObject* getCollisionObject() { return _rigidBody; } virtual void updateShapeIfNecessary() = 0; - virtual void preSimulation() = 0; - virtual void postSimulation() = 0; + // overrides from btCharacterControllerInterface virtual void setWalkDirection(const btVector3 &walkDirection) { assert(false); } + virtual void setVelocityForTimeInterval(const btVector3 &velocity, btScalar timeInterval) override { assert(false); } + virtual void reset(btCollisionWorld* collisionWorld) override { } + virtual void warp(const btVector3& origin) override { } + virtual void debugDraw(btIDebugDraw* debugDrawer) override { } + virtual void setUpInterpolate(bool value) override { } + virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) override { + preStep(collisionWorld); + playerStep(collisionWorld, deltaTime); + } + virtual void preStep(btCollisionWorld *collisionWorld) override; + virtual void playerStep(btCollisionWorld *collisionWorld, btScalar dt) override; + virtual bool canJump() const override { assert(false); return false; } // never call this + virtual void jump() override; + virtual bool onGround() const; + + void preSimulation(); + void postSimulation(); + + void setPositionAndOrientation( const glm::vec3& position, const glm::quat& orientation); + void getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const; + + void setTargetVelocity(const glm::vec3& velocity); + void setFollowVelocity(const glm::vec3& velocity); + float getFollowTime() const { return _followTime; } + + glm::vec3 getLinearVelocity() const; + + bool isHovering() const { return _isHovering; } + void setHovering(bool enabled); + + void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale); + + void setEnabled(bool enabled); + bool isEnabled() const { return _enabled && _dynamicsWorld; } - /* these from btCharacterControllerInterface remain to be overridden - virtual void setVelocityForTimeInterval(const btVector3 &velocity, btScalar timeInterval) = 0; - virtual void reset() = 0; - virtual void warp(const btVector3 &origin) = 0; - virtual void preStep(btCollisionWorld *collisionWorld) = 0; - virtual void playerStep(btCollisionWorld *collisionWorld, btScalar dt) = 0; - virtual bool canJump() const = 0; - virtual void jump() = 0; - virtual bool onGround() const = 0; - */ protected: + void updateUpAxis(const glm::quat& rotation); + +protected: + btVector3 _currentUp; + btVector3 _walkVelocity; + btVector3 _followVelocity; + btTransform _characterBodyTransform; + + glm::vec3 _shapeLocalOffset; + + glm::vec3 _boxScale; // used to compute capsule shape + + quint64 _jumpToHoverStart; + + btScalar _halfHeight; + btScalar _radius; + + btScalar _floorDistance; + + btScalar _gravity; + + btScalar _jumpSpeed; + btScalar _followTime; + + bool _enabled; + bool _isOnGround; + bool _isJumping; + bool _isFalling; + bool _isHovering; + bool _isPushingUp; + btDynamicsWorld* _dynamicsWorld { nullptr }; btRigidBody* _rigidBody { nullptr }; uint32_t _pendingFlags { 0 }; + }; #endif // hifi_CharacterControllerInterface_h