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
getNormalizedVector(const btVector3 & v)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:
btKinematicClosestNotMeRayResultCallback(btCollisionObject * me)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
addSingleResult(btCollisionWorld::LocalRayResult & rayResult,bool normalInWorldSpace)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:
btKinematicClosestNotMeConvexResultCallback(btCollisionObject * me,const btVector3 & up,btScalar minSlopeDot)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
addSingleResult(btCollisionWorld::LocalConvexResult & convexResult,bool normalInWorldSpace)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 */
computeReflectionDirection(const btVector3 & direction,const btVector3 & normal)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 */
parallelComponent(const btVector3 & direction,const btVector3 & normal)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 */
perpindicularComponent(const btVector3 & direction,const btVector3 & normal)130 btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
131 {
132 return direction - parallelComponent(direction, normal);
133 }
134
btKinematicCharacterController(btPairCachingGhostObject * ghostObject,btConvexShape * convexShape,btScalar stepHeight,int upAxis)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
~btKinematicCharacterController()161 btKinematicCharacterController::~btKinematicCharacterController ()
162 {
163 }
164
getGhostObject()165 btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
166 {
167 return m_ghostObject;
168 }
169
recoverFromPenetration(btCollisionWorld * collisionWorld)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
stepUp(btCollisionWorld * world)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
updateTargetPositionBasedOnCollision(const btVector3 & hitNormal,btScalar tangentMag,btScalar normalMag)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
stepForwardAndStrafe(btCollisionWorld * collisionWorld,const btVector3 & walkMove)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
stepDown(btCollisionWorld * collisionWorld,btScalar dt)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
setWalkDirection(const btVector3 & walkDirection)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
setVelocityForTimeInterval(const btVector3 & velocity,btScalar timeInterval)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
reset(btCollisionWorld * collisionWorld)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
warp(const btVector3 & origin)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
preStep(btCollisionWorld * collisionWorld)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
playerStep(btCollisionWorld * collisionWorld,btScalar dt)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
setFallSpeed(btScalar fallSpeed)691 void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
692 {
693 m_fallSpeed = fallSpeed;
694 }
695
setJumpSpeed(btScalar jumpSpeed)696 void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
697 {
698 m_jumpSpeed = jumpSpeed;
699 }
700
setMaxJumpHeight(btScalar maxJumpHeight)701 void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
702 {
703 m_maxJumpHeight = maxJumpHeight;
704 }
705
canJump() const706 bool btKinematicCharacterController::canJump () const
707 {
708 return onGround();
709 }
710
jump()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
setGravity(btScalar gravity)730 void btKinematicCharacterController::setGravity(btScalar gravity)
731 {
732 m_gravity = gravity;
733 }
734
getGravity() const735 btScalar btKinematicCharacterController::getGravity() const
736 {
737 return m_gravity;
738 }
739
setMaxSlope(btScalar slopeRadians)740 void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
741 {
742 m_maxSlopeRadians = slopeRadians;
743 m_maxSlopeCosine = btCos(slopeRadians);
744 }
745
getMaxSlope() const746 btScalar btKinematicCharacterController::getMaxSlope() const
747 {
748 return m_maxSlopeRadians;
749 }
750
onGround() const751 bool btKinematicCharacterController::onGround () const
752 {
753 return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
754 }
755
756
getUpAxisDirections()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
debugDraw(btIDebugDraw * debugDrawer)764 void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
765 {
766 }
767
setUpInterpolate(bool value)768 void btKinematicCharacterController::setUpInterpolate(bool value)
769 {
770 m_interpolateUp = value;
771 }
772