From a1a12465da8e270961337f4c76bf86083cfc3147 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 12 Mar 2015 16:36:41 -0700 Subject: [PATCH] able to update avatar controller in PhysicsEngine --- libraries/avatars/src/AvatarData.cpp | 7 +- libraries/avatars/src/AvatarData.h | 5 +- libraries/physics/src/CharacterController.cpp | 706 ++++++++++++++++++ libraries/physics/src/CharacterController.h | 172 +++++ libraries/physics/src/PhysicsEngine.cpp | 106 ++- libraries/physics/src/PhysicsEngine.h | 16 +- 6 files changed, 971 insertions(+), 41 deletions(-) create mode 100644 libraries/physics/src/CharacterController.cpp create mode 100644 libraries/physics/src/CharacterController.h diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp index 3ab8f38fe6..6d7395754c 100644 --- a/libraries/avatars/src/AvatarData.cpp +++ b/libraries/avatars/src/AvatarData.cpp @@ -32,6 +32,9 @@ quint64 DEFAULT_FILTERED_LOG_EXPIRY = 2 * USECS_PER_SECOND; using namespace std; +const glm::vec3 DEFAULT_LOCAL_AABOX_CORNER(-0.5f); +const glm::vec3 DEFAULT_LOCAL_AABOX_SCALE(1.0f); + AvatarData::AvatarData() : _sessionUUID(), _position(0.0f), @@ -55,9 +58,9 @@ AvatarData::AvatarData() : _errorLogExpiry(0), _owningAvatarMixer(), _lastUpdateTimer(), - _velocity(0.0f) + _velocity(0.0f), + _localAABox(DEFAULT_LOCAL_AABOX_CORNER, DEFAULT_LOCAL_AABOX_SCALE) { - } AvatarData::~AvatarData() { diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index 02b2c364c6..5490938e30 100644 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -50,6 +50,7 @@ typedef unsigned long long quint64; #include +#include "AABox.h" #include "HandData.h" #include "HeadData.h" #include "Player.h" @@ -296,8 +297,7 @@ public: QElapsedTimer& getLastUpdateTimer() { return _lastUpdateTimer; } - virtual float getBoundingRadius() const { return 1.0f; } - + const AABox& getLocalAABox() const { return _localAABox; } const Referential* getReferential() const { return _referential; } void togglePhysicsEnabled() { _enablePhysics = !_enablePhysics; } @@ -410,6 +410,7 @@ private: QReadWriteLock _lock; bool _enablePhysics = false; + AABox _localAABox; }; Q_DECLARE_METATYPE(AvatarData*) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp new file mode 100644 index 0000000000..5173b368c1 --- /dev/null +++ b/libraries/physics/src/CharacterController.cpp @@ -0,0 +1,706 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +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 + +#include "BulletCollision/CollisionDispatch/btGhostObject.h" + +#include "CharacterController.h" + + +// 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; +} + +///@todo Interact with dynamic objects, +///Ride kinematicly animated platforms properly +///More realistic (or maybe just a config option) falling +/// -> Should integrate falling velocity manually and use that in stepDown() +///Support jumping +///Support ducking + +/* This callback is unused, but we're keeping it around just in case we figure out how to use it. +class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback +{ +public: +btKinematicClosestNotMeRayResultCallback(btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) +{ +m_me = me; +} + +virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) +{ +if(rayResult.m_collisionObject == m_me) +return 1.0; + +return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); +} +protected: +btCollisionObject* m_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)) + , m_me(me) + , m_up(up) + , m_minSlopeDot(minSlopeDot) + { + } + + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace) { + if (convexResult.m_hitCollisionObject == m_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; + } + + btScalar dotUp = m_up.dot(hitNormalWorld); + if (dotUp < m_minSlopeDot) { + return btScalar(1.0); + } + + return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace); + } + +protected: + btCollisionObject* m_me; + const btVector3 m_up; + btScalar m_minSlopeDot; +}; + +/* + * 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); +} + +CharacterController::CharacterController( + btPairCachingGhostObject* ghostObject, + btConvexShape* convexShape, + btScalar stepHeight, + int upAxis) { + m_upAxis = upAxis; + m_addedMargin = 0.02; + m_walkDirection.setValue(0,0,0); + m_useGhostObjectSweepTest = true; + m_ghostObject = ghostObject; + m_stepHeight = stepHeight; + m_turnAngle = btScalar(0.0); + m_convexShape = convexShape; + m_useWalkDirection = true; // use walk direction by default, legacy behavior + m_velocityTimeInterval = 0.0; + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + m_gravity = 9.8 * 3 ; // 3G acceleration. + m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s. + m_jumpSpeed = 10.0; // ? + m_wasOnGround = false; + m_wasJumping = false; + m_interpolateUp = true; + setMaxSlope(btRadians(45.0)); + m_currentStepOffset = 0; + + // internal state data members + full_drop = false; + bounce_fix = false; +} + +CharacterController::~CharacterController() { +} + +btPairCachingGhostObject* CharacterController::getGhostObject() { + return m_ghostObject; +} + +bool CharacterController::recoverFromPenetration(btCollisionWorld* collisionWorld) { + // 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; + m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb, maxAabb); + collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(), + minAabb, + maxAabb, + collisionWorld->getDispatcher()); + + bool penetration = false; + + collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher()); + + m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); + + btScalar maxPen = btScalar(0.0); + for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) { + m_manifoldArray.resize(0); + + btBroadphasePair* collisionPair = &m_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(m_manifoldArray); + } + + for (int j=0;jgetBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0); + for (int p=0;pgetNumContacts();p++) { + const btManifoldPoint&pt = manifold->getContactPoint(p); + + btScalar dist = pt.getDistance(); + + if (dist < 0.0) { + if (dist < maxPen) { + maxPen = dist; + m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? + + } + m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2); + penetration = true; + } else { + //printf("touching %f\n", dist); + } + } + + //manifold->clearManifold(); + } + } + btTransform newTrans = m_ghostObject->getWorldTransform(); + newTrans.setOrigin(m_currentPosition); + m_ghostObject->setWorldTransform(newTrans); + //printf("m_touchingNormal = %f,%f,%f\n", m_touchingNormal[0], m_touchingNormal[1], m_touchingNormal[2]); + return penetration; +} + +void CharacterController::stepUp( btCollisionWorld* world) { + // phase 1: up + btTransform start, end; + m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f)); + + start.setIdentity(); + end.setIdentity(); + + /* FIXME: Handle penetration properly */ + start.setOrigin(m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin)); + end.setOrigin(m_targetPosition); + + btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071)); + callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + + if (m_useGhostObjectSweepTest) { + m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); + } + else { + world->convexSweepTest(m_convexShape, start, end, callback); + } + + if (callback.hasHit()) { + // Only modify the position if the hit was a slope and not a wall or ceiling. + if (callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0) { + // we moved up only a fraction of the step height + m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction; + if (m_interpolateUp == true) { + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + } else { + m_currentPosition = m_targetPosition; + } + } + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + } else { + m_currentStepOffset = m_stepHeight; + m_currentPosition = m_targetPosition; + } +} + +void CharacterController::updateTargetPositionBasedOnCollision(const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) { + btVector3 movementDirection = m_targetPosition - m_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); + + m_targetPosition = m_currentPosition; + //if (tangentMag != 0.0) { + if (0) { + btVector3 parComponent = parallelDir * btScalar(tangentMag*movementLength); + //printf("parComponent=%f,%f,%f\n", parComponent[0], parComponent[1], parComponent[2]); + m_targetPosition += parComponent; + } + + if (normalMag != 0.0) { + btVector3 perpComponent = perpindicularDir * btScalar(normalMag*movementLength); + //printf("perpComponent=%f,%f,%f\n", perpComponent[0], perpComponent[1], perpComponent[2]); + m_targetPosition += perpComponent; + } + } else { + //printf("movementLength don't normalize a zero vector\n"); + } +} + +void CharacterController::stepForwardAndStrafe( btCollisionWorld* collisionWorld, const btVector3& walkMove) { + //printf("m_normalizedDirection=%f,%f,%f\n", + // m_normalizedDirection[0], m_normalizedDirection[1], m_normalizedDirection[2]); + // phase 2: forward and strafe + btTransform start, end; + m_targetPosition = m_currentPosition + walkMove; + + start.setIdentity(); + end.setIdentity(); + + btScalar fraction = 1.0; + btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); + //printf("distance2=%f\n", distance2); + + if (m_touchingContact) { + if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) { + //interferes with step movement + //updateTargetPositionBasedOnCollision(m_touchingNormal); + } + } + + int maxIter = 10; + + while (fraction > btScalar(0.01) && maxIter-- > 0) { + start.setOrigin(m_currentPosition); + end.setOrigin(m_targetPosition); + btVector3 sweepDirNegative(m_currentPosition - m_targetPosition); + + btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, sweepDirNegative, btScalar(0.0)); + callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + + + btScalar margin = m_convexShape->getMargin(); + m_convexShape->setMargin(margin + m_addedMargin); + + + if (m_useGhostObjectSweepTest) { + m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } else { + collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } + + m_convexShape->setMargin(margin); + + + fraction -= callback.m_closestHitFraction; + + if (callback.hasHit()) { + // we moved only a fraction + //btScalar hitDistance; + //hitDistance = (callback.m_hitPointWorld - m_currentPosition).length(); + + //m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + + updateTargetPositionBasedOnCollision(callback.m_hitNormalWorld); + btVector3 currentDir = m_targetPosition - m_currentPosition; + distance2 = currentDir.length2(); + if (distance2 > SIMD_EPSILON) { + currentDir.normalize(); + /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */ + if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0)) { + break; + } + } else { + //printf("currentDir: don't normalize a zero vector\n"); + break; + } + } else { + // we moved whole way + m_currentPosition = m_targetPosition; + } + + //if (callback.m_closestHitFraction == 0.f) { + // break; + //} + + } +} + +void CharacterController::stepDown( btCollisionWorld* collisionWorld, btScalar dt) { + btTransform start, end, end_double; + bool runonce = false; + + // phase 3: down + /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0; + btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep); + btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt; + btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; + m_targetPosition -= (step_drop + gravity_drop);*/ + + btVector3 orig_position = m_targetPosition; + + btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + + if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { + downVelocity = m_fallSpeed; + } + + btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + m_targetPosition -= step_drop; + + btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + + btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + + while (1) { + start.setIdentity(); + end.setIdentity(); + + end_double.setIdentity(); + + start.setOrigin(m_currentPosition); + end.setOrigin(m_targetPosition); + + //set double test for 2x the step drop, to check for a large drop vs small drop + end_double.setOrigin(m_targetPosition - step_drop); + + if (m_useGhostObjectSweepTest) { + m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + + if (!callback.hasHit()) { + //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial) + m_ghostObject->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } + } else { + collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + + if (!callback.hasHit()) { + //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) + collisionWorld->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } + } + + btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + bool has_hit = false; + if(bounce_fix == true) { + has_hit = callback.hasHit() || callback2.hasHit(); + } else { + has_hit = callback2.hasHit(); + } + + if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false + && (m_wasOnGround || !m_wasJumping)) { + //redo the velocity calculation when falling a small amount, for fast stairs motion + //for larger falls, use the smoother/slower interpolated movement by not touching the target position + + m_targetPosition = orig_position; + downVelocity = m_stepHeight; + + btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + m_targetPosition -= step_drop; + runonce = true; + continue; //re-run previous tests + } + break; + } + + if (callback.hasHit() || runonce == true) { + // we dropped a fraction of the height -> hit floor + + btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2; + + //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY()); + + if (bounce_fix == true) { + if (full_drop == true) { + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + } else { + //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction); + } + } + else + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + + full_drop = false; + + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + m_wasJumping = false; + } else { + // we dropped the full height + + full_drop = true; + + if (bounce_fix == true) { + downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { + m_targetPosition += step_drop; //undo previous target change + downVelocity = m_fallSpeed; + step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + m_targetPosition -= step_drop; + } + } + //printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY()); + + m_currentPosition = m_targetPosition; + } +} + + + +void CharacterController::setWalkDirection(const btVector3& walkDirection) { + m_useWalkDirection = true; + m_walkDirection = walkDirection; + m_normalizedDirection = getNormalizedVector(m_walkDirection); +} + + + +void CharacterController::setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) { + //printf("setVelocity!\n"); + //printf(" interval: %f\n", timeInterval); + //printf(" velocity: (%f, %f, %f)\n", velocity.x(), velocity.y(), velocity.z()); + + m_useWalkDirection = false; + m_walkDirection = velocity; + m_normalizedDirection = getNormalizedVector(m_walkDirection); + m_velocityTimeInterval += timeInterval; +} + +void CharacterController::reset( btCollisionWorld* collisionWorld ) { + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + m_wasOnGround = false; + m_wasJumping = false; + m_walkDirection.setValue(0,0,0); + m_velocityTimeInterval = 0.0; + + //clear pair cache + btHashedOverlappingPairCache *cache = m_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); + m_ghostObject->setWorldTransform(xform); +} + + +void CharacterController::preStep( btCollisionWorld* collisionWorld) { + int numPenetrationLoops = 0; + m_touchingContact = false; + while (recoverFromPenetration(collisionWorld)) { + numPenetrationLoops++; + m_touchingContact = true; + if (numPenetrationLoops > 4) { + //printf("character could not recover from penetration = %d\n", numPenetrationLoops); + break; + } + } + + m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); + m_targetPosition = m_currentPosition; + //printf("m_targetPosition=%f,%f,%f\n", m_targetPosition[0], m_targetPosition[1], m_targetPosition[2]); +} + +void CharacterController::playerStep( btCollisionWorld* collisionWorld, btScalar dt) { + //printf("playerStep(): "); + //printf(" dt = %f", dt); + + // quick check... + if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) { + //printf("\n"); + return; // no motion + } + + m_wasOnGround = onGround(); + + // Update fall velocity. + m_verticalVelocity -= m_gravity * dt; + if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) { + m_verticalVelocity = m_jumpSpeed; + } + if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) { + m_verticalVelocity = -btFabs(m_fallSpeed); + } + m_verticalOffset = m_verticalVelocity * dt; + + + btTransform xform; + xform = m_ghostObject->getWorldTransform(); + + //printf("walkDirection(%f,%f,%f)\n", walkDirection[0], walkDirection[1], walkDirection[2]); + //printf("walkSpeed=%f\n", walkSpeed); + + stepUp (collisionWorld); + if (m_useWalkDirection) { + stepForwardAndStrafe(collisionWorld, m_walkDirection); + } else { + //printf(" time: %f", m_velocityTimeInterval); + // still have some time left for moving! + btScalar dtMoving = + (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval; + m_velocityTimeInterval -= dt; + + // how far will we move while we are moving? + btVector3 move = m_walkDirection * dtMoving; + + //printf(" dtMoving: %f", dtMoving); + + // okay, step + stepForwardAndStrafe(collisionWorld, move); + } + stepDown(collisionWorld, dt); + + //printf("\n"); + + xform.setOrigin(m_currentPosition); + m_ghostObject->setWorldTransform(xform); +} + +void CharacterController::setFallSpeed(btScalar fallSpeed) { + m_fallSpeed = fallSpeed; +} + +void CharacterController::setJumpSpeed(btScalar jumpSpeed) { + m_jumpSpeed = jumpSpeed; +} + +void CharacterController::setMaxJumpHeight(btScalar maxJumpHeight) { + m_maxJumpHeight = maxJumpHeight; +} + +bool CharacterController::canJump() const { + return onGround(); +} + +void CharacterController::jump() { + if (!canJump()) { + return; + } + + m_verticalVelocity = m_jumpSpeed; + m_wasJumping = true; + +#if 0 + currently no jumping. + btTransform xform; + m_rigidBody->getMotionState()->getWorldTransform(xform); + btVector3 up = xform.getBasis()[1]; + up.normalize(); + btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0); + m_rigidBody->applyCentralImpulse (up * magnitude); +#endif +} + +void CharacterController::setGravity(btScalar gravity) { + m_gravity = gravity; +} + +btScalar CharacterController::getGravity() const { + return m_gravity; +} + +void CharacterController::setMaxSlope(btScalar slopeRadians) { + m_maxSlopeRadians = slopeRadians; + m_maxSlopeCosine = btCos(slopeRadians); +} + +btScalar CharacterController::getMaxSlope() const { + return m_maxSlopeRadians; +} + +bool CharacterController::onGround() const { + return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0; +} + +btVector3* CharacterController::getUpAxisDirections() { + static btVector3 sUpAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) }; + + return sUpAxisDirection; +} + +void CharacterController::debugDraw(btIDebugDraw* debugDrawer) { +} + +void CharacterController::setUpInterpolate(bool value) { + m_interpolateUp = value; +} diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h new file mode 100644 index 0000000000..301253b2bd --- /dev/null +++ b/libraries/physics/src/CharacterController.h @@ -0,0 +1,172 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +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 + + +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: + + btScalar m_halfHeight; + + btPairCachingGhostObject* m_ghostObject; + btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast + + btScalar m_verticalVelocity; + btScalar m_verticalOffset; + btScalar m_fallSpeed; + btScalar m_jumpSpeed; + btScalar m_maxJumpHeight; + btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value) + btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization) + btScalar m_gravity; + + btScalar m_turnAngle; + + btScalar m_stepHeight; + + btScalar m_addedMargin;//@todo: remove this and fix the code + + ///this is the desired walk direction, set by the user + btVector3 m_walkDirection; + btVector3 m_normalizedDirection; + + //some internal variables + btVector3 m_currentPosition; + btScalar m_currentStepOffset; + btVector3 m_targetPosition; + + ///keep track of the contact manifolds + btManifoldArray m_manifoldArray; + + bool m_touchingContact; + btVector3 m_touchingNormal; + + bool m_wasOnGround; + bool m_wasJumping; + bool m_useGhostObjectSweepTest; + bool m_useWalkDirection; + btScalar m_velocityTimeInterval; + int m_upAxis; + + static btVector3* getUpAxisDirections(); + bool m_interpolateUp; + bool full_drop; + bool bounce_fix; + + 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 stepUp(btCollisionWorld* collisionWorld); + void updateTargetPositionBasedOnCollision(const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0)); + void stepForwardAndStrafe(btCollisionWorld* collisionWorld, const btVector3& walkMove); + void stepDown(btCollisionWorld* collisionWorld, btScalar dt); +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + CharacterController( + btPairCachingGhostObject* ghostObject, + btConvexShape* convexShape, + btScalar stepHeight, + int upAxis = 1); + ~CharacterController(); + + + ///btActionInterface interface + virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) { + preStep(collisionWorld); + playerStep(collisionWorld, deltaTime); + } + + ///btActionInterface interface + void debugDraw(btIDebugDraw* debugDrawer); + + void setUpAxis(int axis) { + if (axis < 0) + axis = 0; + if (axis > 2) + axis = 2; + m_upAxis = axis; + } + + /// 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); + + void reset(btCollisionWorld* collisionWorld ); + void warp(const btVector3& origin); + + void preStep(btCollisionWorld* collisionWorld); + void playerStep(btCollisionWorld* collisionWorld, btScalar dt); + + void setFallSpeed(btScalar fallSpeed); + void setJumpSpeed(btScalar jumpSpeed); + void setMaxJumpHeight(btScalar maxJumpHeight); + bool canJump() const; + + void jump(); + + 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 setUseGhostSweepTest(bool useGhostObjectSweepTest) { + m_useGhostObjectSweepTest = useGhostObjectSweepTest; + } + + bool onGround() const; + void setUpInterpolate(bool value); +}; + +#endif // hifi_CharacterController_h diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 634c3707a8..a46095ee0c 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -9,11 +9,12 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // +#include + #include "PhysicsEngine.h" #include "ShapeInfoUtil.h" #include "PhysicsHelpers.h" #include "ThreadSafeDynamicsWorld.h" -#include "AvatarData.h" static uint32_t _numSubsteps; @@ -23,10 +24,12 @@ uint32_t PhysicsEngine::getNumSubsteps() { } PhysicsEngine::PhysicsEngine(const glm::vec3& offset) - : _originOffset(offset) { + : _originOffset(offset), + _avatarShapeLocalOffset(0.0f) { } PhysicsEngine::~PhysicsEngine() { + // TODO: delete engine components... if we ever plan to create more than one instance } // begin EntitySimulation overrides @@ -260,6 +263,9 @@ void PhysicsEngine::init(EntityEditPacketSender* packetSender) { _constraintSolver = new btSequentialImpulseConstraintSolver; _dynamicsWorld = new ThreadSafeDynamicsWorld(_collisionDispatcher, _broadphaseFilter, _constraintSolver, _collisionConfig); + _ghostPairCallback = new btGhostPairCallback(); + _dynamicsWorld->getPairCache()->setInternalGhostPairCallback(_ghostPairCallback); + // default gravity of the world is zero, so each object must specify its own gravity // TODO: set up gravity zones _dynamicsWorld->setGravity(btVector3(0.0f, 0.0f, 0.0f)); @@ -271,6 +277,9 @@ void PhysicsEngine::init(EntityEditPacketSender* packetSender) { } void PhysicsEngine::stepSimulation() { + // expect the engine to have an avatar (and hence: a character controller) + assert(_avatarData); + lock(); // NOTE: the grand order of operations is: // (1) relay incoming changes @@ -288,17 +297,13 @@ void PhysicsEngine::stepSimulation() { float timeStep = btMin(dt, MAX_TIMESTEP); if (_avatarData->isPhysicsEnabled()) { - _avatarGhostObject->setWorldTransform(btTransform(glmToBullet(_avatarData->getOrientation()), - glmToBullet(_avatarData->getPosition()))); - // WORKAROUND: there is a bug in the debug Bullet-2.82 libs where a zero length walk velocity will trigger - // an assert when the getNormalizedVector() helper function in btKinematicCharacterController.cpp tries to - // first normalize a vector before checking its length. Here we workaround the problem by checking the - // length first. NOTE: the character's velocity is reset to zero after each step, so when we DON'T set - // the velocity for this time interval it is the same thing as setting its velocity to zero. + // update character controller + glm::quat rotation = _avatarData->getOrientation(); + glm::vec3 position = _avatarData->getPosition() + rotation * _avatarShapeLocalOffset; + _avatarGhostObject->setWorldTransform(btTransform(glmToBullet(rotation), glmToBullet(position))); + btVector3 walkVelocity = glmToBullet(_avatarData->getVelocity()); - if (walkVelocity.length2() > FLT_EPSILON * FLT_EPSILON) { - _characterController->setVelocityForTimeInterval(walkVelocity, timeStep); - } + _characterController->setVelocityForTimeInterval(walkVelocity, timeStep); } // This is step (2). @@ -323,8 +328,10 @@ void PhysicsEngine::stepSimulation() { if (_avatarData->isPhysicsEnabled()) { const btTransform& avatarTransform = _avatarGhostObject->getWorldTransform(); - _avatarData->setOrientation(bulletToGLM(avatarTransform.getRotation())); - _avatarData->setPosition(bulletToGLM(avatarTransform.getOrigin())); + glm::quat rotation = bulletToGLM(avatarTransform.getRotation()); + glm::vec3 offset = rotation * _avatarShapeLocalOffset; + _avatarData->setOrientation(rotation); + _avatarData->setPosition(bulletToGLM(avatarTransform.getOrigin()) - offset); } unlock(); @@ -610,28 +617,69 @@ bool PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio void PhysicsEngine::setAvatarData(AvatarData *avatarData) { - _avatarData = avatarData; + assert(avatarData); // don't pass NULL argument + + // compute capsule dimensions + AABox box = avatarData->getLocalAABox(); + const glm::vec3& diagonal = box.getScale(); + float radius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z)); + float halfHeight = 0.5f * diagonal.y; + glm::vec3 offset = box.getCorner() + 0.5f * diagonal; + + if (!_avatarData) { + // _avatarData is being initialized + _avatarData = avatarData; + } else { + // _avatarData is being updated + assert(_avatarData == avatarData); + + // get old dimensions from shape + btCapsuleShape* capsule = static_cast(_avatarGhostObject->getCollisionShape()); + btScalar oldRadius = capsule->getRadius(); + btScalar oldHalfHeight = capsule->getHalfHeight(); + + // compare dimensions (and offset) + float radiusDelta = glm::abs(radius - oldRadius); + float heightDelta = glm::abs(halfHeight - oldHalfHeight); + if (radiusDelta < FLT_EPSILON && heightDelta < FLT_EPSILON) { + // shape hasn't changed --> nothing to do + float offsetDelta = glm::distance(offset, _avatarShapeLocalOffset); + if (offsetDelta > FLT_EPSILON) { + // if only the offset changes then we can update it --> no need to rebuild shape + _avatarShapeLocalOffset = offset; + } + return; + } + + // delete old controller and friends + _dynamicsWorld->removeCollisionObject(_avatarGhostObject); + _dynamicsWorld->removeAction(_characterController); + delete _characterController; + _characterController = NULL; + delete _avatarGhostObject; + _avatarGhostObject = NULL; + delete capsule; + } + + // set offset + _avatarShapeLocalOffset = offset; + + // build ghost, shape, and controller _avatarGhostObject = new btPairCachingGhostObject(); _avatarGhostObject->setWorldTransform(btTransform(glmToBullet(_avatarData->getOrientation()), - glmToBullet(_avatarData->getPosition()))); + glmToBullet(_avatarData->getPosition()))); + // ?TODO: use ShapeManager to get avatar's shape? + btCapsuleShape* capsule = new btCapsuleShape(radius, 2.0f * halfHeight); - // XXX these values should be computed from the character model. - btScalar characterRadius = 0.3f; - btScalar characterHeight = 1.75 - 2.0f * characterRadius; - btScalar stepHeight = btScalar(0.35); - - btConvexShape* capsule = new btCapsuleShape(characterRadius, characterHeight); _avatarGhostObject->setCollisionShape(capsule); _avatarGhostObject->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT); - _characterController = new btKinematicCharacterController(_avatarGhostObject, capsule, stepHeight); + const float MIN_STEP_HEIGHT = 0.35f; + btScalar stepHeight = glm::max(MIN_STEP_HEIGHT, 0.6f * halfHeight); + _characterController = new CharacterController(_avatarGhostObject, capsule, stepHeight); _dynamicsWorld->addCollisionObject(_avatarGhostObject, btBroadphaseProxy::CharacterFilter, - btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter); + btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter); _dynamicsWorld->addAction(_characterController); - _characterController->reset (_dynamicsWorld); - // _characterController->warp (btVector3(10.210001,-2.0306311,16.576973)); - - btGhostPairCallback* ghostPairCallback = new btGhostPairCallback(); - _dynamicsWorld->getPairCache()->setInternalGhostPairCallback(ghostPairCallback); + _characterController->reset(_dynamicsWorld); } diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index 649aec2755..acf1617b16 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -16,21 +16,19 @@ #include #include -#include #include -#include -#include -#include +//#include +#include #include #include #include "BulletUtil.h" +#include "CharacterController.h" #include "ContactInfo.h" #include "EntityMotionState.h" #include "ShapeManager.h" #include "ThreadSafeDynamicsWorld.h" -#include "AvatarData.h" const float HALF_SIMULATION_EXTENT = 512.0f; // meters @@ -106,6 +104,7 @@ private: btBroadphaseInterface* _broadphaseFilter = NULL; btSequentialImpulseConstraintSolver* _constraintSolver = NULL; ThreadSafeDynamicsWorld* _dynamicsWorld = NULL; + btGhostPairCallback* _ghostPairCallback = NULL; ShapeManager _shapeManager; glm::vec3 _originOffset; @@ -123,9 +122,10 @@ private: uint32_t _lastNumSubstepsAtUpdateInternal = 0; /// character collisions - btCharacterControllerInterface* _characterController = 0; - class btPairCachingGhostObject* _avatarGhostObject = 0; - AvatarData *_avatarData = 0; + CharacterController* _characterController = NULL; + class btPairCachingGhostObject* _avatarGhostObject = NULL; + AvatarData* _avatarData = NULL; + glm::vec3 _avatarShapeLocalOffset; }; #endif // hifi_PhysicsEngine_h