| /* |
| 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 "LinearMath/btIDebugDraw.h" |
| #include "BulletCollision/CollisionDispatch/btGhostObject.h" |
| #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" |
| #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" |
| #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" |
| #include "BulletCollision/CollisionDispatch/btCollisionWorld.h" |
| #include "LinearMath/btDefaultMotionState.h" |
| #include "BulletDynamics/Character/btKinematicCharacterController.h" |
| |
| static btVector3 upAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) }; |
| |
| |
| // static helper method |
| static btVector3 |
| getNormalizedVector(const btVector3& v) |
| { |
| btVector3 n = v.normalized(); |
| if (n.length() < SIMD_EPSILON) { |
| n.setValue(0, 0, 0); |
| } |
| 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 |
| 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) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) |
| { |
| m_me = me; |
| } |
| |
| virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) |
| { |
| if (convexResult.m_hitCollisionObject == m_me) |
| return 1.0; |
| |
| return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); |
| } |
| protected: |
| btCollisionObject* m_me; |
| }; |
| |
| /* |
| * 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 btKinematicCharacterController::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 btKinematicCharacterController::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 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal) |
| { |
| return direction - parallelComponent(direction, normal); |
| } |
| |
| btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis) |
| { |
| m_upAxis = upAxis; |
| m_addedMargin = 0.02f; |
| 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; |
| } |
| |
| btKinematicCharacterController::~btKinematicCharacterController () |
| { |
| } |
| |
| btPairCachingGhostObject* btKinematicCharacterController::getGhostObject() |
| { |
| return m_ghostObject; |
| } |
| |
| bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld) |
| { |
| |
| 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]; |
| |
| if (collisionPair->m_algorithm) |
| collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray); |
| |
| |
| for (int j=0;j<m_manifoldArray.size();j++) |
| { |
| btPersistentManifold* manifold = m_manifoldArray[j]; |
| btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0); |
| for (int p=0;p<manifold->getNumContacts();p++) |
| { |
| const btManifoldPoint&pt = manifold->getContactPoint(p); |
| |
| if (pt.getDistance() < 0.0) |
| { |
| if (pt.getDistance() < maxPen) |
| { |
| maxPen = pt.getDistance(); |
| m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? |
| |
| } |
| m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2); |
| penetration = true; |
| } else { |
| //printf("touching %f\n", pt.getDistance()); |
| } |
| } |
| |
| //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 btKinematicCharacterController::stepUp ( btCollisionWorld* world) |
| { |
| // phase 1: up |
| btTransform start, end; |
| m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight; |
| |
| start.setIdentity (); |
| end.setIdentity (); |
| |
| /* FIXME: Handle penetration properly */ |
| start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f)); |
| end.setOrigin (m_targetPosition); |
| |
| btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); |
| 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()) |
| { |
| // we moved up only a fraction of the step height |
| m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction; |
| m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); |
| } else { |
| m_currentStepOffset = m_stepHeight; |
| m_currentPosition = m_targetPosition; |
| } |
| } |
| |
| void btKinematicCharacterController::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 (0)//tangentMag != 0.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 btKinematicCharacterController::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)) |
| updateTargetPositionBasedOnCollision (m_touchingNormal); |
| } |
| |
| int maxIter = 10; |
| |
| while (fraction > btScalar(0.01) && maxIter-- > 0) |
| { |
| start.setOrigin (m_currentPosition); |
| end.setOrigin (m_targetPosition); |
| |
| btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); |
| 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 = (callback.m_hitPointWorld - m_currentPosition).length(); |
| if (hitDistance<0.f) |
| { |
| // printf("neg dist?\n"); |
| } |
| |
| /* If the distance is farther than the collision margin, move */ |
| if (hitDistance > m_addedMargin) |
| { |
| // printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction); |
| 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 btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt) |
| { |
| btTransform start, end; |
| |
| // phase 3: down |
| btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset; |
| btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight; |
| m_targetPosition -= (step_drop + gravity_drop); |
| |
| start.setIdentity (); |
| end.setIdentity (); |
| |
| start.setOrigin (m_currentPosition); |
| end.setOrigin (m_targetPosition); |
| |
| btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); |
| 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, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); |
| } else |
| { |
| collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); |
| } |
| |
| if (callback.hasHit()) |
| { |
| // we dropped a fraction of the height -> hit floor |
| m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); |
| } else { |
| // we dropped the full height |
| |
| m_currentPosition = m_targetPosition; |
| } |
| } |
| |
| |
| |
| void btKinematicCharacterController::setWalkDirection |
| ( |
| const btVector3& walkDirection |
| ) |
| { |
| m_useWalkDirection = true; |
| m_walkDirection = walkDirection; |
| m_normalizedDirection = getNormalizedVector(m_walkDirection); |
| } |
| |
| |
| |
| void btKinematicCharacterController::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 btKinematicCharacterController::reset () |
| { |
| } |
| |
| void btKinematicCharacterController::warp (const btVector3& origin) |
| { |
| btTransform xform; |
| xform.setIdentity(); |
| xform.setOrigin (origin); |
| m_ghostObject->setWorldTransform (xform); |
| } |
| |
| |
| void btKinematicCharacterController::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 btKinematicCharacterController::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 |
| } |
| |
| 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 btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) |
| { |
| m_fallSpeed = fallSpeed; |
| } |
| |
| void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed) |
| { |
| m_jumpSpeed = jumpSpeed; |
| } |
| |
| void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight) |
| { |
| m_maxJumpHeight = maxJumpHeight; |
| } |
| |
| bool btKinematicCharacterController::canJump () const |
| { |
| return onGround(); |
| } |
| |
| void btKinematicCharacterController::jump () |
| { |
| if (!canJump()) |
| return; |
| |
| #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 |
| } |
| |
| bool btKinematicCharacterController::onGround () const |
| { |
| return true; |
| } |
| |
| |
| void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer) |
| { |
| } |