diff options
Diffstat (limited to 'tests/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp')
| -rw-r--r-- | tests/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp | 641 |
1 files changed, 641 insertions, 0 deletions
diff --git a/tests/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/tests/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp new file mode 100644 index 00000000..f733dc0c --- /dev/null +++ b/tests/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -0,0 +1,641 @@ +/* +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 "btKinematicCharacterController.h" + + +// 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, 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); + + 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 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.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; + setMaxSlope(btRadians(45.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); + + 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 btKinematicCharacterController::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; + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + } + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + } 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); + 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 btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt) +{ + btTransform start, end; + + // 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);*/ + + btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + if(downVelocity > 0.0 && downVelocity < m_stepHeight + && (m_wasOnGround || !m_wasJumping)) + { + downVelocity = m_stepHeight; + } + + btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + m_targetPosition -= step_drop; + + start.setIdentity (); + end.setIdentity (); + + start.setOrigin (m_currentPosition); + end.setOrigin (m_targetPosition); + + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + 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); + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + m_wasJumping = false; + } 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]); + + +} + +#include <stdio.h> + +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 + } + + 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 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; + + 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 btKinematicCharacterController::setGravity(btScalar gravity) +{ + m_gravity = gravity; +} + +btScalar btKinematicCharacterController::getGravity() const +{ + return m_gravity; +} + +void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians) +{ + m_maxSlopeRadians = slopeRadians; + m_maxSlopeCosine = btCos(slopeRadians); +} + +btScalar btKinematicCharacterController::getMaxSlope() const +{ + return m_maxSlopeRadians; +} + +bool btKinematicCharacterController::onGround () const +{ + return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0; +} + + +btVector3* btKinematicCharacterController::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 btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer) +{ +} |