btKinematicCharacterController.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2008 Erwin Coumans  http://bulletphysics.com
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 #include <stdio.h>
00018 #include "LinearMath/btIDebugDraw.h"
00019 #include "BulletCollision/CollisionDispatch/btGhostObject.h"
00020 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
00021 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
00022 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
00023 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
00024 #include "LinearMath/btDefaultMotionState.h"
00025 #include "btKinematicCharacterController.h"
00026 
00027 
00028 // static helper method
00029 static btVector3
00030 getNormalizedVector(const btVector3& v)
00031 {
00032         btVector3 n = v.normalized();
00033         if (n.length() < SIMD_EPSILON) {
00034                 n.setValue(0, 0, 0);
00035         }
00036         return n;
00037 }
00038 
00039 
00046 class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
00047 {
00048 public:
00049         btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
00050         {
00051                 m_me = me;
00052         }
00053 
00054         virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
00055         {
00056                 if (rayResult.m_collisionObject == m_me)
00057                         return 1.0;
00058 
00059                 return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
00060         }
00061 protected:
00062         btCollisionObject* m_me;
00063 };
00064 
00065 class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
00066 {
00067 public:
00068         btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
00069         : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
00070         , m_me(me)
00071         , m_up(up)
00072         , m_minSlopeDot(minSlopeDot)
00073         {
00074         }
00075 
00076         virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
00077         {
00078                 if (convexResult.m_hitCollisionObject == m_me)
00079                         return btScalar(1.0);
00080 
00081                 btVector3 hitNormalWorld;
00082                 if (normalInWorldSpace)
00083                 {
00084                         hitNormalWorld = convexResult.m_hitNormalLocal;
00085                 } else
00086                 {
00088                         hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
00089                 }
00090 
00091                 btScalar dotUp = m_up.dot(hitNormalWorld);
00092                 if (dotUp < m_minSlopeDot) {
00093                         return btScalar(1.0);
00094                 }
00095 
00096                 return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
00097         }
00098 protected:
00099         btCollisionObject* m_me;
00100         const btVector3 m_up;
00101         btScalar m_minSlopeDot;
00102 };
00103 
00104 /*
00105  * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal'
00106  *
00107  * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
00108  */
00109 btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal)
00110 {
00111         return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
00112 }
00113 
00114 /*
00115  * Returns the portion of 'direction' that is parallel to 'normal'
00116  */
00117 btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal)
00118 {
00119         btScalar magnitude = direction.dot(normal);
00120         return normal * magnitude;
00121 }
00122 
00123 /*
00124  * Returns the portion of 'direction' that is perpindicular to 'normal'
00125  */
00126 btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
00127 {
00128         return direction - parallelComponent(direction, normal);
00129 }
00130 
00131 btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
00132 {
00133         m_upAxis = upAxis;
00134         m_addedMargin = 0.02;
00135         m_walkDirection.setValue(0,0,0);
00136         m_useGhostObjectSweepTest = true;
00137         m_ghostObject = ghostObject;
00138         m_stepHeight = stepHeight;
00139         m_turnAngle = btScalar(0.0);
00140         m_convexShape=convexShape;      
00141         m_useWalkDirection = false;     // use walk direction by default, legacy behavior
00142         m_velocityTimeInterval = 0.0;
00143         m_verticalVelocity = 0.0;
00144         m_verticalOffset = 0.0;
00145         m_gravity = 9.8 * 3 ; // 3G acceleration.
00146         m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
00147         m_jumpSpeed = 10.0; // ?
00148         m_wasOnGround = false;
00149         m_wasJumping = false;
00150         m_interpolateUp = true;
00151         setMaxSlope(btRadians(45.0));
00152         m_currentStepOffset = 0;
00153         full_drop = false;
00154 }
00155 
00156 btKinematicCharacterController::~btKinematicCharacterController ()
00157 {
00158 }
00159 
00160 btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
00161 {
00162         return m_ghostObject;
00163 }
00164 
00165 bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
00166 {
00167         // Here we must refresh the overlapping paircache as the penetrating movement itself or the
00168         // previous recovery iteration might have used setWorldTransform and pushed us into an object
00169         // that is not in the previous cache contents from the last timestep, as will happen if we
00170         // are pushed into a new AABB overlap. Unhandled this means the next convex sweep gets stuck.
00171         //
00172         // Do this by calling the broadphase's setAabb with the moved AABB, this will update the broadphase
00173         // paircache and the ghostobject's internal paircache at the same time.    /BW
00174 
00175         btVector3 minAabb, maxAabb;
00176         m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb,maxAabb);
00177         collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(), 
00178                                                  minAabb, 
00179                                                  maxAabb, 
00180                                                  collisionWorld->getDispatcher());
00181                                                  
00182         bool penetration = false;
00183 
00184         collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
00185 
00186         m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
00187         
00188         btScalar maxPen = btScalar(0.0);
00189         for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
00190         {
00191                 m_manifoldArray.resize(0);
00192 
00193                 btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
00194                 
00195                 if (collisionPair->m_algorithm)
00196                         collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
00197 
00198                 
00199                 for (int j=0;j<m_manifoldArray.size();j++)
00200                 {
00201                         btPersistentManifold* manifold = m_manifoldArray[j];
00202                         btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
00203                         for (int p=0;p<manifold->getNumContacts();p++)
00204                         {
00205                                 const btManifoldPoint&pt = manifold->getContactPoint(p);
00206 
00207                                 btScalar dist = pt.getDistance();
00208 
00209                                 if (dist < 0.2)
00210                                 {
00211                                         if (dist < maxPen)
00212                                         {
00213                                                 maxPen = dist;
00214                                                 m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
00215 
00216                                         }
00217                                         m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
00218                                         penetration = true;
00219                                 } else {
00220                                         //printf("touching %f\n", dist);
00221                                 }
00222                         }
00223                         
00224                         //manifold->clearManifold();
00225                 }
00226         }
00227         btTransform newTrans = m_ghostObject->getWorldTransform();
00228         newTrans.setOrigin(m_currentPosition);
00229         m_ghostObject->setWorldTransform(newTrans);
00230 //      printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
00231         return penetration;
00232 }
00233 
00234 void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
00235 {
00236         // phase 1: up
00237         btTransform start, end;
00238         m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
00239 
00240         start.setIdentity ();
00241         end.setIdentity ();
00242 
00243         /* FIXME: Handle penetration properly */
00244         start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
00245         end.setOrigin (m_targetPosition);
00246 
00247         btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
00248         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
00249         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
00250         
00251         if (m_useGhostObjectSweepTest)
00252         {
00253                 m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
00254         }
00255         else
00256         {
00257                 world->convexSweepTest (m_convexShape, start, end, callback);
00258         }
00259         
00260         if (callback.hasHit())
00261         {
00262                 // Only modify the position if the hit was a slope and not a wall or ceiling.
00263                 if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
00264                 {
00265                         // we moved up only a fraction of the step height
00266                         m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
00267                         if (m_interpolateUp == true)
00268                                 m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
00269                         else
00270                                 m_currentPosition = m_targetPosition;
00271                 }
00272                 m_verticalVelocity = 0.0;
00273                 m_verticalOffset = 0.0;
00274         } else {
00275                 m_currentStepOffset = m_stepHeight;
00276                 m_currentPosition = m_targetPosition;
00277         }
00278 }
00279 
00280 void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
00281 {
00282         btVector3 movementDirection = m_targetPosition - m_currentPosition;
00283         btScalar movementLength = movementDirection.length();
00284         if (movementLength>SIMD_EPSILON)
00285         {
00286                 movementDirection.normalize();
00287 
00288                 btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
00289                 reflectDir.normalize();
00290 
00291                 btVector3 parallelDir, perpindicularDir;
00292 
00293                 parallelDir = parallelComponent (reflectDir, hitNormal);
00294                 perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
00295 
00296                 m_targetPosition = m_currentPosition;
00297                 if (0)//tangentMag != 0.0)
00298                 {
00299                         btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
00300 //                      printf("parComponent1=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
00301                         m_targetPosition +=  parComponent;
00302                 }
00303 
00304                 if (normalMag != 0.0)
00305                 {
00306                         btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
00307 //                      printf("perpComponent2=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
00308                         m_targetPosition += perpComponent;
00309                 }
00310         } else
00311         {
00312 //              printf("movementLength don't normalize a zero vector\n");
00313         }
00314 }
00315 
00316 void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
00317 {
00318         // printf("m_normalizedDirection=%f,%f,%f\n",
00319         //      m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
00320         // phase 2: forward and strafe
00321         btTransform start, end;
00322         m_targetPosition = m_currentPosition + walkMove;
00323 
00324         start.setIdentity ();
00325         end.setIdentity ();
00326         
00327         btScalar fraction = 1.0;
00328         btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
00329 //      printf("distance2=%f\n",distance2);
00330 
00331         if (m_touchingContact)
00332         {
00333                 if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
00334                 {
00335                         //interferes with step movement
00336                         //updateTargetPositionBasedOnCollision (m_touchingNormal);
00337                 }
00338         }
00339 
00340         int maxIter = 10;
00341 
00342         while (fraction > btScalar(0.01) && maxIter-- > 0)
00343         {
00344                 start.setOrigin (m_currentPosition);
00345                 end.setOrigin (m_targetPosition);
00346                 btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
00347 
00348                 btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
00349                 callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
00350                 callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
00351 
00352 
00353                 btScalar margin = m_convexShape->getMargin();
00354                 m_convexShape->setMargin(margin + m_addedMargin);
00355 
00356 
00357                 if (m_useGhostObjectSweepTest)
00358                 {
00359                         m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
00360                 } else
00361                 {
00362                         collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
00363                 }
00364                 
00365                 m_convexShape->setMargin(margin);
00366 
00367                 
00368                 fraction -= callback.m_closestHitFraction;
00369 
00370                 if (callback.hasHit())
00371                 {       
00372                         // we moved only a fraction
00373                         btScalar hitDistance;
00374                         hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
00375 
00376 //                      m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
00377 
00378                         updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
00379                         btVector3 currentDir = m_targetPosition - m_currentPosition;
00380                         distance2 = currentDir.length2();
00381                         if (distance2 > SIMD_EPSILON)
00382                         {
00383                                 currentDir.normalize();
00384                                 /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
00385                                 if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0))
00386                                 {
00387                                         break;
00388                                 }
00389                         } else
00390                         {
00391 //                              printf("currentDir: don't normalize a zero vector\n");
00392                                 break;
00393                         }
00394 
00395                 } else {
00396                         // we moved whole way
00397                         m_currentPosition = m_targetPosition;
00398                 }
00399 
00400         //      if (callback.m_closestHitFraction == 0.f)
00401         //              break;
00402 
00403         }
00404 }
00405 
00406 void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
00407 {
00408         btTransform start, end, start2, end2;
00409 
00410         // phase 3: down
00411         /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
00412         btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
00413         btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
00414         btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; 
00415         m_targetPosition -= (step_drop + gravity_drop);*/
00416 
00417         btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
00418         if(downVelocity > 0.0 && downVelocity < m_stepHeight
00419         //if(downVelocity > m_fallSpeed
00420                 && (m_wasOnGround || !m_wasJumping))
00421         {
00422                 downVelocity = m_stepHeight;
00423                 //downVelocity = m_fallSpeed;
00424         }
00425 
00426         btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
00427         m_targetPosition -= step_drop;
00428 
00429         start.setIdentity ();
00430         end.setIdentity ();
00431 
00432         start2.setIdentity ();
00433         end2.setIdentity ();
00434 
00435         start.setOrigin (m_currentPosition);
00436         end.setOrigin (m_targetPosition);
00437 
00438         //secondary test, with slight offset - this overcomes situations where the standard test misses the polygon due to large polygon rounding errors
00439         start2.setOrigin (m_currentPosition + btVector3(0.02, 0.0, 0.02));
00440         end2.setOrigin (m_targetPosition + btVector3(0.02, 0.0, 0.02));
00441 
00442         btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
00443         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
00444         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
00445         
00446         if (m_useGhostObjectSweepTest)
00447         {
00448                 m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
00449                 if (!callback.hasHit())
00450                 {
00451                         //perform second sweep test to prevent check errors
00452                         m_ghostObject->convexSweepTest (m_convexShape, start2, end2, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
00453                 }
00454         } else
00455         {
00456                 collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
00457                 if (!callback.hasHit())
00458                 {
00459                         //perform second sweep test to prevent check errors
00460                         collisionWorld->convexSweepTest (m_convexShape, start2, end2, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
00461                 }
00462         }
00463 
00464         if (callback.hasHit())
00465         {
00466                 // we dropped a fraction of the height -> hit floor
00467 
00468                 btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
00469 
00470                 //printf("normal: %g %g %g\n", callback.m_hitNormalWorld.getX(), callback.m_hitNormalWorld.getY(), callback.m_hitNormalWorld.getZ());
00471                 //printf("current: %g, target: %g, hit point: %g - closest hit fraction: %g - new fraction: %g\n", m_currentPosition.getY(), m_targetPosition.getY(), callback.m_hitPointWorld.getY(), callback.m_closestHitFraction, fraction);
00472 
00473                 if (full_drop == true)
00474                         m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
00475                 else
00476                 //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
00477                 //printf("up set: %g\n", m_currentPosition.getY()); //1.03
00478                         m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
00479                 //printf("up set: %g\n", m_currentPosition.getY()); //0.78
00480                 full_drop = false;
00481 
00482                 m_verticalVelocity = 0.0;
00483                 m_verticalOffset = 0.0;
00484                 m_wasJumping = false;
00485         } else {
00486                 // we dropped the full height
00487                 
00488                 full_drop = true;
00489                 //printf("full %g\n", m_currentPosition.getY());
00490                 downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
00491                 if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
00492                 {
00493                         m_targetPosition += step_drop; //undo previous target change
00494                         downVelocity = m_fallSpeed;
00495                         step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
00496                         m_targetPosition -= step_drop;
00497                 }
00498 
00499                 m_currentPosition = m_targetPosition;
00500 
00501                 //btScalar fraction = (m_currentPosition.getY() - m_targetPosition.getY()) * 2;
00502                 //m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
00503         }
00504 }
00505 
00506 
00507 
00508 void btKinematicCharacterController::setWalkDirection
00509 (
00510 const btVector3& walkDirection
00511 )
00512 {
00513         m_useWalkDirection = true;
00514         m_walkDirection = walkDirection;
00515         m_normalizedDirection = getNormalizedVector(m_walkDirection);
00516 }
00517 
00518 
00519 
00520 void btKinematicCharacterController::setVelocityForTimeInterval
00521 (
00522 const btVector3& velocity,
00523 btScalar timeInterval
00524 )
00525 {
00526 //      printf("setVelocity!\n");
00527 //      printf("  interval: %f\n", timeInterval);
00528 //      printf("  velocity: (%f, %f, %f)\n",
00529 //               velocity.x(), velocity.y(), velocity.z());
00530 
00531         m_useWalkDirection = false;
00532         m_walkDirection = velocity;
00533         m_normalizedDirection = getNormalizedVector(m_walkDirection);
00534         m_velocityTimeInterval = timeInterval;
00535 }
00536 
00537 
00538 
00539 void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
00540 {
00541         m_verticalVelocity = 0.0;
00542         m_verticalOffset = 0.0;
00543         m_wasOnGround = false;
00544         m_wasJumping = false;
00545         m_walkDirection.setValue(0,0,0);
00546         m_velocityTimeInterval = 0.0;
00547 
00548         //clear pair cache
00549         btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
00550         while (cache->getOverlappingPairArray().size() > 0)
00551         {
00552                 cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
00553         }
00554 }
00555 
00556 void btKinematicCharacterController::warp (const btVector3& origin)
00557 {
00558         btTransform xform;
00559         xform.setIdentity();
00560         xform.setOrigin (origin);
00561         m_ghostObject->setWorldTransform (xform);
00562 }
00563 
00564 
00565 void btKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
00566 {
00567 
00568         int numPenetrationLoops = 0;
00569         m_touchingContact = false;
00570         while (recoverFromPenetration (collisionWorld))
00571         {
00572                 numPenetrationLoops++;
00573                 m_touchingContact = true;
00574                 if (numPenetrationLoops > 4)
00575                 {
00576                         //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
00577                         break;
00578                 }
00579         }
00580 
00581         m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
00582         m_targetPosition = m_currentPosition;
00583 //      printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
00584 
00585         
00586 }
00587 
00588 #include <stdio.h>
00589 
00590 void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
00591 {
00592 //      printf("playerStep(): ");
00593 //      printf("  dt = %f", dt);
00594 
00595         // quick check...
00596         if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
00597 //              printf("\n");
00598                 return;         // no motion
00599         }
00600 
00601         m_wasOnGround = onGround();
00602 
00603         // Update fall velocity.
00604         m_verticalVelocity -= m_gravity * dt;
00605         if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
00606         {
00607                 m_verticalVelocity = m_jumpSpeed;
00608         }
00609         if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
00610         {
00611                 m_verticalVelocity = -btFabs(m_fallSpeed);
00612         }
00613         m_verticalOffset = m_verticalVelocity * dt;
00614 
00615 
00616         btTransform xform;
00617         xform = m_ghostObject->getWorldTransform ();
00618 
00619 //      printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
00620 //      printf("walkSpeed=%f\n",walkSpeed);
00621 
00622         stepUp (collisionWorld);
00623         if (m_useWalkDirection) {
00624                 stepForwardAndStrafe (collisionWorld, m_walkDirection);
00625         } else {
00626                 //printf("  time: %f", m_velocityTimeInterval);
00627                 // still have some time left for moving!
00628                 btScalar dtMoving =
00629                         (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
00630                 m_velocityTimeInterval -= dt;
00631 
00632                 // how far will we move while we are moving?
00633                 btVector3 move = m_walkDirection * dtMoving;
00634 
00635                 //printf("  dtMoving: %f", dtMoving);
00636 
00637                 // okay, step
00638                 stepForwardAndStrafe(collisionWorld, move);
00639         }
00640         stepDown (collisionWorld, dt);
00641 
00642         // printf("\n");
00643 
00644         xform.setOrigin (m_currentPosition);
00645         m_ghostObject->setWorldTransform (xform);
00646 }
00647 
00648 void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
00649 {
00650         m_fallSpeed = fallSpeed;
00651 }
00652 
00653 void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
00654 {
00655         m_jumpSpeed = jumpSpeed;
00656 }
00657 
00658 void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
00659 {
00660         m_maxJumpHeight = maxJumpHeight;
00661 }
00662 
00663 bool btKinematicCharacterController::canJump () const
00664 {
00665         return onGround();
00666 }
00667 
00668 void btKinematicCharacterController::jump ()
00669 {
00670         if (!canJump())
00671                 return;
00672 
00673         m_verticalVelocity = m_jumpSpeed;
00674         m_wasJumping = true;
00675 
00676 #if 0
00677         currently no jumping.
00678         btTransform xform;
00679         m_rigidBody->getMotionState()->getWorldTransform (xform);
00680         btVector3 up = xform.getBasis()[1];
00681         up.normalize ();
00682         btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0);
00683         m_rigidBody->applyCentralImpulse (up * magnitude);
00684 #endif
00685 }
00686 
00687 void btKinematicCharacterController::setGravity(btScalar gravity)
00688 {
00689         m_gravity = gravity;
00690 }
00691 
00692 btScalar btKinematicCharacterController::getGravity() const
00693 {
00694         return m_gravity;
00695 }
00696 
00697 void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
00698 {
00699         m_maxSlopeRadians = slopeRadians;
00700         m_maxSlopeCosine = btCos(slopeRadians);
00701 }
00702 
00703 btScalar btKinematicCharacterController::getMaxSlope() const
00704 {
00705         return m_maxSlopeRadians;
00706 }
00707 
00708 bool btKinematicCharacterController::onGround () const
00709 {
00710         return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
00711 }
00712 
00713 
00714 btVector3* btKinematicCharacterController::getUpAxisDirections()
00715 {
00716         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) };
00717         
00718         return sUpAxisDirection;
00719 }
00720 
00721 void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
00722 {
00723 }
00724 
00725 void btKinematicCharacterController::setUpInterpolate(bool value)
00726 {
00727         m_interpolateUp = value;
00728 }
00729