From 435ce106eb9ac5d70a302ffa5c5474053a2db4a2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 6 Apr 2015 16:03:13 -0700 Subject: [PATCH] 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; };