Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletDynamics / Character / btKinematicCharacterController.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2008 Erwin Coumans  http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16
17 #include <stdio.h>
18 #include "LinearMath/btIDebugDraw.h"
19 #include "BulletCollision/CollisionDispatch/btGhostObject.h"
20 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
21 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
22 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
23 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
24 #include "LinearMath/btDefaultMotionState.h"
25 #include "btKinematicCharacterController.h"
26
27
28 // static helper method
29 static btVector3
30 getNormalizedVector(const btVector3& v)
31 {
32         btVector3 n(0, 0, 0);
33
34         if (v.length() > SIMD_EPSILON) {
35                 n = v.normalized();
36         }
37         return n;
38 }
39
40
41 ///@todo Interact with dynamic objects,
42 ///Ride kinematicly animated platforms properly
43 ///More realistic (or maybe just a config option) falling
44 /// -> Should integrate falling velocity manually and use that in stepDown()
45 ///Support jumping
46 ///Support ducking
47 class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
48 {
49 public:
50         btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
51         {
52                 m_me = me;
53         }
54
55         virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
56         {
57                 if (rayResult.m_collisionObject == m_me)
58                         return 1.0;
59
60                 return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
61         }
62 protected:
63         btCollisionObject* m_me;
64 };
65
66 class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
67 {
68 public:
69         btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
70         : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
71         , m_me(me)
72         , m_up(up)
73         , m_minSlopeDot(minSlopeDot)
74         {
75         }
76
77         virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
78         {
79                 if (convexResult.m_hitCollisionObject == m_me)
80                         return btScalar(1.0);
81
82                 if (!convexResult.m_hitCollisionObject->hasContactResponse())
83                         return btScalar(1.0);
84
85                 btVector3 hitNormalWorld;
86                 if (normalInWorldSpace)
87                 {
88                         hitNormalWorld = convexResult.m_hitNormalLocal;
89                 } else
90                 {
91                         ///need to transform normal into worldspace
92                         hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
93                 }
94
95                 btScalar dotUp = m_up.dot(hitNormalWorld);
96                 if (dotUp < m_minSlopeDot) {
97                         return btScalar(1.0);
98                 }
99
100                 return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
101         }
102 protected:
103         btCollisionObject* m_me;
104         const btVector3 m_up;
105         btScalar m_minSlopeDot;
106 };
107
108 /*
109  * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal'
110  *
111  * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
112  */
113 btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal)
114 {
115         return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
116 }
117
118 /*
119  * Returns the portion of 'direction' that is parallel to 'normal'
120  */
121 btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal)
122 {
123         btScalar magnitude = direction.dot(normal);
124         return normal * magnitude;
125 }
126
127 /*
128  * Returns the portion of 'direction' that is perpindicular to 'normal'
129  */
130 btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
131 {
132         return direction - parallelComponent(direction, normal);
133 }
134
135 btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
136 {
137         m_upAxis = upAxis;
138         m_addedMargin = 0.02;
139         m_walkDirection.setValue(0,0,0);
140         m_useGhostObjectSweepTest = true;
141         m_ghostObject = ghostObject;
142         m_stepHeight = stepHeight;
143         m_turnAngle = btScalar(0.0);
144         m_convexShape=convexShape;      
145         m_useWalkDirection = true;      // use walk direction by default, legacy behavior
146         m_velocityTimeInterval = 0.0;
147         m_verticalVelocity = 0.0;
148         m_verticalOffset = 0.0;
149         m_gravity = 9.8 * 3 ; // 3G acceleration.
150         m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
151         m_jumpSpeed = 10.0; // ?
152         m_wasOnGround = false;
153         m_wasJumping = false;
154         m_interpolateUp = true;
155         setMaxSlope(btRadians(45.0));
156         m_currentStepOffset = 0;
157         full_drop = false;
158         bounce_fix = false;
159 }
160
161 btKinematicCharacterController::~btKinematicCharacterController ()
162 {
163 }
164
165 btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
166 {
167         return m_ghostObject;
168 }
169
170 bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
171 {
172         // Here we must refresh the overlapping paircache as the penetrating movement itself or the
173         // previous recovery iteration might have used setWorldTransform and pushed us into an object
174         // that is not in the previous cache contents from the last timestep, as will happen if we
175         // are pushed into a new AABB overlap. Unhandled this means the next convex sweep gets stuck.
176         //
177         // Do this by calling the broadphase's setAabb with the moved AABB, this will update the broadphase
178         // paircache and the ghostobject's internal paircache at the same time.    /BW
179
180         btVector3 minAabb, maxAabb;
181         m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb,maxAabb);
182         collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(), 
183                                                  minAabb, 
184                                                  maxAabb, 
185                                                  collisionWorld->getDispatcher());
186                                                  
187         bool penetration = false;
188
189         collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
190
191         m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
192         
193         btScalar maxPen = btScalar(0.0);
194         for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
195         {
196                 m_manifoldArray.resize(0);
197
198                 btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
199
200                 btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
201                 btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
202
203                 if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
204                         continue;
205                 
206                 if (collisionPair->m_algorithm)
207                         collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
208
209                 
210                 for (int j=0;j<m_manifoldArray.size();j++)
211                 {
212                         btPersistentManifold* manifold = m_manifoldArray[j];
213                         btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
214                         for (int p=0;p<manifold->getNumContacts();p++)
215                         {
216                                 const btManifoldPoint&pt = manifold->getContactPoint(p);
217
218                                 btScalar dist = pt.getDistance();
219
220                                 if (dist < 0.0)
221                                 {
222                                         if (dist < maxPen)
223                                         {
224                                                 maxPen = dist;
225                                                 m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
226
227                                         }
228                                         m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
229                                         penetration = true;
230                                 } else {
231                                         //printf("touching %f\n", dist);
232                                 }
233                         }
234                         
235                         //manifold->clearManifold();
236                 }
237         }
238         btTransform newTrans = m_ghostObject->getWorldTransform();
239         newTrans.setOrigin(m_currentPosition);
240         m_ghostObject->setWorldTransform(newTrans);
241 //      printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
242         return penetration;
243 }
244
245 void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
246 {
247         // phase 1: up
248         btTransform start, end;
249         m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
250
251         start.setIdentity ();
252         end.setIdentity ();
253
254         /* FIXME: Handle penetration properly */
255         start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
256         end.setOrigin (m_targetPosition);
257
258         btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
259         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
260         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
261         
262         if (m_useGhostObjectSweepTest)
263         {
264                 m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
265         }
266         else
267         {
268                 world->convexSweepTest (m_convexShape, start, end, callback);
269         }
270         
271         if (callback.hasHit())
272         {
273                 // Only modify the position if the hit was a slope and not a wall or ceiling.
274                 if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
275                 {
276                         // we moved up only a fraction of the step height
277                         m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
278                         if (m_interpolateUp == true)
279                                 m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
280                         else
281                                 m_currentPosition = m_targetPosition;
282                 }
283                 m_verticalVelocity = 0.0;
284                 m_verticalOffset = 0.0;
285         } else {
286                 m_currentStepOffset = m_stepHeight;
287                 m_currentPosition = m_targetPosition;
288         }
289 }
290
291 void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
292 {
293         btVector3 movementDirection = m_targetPosition - m_currentPosition;
294         btScalar movementLength = movementDirection.length();
295         if (movementLength>SIMD_EPSILON)
296         {
297                 movementDirection.normalize();
298
299                 btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
300                 reflectDir.normalize();
301
302                 btVector3 parallelDir, perpindicularDir;
303
304                 parallelDir = parallelComponent (reflectDir, hitNormal);
305                 perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
306
307                 m_targetPosition = m_currentPosition;
308                 if (0)//tangentMag != 0.0)
309                 {
310                         btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
311 //                      printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
312                         m_targetPosition +=  parComponent;
313                 }
314
315                 if (normalMag != 0.0)
316                 {
317                         btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
318 //                      printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
319                         m_targetPosition += perpComponent;
320                 }
321         } else
322         {
323 //              printf("movementLength don't normalize a zero vector\n");
324         }
325 }
326
327 void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
328 {
329         // printf("m_normalizedDirection=%f,%f,%f\n",
330         //      m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
331         // phase 2: forward and strafe
332         btTransform start, end;
333         m_targetPosition = m_currentPosition + walkMove;
334
335         start.setIdentity ();
336         end.setIdentity ();
337         
338         btScalar fraction = 1.0;
339         btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
340 //      printf("distance2=%f\n",distance2);
341
342         if (m_touchingContact)
343         {
344                 if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
345                 {
346                         //interferes with step movement
347                         //updateTargetPositionBasedOnCollision (m_touchingNormal);
348                 }
349         }
350
351         int maxIter = 10;
352
353         while (fraction > btScalar(0.01) && maxIter-- > 0)
354         {
355                 start.setOrigin (m_currentPosition);
356                 end.setOrigin (m_targetPosition);
357                 btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
358
359                 btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
360                 callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
361                 callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
362
363
364                 btScalar margin = m_convexShape->getMargin();
365                 m_convexShape->setMargin(margin + m_addedMargin);
366
367
368                 if (m_useGhostObjectSweepTest)
369                 {
370                         m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
371                 } else
372                 {
373                         collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
374                 }
375                 
376                 m_convexShape->setMargin(margin);
377
378                 
379                 fraction -= callback.m_closestHitFraction;
380
381                 if (callback.hasHit())
382                 {       
383                         // we moved only a fraction
384                         //btScalar hitDistance;
385                         //hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
386
387 //                      m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
388
389                         updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
390                         btVector3 currentDir = m_targetPosition - m_currentPosition;
391                         distance2 = currentDir.length2();
392                         if (distance2 > SIMD_EPSILON)
393                         {
394                                 currentDir.normalize();
395                                 /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
396                                 if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0))
397                                 {
398                                         break;
399                                 }
400                         } else
401                         {
402 //                              printf("currentDir: don't normalize a zero vector\n");
403                                 break;
404                         }
405
406                 } else {
407                         // we moved whole way
408                         m_currentPosition = m_targetPosition;
409                 }
410
411         //      if (callback.m_closestHitFraction == 0.f)
412         //              break;
413
414         }
415 }
416
417 void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
418 {
419         btTransform start, end, end_double;
420         bool runonce = false;
421
422         // phase 3: down
423         /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
424         btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
425         btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
426         btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; 
427         m_targetPosition -= (step_drop + gravity_drop);*/
428
429         btVector3 orig_position = m_targetPosition;
430         
431         btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
432
433         if(downVelocity > 0.0 && downVelocity > m_fallSpeed
434                 && (m_wasOnGround || !m_wasJumping))
435                 downVelocity = m_fallSpeed;
436
437         btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
438         m_targetPosition -= step_drop;
439
440         btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
441         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
442         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
443
444         btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
445         callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
446         callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
447
448         while (1)
449         {
450                 start.setIdentity ();
451                 end.setIdentity ();
452
453                 end_double.setIdentity ();
454
455                 start.setOrigin (m_currentPosition);
456                 end.setOrigin (m_targetPosition);
457
458                 //set double test for 2x the step drop, to check for a large drop vs small drop
459                 end_double.setOrigin (m_targetPosition - step_drop);
460
461                 if (m_useGhostObjectSweepTest)
462                 {
463                         m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
464
465                         if (!callback.hasHit())
466                         {
467                                 //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
468                                 m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
469                         }
470                 } else
471                 {
472                         collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
473
474                         if (!callback.hasHit())
475                                         {
476                                                         //test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
477                                                         collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
478                                         }
479                 }
480         
481                 btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
482                 bool has_hit = false;
483                 if (bounce_fix == true)
484                         has_hit = callback.hasHit() || callback2.hasHit();
485                 else
486                         has_hit = callback2.hasHit();
487
488                 if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
489                                         && (m_wasOnGround || !m_wasJumping))
490                 {
491                         //redo the velocity calculation when falling a small amount, for fast stairs motion
492                         //for larger falls, use the smoother/slower interpolated movement by not touching the target position
493
494                         m_targetPosition = orig_position;
495                                         downVelocity = m_stepHeight;
496
497                                 btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
498                         m_targetPosition -= step_drop;
499                         runonce = true;
500                         continue; //re-run previous tests
501                 }
502                 break;
503         }
504
505         if (callback.hasHit() || runonce == true)
506         {
507                 // we dropped a fraction of the height -> hit floor
508
509                 btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
510
511                 //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
512
513                 if (bounce_fix == true)
514                 {
515                         if (full_drop == true)
516                                 m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
517                         else
518                                 //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
519                                 m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
520                 }
521                 else
522                         m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
523
524                 full_drop = false;
525
526                 m_verticalVelocity = 0.0;
527                 m_verticalOffset = 0.0;
528                 m_wasJumping = false;
529         } else {
530                 // we dropped the full height
531                 
532                 full_drop = true;
533
534                 if (bounce_fix == true)
535                 {
536                         downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
537                         if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
538                         {
539                                 m_targetPosition += step_drop; //undo previous target change
540                                 downVelocity = m_fallSpeed;
541                                 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
542                                 m_targetPosition -= step_drop;
543                         }
544                 }
545                 //printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
546
547                 m_currentPosition = m_targetPosition;
548         }
549 }
550
551
552
553 void btKinematicCharacterController::setWalkDirection
554 (
555 const btVector3& walkDirection
556 )
557 {
558         m_useWalkDirection = true;
559         m_walkDirection = walkDirection;
560         m_normalizedDirection = getNormalizedVector(m_walkDirection);
561 }
562
563
564
565 void btKinematicCharacterController::setVelocityForTimeInterval
566 (
567 const btVector3& velocity,
568 btScalar timeInterval
569 )
570 {
571 //      printf("setVelocity!\n");
572 //      printf("  interval: %f\n", timeInterval);
573 //      printf("  velocity: (%f, %f, %f)\n",
574 //               velocity.x(), velocity.y(), velocity.z());
575
576         m_useWalkDirection = false;
577         m_walkDirection = velocity;
578         m_normalizedDirection = getNormalizedVector(m_walkDirection);
579         m_velocityTimeInterval += timeInterval;
580 }
581
582 void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
583 {
584         m_verticalVelocity = 0.0;
585         m_verticalOffset = 0.0;
586         m_wasOnGround = false;
587         m_wasJumping = false;
588         m_walkDirection.setValue(0,0,0);
589         m_velocityTimeInterval = 0.0;
590
591         //clear pair cache
592         btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
593         while (cache->getOverlappingPairArray().size() > 0)
594         {
595                 cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
596         }
597 }
598
599 void btKinematicCharacterController::warp (const btVector3& origin)
600 {
601         btTransform xform;
602         xform.setIdentity();
603         xform.setOrigin (origin);
604         m_ghostObject->setWorldTransform (xform);
605 }
606
607
608 void btKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
609 {
610         
611         int numPenetrationLoops = 0;
612         m_touchingContact = false;
613         while (recoverFromPenetration (collisionWorld))
614         {
615                 numPenetrationLoops++;
616                 m_touchingContact = true;
617                 if (numPenetrationLoops > 4)
618                 {
619                         //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
620                         break;
621                 }
622         }
623
624         m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
625         m_targetPosition = m_currentPosition;
626 //      printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
627
628         
629 }
630
631 #include <stdio.h>
632
633 void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
634 {
635 //      printf("playerStep(): ");
636 //      printf("  dt = %f", dt);
637
638         // quick check...
639         if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
640 //              printf("\n");
641                 return;         // no motion
642         }
643
644         m_wasOnGround = onGround();
645
646         // Update fall velocity.
647         m_verticalVelocity -= m_gravity * dt;
648         if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
649         {
650                 m_verticalVelocity = m_jumpSpeed;
651         }
652         if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
653         {
654                 m_verticalVelocity = -btFabs(m_fallSpeed);
655         }
656         m_verticalOffset = m_verticalVelocity * dt;
657
658
659         btTransform xform;
660         xform = m_ghostObject->getWorldTransform ();
661
662 //      printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
663 //      printf("walkSpeed=%f\n",walkSpeed);
664
665         stepUp (collisionWorld);
666         if (m_useWalkDirection) {
667                 stepForwardAndStrafe (collisionWorld, m_walkDirection);
668         } else {
669                 //printf("  time: %f", m_velocityTimeInterval);
670                 // still have some time left for moving!
671                 btScalar dtMoving =
672                         (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
673                 m_velocityTimeInterval -= dt;
674
675                 // how far will we move while we are moving?
676                 btVector3 move = m_walkDirection * dtMoving;
677
678                 //printf("  dtMoving: %f", dtMoving);
679
680                 // okay, step
681                 stepForwardAndStrafe(collisionWorld, move);
682         }
683         stepDown (collisionWorld, dt);
684
685         // printf("\n");
686
687         xform.setOrigin (m_currentPosition);
688         m_ghostObject->setWorldTransform (xform);
689 }
690
691 void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
692 {
693         m_fallSpeed = fallSpeed;
694 }
695
696 void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
697 {
698         m_jumpSpeed = jumpSpeed;
699 }
700
701 void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
702 {
703         m_maxJumpHeight = maxJumpHeight;
704 }
705
706 bool btKinematicCharacterController::canJump () const
707 {
708         return onGround();
709 }
710
711 void btKinematicCharacterController::jump ()
712 {
713         if (!canJump())
714                 return;
715
716         m_verticalVelocity = m_jumpSpeed;
717         m_wasJumping = true;
718
719 #if 0
720         currently no jumping.
721         btTransform xform;
722         m_rigidBody->getMotionState()->getWorldTransform (xform);
723         btVector3 up = xform.getBasis()[1];
724         up.normalize ();
725         btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0);
726         m_rigidBody->applyCentralImpulse (up * magnitude);
727 #endif
728 }
729
730 void btKinematicCharacterController::setGravity(btScalar gravity)
731 {
732         m_gravity = gravity;
733 }
734
735 btScalar btKinematicCharacterController::getGravity() const
736 {
737         return m_gravity;
738 }
739
740 void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
741 {
742         m_maxSlopeRadians = slopeRadians;
743         m_maxSlopeCosine = btCos(slopeRadians);
744 }
745
746 btScalar btKinematicCharacterController::getMaxSlope() const
747 {
748         return m_maxSlopeRadians;
749 }
750
751 bool btKinematicCharacterController::onGround () const
752 {
753         return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
754 }
755
756
757 btVector3* btKinematicCharacterController::getUpAxisDirections()
758 {
759         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) };
760         
761         return sUpAxisDirection;
762 }
763
764 void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
765 {
766 }
767
768 void btKinematicCharacterController::setUpInterpolate(bool value)
769 {
770         m_interpolateUp = value;
771 }