Bullet Collision Detection & Physics Library
btKinematicCharacterController.cpp
Go to the documentation of this file.
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>
26 
27 
28 // static helper method
29 static btVector3
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 
48 {
49 public:
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:
64 };
65 
67 {
68 public:
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  {
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:
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  */
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  */
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  */
131 {
132  return direction - parallelComponent(direction, normal);
133 }
134 
136 {
137  m_ghostObject = ghostObject;
138  m_up.setValue(0.0f, 0.0f, 1.0f);
139  m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
140  setUp(up);
141  setStepHeight(stepHeight);
142  m_addedMargin = 0.02;
143  m_walkDirection.setValue(0.0,0.0,0.0);
144  m_AngVel.setValue(0.0, 0.0, 0.0);
145  m_useGhostObjectSweepTest = true;
146  m_turnAngle = btScalar(0.0);
147  m_convexShape=convexShape;
148  m_useWalkDirection = true; // use walk direction by default, legacy behavior
149  m_velocityTimeInterval = 0.0;
150  m_verticalVelocity = 0.0;
151  m_verticalOffset = 0.0;
152  m_gravity = 9.8 * 3.0 ; // 3G acceleration.
153  m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
154  m_jumpSpeed = 10.0; // ?
155  m_SetjumpSpeed = m_jumpSpeed;
156  m_wasOnGround = false;
157  m_wasJumping = false;
158  m_interpolateUp = true;
159  setMaxSlope(btRadians(45.0));
160  m_currentStepOffset = 0.0;
161  m_maxPenetrationDepth = 0.2;
162  full_drop = false;
163  bounce_fix = false;
164  m_linearDamping = btScalar(0.0);
165  m_angularDamping = btScalar(0.0);
166 }
167 
169 {
170 }
171 
173 {
174  return m_ghostObject;
175 }
176 
178 {
179  // Here we must refresh the overlapping paircache as the penetrating movement itself or the
180  // previous recovery iteration might have used setWorldTransform and pushed us into an object
181  // that is not in the previous cache contents from the last timestep, as will happen if we
182  // are pushed into a new AABB overlap. Unhandled this means the next convex sweep gets stuck.
183  //
184  // Do this by calling the broadphase's setAabb with the moved AABB, this will update the broadphase
185  // paircache and the ghostobject's internal paircache at the same time. /BW
186 
187  btVector3 minAabb, maxAabb;
188  m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb,maxAabb);
189  collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(),
190  minAabb,
191  maxAabb,
192  collisionWorld->getDispatcher());
193 
194  bool penetration = false;
195 
196  collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
197 
198  m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
199 
200 // btScalar maxPen = btScalar(0.0);
201  for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
202  {
203  m_manifoldArray.resize(0);
204 
205  btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
206 
207  btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
208  btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
209 
210  if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
211  continue;
212 
213  if (!needsCollision(obj0, obj1))
214  continue;
215 
216  if (collisionPair->m_algorithm)
217  collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
218 
219 
220  for (int j=0;j<m_manifoldArray.size();j++)
221  {
222  btPersistentManifold* manifold = m_manifoldArray[j];
223  btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
224  for (int p=0;p<manifold->getNumContacts();p++)
225  {
226  const btManifoldPoint&pt = manifold->getContactPoint(p);
227 
228  btScalar dist = pt.getDistance();
229 
230  if (dist < -m_maxPenetrationDepth)
231  {
232  // TODO: cause problems on slopes, not sure if it is needed
233  //if (dist < maxPen)
234  //{
235  // maxPen = dist;
236  // m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
237 
238  //}
239  m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
240  penetration = true;
241  } else {
242  //printf("touching %f\n", dist);
243  }
244  }
245 
246  //manifold->clearManifold();
247  }
248  }
249  btTransform newTrans = m_ghostObject->getWorldTransform();
250  newTrans.setOrigin(m_currentPosition);
251  m_ghostObject->setWorldTransform(newTrans);
252 // printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
253  return penetration;
254 }
255 
257 {
258  btScalar stepHeight = 0.0f;
259  if (m_verticalVelocity < 0.0)
260  stepHeight = m_stepHeight;
261 
262  // phase 1: up
263  btTransform start, end;
264 
265  start.setIdentity ();
266  end.setIdentity ();
267 
268  /* FIXME: Handle penetration properly */
269  start.setOrigin(m_currentPosition);
270 
271  m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f));
272  m_currentPosition = m_targetPosition;
273 
274  end.setOrigin (m_targetPosition);
275 
276  start.setRotation(m_currentOrientation);
277  end.setRotation(m_targetOrientation);
278 
279  btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
280  callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
281  callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
282 
283  if (m_useGhostObjectSweepTest)
284  {
285  m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
286  }
287  else
288  {
289  world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
290  }
291 
292  if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
293  {
294  // Only modify the position if the hit was a slope and not a wall or ceiling.
295  if (callback.m_hitNormalWorld.dot(m_up) > 0.0)
296  {
297  // we moved up only a fraction of the step height
298  m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
299  if (m_interpolateUp == true)
300  m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
301  else
302  m_currentPosition = m_targetPosition;
303  }
304 
305  btTransform& xform = m_ghostObject->getWorldTransform();
306  xform.setOrigin(m_currentPosition);
307  m_ghostObject->setWorldTransform(xform);
308 
309  // fix penetration if we hit a ceiling for example
310  int numPenetrationLoops = 0;
311  m_touchingContact = false;
312  while (recoverFromPenetration(world))
313  {
314  numPenetrationLoops++;
315  m_touchingContact = true;
316  if (numPenetrationLoops > 4)
317  {
318  //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
319  break;
320  }
321  }
322  m_targetPosition = m_ghostObject->getWorldTransform().getOrigin();
323  m_currentPosition = m_targetPosition;
324 
325  if (m_verticalOffset > 0)
326  {
327  m_verticalOffset = 0.0;
328  m_verticalVelocity = 0.0;
329  m_currentStepOffset = m_stepHeight;
330  }
331  } else {
332  m_currentStepOffset = stepHeight;
333  m_currentPosition = m_targetPosition;
334  }
335 }
336 
338 {
339  bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0;
340  collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask);
341  return collides;
342 }
343 
345 {
346  btVector3 movementDirection = m_targetPosition - m_currentPosition;
347  btScalar movementLength = movementDirection.length();
348  if (movementLength>SIMD_EPSILON)
349  {
350  movementDirection.normalize();
351 
352  btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
353  reflectDir.normalize();
354 
355  btVector3 parallelDir, perpindicularDir;
356 
357  parallelDir = parallelComponent (reflectDir, hitNormal);
358  perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
359 
360  m_targetPosition = m_currentPosition;
361  if (0)//tangentMag != 0.0)
362  {
363  btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
364 // printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
365  m_targetPosition += parComponent;
366  }
367 
368  if (normalMag != 0.0)
369  {
370  btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
371 // printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
372  m_targetPosition += perpComponent;
373  }
374  } else
375  {
376 // printf("movementLength don't normalize a zero vector\n");
377  }
378 }
379 
381 {
382  // printf("m_normalizedDirection=%f,%f,%f\n",
383  // m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
384  // phase 2: forward and strafe
385  btTransform start, end;
386 
387  m_targetPosition = m_currentPosition + walkMove;
388 
389  start.setIdentity ();
390  end.setIdentity ();
391 
392  btScalar fraction = 1.0;
393  btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
394 // printf("distance2=%f\n",distance2);
395 
396  int maxIter = 10;
397 
398  while (fraction > btScalar(0.01) && maxIter-- > 0)
399  {
400  start.setOrigin (m_currentPosition);
401  end.setOrigin (m_targetPosition);
402  btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
403 
404  start.setRotation(m_currentOrientation);
405  end.setRotation(m_targetOrientation);
406 
407  btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
408  callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
409  callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
410 
411 
412  btScalar margin = m_convexShape->getMargin();
413  m_convexShape->setMargin(margin + m_addedMargin);
414 
415  if (!(start == end))
416  {
417  if (m_useGhostObjectSweepTest)
418  {
419  m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
420  }
421  else
422  {
423  collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
424  }
425  }
426  m_convexShape->setMargin(margin);
427 
428 
429  fraction -= callback.m_closestHitFraction;
430 
431  if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
432  {
433  // we moved only a fraction
434  //btScalar hitDistance;
435  //hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
436 
437 // m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
438 
439  updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
440  btVector3 currentDir = m_targetPosition - m_currentPosition;
441  distance2 = currentDir.length2();
442  if (distance2 > SIMD_EPSILON)
443  {
444  currentDir.normalize();
445  /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
446  if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0))
447  {
448  break;
449  }
450  } else
451  {
452 // printf("currentDir: don't normalize a zero vector\n");
453  break;
454  }
455 
456  }
457  else
458  {
459  m_currentPosition = m_targetPosition;
460  }
461  }
462 }
463 
465 {
466  btTransform start, end, end_double;
467  bool runonce = false;
468 
469  // phase 3: down
470  /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
471  btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
472  btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
473  btVector3 gravity_drop = m_up * downVelocity;
474  m_targetPosition -= (step_drop + gravity_drop);*/
475 
476  btVector3 orig_position = m_targetPosition;
477 
478  btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
479 
480  if (m_verticalVelocity > 0.0)
481  return;
482 
483  if(downVelocity > 0.0 && downVelocity > m_fallSpeed
484  && (m_wasOnGround || !m_wasJumping))
485  downVelocity = m_fallSpeed;
486 
487  btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
488  m_targetPosition -= step_drop;
489 
490  btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
491  callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
492  callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
493 
494  btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
495  callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
496  callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
497 
498  while (1)
499  {
500  start.setIdentity ();
501  end.setIdentity ();
502 
503  end_double.setIdentity ();
504 
505  start.setOrigin (m_currentPosition);
506  end.setOrigin (m_targetPosition);
507 
508  start.setRotation(m_currentOrientation);
509  end.setRotation(m_targetOrientation);
510 
511  //set double test for 2x the step drop, to check for a large drop vs small drop
512  end_double.setOrigin (m_targetPosition - step_drop);
513 
514  if (m_useGhostObjectSweepTest)
515  {
516  m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
517 
518  if (!callback.hasHit() && m_ghostObject->hasContactResponse())
519  {
520  //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
521  m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
522  }
523  } else
524  {
525  collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
526 
527  if (!callback.hasHit() && m_ghostObject->hasContactResponse())
528  {
529  //test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
530  collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
531  }
532  }
533 
534  btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
535  bool has_hit;
536  if (bounce_fix == true)
537  has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
538  else
539  has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject);
540 
541  btScalar stepHeight = 0.0f;
542  if (m_verticalVelocity < 0.0)
543  stepHeight = m_stepHeight;
544 
545  if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false
546  && (m_wasOnGround || !m_wasJumping))
547  {
548  //redo the velocity calculation when falling a small amount, for fast stairs motion
549  //for larger falls, use the smoother/slower interpolated movement by not touching the target position
550 
551  m_targetPosition = orig_position;
552  downVelocity = stepHeight;
553 
554  step_drop = m_up * (m_currentStepOffset + downVelocity);
555  m_targetPosition -= step_drop;
556  runonce = true;
557  continue; //re-run previous tests
558  }
559  break;
560  }
561 
562  if (m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) || runonce == true)
563  {
564  // we dropped a fraction of the height -> hit floor
565  btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
566 
567  //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
568 
569  if (bounce_fix == true)
570  {
571  if (full_drop == true)
572  m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
573  else
574  //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
575  m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
576  }
577  else
578  m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
579 
580  full_drop = false;
581 
582  m_verticalVelocity = 0.0;
583  m_verticalOffset = 0.0;
584  m_wasJumping = false;
585  } else {
586  // we dropped the full height
587 
588  full_drop = true;
589 
590  if (bounce_fix == true)
591  {
592  downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
593  if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
594  {
595  m_targetPosition += step_drop; //undo previous target change
596  downVelocity = m_fallSpeed;
597  step_drop = m_up * (m_currentStepOffset + downVelocity);
598  m_targetPosition -= step_drop;
599  }
600  }
601  //printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
602 
603  m_currentPosition = m_targetPosition;
604  }
605 }
606 
607 
608 
610 (
611 const btVector3& walkDirection
612 )
613 {
614  m_useWalkDirection = true;
615  m_walkDirection = walkDirection;
616  m_normalizedDirection = getNormalizedVector(m_walkDirection);
617 }
618 
619 
620 
622 (
623 const btVector3& velocity,
624 btScalar timeInterval
625 )
626 {
627 // printf("setVelocity!\n");
628 // printf(" interval: %f\n", timeInterval);
629 // printf(" velocity: (%f, %f, %f)\n",
630 // velocity.x(), velocity.y(), velocity.z());
631 
632  m_useWalkDirection = false;
633  m_walkDirection = velocity;
634  m_normalizedDirection = getNormalizedVector(m_walkDirection);
635  m_velocityTimeInterval += timeInterval;
636 }
637 
639 {
640  m_AngVel = velocity;
641 }
642 
644 {
645  return m_AngVel;
646 }
647 
649 {
650  m_walkDirection = velocity;
651 
652  // HACK: if we are moving in the direction of the up, treat it as a jump :(
653  if (m_walkDirection.length2() > 0)
654  {
655  btVector3 w = velocity.normalized();
656  btScalar c = w.dot(m_up);
657  if (c != 0)
658  {
659  //there is a component in walkdirection for vertical velocity
660  btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length());
661  m_walkDirection -= upComponent;
662  m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
663 
664  if (c > 0.0f)
665  {
666  m_wasJumping = true;
667  m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
668  }
669  }
670  }
671  else
672  m_verticalVelocity = 0.0f;
673 }
674 
676 {
677  return m_walkDirection + (m_verticalVelocity * m_up);
678 }
679 
681 {
682  m_verticalVelocity = 0.0;
683  m_verticalOffset = 0.0;
684  m_wasOnGround = false;
685  m_wasJumping = false;
686  m_walkDirection.setValue(0,0,0);
687  m_velocityTimeInterval = 0.0;
688 
689  //clear pair cache
690  btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
691  while (cache->getOverlappingPairArray().size() > 0)
692  {
693  cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
694  }
695 }
696 
698 {
699  btTransform xform;
700  xform.setIdentity();
701  xform.setOrigin (origin);
702  m_ghostObject->setWorldTransform (xform);
703 }
704 
705 
707 {
708  m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
709  m_targetPosition = m_currentPosition;
710 
711  m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
712  m_targetOrientation = m_currentOrientation;
713 // printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
714 }
715 
717 {
718 // printf("playerStep(): ");
719 // printf(" dt = %f", dt);
720 
721  if (m_AngVel.length2() > 0.0f)
722  {
723  m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt);
724  }
725 
726  // integrate for angular velocity
727  if (m_AngVel.length2() > 0.0f)
728  {
729  btTransform xform;
730  xform = m_ghostObject->getWorldTransform();
731 
732  btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt);
733 
734  btQuaternion orn = rot * xform.getRotation();
735 
736  xform.setRotation(orn);
737  m_ghostObject->setWorldTransform(xform);
738 
739  m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
740  m_targetPosition = m_currentPosition;
741  m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
742  m_targetOrientation = m_currentOrientation;
743  }
744 
745  // quick check...
746  if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) {
747 // printf("\n");
748  return; // no motion
749  }
750 
751  m_wasOnGround = onGround();
752 
753  //btVector3 lvel = m_walkDirection;
754  btScalar c = 0.0f;
755 
756  if (m_walkDirection.length2() > 0)
757  {
758  // apply damping
759  m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt);
760  }
761 
762  m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt);
763 
764  // Update fall velocity.
765  m_verticalVelocity -= m_gravity * dt;
766  if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
767  {
768  m_verticalVelocity = m_jumpSpeed;
769  }
770  if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
771  {
772  m_verticalVelocity = -btFabs(m_fallSpeed);
773  }
774  m_verticalOffset = m_verticalVelocity * dt;
775 
776  btTransform xform;
777  xform = m_ghostObject->getWorldTransform();
778 
779 // printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
780 // printf("walkSpeed=%f\n",walkSpeed);
781 
782  stepUp(collisionWorld);
783  //todo: Experimenting with behavior of controller when it hits a ceiling..
784  //bool hitUp = stepUp (collisionWorld);
785  //if (hitUp)
786  //{
787  // m_verticalVelocity -= m_gravity * dt;
788  // if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
789  // {
790  // m_verticalVelocity = m_jumpSpeed;
791  // }
792  // if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
793  // {
794  // m_verticalVelocity = -btFabs(m_fallSpeed);
795  // }
796  // m_verticalOffset = m_verticalVelocity * dt;
797 
798  // xform = m_ghostObject->getWorldTransform();
799  //}
800 
801  if (m_useWalkDirection) {
802  stepForwardAndStrafe (collisionWorld, m_walkDirection);
803  } else {
804  //printf(" time: %f", m_velocityTimeInterval);
805  // still have some time left for moving!
806  btScalar dtMoving =
807  (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
808  m_velocityTimeInterval -= dt;
809 
810  // how far will we move while we are moving?
811  btVector3 move = m_walkDirection * dtMoving;
812 
813  //printf(" dtMoving: %f", dtMoving);
814 
815  // okay, step
816  stepForwardAndStrafe(collisionWorld, move);
817  }
818  stepDown (collisionWorld, dt);
819 
820  //todo: Experimenting with max jump height
821  //if (m_wasJumping)
822  //{
823  // btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis];
824  // if (ds > m_maxJumpHeight)
825  // {
826  // // substract the overshoot
827  // m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
828 
829  // // max height was reached, so potential energy is at max
830  // // and kinematic energy is 0, thus velocity is 0.
831  // if (m_verticalVelocity > 0.0)
832  // m_verticalVelocity = 0.0;
833  // }
834  //}
835  // printf("\n");
836 
837  xform.setOrigin (m_currentPosition);
838  m_ghostObject->setWorldTransform (xform);
839 
840  int numPenetrationLoops = 0;
841  m_touchingContact = false;
842  while (recoverFromPenetration(collisionWorld))
843  {
844  numPenetrationLoops++;
845  m_touchingContact = true;
846  if (numPenetrationLoops > 4)
847  {
848  //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
849  break;
850  }
851  }
852 }
853 
855 {
856  m_fallSpeed = fallSpeed;
857 }
858 
860 {
861  m_jumpSpeed = jumpSpeed;
862  m_SetjumpSpeed = m_jumpSpeed;
863 }
864 
866 {
867  m_maxJumpHeight = maxJumpHeight;
868 }
869 
871 {
872  return onGround();
873 }
874 
876 {
877  m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length();
878  m_verticalVelocity = m_jumpSpeed;
879  m_wasJumping = true;
880 
881  m_jumpAxis = v.length2() == 0 ? m_up : v.normalized();
882 
883  m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
884 
885 #if 0
886  currently no jumping.
887  btTransform xform;
888  m_rigidBody->getMotionState()->getWorldTransform (xform);
889  btVector3 up = xform.getBasis()[1];
890  up.normalize ();
891  btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0);
892  m_rigidBody->applyCentralImpulse (up * magnitude);
893 #endif
894 }
895 
897 {
898  if (gravity.length2() > 0) setUpVector(-gravity);
899 
900  m_gravity = gravity.length();
901 }
902 
904 {
905  return -m_gravity * m_up;
906 }
907 
909 {
910  m_maxSlopeRadians = slopeRadians;
911  m_maxSlopeCosine = btCos(slopeRadians);
912 }
913 
915 {
916  return m_maxSlopeRadians;
917 }
918 
920 {
921  m_maxPenetrationDepth = d;
922 }
923 
925 {
926  return m_maxPenetrationDepth;
927 }
928 
930 {
931  return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
932 }
933 
935 {
936  m_stepHeight = h;
937 }
938 
940 {
941  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) };
942 
943  return sUpAxisDirection;
944 }
945 
947 {
948 }
949 
951 {
952  m_interpolateUp = value;
953 }
954 
956 {
957  if (up.length2() > 0 && m_gravity > 0.0f)
958  {
959  setGravity(-m_gravity * up.normalized());
960  return;
961  }
962 
963  setUpVector(up);
964 }
965 
967 {
968  if (m_up == up)
969  return;
970 
971  btVector3 u = m_up;
972 
973  if (up.length2() > 0)
974  m_up = up.normalized();
975  else
976  m_up = btVector3(0.0, 0.0, 0.0);
977 
978  if (!m_ghostObject) return;
979  btQuaternion rot = getRotation(m_up, u);
980 
981  //set orientation with new up
982  btTransform xform;
983  xform = m_ghostObject->getWorldTransform();
984  btQuaternion orn = rot.inverse() * xform.getRotation();
985  xform.setRotation(orn);
986  m_ghostObject->setWorldTransform(xform);
987 }
988 
990 {
991  if (v0.length2() == 0.0f || v1.length2() == 0.0f)
992  {
993  btQuaternion q;
994  return q;
995  }
996 
997  return shortestArcQuatNormalize2(v0, v1);
998 }
999 
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
#define SIMD_EPSILON
Definition: btScalar.h:495
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
void playerStep(btCollisionWorld *collisionWorld, btScalar dt)
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult &rayResult, bool normalInWorldSpace)
void stepDown(btCollisionWorld *collisionWorld, btScalar dt)
btScalar btRadians(btScalar x)
Definition: btScalar.h:533
btQuaternion getRotation(btVector3 &v0, btVector3 &v1) const
short int m_collisionFilterGroup
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
virtual void dispatchAllCollisionPairs(btOverlappingPairCache *pairCache, const btDispatcherInfo &dispatchInfo, btDispatcher *dispatcher)=0
btVector3 computeReflectionDirection(const btVector3 &direction, const btVector3 &normal)
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btBroadphasePairArray & getOverlappingPairArray()
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:573
ManifoldContactPoint collects and maintains persistent contactpoints.
virtual void setVelocityForTimeInterval(const btVector3 &velocity, btScalar timeInterval)
Caller provides a velocity with which the character should move for the given time period...
#define SIMD_HALF_PI
Definition: btScalar.h:479
void debugDraw(btIDebugDraw *debugDrawer)
btActionInterface interface
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:307
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:962
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
const btManifoldPoint & getContactPoint(int index) const
const btCollisionObject * m_hitCollisionObject
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:452
btTransform & getWorldTransform()
btVector3 m_normalWorldOnB
btBroadphaseProxy * getBroadphaseHandle()
static btVector3 getNormalizedVector(const btVector3 &v)
const btCollisionObject * getBody0() const
virtual void setLinearVelocity(const btVector3 &velocity)
btQuaternion shortestArcQuatNormalize2(btVector3 &v0, btVector3 &v1)
Definition: btQuaternion.h:920
virtual bool needsCollision(btBroadphaseProxy *proxy0) const
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
void jump(const btVector3 &v=btVector3())
void setMaxSlope(btScalar slopeRadians)
The max slope determines the maximum angle that the controller can walk up.
btCollisionObject can be used to manage collision detection objects.
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:165
bool recoverFromPenetration(btCollisionWorld *collisionWorld)
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
bool hasContactResponse() const
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
const btCollisionObject * m_collisionObject
btDispatcher * getDispatcher()
btVector3 parallelComponent(const btVector3 &direction, const btVector3 &normal)
btBroadphaseProxy * m_pProxy1
btCollisionAlgorithm * m_algorithm
virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)
btScalar btPow(btScalar x, btScalar y)
Definition: btScalar.h:472
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)=0
int size() const
return the number of elements in the array
btBroadphaseProxy * m_pProxy0
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
void updateTargetPositionBasedOnCollision(const btVector3 &hit_normal, btScalar tangentMag=btScalar(0.0), btScalar normalMag=btScalar(1.0))
CollisionWorld is interface and container for the collision detection.
void convexSweepTest(const btConvexShape *castShape, const btTransform &from, const btTransform &to, ConvexResultCallback &resultCallback, btScalar allowedCcdPenetration=btScalar(0.)) const
convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultC...
btDispatcherInfo & getDispatchInfo()
void stepUp(btCollisionWorld *collisionWorld)
btScalar m_allowedCcdPenetration
Definition: btDispatcher.h:62
virtual void setAngularVelocity(const btVector3 &velocity)
btVector3 perpindicularComponent(const btVector3 &direction, const btVector3 &normal)
void reset(btCollisionWorld *collisionWorld)
virtual const btVector3 & getAngularVelocity() const
btKinematicClosestNotMeConvexResultCallback(btCollisionObject *me, const btVector3 &up, btScalar minSlopeDot)
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)=0
short int m_collisionFilterMask
void stepForwardAndStrafe(btCollisionWorld *collisionWorld, const btVector3 &walkMove)
void preStep(btCollisionWorld *collisionWorld)
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
virtual void setWalkDirection(const btVector3 &walkDirection)
This should probably be called setPositionIncrementPerSimulatorStep.
virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1)
btKinematicCharacterController(btPairCachingGhostObject *ghostObject, btConvexShape *convexShape, btScalar stepHeight, const btVector3 &up=btVector3(1.0, 0.0, 0.0))
void setInterpolate3(const btVector3 &v0, const btVector3 &v1, btScalar rt)
Definition: btVector3.h:501
const btBroadphaseInterface * getBroadphase() const
Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman...
btScalar getDistance() const
ClosestRayResultCallback(const btVector3 &rayFromWorld, const btVector3 &rayToWorld)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
btScalar btCos(btScalar x)
Definition: btScalar.h:451
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btScalar btFabs(btScalar x)
Definition: btScalar.h:450
The btBroadphasePair class contains a pair of aabb-overlapping objects.