From 435ce106eb9ac5d70a302ffa5c5474053a2db4a2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 6 Apr 2015 16:03:13 -0700 Subject: [PATCH 1/6] Add DynamicCharacterController prototype --- interface/src/avatar/MyAvatar.h | 6 +- .../src/DynamicCharacterController.cpp | 411 ++++++++++++++++++ .../physics/src/DynamicCharacterController.h | 105 +++++ libraries/physics/src/PhysicsEngine.cpp | 2 +- libraries/physics/src/PhysicsEngine.h | 6 +- 5 files changed, 523 insertions(+), 7 deletions(-) create mode 100644 libraries/physics/src/DynamicCharacterController.cpp create mode 100644 libraries/physics/src/DynamicCharacterController.h diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 2bc3a4e4ba..5ba5c407b0 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -13,7 +13,7 @@ #define hifi_MyAvatar_h #include -#include +#include #include "Avatar.h" @@ -122,7 +122,7 @@ public: virtual glm::vec3 getSkeletonPosition() const; void updateLocalAABox(); - CharacterController* getCharacterController() { return &_characterController; } + DynamicCharacterController* getCharacterController() { return &_characterController; } void updateCharacterController(); void clearJointAnimationPriorities(); @@ -203,7 +203,7 @@ private: int _scriptedMotorFrame; quint32 _motionBehaviors; - CharacterController _characterController; + DynamicCharacterController _characterController; QWeakPointer _lookAtTargetAvatar; glm::vec3 _targetAvatarPosition; diff --git a/libraries/physics/src/DynamicCharacterController.cpp b/libraries/physics/src/DynamicCharacterController.cpp new file mode 100644 index 0000000000..525d918c4d --- /dev/null +++ b/libraries/physics/src/DynamicCharacterController.cpp @@ -0,0 +1,411 @@ +#include +#include +#include +#include + +#include "BulletUtil.h" +#include "DynamicCharacterController.h" + +const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f); +const float DEFAULT_GRAVITY = 5.0f; +const float TERMINAL_VELOCITY = 55.0f; +const float JUMP_SPEED = 3.5f; + +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; + +DynamicCharacterController::DynamicCharacterController(AvatarData* avatarData) { + _rayLambda[0] = 1.0f; + _rayLambda[1] = 1.0f; + _halfHeight = 1.0f; + //_turnVelocity = 1.0f; // radians/sec + _shape = NULL; + _rigidBody = NULL; + + assert(avatarData); + _avatarData = avatarData; + + _enabled = false; + + _walkVelocity.setValue(0.0f,0.0f,0.0f); + //_verticalVelocity = 0.0f; + _jumpSpeed = JUMP_SPEED; + _isOnGround = false; + _isJumping = false; + _isHovering = true; + + _pendingFlags = PENDING_FLAG_UPDATE_SHAPE; + updateShapeIfNecessary(); +} + +DynamicCharacterController::~DynamicCharacterController() { +} + +// virtual +void DynamicCharacterController::setWalkDirection(const btVector3& walkDirection) { + _walkVelocity = walkDirection; +} + +/* +void DynamicCharacterController::setup(btScalar height, btScalar width, btScalar stepHeight) { + btVector3 spherePositions[2]; + btScalar sphereRadii[2]; + + sphereRadii[0] = width; + sphereRadii[1] = width; + spherePositions[0] = btVector3 (0.0f, (height/btScalar(2.0f) - width), 0.0f); + spherePositions[1] = btVector3 (0.0f, (-height/btScalar(2.0f) + width), 0.0f); + + _halfHeight = height/btScalar(2.0f); + + _shape = new btMultiSphereShape(&spherePositions[0], &sphereRadii[0], 2); + + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0.0f, 2.0f, 0.0f)); + btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + btRigidBody::btRigidBodyConstructionInfo cInfo(1.0f, myMotionState, _shape); + _rigidBody = new btRigidBody(cInfo); + // kinematic vs. static doesn't work + //_rigidBody->setCollisionFlags( _rigidBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); + _rigidBody->setSleepingThresholds(0.0f, 0.0f); + _rigidBody->setAngularFactor(0.0f); +} + +void DynamicCharacterController::destroy() { + if (_shape) { + delete _shape; + } + + if (_rigidBody) { + delete _rigidBody; + _rigidBody = NULL; + } +} +*/ + +btCollisionObject* DynamicCharacterController::getCollisionObject() { + return _rigidBody; +} + +void DynamicCharacterController::preStep(btCollisionWorld* collisionWorld) { + const btTransform& xform = _rigidBody->getCenterOfMassTransform(); + + btVector3 down = -xform.getBasis()[1]; + btVector3 forward = xform.getBasis()[2]; + down.normalize(); + forward.normalize(); + + _raySource[0] = xform.getOrigin(); + _raySource[1] = xform.getOrigin(); + + _rayTarget[0] = _raySource[0] + down * _halfHeight * btScalar(1.1f); + _rayTarget[1] = _raySource[1] + forward * _halfHeight * btScalar(1.1f); + + 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; + }; + + ClosestNotMe rayCallback(_rigidBody); + + int i = 0; + for (i = 0; i < 2; i++) { + rayCallback.m_closestHitFraction = 1.0f; + collisionWorld->rayTest(_raySource[i], _rayTarget[i], rayCallback); + if (rayCallback.hasHit()) { + _rayLambda[i] = rayCallback.m_closestHitFraction; + } else { + _rayLambda[i] = 1.0f; + } + } +} + +void DynamicCharacterController::playerStep(btCollisionWorld* dynaWorld,btScalar dt) { + /* Handle turning + const btTransform& xform = _rigidBody->getCenterOfMassTransform(); + if (left) + _turnAngle -= dt * _turnVelocity; + if (right) + _turnAngle += dt * _turnVelocity; + + xform.setRotation(btQuaternion(btVector3(0.0, 1.0, 0.0), _turnAngle)); + */ + + btVector3 currentVelocity = _rigidBody->getLinearVelocity(); + btScalar currentSpeed = currentVelocity.length(); + + btVector3 desiredVelocity = _walkVelocity; + btScalar desiredSpeed = desiredVelocity.length(); + const btScalar MIN_SPEED = 0.001f; + if (desiredSpeed < MIN_SPEED) { + if (currentSpeed < MIN_SPEED) { + _rigidBody->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f)); + } else { + const btScalar BRAKING_TIMESCALE = 0.2f; + btScalar tau = dt / BRAKING_TIMESCALE; + _rigidBody->setLinearVelocity((1.0f - tau) * currentVelocity); + } + } else { + const btScalar WALKING_TIMESCALE = 0.5f; + btScalar tau = dt / WALKING_TIMESCALE; + if (onGround()) { + // subtract vertical component + desiredVelocity = desiredVelocity - desiredVelocity.dot(_currentUp) * _currentUp; + } + _rigidBody->setLinearVelocity(currentVelocity - tau * (currentVelocity - desiredVelocity)); + } + + /* + _rigidBody->getMotionState()->setWorldTransform(xform); + _rigidBody->setCenterOfMassTransform(xform); + */ +} + +bool DynamicCharacterController::canJump() const { + return false; // temporarily disabled + //return onGround(); +} + +void DynamicCharacterController::jump() { + /* + if (!canJump()) { + return; + } + + btTransform xform = _rigidBody->getCenterOfMassTransform(); + btVector3 up = xform.getBasis()[1]; + up.normalize(); + btScalar magnitude = (btScalar(1.0)/_rigidBody->getInvMass()) * btScalar(8.0); + _rigidBody->applyCentralImpulse(up * magnitude); + */ +} + +bool DynamicCharacterController::onGround() const { + return _rayLambda[0] < btScalar(1.0); +} + +void DynamicCharacterController::debugDraw(btIDebugDraw* debugDrawer) { +} + +void DynamicCharacterController::setUpInterpolate(bool value) { + // This method is required by btCharacterControllerInterface, but it does nothing. + // What it used to do was determine whether stepUp() would: stop where it hit the ceiling + // (interpolate = true, and now default behavior) or happily penetrate objects above the avatar. +} + +void DynamicCharacterController::warp(const btVector3& origin) { +} + +void DynamicCharacterController::reset(btCollisionWorld* foo) { +} + +void DynamicCharacterController::registerPairCacheAndDispatcher(btOverlappingPairCache* pairCache, btCollisionDispatcher* dispatcher) { +} + +void DynamicCharacterController::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; +} + +bool DynamicCharacterController::needsRemoval() const { + return (bool)(_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION); +} + +bool DynamicCharacterController::needsAddition() const { + return (bool)(_pendingFlags & PENDING_FLAG_ADD_TO_SIMULATION); +} + +void DynamicCharacterController::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; + _isHovering = true; + } else { + if (_dynamicsWorld) { + _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION; + } + _pendingFlags &= ~ PENDING_FLAG_ADD_TO_SIMULATION; + _isOnGround = false; + } + _enabled = enabled; + } +} + +void DynamicCharacterController::setDynamicsWorld(btDynamicsWorld* world) { + if (_dynamicsWorld != world) { + if (_dynamicsWorld) { + if (_rigidBody) { + _dynamicsWorld->removeRigidBody(_rigidBody); + _dynamicsWorld->removeAction(this); + } + _dynamicsWorld = NULL; + } + if (world && _rigidBody) { + _dynamicsWorld = world; + _pendingFlags &= ~ PENDING_FLAG_JUMP; + _dynamicsWorld->addRigidBody(_rigidBody); +// _dynamicsWorld->addCollisionObject(_rigidBody, +// btBroadphaseProxy::CharacterFilter, +// btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter); + _dynamicsWorld->addAction(this); +// reset(_dynamicsWorld); + } + } + if (_dynamicsWorld) { + if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { + // shouldn't fall in here, but if we do make sure both ADD and REMOVE bits are still set + _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_REMOVE_FROM_SIMULATION; + } else { + _pendingFlags &= ~PENDING_FLAG_ADD_TO_SIMULATION; + } + } else { + _pendingFlags &= ~ PENDING_FLAG_REMOVE_FROM_SIMULATION; + } +} + +void DynamicCharacterController::updateShapeIfNecessary() { + if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { + // make sure there is NO pending removal from simulation at this point + // (don't want to delete _rigidBody out from under the simulation) + assert(!(_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION)); + _pendingFlags &= ~ PENDING_FLAG_UPDATE_SHAPE; + // delete shape and GhostObject + delete _rigidBody; + _rigidBody = NULL; + delete _shape; + _shape = NULL; + + // compute new dimensions from avatar's bounding box + float x = _boxScale.x; + float z = _boxScale.z; + _radius = 0.5f * sqrtf(0.5f * (x * x + z * z)); + _halfHeight = 0.5f * _boxScale.y - _radius; + float MIN_HALF_HEIGHT = 0.1f; + if (_halfHeight < MIN_HALF_HEIGHT) { + _halfHeight = MIN_HALF_HEIGHT; + } + // NOTE: _shapeLocalOffset is already computed + + if (_radius > 0.0f) { + // create new shape + _shape = new btCapsuleShape(_radius, 2.0f * _halfHeight); + + // create new body + float mass = 1.0f; + btVector3 inertia(1.0f, 1.0f, 1.0f); + _rigidBody = new btRigidBody(mass, NULL, _shape, inertia); + _rigidBody->setSleepingThresholds (0.0f, 0.0f); + _rigidBody->setAngularFactor (0.0f); + _rigidBody->setWorldTransform(btTransform(glmToBullet(_avatarData->getOrientation()), + glmToBullet(_avatarData->getPosition()))); + // stepHeight affects the heights of ledges that the character can ascend + //_stepUpHeight = _radius + 0.25f * _halfHeight + 0.1f; + //_stepDownHeight = _radius; + + //_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT); + } else { + // TODO: handle this failure case + } + } +} + +void DynamicCharacterController::preSimulation(btScalar timeStep) { + if (_enabled && _dynamicsWorld) { + glm::quat rotation = _avatarData->getOrientation(); + _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS); + glm::vec3 position = _avatarData->getPosition() + rotation * _shapeLocalOffset; + + // TODO: get intended WALK velocity from _avatarData, not its actual velocity + btVector3 walkVelocity = glmToBullet(_avatarData->getVelocity()); + + _rigidBody->setWorldTransform(btTransform(glmToBullet(rotation), glmToBullet(position))); + _rigidBody->setLinearVelocity(walkVelocity); + //setVelocityForTimeInterval(walkVelocity, timeStep); + if (_pendingFlags & PENDING_FLAG_JUMP) { + _pendingFlags &= ~ PENDING_FLAG_JUMP; + if (canJump()) { + //_verticalVelocity = _jumpSpeed; + _isJumping = true; + } + } + // remember last position so we can throttle the total motion from the next step +// _lastPosition = position; +// _stepDt = 0.0f; + + // the rotation is determined by AvatarData + btTransform xform = _rigidBody->getCenterOfMassTransform(); + xform.setRotation(glmToBullet(rotation)); + _rigidBody->setCenterOfMassTransform(xform); + } +} + +void DynamicCharacterController::postSimulation() { + if (_enabled && _rigidBody) { + const btTransform& avatarTransform = _rigidBody->getCenterOfMassTransform(); + glm::quat rotation = bulletToGLM(avatarTransform.getRotation()); + glm::vec3 position = bulletToGLM(avatarTransform.getOrigin()); + + /* + // cap the velocity of the step so that the character doesn't POP! so hard on steps + glm::vec3 finalStep = position - _lastPosition; + btVector3 finalVelocity = _walkVelocity + _verticalVelocity * _currentUp;; + const btScalar MAX_RESOLUTION_SPEED = 5.0f; // m/sec + btScalar maxStepLength = glm::max(MAX_RESOLUTION_SPEED, 2.0f * finalVelocity.length()) * _stepDt; + btScalar stepLength = glm::length(finalStep); + if (stepLength > maxStepLength) { + position = _lastPosition + (maxStepLength / stepLength) * finalStep; + // NOTE: we don't need to move ghostObject to throttled position unless + // we want to support do async ray-traces/collision-queries against character + } + */ + + _avatarData->setOrientation(rotation); + _avatarData->setPosition(position - rotation * _shapeLocalOffset); + } +} + diff --git a/libraries/physics/src/DynamicCharacterController.h b/libraries/physics/src/DynamicCharacterController.h new file mode 100644 index 0000000000..a9b9ce9bd1 --- /dev/null +++ b/libraries/physics/src/DynamicCharacterController.h @@ -0,0 +1,105 @@ +#ifndef hifi_DynamicCharacterController_h +#define hifi_DynamicCharacterController_h + +#include +#include + +#include + +class btCollisionShape; +class btRigidBody; +class btCollisionWorld; + +///DynamicCharacterController is obsolete/unsupported at the moment +class DynamicCharacterController : public btCharacterControllerInterface +{ +protected: + btScalar _halfHeight; + btScalar _radius; + btCollisionShape* _shape; + btRigidBody* _rigidBody; + + btVector3 _currentUp; + btVector3 _raySource[2]; + btVector3 _rayTarget[2]; + btScalar _rayLambda[2]; + btVector3 _rayNormal[2]; + + btVector3 _walkVelocity; + //btScalar _turnVelocity; + + glm::vec3 _shapeLocalOffset; + glm::vec3 _boxScale; // used to compute capsule shape + AvatarData* _avatarData = NULL; + + bool _enabled; + bool _isOnGround; + bool _isJumping; + bool _isHovering; +// quint64 _jumpToHoverStart; +// btScalar _velocityTimeInterval; +// btScalar _stepDt; + uint32_t _pendingFlags; + + btDynamicsWorld* _dynamicsWorld = NULL; + + btScalar _jumpSpeed; + +public: + DynamicCharacterController(AvatarData* avatarData); + ~DynamicCharacterController (); + +// void setup(btScalar height = 2.0, btScalar width = 0.25, btScalar stepHeight = 0.25); +// void destroy (); + + virtual void setWalkDirection(const btVector3& walkDirection); + virtual void setVelocityForTimeInterval(const btVector3 &velocity, btScalar timeInterval) { assert(false); } + + virtual void reset(btCollisionWorld* collisionWorld); + virtual void warp(const btVector3& origin); + virtual void registerPairCacheAndDispatcher(btOverlappingPairCache* pairCache, btCollisionDispatcher* dispatcher); + + btCollisionObject* getCollisionObject(); + + ///btActionInterface interface + virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) { + preStep(collisionWorld); + playerStep(collisionWorld, deltaTime); + } + + virtual void debugDraw(btIDebugDraw* debugDrawer); + + void setUpInterpolate(bool value); + + virtual void preStep(btCollisionWorld* collisionWorld); + virtual void playerStep(btCollisionWorld* collisionWorld, btScalar dt); + + virtual bool canJump() const; + virtual void jump(); + virtual bool onGround() const; + + bool needsRemoval() const; + bool needsAddition() const; + void setEnabled(bool enabled); + bool isEnabled() const { return _enabled; } + void setDynamicsWorld(btDynamicsWorld* world); + + void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale); + bool needsShapeUpdate() const; + void updateShapeIfNecessary(); + + void preSimulation(btScalar timeStep); + void postSimulation(); +}; + +//virtual void setWalkDirectio(const btVector3 &walkDirection)=0 +//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 + +#endif // hifi_DynamicCharacterController_h diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 67e4e0616f..f20d07fe2e 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -628,7 +628,7 @@ bool PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio return true; } -void PhysicsEngine::setCharacterController(CharacterController* character) { +void PhysicsEngine::setCharacterController(DynamicCharacterController* character) { if (_characterController != character) { lock(); if (_characterController) { diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index d7d3278286..01717be175 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -22,7 +22,7 @@ #include #include "BulletUtil.h" -#include "CharacterController.h" +#include "DynamicCharacterController.h" #include "ContactInfo.h" #include "EntityMotionState.h" #include "ShapeManager.h" @@ -84,7 +84,7 @@ public: /// process queue of changed from external sources void relayIncomingChangesToSimulation(); - void setCharacterController(CharacterController* character); + void setCharacterController(DynamicCharacterController* character); void dumpNextStats() { _dumpNextStats = true; } @@ -122,7 +122,7 @@ private: uint32_t _lastNumSubstepsAtUpdateInternal = 0; /// character collisions - CharacterController* _characterController = NULL; + DynamicCharacterController* _characterController = NULL; bool _dumpNextStats = false; }; From 23951620bba2aded3ba8880111a8df2d95acd91c Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 6 Apr 2015 16:19:14 -0700 Subject: [PATCH 2/6] cleanup DynamicCharacterController --- .../src/DynamicCharacterController.cpp | 110 +----------------- .../physics/src/DynamicCharacterController.h | 27 +---- 2 files changed, 10 insertions(+), 127 deletions(-) diff --git a/libraries/physics/src/DynamicCharacterController.cpp b/libraries/physics/src/DynamicCharacterController.cpp index 525d918c4d..9969619ff2 100644 --- a/libraries/physics/src/DynamicCharacterController.cpp +++ b/libraries/physics/src/DynamicCharacterController.cpp @@ -20,7 +20,6 @@ DynamicCharacterController::DynamicCharacterController(AvatarData* avatarData) { _rayLambda[0] = 1.0f; _rayLambda[1] = 1.0f; _halfHeight = 1.0f; - //_turnVelocity = 1.0f; // radians/sec _shape = NULL; _rigidBody = NULL; @@ -30,7 +29,6 @@ DynamicCharacterController::DynamicCharacterController(AvatarData* avatarData) { _enabled = false; _walkVelocity.setValue(0.0f,0.0f,0.0f); - //_verticalVelocity = 0.0f; _jumpSpeed = JUMP_SPEED; _isOnGround = false; _isJumping = false; @@ -48,48 +46,6 @@ void DynamicCharacterController::setWalkDirection(const btVector3& walkDirection _walkVelocity = walkDirection; } -/* -void DynamicCharacterController::setup(btScalar height, btScalar width, btScalar stepHeight) { - btVector3 spherePositions[2]; - btScalar sphereRadii[2]; - - sphereRadii[0] = width; - sphereRadii[1] = width; - spherePositions[0] = btVector3 (0.0f, (height/btScalar(2.0f) - width), 0.0f); - spherePositions[1] = btVector3 (0.0f, (-height/btScalar(2.0f) + width), 0.0f); - - _halfHeight = height/btScalar(2.0f); - - _shape = new btMultiSphereShape(&spherePositions[0], &sphereRadii[0], 2); - - btTransform startTransform; - startTransform.setIdentity(); - startTransform.setOrigin(btVector3(0.0f, 2.0f, 0.0f)); - btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); - btRigidBody::btRigidBodyConstructionInfo cInfo(1.0f, myMotionState, _shape); - _rigidBody = new btRigidBody(cInfo); - // kinematic vs. static doesn't work - //_rigidBody->setCollisionFlags( _rigidBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); - _rigidBody->setSleepingThresholds(0.0f, 0.0f); - _rigidBody->setAngularFactor(0.0f); -} - -void DynamicCharacterController::destroy() { - if (_shape) { - delete _shape; - } - - if (_rigidBody) { - delete _rigidBody; - _rigidBody = NULL; - } -} -*/ - -btCollisionObject* DynamicCharacterController::getCollisionObject() { - return _rigidBody; -} - void DynamicCharacterController::preStep(btCollisionWorld* collisionWorld) { const btTransform& xform = _rigidBody->getCenterOfMassTransform(); @@ -136,16 +92,6 @@ void DynamicCharacterController::preStep(btCollisionWorld* collisionWorld) { } void DynamicCharacterController::playerStep(btCollisionWorld* dynaWorld,btScalar dt) { - /* Handle turning - const btTransform& xform = _rigidBody->getCenterOfMassTransform(); - if (left) - _turnAngle -= dt * _turnVelocity; - if (right) - _turnAngle += dt * _turnVelocity; - - xform.setRotation(btQuaternion(btVector3(0.0, 1.0, 0.0), _turnAngle)); - */ - btVector3 currentVelocity = _rigidBody->getLinearVelocity(); btScalar currentSpeed = currentVelocity.length(); @@ -169,16 +115,10 @@ void DynamicCharacterController::playerStep(btCollisionWorld* dynaWorld,btScalar } _rigidBody->setLinearVelocity(currentVelocity - tau * (currentVelocity - desiredVelocity)); } - - /* - _rigidBody->getMotionState()->setWorldTransform(xform); - _rigidBody->setCenterOfMassTransform(xform); - */ } bool DynamicCharacterController::canJump() const { - return false; // temporarily disabled - //return onGround(); + return onGround(); } void DynamicCharacterController::jump() { @@ -199,24 +139,6 @@ bool DynamicCharacterController::onGround() const { return _rayLambda[0] < btScalar(1.0); } -void DynamicCharacterController::debugDraw(btIDebugDraw* debugDrawer) { -} - -void DynamicCharacterController::setUpInterpolate(bool value) { - // This method is required by btCharacterControllerInterface, but it does nothing. - // What it used to do was determine whether stepUp() would: stop where it hit the ceiling - // (interpolate = true, and now default behavior) or happily penetrate objects above the avatar. -} - -void DynamicCharacterController::warp(const btVector3& origin) { -} - -void DynamicCharacterController::reset(btCollisionWorld* foo) { -} - -void DynamicCharacterController::registerPairCacheAndDispatcher(btOverlappingPairCache* pairCache, btCollisionDispatcher* dispatcher) { -} - void DynamicCharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale) { _boxScale = scale; @@ -289,11 +211,8 @@ void DynamicCharacterController::setDynamicsWorld(btDynamicsWorld* world) { _dynamicsWorld = world; _pendingFlags &= ~ PENDING_FLAG_JUMP; _dynamicsWorld->addRigidBody(_rigidBody); -// _dynamicsWorld->addCollisionObject(_rigidBody, -// btBroadphaseProxy::CharacterFilter, -// btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter); _dynamicsWorld->addAction(this); -// reset(_dynamicsWorld); + //reset(_dynamicsWorld); } } if (_dynamicsWorld) { @@ -314,7 +233,7 @@ void DynamicCharacterController::updateShapeIfNecessary() { // (don't want to delete _rigidBody out from under the simulation) assert(!(_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION)); _pendingFlags &= ~ PENDING_FLAG_UPDATE_SHAPE; - // delete shape and GhostObject + // delete shape and RigidBody delete _rigidBody; _rigidBody = NULL; delete _shape; @@ -343,10 +262,6 @@ void DynamicCharacterController::updateShapeIfNecessary() { _rigidBody->setAngularFactor (0.0f); _rigidBody->setWorldTransform(btTransform(glmToBullet(_avatarData->getOrientation()), glmToBullet(_avatarData->getPosition()))); - // stepHeight affects the heights of ledges that the character can ascend - //_stepUpHeight = _radius + 0.25f * _halfHeight + 0.1f; - //_stepDownHeight = _radius; - //_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT); } else { // TODO: handle this failure case @@ -365,17 +280,14 @@ void DynamicCharacterController::preSimulation(btScalar timeStep) { _rigidBody->setWorldTransform(btTransform(glmToBullet(rotation), glmToBullet(position))); _rigidBody->setLinearVelocity(walkVelocity); - //setVelocityForTimeInterval(walkVelocity, timeStep); if (_pendingFlags & PENDING_FLAG_JUMP) { _pendingFlags &= ~ PENDING_FLAG_JUMP; if (canJump()) { + // TODO: make jump work //_verticalVelocity = _jumpSpeed; _isJumping = true; } } - // remember last position so we can throttle the total motion from the next step -// _lastPosition = position; -// _stepDt = 0.0f; // the rotation is determined by AvatarData btTransform xform = _rigidBody->getCenterOfMassTransform(); @@ -390,20 +302,6 @@ void DynamicCharacterController::postSimulation() { glm::quat rotation = bulletToGLM(avatarTransform.getRotation()); glm::vec3 position = bulletToGLM(avatarTransform.getOrigin()); - /* - // cap the velocity of the step so that the character doesn't POP! so hard on steps - glm::vec3 finalStep = position - _lastPosition; - btVector3 finalVelocity = _walkVelocity + _verticalVelocity * _currentUp;; - const btScalar MAX_RESOLUTION_SPEED = 5.0f; // m/sec - btScalar maxStepLength = glm::max(MAX_RESOLUTION_SPEED, 2.0f * finalVelocity.length()) * _stepDt; - btScalar stepLength = glm::length(finalStep); - if (stepLength > maxStepLength) { - position = _lastPosition + (maxStepLength / stepLength) * finalStep; - // NOTE: we don't need to move ghostObject to throttled position unless - // we want to support do async ray-traces/collision-queries against character - } - */ - _avatarData->setOrientation(rotation); _avatarData->setPosition(position - rotation * _shapeLocalOffset); } diff --git a/libraries/physics/src/DynamicCharacterController.h b/libraries/physics/src/DynamicCharacterController.h index a9b9ce9bd1..7bbe8504ab 100644 --- a/libraries/physics/src/DynamicCharacterController.h +++ b/libraries/physics/src/DynamicCharacterController.h @@ -49,17 +49,16 @@ public: DynamicCharacterController(AvatarData* avatarData); ~DynamicCharacterController (); -// void setup(btScalar height = 2.0, btScalar width = 0.25, btScalar stepHeight = 0.25); -// void destroy (); - virtual void setWalkDirection(const btVector3& walkDirection); virtual void setVelocityForTimeInterval(const btVector3 &velocity, btScalar timeInterval) { assert(false); } - virtual void reset(btCollisionWorld* collisionWorld); - virtual void warp(const btVector3& origin); - virtual void registerPairCacheAndDispatcher(btOverlappingPairCache* pairCache, btCollisionDispatcher* dispatcher); + // TODO: implement these when needed + virtual void reset(btCollisionWorld* collisionWorld) { } + virtual void warp(const btVector3& origin) { } + virtual void debugDraw(btIDebugDraw* debugDrawer) { } + virtual void setUpInterpolate(bool value) { } - btCollisionObject* getCollisionObject(); + btCollisionObject* getCollisionObject() { return _rigidBody; } ///btActionInterface interface virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) { @@ -67,10 +66,6 @@ public: playerStep(collisionWorld, deltaTime); } - virtual void debugDraw(btIDebugDraw* debugDrawer); - - void setUpInterpolate(bool value); - virtual void preStep(btCollisionWorld* collisionWorld); virtual void playerStep(btCollisionWorld* collisionWorld, btScalar dt); @@ -92,14 +87,4 @@ public: void postSimulation(); }; -//virtual void setWalkDirectio(const btVector3 &walkDirection)=0 -//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 - #endif // hifi_DynamicCharacterController_h From e2ea940249fd9275b1eae4c8a6088125b1853e3d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 6 Apr 2015 16:21:04 -0700 Subject: [PATCH 3/6] more cleanup --- libraries/physics/src/DynamicCharacterController.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/physics/src/DynamicCharacterController.h b/libraries/physics/src/DynamicCharacterController.h index 7bbe8504ab..8489bf6dad 100644 --- a/libraries/physics/src/DynamicCharacterController.h +++ b/libraries/physics/src/DynamicCharacterController.h @@ -36,9 +36,6 @@ protected: bool _isOnGround; bool _isJumping; bool _isHovering; -// quint64 _jumpToHoverStart; -// btScalar _velocityTimeInterval; -// btScalar _stepDt; uint32_t _pendingFlags; btDynamicsWorld* _dynamicsWorld = NULL; From b5f0c57dd110b0f7e8cd5aa8397061df6f790838 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 7 Apr 2015 15:07:46 -0700 Subject: [PATCH 4/6] jumping and hovering works with dynamic controller --- interface/src/avatar/MyAvatar.cpp | 26 +- interface/src/avatar/MyAvatar.h | 2 +- .../src/DynamicCharacterController.cpp | 259 ++++++++++++------ .../physics/src/DynamicCharacterController.h | 17 +- 4 files changed, 206 insertions(+), 98 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index e4eb6e7869..b60d7316d9 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1116,7 +1116,7 @@ void MyAvatar::updateOrientation(float deltaTime) { } -glm::vec3 MyAvatar::applyKeyboardMotor(float deltaTime, const glm::vec3& localVelocity, bool hasFloor) { +glm::vec3 MyAvatar::applyKeyboardMotor(float deltaTime, const glm::vec3& localVelocity, bool isHovering) { if (! (_motionBehaviors & AVATAR_MOTION_KEYBOARD_MOTOR_ENABLED)) { return localVelocity; } @@ -1137,7 +1137,7 @@ glm::vec3 MyAvatar::applyKeyboardMotor(float deltaTime, const glm::vec3& localVe if (_isPushing || isThrust || (_scriptedMotorTimescale < MAX_KEYBOARD_MOTOR_TIMESCALE && _motionBehaviors | AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED)) { - // we don't want to break if anything is pushing the avatar around + // we don't want to brake if something is pushing the avatar around timescale = _keyboardMotorTimescale; _isBraking = false; } else { @@ -1168,14 +1168,8 @@ glm::vec3 MyAvatar::applyKeyboardMotor(float deltaTime, const glm::vec3& localVe if (directionLength > EPSILON) { direction /= directionLength; - if (hasFloor) { - // we're walking --> simple exponential decay toward target walk speed - const float WALK_ACCELERATION_TIMESCALE = 0.7f; // seconds to decrease delta to 1/e - _keyboardMotorVelocity = MAX_WALKING_SPEED * direction; - motorEfficiency = glm::clamp(deltaTime / WALK_ACCELERATION_TIMESCALE, 0.0f, 1.0f); - - } else { - // we're flying --> more complex curve + if (isHovering) { + // we're flying --> complex acceleration curve with high max speed float motorSpeed = glm::length(_keyboardMotorVelocity); float finalMaxMotorSpeed = _scale * MAX_KEYBOARD_MOTOR_SPEED; float speedGrowthTimescale = 2.0f; @@ -1191,6 +1185,12 @@ glm::vec3 MyAvatar::applyKeyboardMotor(float deltaTime, const glm::vec3& localVe motorSpeed = finalMaxMotorSpeed; } _keyboardMotorVelocity = motorSpeed * direction; + + } else { + // we're using a floor --> simple exponential decay toward target walk speed + const float WALK_ACCELERATION_TIMESCALE = 0.7f; // seconds to decrease delta to 1/e + _keyboardMotorVelocity = MAX_WALKING_SPEED * direction; + motorEfficiency = glm::clamp(deltaTime / WALK_ACCELERATION_TIMESCALE, 0.0f, 1.0f); } _isPushing = true; } @@ -1198,7 +1198,7 @@ glm::vec3 MyAvatar::applyKeyboardMotor(float deltaTime, const glm::vec3& localVe } else { _keyboardMotorVelocity = glm::vec3(0.0f); newLocalVelocity = (1.0f - motorEfficiency) * localVelocity; - if (hasFloor && !_wasPushing) { + if (!isHovering && !_wasPushing) { float speed = glm::length(newLocalVelocity); if (speed > MIN_AVATAR_SPEED) { // add small constant friction to help avatar drift to a stop sooner at low speeds @@ -1238,8 +1238,8 @@ void MyAvatar::updatePosition(float deltaTime) { glm::quat rotation = getHead()->getCameraOrientation(); glm::vec3 localVelocity = glm::inverse(rotation) * _velocity; - bool isOnGround = _characterController.onGround(); - glm::vec3 newLocalVelocity = applyKeyboardMotor(deltaTime, localVelocity, isOnGround); + bool isHovering = _characterController.isHovering(); + glm::vec3 newLocalVelocity = applyKeyboardMotor(deltaTime, localVelocity, isHovering); newLocalVelocity = applyScriptedMotor(deltaTime, newLocalVelocity); // rotate back into world-frame diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 5ba5c407b0..12f81f09a2 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -224,7 +224,7 @@ private: // private methods void updateOrientation(float deltaTime); - glm::vec3 applyKeyboardMotor(float deltaTime, const glm::vec3& velocity, bool walkingOnFloor); + glm::vec3 applyKeyboardMotor(float deltaTime, const glm::vec3& velocity, bool isHovering); glm::vec3 applyScriptedMotor(float deltaTime, const glm::vec3& velocity); void updatePosition(float deltaTime); void updateCollisionSound(const glm::vec3& penetration, float deltaTime, float frequency); diff --git a/libraries/physics/src/DynamicCharacterController.cpp b/libraries/physics/src/DynamicCharacterController.cpp index 9969619ff2..349e2666dd 100644 --- a/libraries/physics/src/DynamicCharacterController.cpp +++ b/libraries/physics/src/DynamicCharacterController.cpp @@ -7,18 +7,41 @@ #include "DynamicCharacterController.h" const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f); -const float DEFAULT_GRAVITY = 5.0f; +const float DEFAULT_GRAVITY = -5.0f; const float TERMINAL_VELOCITY = 55.0f; const float JUMP_SPEED = 3.5f; +const float MAX_FALL_HEIGHT = 20.0f; +const float MIN_HOVER_HEIGHT = 3.0f; + 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; +// 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; +}; + DynamicCharacterController::DynamicCharacterController(AvatarData* avatarData) { - _rayLambda[0] = 1.0f; - _rayLambda[1] = 1.0f; _halfHeight = 1.0f; _shape = NULL; _rigidBody = NULL; @@ -28,11 +51,15 @@ DynamicCharacterController::DynamicCharacterController(AvatarData* avatarData) { _enabled = false; + _floorDistance = MAX_FALL_HEIGHT; + _walkVelocity.setValue(0.0f,0.0f,0.0f); _jumpSpeed = JUMP_SPEED; _isOnGround = false; _isJumping = false; + _isFalling = false; _isHovering = true; + _jumpToHoverStart = 0; _pendingFlags = PENDING_FLAG_UPDATE_SHAPE; updateShapeIfNecessary(); @@ -43,100 +70,127 @@ DynamicCharacterController::~DynamicCharacterController() { // virtual void DynamicCharacterController::setWalkDirection(const btVector3& walkDirection) { - _walkVelocity = walkDirection; + // do nothing -- walkVelocity is upated in preSimulation() + //_walkVelocity = walkDirection; } void DynamicCharacterController::preStep(btCollisionWorld* collisionWorld) { - const btTransform& xform = _rigidBody->getCenterOfMassTransform(); + // trace a ray straight down to see if we're standing on the ground + const btTransform& xform = _rigidBody->getWorldTransform(); - btVector3 down = -xform.getBasis()[1]; - btVector3 forward = xform.getBasis()[2]; - down.normalize(); - forward.normalize(); + // rayStart is at center of bottom sphere + btVector3 rayStart = xform.getOrigin() - _halfHeight * _currentUp; - _raySource[0] = xform.getOrigin(); - _raySource[1] = xform.getOrigin(); - - _rayTarget[0] = _raySource[0] + down * _halfHeight * btScalar(1.1f); - _rayTarget[1] = _raySource[1] + forward * _halfHeight * btScalar(1.1f); - - 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; - }; + // 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); - - int i = 0; - for (i = 0; i < 2; i++) { - rayCallback.m_closestHitFraction = 1.0f; - collisionWorld->rayTest(_raySource[i], _rayTarget[i], rayCallback); - if (rayCallback.hasHit()) { - _rayLambda[i] = rayCallback.m_closestHitFraction; - } else { - _rayLambda[i] = 1.0f; - } + rayCallback.m_closestHitFraction = 1.0f; + collisionWorld->rayTest(rayStart, rayEnd, rayCallback); + if (rayCallback.hasHit()) { + _floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius; } } void DynamicCharacterController::playerStep(btCollisionWorld* dynaWorld,btScalar dt) { - btVector3 currentVelocity = _rigidBody->getLinearVelocity(); - btScalar currentSpeed = currentVelocity.length(); + btVector3 actualVelocity = _rigidBody->getLinearVelocity(); + btScalar actualSpeed = actualVelocity.length(); btVector3 desiredVelocity = _walkVelocity; btScalar desiredSpeed = desiredVelocity.length(); + const btScalar MIN_SPEED = 0.001f; - if (desiredSpeed < MIN_SPEED) { - if (currentSpeed < MIN_SPEED) { - _rigidBody->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f)); + + 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 BRAKING_TIMESCALE = 0.2f; - btScalar tau = dt / BRAKING_TIMESCALE; - _rigidBody->setLinearVelocity((1.0f - tau) * currentVelocity); + const btScalar HOVER_ACCELERATION_TIMESCALE = 0.1f; + btScalar tau = dt / HOVER_ACCELERATION_TIMESCALE; + _rigidBody->setLinearVelocity(actualVelocity - tau * (actualVelocity - desiredVelocity)); } } else { - const btScalar WALKING_TIMESCALE = 0.5f; - btScalar tau = dt / WALKING_TIMESCALE; 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 { + // falling or jumping + const btScalar FALL_ACCELERATION_TIMESCALE = 2.0f; + btScalar tau = dt / FALL_ACCELERATION_TIMESCALE; + btVector3 velocityCorrection = tau * (desiredVelocity - actualVelocity); // subtract vertical component - desiredVelocity = desiredVelocity - desiredVelocity.dot(_currentUp) * _currentUp; - } - _rigidBody->setLinearVelocity(currentVelocity - tau * (currentVelocity - desiredVelocity)); + velocityCorrection -= velocityCorrection.dot(_currentUp) * _currentUp; + _rigidBody->setLinearVelocity(actualVelocity + velocityCorrection); + } } } bool DynamicCharacterController::canJump() const { - return onGround(); + return onGround() && !_isJumping; } void DynamicCharacterController::jump() { - /* - if (!canJump()) { - return; - } + _pendingFlags |= PENDING_FLAG_JUMP; - btTransform xform = _rigidBody->getCenterOfMassTransform(); - btVector3 up = xform.getBasis()[1]; - up.normalize(); - btScalar magnitude = (btScalar(1.0)/_rigidBody->getInvMass()) * btScalar(8.0); - _rigidBody->applyCentralImpulse(up * magnitude); - */ + // check for case where user is holding down "jump" key... + // we'll eventually tansition to "hover" + if (!_isHovering) { + if (!_isJumping) { + _jumpToHoverStart = usecTimestampNow(); + } else { + quint64 now = usecTimestampNow(); + const quint64 JUMP_TO_HOVER_PERIOD = USECS_PER_SECOND; + if (now - _jumpToHoverStart > JUMP_TO_HOVER_PERIOD) { + _isHovering = true; + } + } + } } bool DynamicCharacterController::onGround() const { - return _rayLambda[0] < btScalar(1.0); + const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius; + return _floorDistance < FLOOR_PROXIMITY_THRESHOLD; +} + +void DynamicCharacterController::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); + } + btVector3 g = _rigidBody->getGravity(); + } + } } void DynamicCharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale) { @@ -186,7 +240,7 @@ void DynamicCharacterController::setEnabled(bool 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; - _isHovering = true; + setHovering(true); } else { if (_dynamicsWorld) { _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION; @@ -262,6 +316,11 @@ void DynamicCharacterController::updateShapeIfNecessary() { _rigidBody->setAngularFactor (0.0f); _rigidBody->setWorldTransform(btTransform(glmToBullet(_avatarData->getOrientation()), glmToBullet(_avatarData->getPosition()))); + if (_isHovering) { + _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); + } else { + _rigidBody->setGravity(DEFAULT_GRAVITY * _currentUp); + } //_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT); } else { // TODO: handle this failure case @@ -269,36 +328,78 @@ void DynamicCharacterController::updateShapeIfNecessary() { } } +void DynamicCharacterController::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 DynamicCharacterController::preSimulation(btScalar timeStep) { if (_enabled && _dynamicsWorld) { glm::quat rotation = _avatarData->getOrientation(); - _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS); + + // TODO: update gravity if up has changed + updateUpAxis(rotation); + glm::vec3 position = _avatarData->getPosition() + rotation * _shapeLocalOffset; - - // TODO: get intended WALK velocity from _avatarData, not its actual velocity - btVector3 walkVelocity = glmToBullet(_avatarData->getVelocity()); - _rigidBody->setWorldTransform(btTransform(glmToBullet(rotation), glmToBullet(position))); - _rigidBody->setLinearVelocity(walkVelocity); + + // the rotation is dictated by AvatarData + btTransform xform = _rigidBody->getWorldTransform(); + xform.setRotation(glmToBullet(rotation)); + _rigidBody->setWorldTransform(xform); + + // scan for distant floor + // rayStart is at center of bottom sphere + btVector3 rayStart = xform.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) { + 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); + } + + _walkVelocity = glmToBullet(_avatarData->getVelocity()); + if (_pendingFlags & PENDING_FLAG_JUMP) { _pendingFlags &= ~ PENDING_FLAG_JUMP; if (canJump()) { // TODO: make jump work //_verticalVelocity = _jumpSpeed; _isJumping = true; + btVector3 velocity = _rigidBody->getLinearVelocity(); + velocity += _jumpSpeed * _currentUp; + _rigidBody->setLinearVelocity(velocity); } } - - // the rotation is determined by AvatarData - btTransform xform = _rigidBody->getCenterOfMassTransform(); - xform.setRotation(glmToBullet(rotation)); - _rigidBody->setCenterOfMassTransform(xform); } } void DynamicCharacterController::postSimulation() { if (_enabled && _rigidBody) { - const btTransform& avatarTransform = _rigidBody->getCenterOfMassTransform(); + const btTransform& avatarTransform = _rigidBody->getWorldTransform(); glm::quat rotation = bulletToGLM(avatarTransform.getRotation()); glm::vec3 position = bulletToGLM(avatarTransform.getOrigin()); diff --git a/libraries/physics/src/DynamicCharacterController.h b/libraries/physics/src/DynamicCharacterController.h index 8489bf6dad..2fad88b4db 100644 --- a/libraries/physics/src/DynamicCharacterController.h +++ b/libraries/physics/src/DynamicCharacterController.h @@ -10,6 +10,8 @@ class btCollisionShape; class btRigidBody; class btCollisionWorld; +const int NUM_CHARACTER_CONTROLLER_RAYS = 2; + ///DynamicCharacterController is obsolete/unsupported at the moment class DynamicCharacterController : public btCharacterControllerInterface { @@ -20,13 +22,11 @@ protected: btRigidBody* _rigidBody; btVector3 _currentUp; - btVector3 _raySource[2]; - btVector3 _rayTarget[2]; - btScalar _rayLambda[2]; - btVector3 _rayNormal[2]; + + btScalar _floorDistance; btVector3 _walkVelocity; - //btScalar _turnVelocity; + btScalar _gravity; glm::vec3 _shapeLocalOffset; glm::vec3 _boxScale; // used to compute capsule shape @@ -35,7 +35,9 @@ protected: bool _enabled; bool _isOnGround; bool _isJumping; + bool _isFalling; bool _isHovering; + quint64 _jumpToHoverStart; uint32_t _pendingFlags; btDynamicsWorld* _dynamicsWorld = NULL; @@ -69,6 +71,8 @@ public: virtual bool canJump() const; virtual void jump(); virtual bool onGround() const; + bool isHovering() const { return _isHovering; } + void setHovering(bool enabled); bool needsRemoval() const; bool needsAddition() const; @@ -82,6 +86,9 @@ public: void preSimulation(btScalar timeStep); void postSimulation(); + +protected: + void updateUpAxis(const glm::quat& rotation); }; #endif // hifi_DynamicCharacterController_h From d9146b1033d9b36e516fa4513dc8235b70d77839 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 7 Apr 2015 16:23:06 -0700 Subject: [PATCH 5/6] jump to hover works again --- .../src/DynamicCharacterController.cpp | 57 ++++++++++--------- .../physics/src/DynamicCharacterController.h | 5 +- 2 files changed, 33 insertions(+), 29 deletions(-) diff --git a/libraries/physics/src/DynamicCharacterController.cpp b/libraries/physics/src/DynamicCharacterController.cpp index 349e2666dd..db84bea540 100644 --- a/libraries/physics/src/DynamicCharacterController.cpp +++ b/libraries/physics/src/DynamicCharacterController.cpp @@ -59,6 +59,7 @@ DynamicCharacterController::DynamicCharacterController(AvatarData* avatarData) { _isJumping = false; _isFalling = false; _isHovering = true; + _isPushingUp = false; _jumpToHoverStart = 0; _pendingFlags = PENDING_FLAG_UPDATE_SHAPE; @@ -102,8 +103,12 @@ void DynamicCharacterController::playerStep(btCollisionWorld* dynaWorld,btScalar btVector3 desiredVelocity = _walkVelocity; btScalar desiredSpeed = desiredVelocity.length(); - const btScalar MIN_SPEED = 0.001f; + 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) { @@ -139,35 +144,36 @@ void DynamicCharacterController::playerStep(btCollisionWorld* dynaWorld,btScalar _rigidBody->setLinearVelocity(actualVelocity + velocityCorrection); } } else { - // falling or jumping - const btScalar FALL_ACCELERATION_TIMESCALE = 2.0f; - btScalar tau = dt / FALL_ACCELERATION_TIMESCALE; - btVector3 velocityCorrection = tau * (desiredVelocity - actualVelocity); - // subtract vertical component - velocityCorrection -= velocityCorrection.dot(_currentUp) * _currentUp; - _rigidBody->setLinearVelocity(actualVelocity + velocityCorrection); + // 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); } } } -bool DynamicCharacterController::canJump() const { - return onGround() && !_isJumping; -} - void DynamicCharacterController::jump() { - _pendingFlags |= PENDING_FLAG_JUMP; - // check for case where user is holding down "jump" key... // we'll eventually tansition to "hover" - if (!_isHovering) { - if (!_isJumping) { + if (!_isJumping) { + if (!_isHovering) { _jumpToHoverStart = usecTimestampNow(); - } else { - quint64 now = usecTimestampNow(); - const quint64 JUMP_TO_HOVER_PERIOD = USECS_PER_SECOND; - if (now - _jumpToHoverStart > JUMP_TO_HOVER_PERIOD) { - _isHovering = true; - } + _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); } } } @@ -188,7 +194,6 @@ void DynamicCharacterController::setHovering(bool hover) { } else { _rigidBody->setGravity(DEFAULT_GRAVITY * _currentUp); } - btVector3 g = _rigidBody->getGravity(); } } } @@ -368,7 +373,7 @@ void DynamicCharacterController::preSimulation(btScalar timeStep) { if (rayCallback.hasHit()) { _floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius; const btScalar MIN_HOVER_HEIGHT = 3.0f; - if (_isHovering && _floorDistance < MIN_HOVER_HEIGHT) { + if (_isHovering && _floorDistance < MIN_HOVER_HEIGHT && !_isPushingUp) { setHovering(false); } // TODO: use collision events rather than ray-trace test to disable jumping @@ -385,9 +390,7 @@ void DynamicCharacterController::preSimulation(btScalar timeStep) { if (_pendingFlags & PENDING_FLAG_JUMP) { _pendingFlags &= ~ PENDING_FLAG_JUMP; - if (canJump()) { - // TODO: make jump work - //_verticalVelocity = _jumpSpeed; + if (onGround()) { _isJumping = true; btVector3 velocity = _rigidBody->getLinearVelocity(); velocity += _jumpSpeed * _currentUp; diff --git a/libraries/physics/src/DynamicCharacterController.h b/libraries/physics/src/DynamicCharacterController.h index 2fad88b4db..f98343c49b 100644 --- a/libraries/physics/src/DynamicCharacterController.h +++ b/libraries/physics/src/DynamicCharacterController.h @@ -37,6 +37,7 @@ protected: bool _isJumping; bool _isFalling; bool _isHovering; + bool _isPushingUp; quint64 _jumpToHoverStart; uint32_t _pendingFlags; @@ -68,8 +69,8 @@ public: virtual void preStep(btCollisionWorld* collisionWorld); virtual void playerStep(btCollisionWorld* collisionWorld, btScalar dt); - virtual bool canJump() const; - virtual void jump(); + virtual bool canJump() const { assert(false); return false; } // never call this + virtual void jump(); // call this every frame the jump button is pressed virtual bool onGround() const; bool isHovering() const { return _isHovering; } void setHovering(bool enabled); From 2ccc25e380f4ab84006a16fc7dacb4f23bc2a6a3 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 7 Apr 2015 16:29:52 -0700 Subject: [PATCH 6/6] remove old kinematic CharacterController --- libraries/physics/src/CharacterController.cpp | 931 ------------------ libraries/physics/src/CharacterController.h | 181 ---- 2 files changed, 1112 deletions(-) delete mode 100755 libraries/physics/src/CharacterController.cpp delete mode 100644 libraries/physics/src/CharacterController.h diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp deleted file mode 100755 index 1d7a84c177..0000000000 --- a/libraries/physics/src/CharacterController.cpp +++ /dev/null @@ -1,931 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com -2015.03.25 -- modified by Andrew Meadows andrew@highfidelity.io - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. - If you use this software in a product, an acknowledgment in the product documentation would be appreciated - but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "BulletCollision/CollisionDispatch/btGhostObject.h" - -#include "BulletUtil.h" -#include "CharacterController.h" - -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; - -// static helper method -static btVector3 getNormalizedVector(const btVector3& v) { - // NOTE: check the length first, then normalize - // --> avoids assert when trying to normalize zero-length vectors - btScalar vLength = v.length(); - if (vLength < FLT_EPSILON) { - return btVector3(0.0f, 0.0f, 0.0f); - } - btVector3 n = v; - n /= vLength; - return n; -} - -class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback { -public: - btKinematicClosestNotMeRayResultCallback (btCollisionObject* 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: - btCollisionObject* _me; -}; - - -class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback { - public: - btKinematicClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& up, btScalar minSlopeDot) - : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) - , _me(me) - , _up(up) - , _minSlopeDot(minSlopeDot) - { - } - - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace) { - if (convexResult.m_hitCollisionObject == _me) { - return btScalar(1.0); - } - - if (!convexResult.m_hitCollisionObject->hasContactResponse()) { - return btScalar(1.0); - } - - btVector3 hitNormalWorld; - if (normalInWorldSpace) { - hitNormalWorld = convexResult.m_hitNormalLocal; - } else { - ///need to transform normal into worldspace - hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal; - } - - // Note: hitNormalWorld points into character, away from object - // and _up points opposite to movement - - btScalar dotUp = _up.dot(hitNormalWorld); - if (dotUp < _minSlopeDot) { - return btScalar(1.0); - } - - return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace); - } - -protected: - btCollisionObject* _me; - const btVector3 _up; - btScalar _minSlopeDot; -}; - -class StepDownConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback { - // special convex sweep callback for character during the stepDown() phase - public: - StepDownConvexResultCallback(btCollisionObject* me, - const btVector3& up, - const btVector3& start, - const btVector3& step, - const btVector3& pushDirection, - btScalar minSlopeDot, - btScalar radius, - btScalar halfHeight) - : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) - , _me(me) - , _up(up) - , _start(start) - , _step(step) - , _pushDirection(pushDirection) - , _minSlopeDot(minSlopeDot) - , _radius(radius) - , _halfHeight(halfHeight) - { - } - - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace) { - if (convexResult.m_hitCollisionObject == _me) { - return btScalar(1.0); - } - - if (!convexResult.m_hitCollisionObject->hasContactResponse()) { - return btScalar(1.0); - } - - btVector3 hitNormalWorld; - if (normalInWorldSpace) { - hitNormalWorld = convexResult.m_hitNormalLocal; - } else { - ///need to transform normal into worldspace - hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal; - } - - // Note: hitNormalWorld points into character, away from object - // and _up points opposite to movement - - btScalar dotUp = _up.dot(hitNormalWorld); - if (dotUp < _minSlopeDot) { - if (hitNormalWorld.dot(_pushDirection) > 0.0f) { - // ignore hits that push in same direction as character is moving - // which helps character NOT snag when stepping off ledges - return btScalar(1.0f); - } - - // compute the angle between "down" and the line from character center to "hit" point - btVector3 fractionalStep = convexResult.m_hitFraction * _step; - btVector3 localHit = convexResult.m_hitPointLocal - _start + fractionalStep; - btScalar angle = localHit.angle(-_up); - - // compute a maxAngle based on size of _step - btVector3 side(_radius, - (_halfHeight - _step.length() + fractionalStep.dot(_up)), 0.0f); - btScalar maxAngle = side.angle(-_up); - - // Ignore hits that are larger than maxAngle. Effectively what is happening here is: - // we're ignoring hits at contacts that have non-vertical normals... if they hit higher - // than the character's "feet". Ignoring the contact allows the character to slide down - // for these hits. In other words, vertical walls against the character's torso will - // not prevent them from "stepping down" to find the floor. - if (angle > maxAngle) { - return btScalar(1.0f); - } - } - - btScalar fraction = ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace); - return fraction; - } - -protected: - btCollisionObject* _me; - const btVector3 _up; - btVector3 _start; - btVector3 _step; - btVector3 _pushDirection; - btScalar _minSlopeDot; - btScalar _radius; - btScalar _halfHeight; -}; - -/* - * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal' - * - * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html - */ -btVector3 CharacterController::computeReflectionDirection(const btVector3& direction, const btVector3& normal) { - return direction - (btScalar(2.0) * direction.dot(normal)) * normal; -} - -/* - * Returns the portion of 'direction' that is parallel to 'normal' - */ -btVector3 CharacterController::parallelComponent(const btVector3& direction, const btVector3& normal) { - btScalar magnitude = direction.dot(normal); - return normal * magnitude; -} - -/* - * Returns the portion of 'direction' that is perpindicular to 'normal' - */ -btVector3 CharacterController::perpindicularComponent(const btVector3& direction, const btVector3& normal) { - return direction - parallelComponent(direction, normal); -} - -const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f); -const float DEFAULT_GRAVITY = 5.0f; -const float TERMINAL_VELOCITY = 55.0f; -const float JUMP_SPEED = 3.5f; - -CharacterController::CharacterController(AvatarData* avatarData) { - assert(avatarData); - _avatarData = avatarData; - - _enabled = false; - _ghostObject = NULL; - _convexShape = NULL; - - _addedMargin = 0.02f; - _walkDirection.setValue(0.0f,0.0f,0.0f); - _velocityTimeInterval = 0.0f; - _verticalVelocity = 0.0f; - _verticalOffset = 0.0f; - _gravity = DEFAULT_GRAVITY; - _maxFallSpeed = TERMINAL_VELOCITY; - _jumpSpeed = JUMP_SPEED; - _isOnGround = false; - _isJumping = false; - _isHovering = true; - _jumpToHoverStart = 0; - setMaxSlope(btRadians(45.0f)); - _lastStepUp = 0.0f; - - _pendingFlags = PENDING_FLAG_UPDATE_SHAPE; - updateShapeIfNecessary(); -} - -CharacterController::~CharacterController() { - delete _ghostObject; - _ghostObject = NULL; - delete _convexShape; - _convexShape = NULL; - // make sure you remove this Character from its DynamicsWorld before reaching this spot - assert(_dynamicsWorld == NULL); -} - -btPairCachingGhostObject* CharacterController::getGhostObject() { - return _ghostObject; -} - -bool CharacterController::recoverFromPenetration(btCollisionWorld* collisionWorld) { - BT_PROFILE("recoverFromPenetration"); - // Here we must refresh the overlapping paircache as the penetrating movement itself or the - // previous recovery iteration might have used setWorldTransform and pushed us into an object - // that is not in the previous cache contents from the last timestep, as will happen if we - // are pushed into a new AABB overlap. Unhandled this means the next convex sweep gets stuck. - // - // Do this by calling the broadphase's setAabb with the moved AABB, this will update the broadphase - // paircache and the ghostobject's internal paircache at the same time. /BW - - btVector3 minAabb, maxAabb; - _convexShape->getAabb(_ghostObject->getWorldTransform(), minAabb, maxAabb); - collisionWorld->getBroadphase()->setAabb(_ghostObject->getBroadphaseHandle(), - minAabb, - maxAabb, - collisionWorld->getDispatcher()); - - bool penetration = false; - - collisionWorld->getDispatcher()->dispatchAllCollisionPairs(_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher()); - - _currentPosition = _ghostObject->getWorldTransform().getOrigin(); - - btVector3 currentPosition = _currentPosition; - - btScalar maxPen = btScalar(0.0); - for (int i = 0; i < _ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) { - _manifoldArray.resize(0); - - btBroadphasePair* collisionPair = &_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; - - btCollisionObject* obj0 = static_cast(collisionPair->m_pProxy0->m_clientObject); - btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); - - if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse())) { - continue; - } - - if (collisionPair->m_algorithm) { - collisionPair->m_algorithm->getAllContactManifolds(_manifoldArray); - } - - for (int j = 0;j < _manifoldArray.size(); j++) { - btPersistentManifold* manifold = _manifoldArray[j]; - btScalar directionSign = (manifold->getBody0() == _ghostObject) ? btScalar(1.0) : btScalar(-1.0); - for (int p = 0;p < manifold->getNumContacts(); p++) { - const btManifoldPoint&pt = manifold->getContactPoint(p); - - btScalar dist = pt.getDistance(); - - if (dist < 0.0) { - bool useContact = true; - btVector3 normal = pt.m_normalWorldOnB; - normal *= directionSign; // always points from object to character - - btScalar normalDotUp = normal.dot(_currentUp); - if (normalDotUp < _maxSlopeCosine) { - // this contact has a non-vertical normal... might need to ignored - btVector3 collisionPoint; - if (directionSign > 0.0) { - collisionPoint = pt.getPositionWorldOnB(); - } else { - collisionPoint = pt.getPositionWorldOnA(); - } - - // we do math in frame where character base is origin - btVector3 characterBase = currentPosition - (_radius + _halfHeight) * _currentUp; - collisionPoint -= characterBase; - btScalar collisionHeight = collisionPoint.dot(_currentUp); - - if (collisionHeight < _lastStepUp) { - // This contact is below the lastStepUp, so we ignore it for penetration resolution, - // otherwise it may prevent the character from getting close enough to find any available - // horizontal foothold that would allow it to climbe the ledge. In other words, we're - // making the character's "feet" soft for collisions against steps, but not floors. - useContact = false; - } - } - if (useContact) { - - if (dist < maxPen) { - maxPen = dist; - _floorNormal = normal; - } - const btScalar INCREMENTAL_RESOLUTION_FACTOR = 0.2f; - _currentPosition += normal * (fabsf(dist) * INCREMENTAL_RESOLUTION_FACTOR); - penetration = true; - } - } - } - } - } - btTransform newTrans = _ghostObject->getWorldTransform(); - newTrans.setOrigin(_currentPosition); - _ghostObject->setWorldTransform(newTrans); - return penetration; -} - - -void CharacterController::scanDown(btCollisionWorld* world) { - BT_PROFILE("scanDown"); - // we test with downward raycast and if we don't find floor close enough then turn on "hover" - btKinematicClosestNotMeRayResultCallback callback(_ghostObject); - callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - - btVector3 start = _currentPosition; - const btScalar MAX_SCAN_HEIGHT = 20.0f + _halfHeight + _radius; // closest possible floor for disabling hover - const btScalar MIN_HOVER_HEIGHT = 3.0f + _halfHeight + _radius; // distance to floor for enabling hover - btVector3 end = start - MAX_SCAN_HEIGHT * _currentUp; - - world->rayTest(start, end, callback); - if (!callback.hasHit()) { - _isHovering = true; - } else if (_isHovering && callback.m_closestHitFraction * MAX_SCAN_HEIGHT < MIN_HOVER_HEIGHT) { - _isHovering = false; - } -} - -void CharacterController::stepUp(btCollisionWorld* world) { - BT_PROFILE("stepUp"); - // phase 1: up - - // compute start and end - btTransform start, end; - start.setIdentity(); - start.setOrigin(_currentPosition + _currentUp * (_convexShape->getMargin() + _addedMargin)); - - _targetPosition = _currentPosition + _currentUp * _stepUpHeight; - end.setIdentity(); - end.setOrigin(_targetPosition); - - // sweep up - btVector3 sweepDirNegative = - _currentUp; - btKinematicClosestNotMeConvexResultCallback callback(_ghostObject, sweepDirNegative, btScalar(0.7071)); - callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - _ghostObject->convexSweepTest(_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); - - if (callback.hasHit()) { - // we hit something, so zero our vertical velocity - _verticalVelocity = 0.0f; - _verticalOffset = 0.0f; - - // Only modify the position if the hit was a slope and not a wall or ceiling. - if (callback.m_hitNormalWorld.dot(_currentUp) > 0.0f) { - _lastStepUp = _stepUpHeight * callback.m_closestHitFraction; - _currentPosition.setInterpolate3(_currentPosition, _targetPosition, callback.m_closestHitFraction); - } else { - _lastStepUp = _stepUpHeight; - _currentPosition = _targetPosition; - } - } else { - _currentPosition = _targetPosition; - _lastStepUp = _stepUpHeight; - } -} - -void CharacterController::updateTargetPositionBasedOnCollision(const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) { - btVector3 movementDirection = _targetPosition - _currentPosition; - btScalar movementLength = movementDirection.length(); - if (movementLength > SIMD_EPSILON) { - movementDirection.normalize(); - - btVector3 reflectDir = computeReflectionDirection(movementDirection, hitNormal); - reflectDir.normalize(); - - btVector3 parallelDir, perpindicularDir; - - parallelDir = parallelComponent(reflectDir, hitNormal); - perpindicularDir = perpindicularComponent(reflectDir, hitNormal); - - _targetPosition = _currentPosition; - //if (tangentMag != 0.0) { - if (0) { - btVector3 parComponent = parallelDir * btScalar(tangentMag * movementLength); - _targetPosition += parComponent; - } - - if (normalMag != 0.0) { - btVector3 perpComponent = perpindicularDir * btScalar(normalMag * movementLength); - _targetPosition += perpComponent; - } - } -} - -void CharacterController::stepForward(btCollisionWorld* collisionWorld, const btVector3& movement) { - BT_PROFILE("stepForward"); - // phase 2: forward - _targetPosition = _currentPosition + movement; - - btTransform start, end; - start.setIdentity(); - end.setIdentity(); - - /* TODO: experiment with this to see if we can use this to help direct motion when a floor is available - if (_touchingContact) { - if (_normalizedDirection.dot(_floorNormal) < btScalar(0.0)) { - updateTargetPositionBasedOnCollision(_floorNormal, 1.0f, 1.0f); - } - }*/ - - // modify shape's margin for the sweeps - btScalar margin = _convexShape->getMargin(); - _convexShape->setMargin(margin + _addedMargin); - - const btScalar MIN_STEP_DISTANCE_SQUARED = 1.0e-6f; - btVector3 step = _targetPosition - _currentPosition; - btScalar stepLength2 = step.length2(); - int maxIter = 10; - - while (stepLength2 > MIN_STEP_DISTANCE_SQUARED && maxIter-- > 0) { - start.setOrigin(_currentPosition); - end.setOrigin(_targetPosition); - - // sweep forward - btVector3 sweepDirNegative(_currentPosition - _targetPosition); - btKinematicClosestNotMeConvexResultCallback callback(_ghostObject, sweepDirNegative, btScalar(0.0)); - callback.m_collisionFilterGroup = _ghostObject->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = _ghostObject->getBroadphaseHandle()->m_collisionFilterMask; - _ghostObject->convexSweepTest(_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - - if (callback.hasHit()) { - // we hit soemthing! - // Compute new target position by removing portion cut-off by collision, which will produce a new target - // that is the closest approach of the the obstacle plane to the original target. - step = _targetPosition - _currentPosition; - btScalar stepDotNormal = step.dot(callback.m_hitNormalWorld); // we expect this dot to be negative - step += (stepDotNormal * (1.0f - callback.m_closestHitFraction)) * callback.m_hitNormalWorld; - _targetPosition = _currentPosition + step; - - stepLength2 = step.length2(); - } else { - // we swept to the end without hitting anything - _currentPosition = _targetPosition; - break; - } - } - - // restore shape's margin - _convexShape->setMargin(margin); -} - -void CharacterController::stepDown(btCollisionWorld* collisionWorld, btScalar dt) { - BT_PROFILE("stepDown"); - // phase 3: down - // - // The "stepDown" phase first makes a normal sweep down that cancels the lift from the "stepUp" phase. - // If it hits a ledge then it stops otherwise it makes another sweep down in search of a floor within - // reach of the character's feet. - - // first sweep for ledge - btVector3 step = (_verticalVelocity * dt - _lastStepUp) * _currentUp; - - StepDownConvexResultCallback callback(_ghostObject, - _currentUp, - _currentPosition, step, - _walkDirection, - _maxSlopeCosine, - _radius, _halfHeight); - callback.m_collisionFilterGroup = _ghostObject->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = _ghostObject->getBroadphaseHandle()->m_collisionFilterMask; - - btTransform start, end; - start.setIdentity(); - end.setIdentity(); - - start.setOrigin(_currentPosition); - _targetPosition = _currentPosition + step; - end.setOrigin(_targetPosition); - _ghostObject->convexSweepTest(_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - - _isOnGround = false; - if (callback.hasHit()) { - _currentPosition += callback.m_closestHitFraction * step; - _verticalVelocity = 0.0f; - _verticalOffset = 0.0f; - _isJumping = false; - _isHovering = false; - _isOnGround = true; - } else if (!_isJumping) { - // sweep again for floor within downStep threshold - step = -_stepDownHeight * _currentUp; - StepDownConvexResultCallback callback2 (_ghostObject, - _currentUp, - _currentPosition, step, - _walkDirection, - _maxSlopeCosine, - _radius, _halfHeight); - - callback2.m_collisionFilterGroup = _ghostObject->getBroadphaseHandle()->m_collisionFilterGroup; - callback2.m_collisionFilterMask = _ghostObject->getBroadphaseHandle()->m_collisionFilterMask; - - _currentPosition = _targetPosition; - _targetPosition = _currentPosition + step; - - start.setOrigin(_currentPosition); - end.setOrigin(_targetPosition); - _ghostObject->convexSweepTest(_convexShape, start, end, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - - if (callback2.hasHit()) { - _currentPosition += callback2.m_closestHitFraction * step; - _verticalVelocity = 0.0f; - _verticalOffset = 0.0f; - _isJumping = false; - _isHovering = false; - _isOnGround = true; - } else { - // nothing to step down on - _lastStepUp = 0.0f; - } - } else { - // we're jumping, and didn't hit anything, so our target position is where we would have fallen to - _currentPosition = _targetPosition; - } -} - -void CharacterController::setWalkDirection(const btVector3& walkDirection) { - // This must be implemented to satisfy base-class interface but does nothing. - // Use setVelocityForTimeInterval() instead. - assert(false); -} - -void CharacterController::setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) { - _walkDirection = velocity; - _normalizedDirection = getNormalizedVector(_walkDirection); - _velocityTimeInterval += timeInterval; -} - -void CharacterController::reset(btCollisionWorld* collisionWorld) { - _verticalVelocity = 0.0; - _verticalOffset = 0.0; - _isOnGround = false; - _isJumping = false; - _isHovering = true; - _walkDirection.setValue(0,0,0); - _velocityTimeInterval = 0.0; - - //clear pair cache - btHashedOverlappingPairCache *cache = _ghostObject->getOverlappingPairCache(); - while (cache->getOverlappingPairArray().size() > 0) { - cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, - cache->getOverlappingPairArray()[0].m_pProxy1, - collisionWorld->getDispatcher()); - } -} - -void CharacterController::warp(const btVector3& origin) { - btTransform xform; - xform.setIdentity(); - xform.setOrigin(origin); - _ghostObject->setWorldTransform(xform); -} - - -void CharacterController::preStep(btCollisionWorld* collisionWorld) { - BT_PROFILE("preStep"); - if (!_enabled) { - return; - } - int numPenetrationLoops = 0; - _touchingContact = false; - while (recoverFromPenetration(collisionWorld)) { - numPenetrationLoops++; - _touchingContact = true; - if (numPenetrationLoops > 4) { - break; - } - } - - // the CharacterController algorithm can only change the position, - // so we don't bother to pull the rotation out of the transform - const btTransform& transform = _ghostObject->getWorldTransform(); - _currentPosition = transform.getOrigin(); -} - -void CharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt) { - BT_PROFILE("playerStep"); - if (!_enabled) { - return; // no motion - } - - // Update fall velocity. - if (_isHovering) { - const btScalar MIN_HOVER_VERTICAL_VELOCITY = 0.1f; - if (fabsf(_verticalVelocity) < MIN_HOVER_VERTICAL_VELOCITY) { - _verticalVelocity = 0.0f; - } else { - const btScalar HOVER_RELAXATION_TIMESCALE = 0.8f; - _verticalVelocity *= (1.0f - dt / HOVER_RELAXATION_TIMESCALE); - } - } else { - _verticalVelocity -= _gravity * dt; - if (_verticalVelocity > _jumpSpeed) { - _verticalVelocity = _jumpSpeed; - } else if (_verticalVelocity < -_maxFallSpeed) { - _verticalVelocity = -_maxFallSpeed; - } - } - _verticalOffset = _verticalVelocity * dt; - - btTransform xform; - xform = _ghostObject->getWorldTransform(); - - // the algorithm is as follows: - // (1) step the character up a little bit so that its forward step doesn't hit the floor - // (2) step the character forward - // (3) step the character down looking for new ledges, the original floor, or a floor one step below where we started - - scanDown(collisionWorld); - - stepUp(collisionWorld); - - // compute substep and decrement total interval - btScalar dtMoving = (dt < _velocityTimeInterval) ? dt : _velocityTimeInterval; - _velocityTimeInterval -= dt; - _stepDt += dt; - - // stepForward substep - btVector3 move = _walkDirection * dtMoving; - stepForward(collisionWorld, move); - - stepDown(collisionWorld, dt); - - xform.setOrigin(_currentPosition); - _ghostObject->setWorldTransform(xform); -} - -void CharacterController::setMaxFallSpeed(btScalar speed) { - _maxFallSpeed = speed; -} - -void CharacterController::setJumpSpeed(btScalar jumpSpeed) { - _jumpSpeed = jumpSpeed; -} - -void CharacterController::setMaxJumpHeight(btScalar maxJumpHeight) { - _maxJumpHeight = maxJumpHeight; -} - -bool CharacterController::canJump() const { - return _isOnGround; -} - -void CharacterController::jump() { - _pendingFlags |= PENDING_FLAG_JUMP; - - // check for case where user is holding down "jump" key... - // we'll eventually tansition to "hover" - if (!_isHovering) { - if (!_isJumping) { - _jumpToHoverStart = usecTimestampNow(); - } else { - quint64 now = usecTimestampNow(); - const quint64 JUMP_TO_HOVER_PERIOD = USECS_PER_SECOND; - if (now - _jumpToHoverStart > JUMP_TO_HOVER_PERIOD) { - _isHovering = true; - } - } - } -} - -void CharacterController::setGravity(btScalar gravity) { - _gravity = gravity; -} - -btScalar CharacterController::getGravity() const { - return _gravity; -} - -void CharacterController::setMaxSlope(btScalar slopeRadians) { - _maxSlopeRadians = slopeRadians; - _maxSlopeCosine = btCos(slopeRadians); -} - -btScalar CharacterController::getMaxSlope() const { - return _maxSlopeRadians; -} - -bool CharacterController::onGround() const { - return _isOnGround; -} - -void CharacterController::debugDraw(btIDebugDraw* debugDrawer) { -} - -void CharacterController::setUpInterpolate(bool value) { - // This method is required by btCharacterControllerInterface, but it does nothing. - // What it used to do was determine whether stepUp() would: stop where it hit the ceiling - // (interpolate = true, and now default behavior) or happily penetrate objects above the avatar. -} - -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; -} - -bool CharacterController::needsAddition() const { - return (bool)(_pendingFlags & PENDING_FLAG_ADD_TO_SIMULATION); -} - -bool CharacterController::needsRemoval() const { - return (bool)(_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION); -} - -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; - _isHovering = true; - _verticalVelocity = 0.0f; - } else { - if (_dynamicsWorld) { - _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION; - } - _pendingFlags &= ~ PENDING_FLAG_ADD_TO_SIMULATION; - _isOnGround = false; - } - _enabled = enabled; - } -} - -void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { - if (_dynamicsWorld != world) { - if (_dynamicsWorld) { - if (_ghostObject) { - _dynamicsWorld->removeCollisionObject(_ghostObject); - _dynamicsWorld->removeAction(this); - } - _dynamicsWorld = NULL; - } - if (world && _ghostObject) { - _dynamicsWorld = world; - _pendingFlags &= ~ PENDING_FLAG_JUMP; - _dynamicsWorld->addCollisionObject(_ghostObject, - btBroadphaseProxy::CharacterFilter, - btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter); - _dynamicsWorld->addAction(this); - reset(_dynamicsWorld); - } - } - if (_dynamicsWorld) { - if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { - // shouldn't fall in here, but if we do make sure both ADD and REMOVE bits are still set - _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_REMOVE_FROM_SIMULATION; - } else { - _pendingFlags &= ~PENDING_FLAG_ADD_TO_SIMULATION; - } - } else { - _pendingFlags &= ~ PENDING_FLAG_REMOVE_FROM_SIMULATION; - } -} - -void CharacterController::updateShapeIfNecessary() { - if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { - assert(!(_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION)); - _pendingFlags &= ~ PENDING_FLAG_UPDATE_SHAPE; - // make sure there is NO pending removal from simulation at this point - // (don't want to delete _ghostObject out from under the simulation) - // delete shape and GhostObject - delete _ghostObject; - _ghostObject = NULL; - delete _convexShape; - _convexShape = NULL; - - // compute new dimensions from avatar's bounding box - float x = _boxScale.x; - float z = _boxScale.z; - _radius = 0.5f * sqrtf(0.5f * (x * x + z * z)); - _halfHeight = 0.5f * _boxScale.y - _radius; - float MIN_HALF_HEIGHT = 0.1f; - if (_halfHeight < MIN_HALF_HEIGHT) { - _halfHeight = MIN_HALF_HEIGHT; - } - // NOTE: _shapeLocalOffset is already computed - - if (_radius > 0.0f) { - // create new ghost - _ghostObject = new btPairCachingGhostObject(); - _ghostObject->setWorldTransform(btTransform(glmToBullet(_avatarData->getOrientation()), - glmToBullet(_avatarData->getPosition()))); - // stepHeight affects the heights of ledges that the character can ascend - _stepUpHeight = _radius + 0.25f * _halfHeight + 0.1f; - _stepDownHeight = _radius; - - // create new shape - _convexShape = new btCapsuleShape(_radius, 2.0f * _halfHeight); - _ghostObject->setCollisionShape(_convexShape); - _ghostObject->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT); - } else { - // TODO: handle this failure case - } - } -} - -void CharacterController::preSimulation(btScalar timeStep) { - BT_PROFILE("preSimulation"); - if (_enabled && _dynamicsWorld) { - glm::quat rotation = _avatarData->getOrientation(); - _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS); - glm::vec3 position = _avatarData->getPosition() + rotation * _shapeLocalOffset; - btVector3 walkVelocity = glmToBullet(_avatarData->getVelocity()); - - _ghostObject->setWorldTransform(btTransform(glmToBullet(rotation), glmToBullet(position))); - setVelocityForTimeInterval(walkVelocity, timeStep); - if (_pendingFlags & PENDING_FLAG_JUMP) { - _pendingFlags &= ~ PENDING_FLAG_JUMP; - if (canJump()) { - _verticalVelocity = _jumpSpeed; - _isJumping = true; - } - } - // remember last position so we can throttle the total motion from the next step - _lastPosition = position; - _stepDt = 0.0f; - } -} - -void CharacterController::postSimulation() { - BT_PROFILE("postSimulation"); - if (_enabled && _ghostObject) { - const btTransform& avatarTransform = _ghostObject->getWorldTransform(); - glm::quat rotation = bulletToGLM(avatarTransform.getRotation()); - glm::vec3 position = bulletToGLM(avatarTransform.getOrigin()); - - // cap the velocity of the step so that the character doesn't POP! so hard on steps - glm::vec3 finalStep = position - _lastPosition; - btVector3 finalVelocity = _walkDirection; - finalVelocity += _verticalVelocity * _currentUp; - const btScalar MAX_RESOLUTION_SPEED = 5.0f; // m/sec - btScalar maxStepLength = glm::max(MAX_RESOLUTION_SPEED, 2.0f * finalVelocity.length()) * _stepDt; - btScalar stepLength = glm::length(finalStep); - if (stepLength > maxStepLength) { - position = _lastPosition + (maxStepLength / stepLength) * finalStep; - // NOTE: we don't need to move ghostObject to throttled position unless - // we want to support do async ray-traces/collision-queries against character - } - - _avatarData->setOrientation(rotation); - _avatarData->setPosition(position - rotation * _shapeLocalOffset); - } -} - diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h deleted file mode 100644 index e4e73b6d3f..0000000000 --- a/libraries/physics/src/CharacterController.h +++ /dev/null @@ -1,181 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com -2015.03.25 -- modified by Andrew Meadows andrew@highfidelity.io - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. - If you use this software in a product, an acknowledgment in the product documentation would be appreciated but - is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef hifi_CharacterController_h -#define hifi_CharacterController_h - -#include - -#include -#include -#include - - -class btConvexShape; -class btCollisionWorld; -class btCollisionDispatcher; -class btPairCachingGhostObject; - -///CharacterController is a custom version of btKinematicCharacterController - -///btKinematicCharacterController is an object that supports a sliding motion in a world. -///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations. -///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user. - - -ATTRIBUTE_ALIGNED16(class) CharacterController : public btCharacterControllerInterface -{ -protected: - ///this is the desired walk direction, set by the user - btVector3 _walkDirection; - btVector3 _normalizedDirection; - - //some internal variables - btVector3 _currentPosition; - btVector3 _currentUp; - btVector3 _targetPosition; - glm::vec3 _lastPosition; - btVector3 _floorNormal; // points from object to character - - glm::vec3 _shapeLocalOffset; - glm::vec3 _boxScale; // used to compute capsule shape - - AvatarData* _avatarData = NULL; - btPairCachingGhostObject* _ghostObject = NULL; - - btConvexShape* _convexShape;//is also in _ghostObject, but it needs to be convex, so we store it here to avoid upcast - btScalar _radius; - btScalar _halfHeight; - - btScalar _verticalVelocity; - btScalar _verticalOffset; // fall distance from velocity this frame - btScalar _maxFallSpeed; - btScalar _jumpSpeed; - btScalar _maxJumpHeight; - btScalar _maxSlopeRadians; // Slope angle that is set (used for returning the exact value) - btScalar _maxSlopeCosine; // Cosine equivalent of _maxSlopeRadians (calculated once when set, for optimization) - btScalar _gravity; - - btScalar _stepUpHeight; // height of stepUp prior to stepForward - btScalar _stepDownHeight; // height of stepDown - - btScalar _addedMargin;//@todo: remove this and fix the code - - btScalar _lastStepUp; - - ///keep track of the contact manifolds - btManifoldArray _manifoldArray; - - bool _touchingContact; - - bool _enabled; - bool _isOnGround; - bool _isJumping; - bool _isHovering; - quint64 _jumpToHoverStart; - btScalar _velocityTimeInterval; - btScalar _stepDt; - uint32_t _pendingFlags; - - btDynamicsWorld* _dynamicsWorld = NULL; - - btVector3 computeReflectionDirection(const btVector3& direction, const btVector3& normal); - btVector3 parallelComponent(const btVector3& direction, const btVector3& normal); - btVector3 perpindicularComponent(const btVector3& direction, const btVector3& normal); - - bool recoverFromPenetration(btCollisionWorld* collisionWorld); - void scanDown(btCollisionWorld* collisionWorld); - void stepUp(btCollisionWorld* collisionWorld); - void updateTargetPositionBasedOnCollision(const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0)); - void stepForward(btCollisionWorld* collisionWorld, const btVector3& walkMove); - void stepDown(btCollisionWorld* collisionWorld, btScalar dt); - void createShapeAndGhost(); -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - CharacterController(AvatarData* avatarData); - ~CharacterController(); - - - ///btActionInterface interface - virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) { - preStep(collisionWorld); - playerStep(collisionWorld, deltaTime); - } - - ///btActionInterface interface - void debugDraw(btIDebugDraw* debugDrawer); - - /// This should probably be called setPositionIncrementPerSimulatorStep. - /// This is neither a direction nor a velocity, but the amount to - /// increment the position each simulation iteration, regardless - /// of dt. - /// This call will reset any velocity set by setVelocityForTimeInterval(). - virtual void setWalkDirection(const btVector3& walkDirection); - - /// Caller provides a velocity with which the character should move for - /// the given time period. After the time period, velocity is reset - /// to zero. - /// This call will reset any walk direction set by setWalkDirection(). - /// Negative time intervals will result in no motion. - virtual void setVelocityForTimeInterval(const btVector3& velocity, - btScalar timeInterval); - - virtual void reset(btCollisionWorld* collisionWorld ); - virtual void warp(const btVector3& origin); - - virtual void preStep(btCollisionWorld* collisionWorld); - virtual void playerStep(btCollisionWorld* collisionWorld, btScalar dt); - - virtual bool canJump() const; - virtual void jump(); - virtual bool onGround() const; - - void setMaxFallSpeed(btScalar speed); - void setJumpSpeed(btScalar jumpSpeed); - void setMaxJumpHeight(btScalar maxJumpHeight); - - void setGravity(btScalar gravity); - btScalar getGravity() const; - - /// The max slope determines the maximum angle that the controller can walk up. - /// The slope angle is measured in radians. - void setMaxSlope(btScalar slopeRadians); - btScalar getMaxSlope() const; - - btPairCachingGhostObject* getGhostObject(); - - void setUpInterpolate(bool value); - - bool needsRemoval() const; - bool needsAddition() const; - void setEnabled(bool enabled); - bool isEnabled() const { return _enabled; } - void setDynamicsWorld(btDynamicsWorld* world); - - void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale); - bool needsShapeUpdate() const; - void updateShapeIfNecessary(); - - void preSimulation(btScalar timeStep); - void postSimulation(); -}; - -#endif // hifi_CharacterController_h