== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / 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 "LinearMath/btIDebugDraw.h"
18 #include "BulletCollision/CollisionDispatch/btGhostObject.h"
19 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
20 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
21 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
22 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
23 #include "LinearMath/btDefaultMotionState.h"
24 #include "btKinematicCharacterController.h"
25
26
27 // static helper method
28 static btVector3
29 getNormalizedVector(const btVector3& v)
30 {
31         btVector3 n = v.normalized();
32         if (n.length() < SIMD_EPSILON) {
33                 n.setValue(0, 0, 0);
34         }
35         return n;
36 }
37
38
39 ///@todo Interact with dynamic objects,
40 ///Ride kinematicly animated platforms properly
41 ///More realistic (or maybe just a config option) falling
42 /// -> Should integrate falling velocity manually and use that in stepDown()
43 ///Support jumping
44 ///Support ducking
45 class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
46 {
47 public:
48         btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
49         {
50                 m_me = me;
51         }
52
53         virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
54         {
55                 if (rayResult.m_collisionObject == m_me)
56                         return 1.0;
57
58                 return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
59         }
60 protected:
61         btCollisionObject* m_me;
62 };
63
64 class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
65 {
66 public:
67         btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
68         {
69                 m_me = me;
70         }
71
72         virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
73         {
74                 if (convexResult.m_hitCollisionObject == m_me)
75                         return 1.0;
76
77                 return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
78         }
79 protected:
80         btCollisionObject* m_me;
81 };
82
83 /*
84  * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal'
85  *
86  * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
87  */
88 btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal)
89 {
90         return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
91 }
92
93 /*
94  * Returns the portion of 'direction' that is parallel to 'normal'
95  */
96 btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal)
97 {
98         btScalar magnitude = direction.dot(normal);
99         return normal * magnitude;
100 }
101
102 /*
103  * Returns the portion of 'direction' that is perpindicular to 'normal'
104  */
105 btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
106 {
107         return direction - parallelComponent(direction, normal);
108 }
109
110 btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
111 {
112         m_upAxis = upAxis;
113         m_addedMargin = 0.02f;
114         m_walkDirection.setValue(0,0,0);
115         m_useGhostObjectSweepTest = true;
116         m_ghostObject = ghostObject;
117         m_stepHeight = stepHeight;
118         m_turnAngle = btScalar(0.0);
119         m_convexShape=convexShape;      
120         m_useWalkDirection = true;      // use walk direction by default, legacy behavior
121         m_velocityTimeInterval = 0.0;
122 }
123
124 btKinematicCharacterController::~btKinematicCharacterController ()
125 {
126 }
127
128 btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
129 {
130         return m_ghostObject;
131 }
132
133 bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
134 {
135
136         bool penetration = false;
137
138         collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
139
140         m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
141         
142         btScalar maxPen = btScalar(0.0);
143         for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
144         {
145                 m_manifoldArray.resize(0);
146
147                 btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
148                 
149                 if (collisionPair->m_algorithm)
150                         collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
151
152                 
153                 for (int j=0;j<m_manifoldArray.size();j++)
154                 {
155                         btPersistentManifold* manifold = m_manifoldArray[j];
156                         btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
157                         for (int p=0;p<manifold->getNumContacts();p++)
158                         {
159                                 const btManifoldPoint&pt = manifold->getContactPoint(p);
160
161                                 if (pt.getDistance() < 0.0)
162                                 {
163                                         if (pt.getDistance() < maxPen)
164                                         {
165                                                 maxPen = pt.getDistance();
166                                                 m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
167
168                                         }
169                                         m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2);
170                                         penetration = true;
171                                 } else {
172                                         //printf("touching %f\n", pt.getDistance());
173                                 }
174                         }
175                         
176                         //manifold->clearManifold();
177                 }
178         }
179         btTransform newTrans = m_ghostObject->getWorldTransform();
180         newTrans.setOrigin(m_currentPosition);
181         m_ghostObject->setWorldTransform(newTrans);
182 //      printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
183         return penetration;
184 }
185
186 void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
187 {
188         // phase 1: up
189         btTransform start, end;
190         m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * m_stepHeight;
191
192         start.setIdentity ();
193         end.setIdentity ();
194
195         /* FIXME: Handle penetration properly */
196         start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * btScalar(0.1f));
197         end.setOrigin (m_targetPosition);
198
199         btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
200         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
201         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
202         
203         if (m_useGhostObjectSweepTest)
204         {
205                 m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
206         }
207         else
208         {
209                 world->convexSweepTest (m_convexShape, start, end, callback);
210         }
211         
212         if (callback.hasHit())
213         {
214                 // we moved up only a fraction of the step height
215                 m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
216                 m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
217         } else {
218                 m_currentStepOffset = m_stepHeight;
219                 m_currentPosition = m_targetPosition;
220         }
221 }
222
223 void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
224 {
225         btVector3 movementDirection = m_targetPosition - m_currentPosition;
226         btScalar movementLength = movementDirection.length();
227         if (movementLength>SIMD_EPSILON)
228         {
229                 movementDirection.normalize();
230
231                 btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
232                 reflectDir.normalize();
233
234                 btVector3 parallelDir, perpindicularDir;
235
236                 parallelDir = parallelComponent (reflectDir, hitNormal);
237                 perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
238
239                 m_targetPosition = m_currentPosition;
240                 if (0)//tangentMag != 0.0)
241                 {
242                         btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
243 //                      printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
244                         m_targetPosition +=  parComponent;
245                 }
246
247                 if (normalMag != 0.0)
248                 {
249                         btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
250 //                      printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
251                         m_targetPosition += perpComponent;
252                 }
253         } else
254         {
255 //              printf("movementLength don't normalize a zero vector\n");
256         }
257 }
258
259 void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
260 {
261         // printf("m_normalizedDirection=%f,%f,%f\n",
262         //      m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
263         // phase 2: forward and strafe
264         btTransform start, end;
265         m_targetPosition = m_currentPosition + walkMove;
266         start.setIdentity ();
267         end.setIdentity ();
268         
269         btScalar fraction = 1.0;
270         btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
271 //      printf("distance2=%f\n",distance2);
272
273         if (m_touchingContact)
274         {
275                 if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
276                         updateTargetPositionBasedOnCollision (m_touchingNormal);
277         }
278
279         int maxIter = 10;
280
281         while (fraction > btScalar(0.01) && maxIter-- > 0)
282         {
283                 start.setOrigin (m_currentPosition);
284                 end.setOrigin (m_targetPosition);
285
286                 btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
287                 callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
288                 callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
289
290
291                 btScalar margin = m_convexShape->getMargin();
292                 m_convexShape->setMargin(margin + m_addedMargin);
293
294
295                 if (m_useGhostObjectSweepTest)
296                 {
297                         m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
298                 } else
299                 {
300                         collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
301                 }
302                 
303                 m_convexShape->setMargin(margin);
304
305                 
306                 fraction -= callback.m_closestHitFraction;
307
308                 if (callback.hasHit())
309                 {       
310                         // we moved only a fraction
311                         btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
312                         if (hitDistance<0.f)
313                         {
314 //                              printf("neg dist?\n");
315                         }
316
317                         /* If the distance is farther than the collision margin, move */
318                         if (hitDistance > m_addedMargin)
319                         {
320 //                              printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
321                                 m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
322                         }
323
324                         updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
325                         btVector3 currentDir = m_targetPosition - m_currentPosition;
326                         distance2 = currentDir.length2();
327                         if (distance2 > SIMD_EPSILON)
328                         {
329                                 currentDir.normalize();
330                                 /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
331                                 if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0))
332                                 {
333                                         break;
334                                 }
335                         } else
336                         {
337 //                              printf("currentDir: don't normalize a zero vector\n");
338                                 break;
339                         }
340                 } else {
341                         // we moved whole way
342                         m_currentPosition = m_targetPosition;
343                 }
344
345         //      if (callback.m_closestHitFraction == 0.f)
346         //              break;
347
348         }
349 }
350
351 void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
352 {
353         btTransform start, end;
354
355         // phase 3: down
356         btVector3 step_drop = getUpAxisDirections()[m_upAxis] * m_currentStepOffset;
357         btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * m_stepHeight; 
358         m_targetPosition -= (step_drop + gravity_drop);
359
360         start.setIdentity ();
361         end.setIdentity ();
362
363         start.setOrigin (m_currentPosition);
364         end.setOrigin (m_targetPosition);
365
366         btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
367         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
368         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
369         
370         if (m_useGhostObjectSweepTest)
371         {
372                 m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
373         } else
374         {
375                 collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
376         }
377
378         if (callback.hasHit())
379         {
380                 // we dropped a fraction of the height -> hit floor
381                 m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
382         } else {
383                 // we dropped the full height
384                 
385                 m_currentPosition = m_targetPosition;
386         }
387 }
388
389
390
391 void btKinematicCharacterController::setWalkDirection
392 (
393 const btVector3& walkDirection
394 )
395 {
396         m_useWalkDirection = true;
397         m_walkDirection = walkDirection;
398         m_normalizedDirection = getNormalizedVector(m_walkDirection);
399 }
400
401
402
403 void btKinematicCharacterController::setVelocityForTimeInterval
404 (
405 const btVector3& velocity,
406 btScalar timeInterval
407 )
408 {
409 //      printf("setVelocity!\n");
410 //      printf("  interval: %f\n", timeInterval);
411 //      printf("  velocity: (%f, %f, %f)\n",
412 //          velocity.x(), velocity.y(), velocity.z());
413
414         m_useWalkDirection = false;
415         m_walkDirection = velocity;
416         m_normalizedDirection = getNormalizedVector(m_walkDirection);
417         m_velocityTimeInterval = timeInterval;
418 }
419
420
421
422 void btKinematicCharacterController::reset ()
423 {
424 }
425
426 void btKinematicCharacterController::warp (const btVector3& origin)
427 {
428         btTransform xform;
429         xform.setIdentity();
430         xform.setOrigin (origin);
431         m_ghostObject->setWorldTransform (xform);
432 }
433
434
435 void btKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
436 {
437         
438         int numPenetrationLoops = 0;
439         m_touchingContact = false;
440         while (recoverFromPenetration (collisionWorld))
441         {
442                 numPenetrationLoops++;
443                 m_touchingContact = true;
444                 if (numPenetrationLoops > 4)
445                 {
446 //                      printf("character could not recover from penetration = %d\n", numPenetrationLoops);
447                         break;
448                 }
449         }
450
451         m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
452         m_targetPosition = m_currentPosition;
453 //      printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
454
455         
456 }
457
458 void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
459 {
460 //      printf("playerStep(): ");
461 //      printf("  dt = %f", dt);
462
463         // quick check...
464         if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
465 //              printf("\n");
466                 return;         // no motion
467         }
468
469         btTransform xform;
470         xform = m_ghostObject->getWorldTransform ();
471
472 //      printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
473 //      printf("walkSpeed=%f\n",walkSpeed);
474
475         stepUp (collisionWorld);
476         if (m_useWalkDirection) {
477                 stepForwardAndStrafe (collisionWorld, m_walkDirection);
478         } else {
479                 //printf("  time: %f", m_velocityTimeInterval);
480                 // still have some time left for moving!
481                 btScalar dtMoving =
482                    (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
483                 m_velocityTimeInterval -= dt;
484
485                 // how far will we move while we are moving?
486                 btVector3 move = m_walkDirection * dtMoving;
487
488                 // printf("  dtMoving: %f", dtMoving);
489
490                 // okay, step
491                 stepForwardAndStrafe(collisionWorld, move);
492         }
493         stepDown (collisionWorld, dt);
494
495         // printf("\n");
496
497         xform.setOrigin (m_currentPosition);
498         m_ghostObject->setWorldTransform (xform);
499 }
500
501 void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
502 {
503         m_fallSpeed = fallSpeed;
504 }
505
506 void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
507 {
508         m_jumpSpeed = jumpSpeed;
509 }
510
511 void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
512 {
513         m_maxJumpHeight = maxJumpHeight;
514 }
515
516 bool btKinematicCharacterController::canJump () const
517 {
518         return onGround();
519 }
520
521 void btKinematicCharacterController::jump ()
522 {
523         if (!canJump())
524                 return;
525
526 #if 0
527         currently no jumping.
528         btTransform xform;
529         m_rigidBody->getMotionState()->getWorldTransform (xform);
530         btVector3 up = xform.getBasis()[1];
531         up.normalize ();
532         btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0);
533         m_rigidBody->applyCentralImpulse (up * magnitude);
534 #endif
535 }
536
537 bool btKinematicCharacterController::onGround () const
538 {
539         return true;
540 }
541
542
543 void    btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
544 {
545 }
546
547
548 btVector3* btKinematicCharacterController::getUpAxisDirections()
549 {
550         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) };
551         
552         return sUpAxisDirection;
553 }