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 #include <stdio.h>
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 // static helper method
27 static btVector3
getNormalizedVector(const btVector3 & v)28 getNormalizedVector(const btVector3& v)
29 {
30 btVector3 n(0, 0, 0);
31
32 if (v.length() > SIMD_EPSILON)
33 {
34 n = v.normalized();
35 }
36 return n;
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:
btKinematicClosestNotMeRayResultCallback(btCollisionObject * me)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
addSingleResult(btCollisionWorld::LocalRayResult & rayResult,bool normalInWorldSpace)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
61 protected:
62 btCollisionObject* m_me;
63 };
64
65 class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
66 {
67 public:
btKinematicClosestNotMeConvexResultCallback(btCollisionObject * me,const btVector3 & up,btScalar minSlopeDot)68 btKinematicClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
69 : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)), m_me(me), m_up(up), m_minSlopeDot(minSlopeDot)
70 {
71 }
72
addSingleResult(btCollisionWorld::LocalConvexResult & convexResult,bool normalInWorldSpace)73 virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace)
74 {
75 if (convexResult.m_hitCollisionObject == m_me)
76 return btScalar(1.0);
77
78 if (!convexResult.m_hitCollisionObject->hasContactResponse())
79 return btScalar(1.0);
80
81 btVector3 hitNormalWorld;
82 if (normalInWorldSpace)
83 {
84 hitNormalWorld = convexResult.m_hitNormalLocal;
85 }
86 else
87 {
88 ///need to transform normal into worldspace
89 hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal;
90 }
91
92 btScalar dotUp = m_up.dot(hitNormalWorld);
93 if (dotUp < m_minSlopeDot)
94 {
95 return btScalar(1.0);
96 }
97
98 return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
99 }
100
101 protected:
102 btCollisionObject* m_me;
103 const btVector3 m_up;
104 btScalar m_minSlopeDot;
105 };
106
107 /*
108 * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal'
109 *
110 * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
111 */
computeReflectionDirection(const btVector3 & direction,const btVector3 & normal)112 btVector3 btKinematicCharacterController::computeReflectionDirection(const btVector3& direction, const btVector3& normal)
113 {
114 return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
115 }
116
117 /*
118 * Returns the portion of 'direction' that is parallel to 'normal'
119 */
parallelComponent(const btVector3 & direction,const btVector3 & normal)120 btVector3 btKinematicCharacterController::parallelComponent(const btVector3& direction, const btVector3& normal)
121 {
122 btScalar magnitude = direction.dot(normal);
123 return normal * magnitude;
124 }
125
126 /*
127 * Returns the portion of 'direction' that is perpindicular to 'normal'
128 */
perpindicularComponent(const btVector3 & direction,const btVector3 & normal)129 btVector3 btKinematicCharacterController::perpindicularComponent(const btVector3& direction, const btVector3& normal)
130 {
131 return direction - parallelComponent(direction, normal);
132 }
133
btKinematicCharacterController(btPairCachingGhostObject * ghostObject,btConvexShape * convexShape,btScalar stepHeight,const btVector3 & up)134 btKinematicCharacterController::btKinematicCharacterController(btPairCachingGhostObject* ghostObject, btConvexShape* convexShape, btScalar stepHeight, const btVector3& up)
135 {
136 m_ghostObject = ghostObject;
137 m_up.setValue(0.0f, 0.0f, 1.0f);
138 m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
139 m_addedMargin = 0.02;
140 m_walkDirection.setValue(0.0, 0.0, 0.0);
141 m_AngVel.setValue(0.0, 0.0, 0.0);
142 m_useGhostObjectSweepTest = true;
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.0; // 3G acceleration.
150 m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
151 m_jumpSpeed = 10.0; // ?
152 m_SetjumpSpeed = m_jumpSpeed;
153 m_wasOnGround = false;
154 m_wasJumping = false;
155 m_interpolateUp = true;
156 m_currentStepOffset = 0.0;
157 m_maxPenetrationDepth = 0.2;
158 full_drop = false;
159 bounce_fix = false;
160 m_linearDamping = btScalar(0.0);
161 m_angularDamping = btScalar(0.0);
162
163 setUp(up);
164 setStepHeight(stepHeight);
165 setMaxSlope(btRadians(45.0));
166 }
167
~btKinematicCharacterController()168 btKinematicCharacterController::~btKinematicCharacterController()
169 {
170 }
171
getGhostObject()172 btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
173 {
174 return m_ghostObject;
175 }
176
recoverFromPenetration(btCollisionWorld * collisionWorld)177 bool btKinematicCharacterController::recoverFromPenetration(btCollisionWorld* collisionWorld)
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 for (int j = 0; j < m_manifoldArray.size(); j++)
220 {
221 btPersistentManifold* manifold = m_manifoldArray[j];
222 btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
223 for (int p = 0; p < manifold->getNumContacts(); p++)
224 {
225 const btManifoldPoint& pt = manifold->getContactPoint(p);
226
227 btScalar dist = pt.getDistance();
228
229 if (dist < -m_maxPenetrationDepth)
230 {
231 // TODO: cause problems on slopes, not sure if it is needed
232 //if (dist < maxPen)
233 //{
234 // maxPen = dist;
235 // m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
236
237 //}
238 m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
239 penetration = true;
240 }
241 else
242 {
243 //printf("touching %f\n", dist);
244 }
245 }
246
247 //manifold->clearManifold();
248 }
249 }
250 btTransform newTrans = m_ghostObject->getWorldTransform();
251 newTrans.setOrigin(m_currentPosition);
252 m_ghostObject->setWorldTransform(newTrans);
253 // printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
254 return penetration;
255 }
256
stepUp(btCollisionWorld * world)257 void btKinematicCharacterController::stepUp(btCollisionWorld* world)
258 {
259 btScalar stepHeight = 0.0f;
260 if (m_verticalVelocity < 0.0)
261 stepHeight = m_stepHeight;
262
263 // phase 1: up
264 btTransform start, end;
265
266 start.setIdentity();
267 end.setIdentity();
268
269 /* FIXME: Handle penetration properly */
270 start.setOrigin(m_currentPosition);
271
272 m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f));
273 m_currentPosition = m_targetPosition;
274
275 end.setOrigin(m_targetPosition);
276
277 start.setRotation(m_currentOrientation);
278 end.setRotation(m_targetOrientation);
279
280 btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
281 callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
282 callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
283
284 if (m_useGhostObjectSweepTest)
285 {
286 m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
287 }
288 else
289 {
290 world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
291 }
292
293 if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
294 {
295 // Only modify the position if the hit was a slope and not a wall or ceiling.
296 if (callback.m_hitNormalWorld.dot(m_up) > 0.0)
297 {
298 // we moved up only a fraction of the step height
299 m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
300 if (m_interpolateUp == true)
301 m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
302 else
303 m_currentPosition = m_targetPosition;
304 }
305
306 btTransform& xform = m_ghostObject->getWorldTransform();
307 xform.setOrigin(m_currentPosition);
308 m_ghostObject->setWorldTransform(xform);
309
310 // fix penetration if we hit a ceiling for example
311 int numPenetrationLoops = 0;
312 m_touchingContact = false;
313 while (recoverFromPenetration(world))
314 {
315 numPenetrationLoops++;
316 m_touchingContact = true;
317 if (numPenetrationLoops > 4)
318 {
319 //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
320 break;
321 }
322 }
323 m_targetPosition = m_ghostObject->getWorldTransform().getOrigin();
324 m_currentPosition = m_targetPosition;
325
326 if (m_verticalOffset > 0)
327 {
328 m_verticalOffset = 0.0;
329 m_verticalVelocity = 0.0;
330 m_currentStepOffset = m_stepHeight;
331 }
332 }
333 else
334 {
335 m_currentStepOffset = stepHeight;
336 m_currentPosition = m_targetPosition;
337 }
338 }
339
needsCollision(const btCollisionObject * body0,const btCollisionObject * body1)340 bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
341 {
342 bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0;
343 collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask);
344 return collides;
345 }
346
updateTargetPositionBasedOnCollision(const btVector3 & hitNormal,btScalar tangentMag,btScalar normalMag)347 void btKinematicCharacterController::updateTargetPositionBasedOnCollision(const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
348 {
349 btVector3 movementDirection = m_targetPosition - m_currentPosition;
350 btScalar movementLength = movementDirection.length();
351 if (movementLength > SIMD_EPSILON)
352 {
353 movementDirection.normalize();
354
355 btVector3 reflectDir = computeReflectionDirection(movementDirection, hitNormal);
356 reflectDir.normalize();
357
358 btVector3 parallelDir, perpindicularDir;
359
360 parallelDir = parallelComponent(reflectDir, hitNormal);
361 perpindicularDir = perpindicularComponent(reflectDir, hitNormal);
362
363 m_targetPosition = m_currentPosition;
364 if (0) //tangentMag != 0.0)
365 {
366 btVector3 parComponent = parallelDir * btScalar(tangentMag * movementLength);
367 // printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
368 m_targetPosition += parComponent;
369 }
370
371 if (normalMag != 0.0)
372 {
373 btVector3 perpComponent = perpindicularDir * btScalar(normalMag * movementLength);
374 // printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
375 m_targetPosition += perpComponent;
376 }
377 }
378 else
379 {
380 // printf("movementLength don't normalize a zero vector\n");
381 }
382 }
383
stepForwardAndStrafe(btCollisionWorld * collisionWorld,const btVector3 & walkMove)384 void btKinematicCharacterController::stepForwardAndStrafe(btCollisionWorld* collisionWorld, const btVector3& walkMove)
385 {
386 // printf("m_normalizedDirection=%f,%f,%f\n",
387 // m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
388 // phase 2: forward and strafe
389 btTransform start, end;
390
391 m_targetPosition = m_currentPosition + walkMove;
392
393 start.setIdentity();
394 end.setIdentity();
395
396 btScalar fraction = 1.0;
397 btScalar distance2 = (m_currentPosition - m_targetPosition).length2();
398 // printf("distance2=%f\n",distance2);
399
400 int maxIter = 10;
401
402 while (fraction > btScalar(0.01) && maxIter-- > 0)
403 {
404 start.setOrigin(m_currentPosition);
405 end.setOrigin(m_targetPosition);
406 btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
407
408 start.setRotation(m_currentOrientation);
409 end.setRotation(m_targetOrientation);
410
411 btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, sweepDirNegative, btScalar(0.0));
412 callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
413 callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
414
415 btScalar margin = m_convexShape->getMargin();
416 m_convexShape->setMargin(margin + m_addedMargin);
417
418 if (!(start == end))
419 {
420 if (m_useGhostObjectSweepTest)
421 {
422 m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
423 }
424 else
425 {
426 collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
427 }
428 }
429 m_convexShape->setMargin(margin);
430
431 fraction -= callback.m_closestHitFraction;
432
433 if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
434 {
435 // we moved only a fraction
436 //btScalar hitDistance;
437 //hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
438
439 // m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
440
441 updateTargetPositionBasedOnCollision(callback.m_hitNormalWorld);
442 btVector3 currentDir = m_targetPosition - m_currentPosition;
443 distance2 = currentDir.length2();
444 if (distance2 > SIMD_EPSILON)
445 {
446 currentDir.normalize();
447 /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
448 if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0))
449 {
450 break;
451 }
452 }
453 else
454 {
455 // printf("currentDir: don't normalize a zero vector\n");
456 break;
457 }
458 }
459 else
460 {
461 m_currentPosition = m_targetPosition;
462 }
463 }
464 }
465
stepDown(btCollisionWorld * collisionWorld,btScalar dt)466 void btKinematicCharacterController::stepDown(btCollisionWorld* collisionWorld, btScalar dt)
467 {
468 btTransform start, end, end_double;
469 bool runonce = false;
470
471 // phase 3: down
472 /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
473 btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
474 btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
475 btVector3 gravity_drop = m_up * downVelocity;
476 m_targetPosition -= (step_drop + gravity_drop);*/
477
478 btVector3 orig_position = m_targetPosition;
479
480 btScalar downVelocity = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
481
482 if (m_verticalVelocity > 0.0)
483 return;
484
485 if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
486 downVelocity = m_fallSpeed;
487
488 btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
489 m_targetPosition -= step_drop;
490
491 btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
492 callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
493 callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
494
495 btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
496 callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
497 callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
498
499 while (1)
500 {
501 start.setIdentity();
502 end.setIdentity();
503
504 end_double.setIdentity();
505
506 start.setOrigin(m_currentPosition);
507 end.setOrigin(m_targetPosition);
508
509 start.setRotation(m_currentOrientation);
510 end.setRotation(m_targetOrientation);
511
512 //set double test for 2x the step drop, to check for a large drop vs small drop
513 end_double.setOrigin(m_targetPosition - step_drop);
514
515 if (m_useGhostObjectSweepTest)
516 {
517 m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
518
519 if (!callback.hasHit() && m_ghostObject->hasContactResponse())
520 {
521 //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
522 m_ghostObject->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
523 }
524 }
525 else
526 {
527 collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
528
529 if (!callback.hasHit() && m_ghostObject->hasContactResponse())
530 {
531 //test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
532 collisionWorld->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
533 }
534 }
535
536 btScalar downVelocity2 = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
537 bool has_hit;
538 if (bounce_fix == true)
539 has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
540 else
541 has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject);
542
543 btScalar stepHeight = 0.0f;
544 if (m_verticalVelocity < 0.0)
545 stepHeight = m_stepHeight;
546
547 if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false && (m_wasOnGround || !m_wasJumping))
548 {
549 //redo the velocity calculation when falling a small amount, for fast stairs motion
550 //for larger falls, use the smoother/slower interpolated movement by not touching the target position
551
552 m_targetPosition = orig_position;
553 downVelocity = stepHeight;
554
555 step_drop = m_up * (m_currentStepOffset + downVelocity);
556 m_targetPosition -= step_drop;
557 runonce = true;
558 continue; //re-run previous tests
559 }
560 break;
561 }
562
563 if ((m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))) || runonce == true)
564 {
565 // we dropped a fraction of the height -> hit floor
566 btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
567
568 //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
569
570 if (bounce_fix == true)
571 {
572 if (full_drop == true)
573 m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
574 else
575 //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
576 m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, fraction);
577 }
578 else
579 m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
580
581 full_drop = false;
582
583 m_verticalVelocity = 0.0;
584 m_verticalOffset = 0.0;
585 m_wasJumping = false;
586 }
587 else
588 {
589 // we dropped the full height
590
591 full_drop = true;
592
593 if (bounce_fix == true)
594 {
595 downVelocity = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
596 if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
597 {
598 m_targetPosition += step_drop; //undo previous target change
599 downVelocity = m_fallSpeed;
600 step_drop = m_up * (m_currentStepOffset + downVelocity);
601 m_targetPosition -= step_drop;
602 }
603 }
604 //printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
605
606 m_currentPosition = m_targetPosition;
607 }
608 }
609
setWalkDirection(const btVector3 & walkDirection)610 void btKinematicCharacterController::setWalkDirection(
611 const btVector3& walkDirection)
612 {
613 m_useWalkDirection = true;
614 m_walkDirection = walkDirection;
615 m_normalizedDirection = getNormalizedVector(m_walkDirection);
616 }
617
setVelocityForTimeInterval(const btVector3 & velocity,btScalar timeInterval)618 void btKinematicCharacterController::setVelocityForTimeInterval(
619 const btVector3& velocity,
620 btScalar timeInterval)
621 {
622 // printf("setVelocity!\n");
623 // printf(" interval: %f\n", timeInterval);
624 // printf(" velocity: (%f, %f, %f)\n",
625 // velocity.x(), velocity.y(), velocity.z());
626
627 m_useWalkDirection = false;
628 m_walkDirection = velocity;
629 m_normalizedDirection = getNormalizedVector(m_walkDirection);
630 m_velocityTimeInterval += timeInterval;
631 }
632
setAngularVelocity(const btVector3 & velocity)633 void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity)
634 {
635 m_AngVel = velocity;
636 }
637
getAngularVelocity() const638 const btVector3& btKinematicCharacterController::getAngularVelocity() const
639 {
640 return m_AngVel;
641 }
642
setLinearVelocity(const btVector3 & velocity)643 void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity)
644 {
645 m_walkDirection = velocity;
646
647 // HACK: if we are moving in the direction of the up, treat it as a jump :(
648 if (m_walkDirection.length2() > 0)
649 {
650 btVector3 w = velocity.normalized();
651 btScalar c = w.dot(m_up);
652 if (c != 0)
653 {
654 //there is a component in walkdirection for vertical velocity
655 btVector3 upComponent = m_up * (btSin(SIMD_HALF_PI - btAcos(c)) * m_walkDirection.length());
656 m_walkDirection -= upComponent;
657 m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
658
659 if (c > 0.0f)
660 {
661 m_wasJumping = true;
662 m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
663 }
664 }
665 }
666 else
667 m_verticalVelocity = 0.0f;
668 }
669
getLinearVelocity() const670 btVector3 btKinematicCharacterController::getLinearVelocity() const
671 {
672 return m_walkDirection + (m_verticalVelocity * m_up);
673 }
674
reset(btCollisionWorld * collisionWorld)675 void btKinematicCharacterController::reset(btCollisionWorld* collisionWorld)
676 {
677 m_verticalVelocity = 0.0;
678 m_verticalOffset = 0.0;
679 m_wasOnGround = false;
680 m_wasJumping = false;
681 m_walkDirection.setValue(0, 0, 0);
682 m_velocityTimeInterval = 0.0;
683
684 //clear pair cache
685 btHashedOverlappingPairCache* cache = m_ghostObject->getOverlappingPairCache();
686 while (cache->getOverlappingPairArray().size() > 0)
687 {
688 cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
689 }
690 }
691
warp(const btVector3 & origin)692 void btKinematicCharacterController::warp(const btVector3& origin)
693 {
694 btTransform xform;
695 xform.setIdentity();
696 xform.setOrigin(origin);
697 m_ghostObject->setWorldTransform(xform);
698 }
699
preStep(btCollisionWorld * collisionWorld)700 void btKinematicCharacterController::preStep(btCollisionWorld* collisionWorld)
701 {
702 m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
703 m_targetPosition = m_currentPosition;
704
705 m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
706 m_targetOrientation = m_currentOrientation;
707 // printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
708 }
709
playerStep(btCollisionWorld * collisionWorld,btScalar dt)710 void btKinematicCharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt)
711 {
712 // printf("playerStep(): ");
713 // printf(" dt = %f", dt);
714
715 if (m_AngVel.length2() > 0.0f)
716 {
717 m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt);
718 }
719
720 // integrate for angular velocity
721 if (m_AngVel.length2() > 0.0f)
722 {
723 btTransform xform;
724 xform = m_ghostObject->getWorldTransform();
725
726 btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt);
727
728 btQuaternion orn = rot * xform.getRotation();
729
730 xform.setRotation(orn);
731 m_ghostObject->setWorldTransform(xform);
732
733 m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
734 m_targetPosition = m_currentPosition;
735 m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
736 m_targetOrientation = m_currentOrientation;
737 }
738
739 // quick check...
740 if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero()))
741 {
742 // printf("\n");
743 return; // no motion
744 }
745
746 m_wasOnGround = onGround();
747
748 //btVector3 lvel = m_walkDirection;
749 //btScalar c = 0.0f;
750
751 if (m_walkDirection.length2() > 0)
752 {
753 // apply damping
754 m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt);
755 }
756
757 m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt);
758
759 // Update fall velocity.
760 m_verticalVelocity -= m_gravity * dt;
761 if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
762 {
763 m_verticalVelocity = m_jumpSpeed;
764 }
765 if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
766 {
767 m_verticalVelocity = -btFabs(m_fallSpeed);
768 }
769 m_verticalOffset = m_verticalVelocity * dt;
770
771 btTransform xform;
772 xform = m_ghostObject->getWorldTransform();
773
774 // printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
775 // printf("walkSpeed=%f\n",walkSpeed);
776
777 stepUp(collisionWorld);
778 //todo: Experimenting with behavior of controller when it hits a ceiling..
779 //bool hitUp = stepUp (collisionWorld);
780 //if (hitUp)
781 //{
782 // m_verticalVelocity -= m_gravity * dt;
783 // if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
784 // {
785 // m_verticalVelocity = m_jumpSpeed;
786 // }
787 // if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
788 // {
789 // m_verticalVelocity = -btFabs(m_fallSpeed);
790 // }
791 // m_verticalOffset = m_verticalVelocity * dt;
792
793 // xform = m_ghostObject->getWorldTransform();
794 //}
795
796 if (m_useWalkDirection)
797 {
798 stepForwardAndStrafe(collisionWorld, m_walkDirection);
799 }
800 else
801 {
802 //printf(" time: %f", m_velocityTimeInterval);
803 // still have some time left for moving!
804 btScalar dtMoving =
805 (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
806 m_velocityTimeInterval -= dt;
807
808 // how far will we move while we are moving?
809 btVector3 move = m_walkDirection * dtMoving;
810
811 //printf(" dtMoving: %f", dtMoving);
812
813 // okay, step
814 stepForwardAndStrafe(collisionWorld, move);
815 }
816 stepDown(collisionWorld, dt);
817
818 //todo: Experimenting with max jump height
819 //if (m_wasJumping)
820 //{
821 // btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis];
822 // if (ds > m_maxJumpHeight)
823 // {
824 // // substract the overshoot
825 // m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
826
827 // // max height was reached, so potential energy is at max
828 // // and kinematic energy is 0, thus velocity is 0.
829 // if (m_verticalVelocity > 0.0)
830 // m_verticalVelocity = 0.0;
831 // }
832 //}
833 // printf("\n");
834
835 xform.setOrigin(m_currentPosition);
836 m_ghostObject->setWorldTransform(xform);
837
838 int numPenetrationLoops = 0;
839 m_touchingContact = false;
840 while (recoverFromPenetration(collisionWorld))
841 {
842 numPenetrationLoops++;
843 m_touchingContact = true;
844 if (numPenetrationLoops > 4)
845 {
846 //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
847 break;
848 }
849 }
850 }
851
setFallSpeed(btScalar fallSpeed)852 void btKinematicCharacterController::setFallSpeed(btScalar fallSpeed)
853 {
854 m_fallSpeed = fallSpeed;
855 }
856
setJumpSpeed(btScalar jumpSpeed)857 void btKinematicCharacterController::setJumpSpeed(btScalar jumpSpeed)
858 {
859 m_jumpSpeed = jumpSpeed;
860 m_SetjumpSpeed = m_jumpSpeed;
861 }
862
setMaxJumpHeight(btScalar maxJumpHeight)863 void btKinematicCharacterController::setMaxJumpHeight(btScalar maxJumpHeight)
864 {
865 m_maxJumpHeight = maxJumpHeight;
866 }
867
canJump() const868 bool btKinematicCharacterController::canJump() const
869 {
870 return onGround();
871 }
872
jump(const btVector3 & v)873 void btKinematicCharacterController::jump(const btVector3& v)
874 {
875 m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length();
876 m_verticalVelocity = m_jumpSpeed;
877 m_wasJumping = true;
878
879 m_jumpAxis = v.length2() == 0 ? m_up : v.normalized();
880
881 m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
882
883 #if 0
884 currently no jumping.
885 btTransform xform;
886 m_rigidBody->getMotionState()->getWorldTransform (xform);
887 btVector3 up = xform.getBasis()[1];
888 up.normalize ();
889 btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0);
890 m_rigidBody->applyCentralImpulse (up * magnitude);
891 #endif
892 }
893
setGravity(const btVector3 & gravity)894 void btKinematicCharacterController::setGravity(const btVector3& gravity)
895 {
896 if (gravity.length2() > 0) setUpVector(-gravity);
897
898 m_gravity = gravity.length();
899 }
900
getGravity() const901 btVector3 btKinematicCharacterController::getGravity() const
902 {
903 return -m_gravity * m_up;
904 }
905
setMaxSlope(btScalar slopeRadians)906 void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
907 {
908 m_maxSlopeRadians = slopeRadians;
909 m_maxSlopeCosine = btCos(slopeRadians);
910 }
911
getMaxSlope() const912 btScalar btKinematicCharacterController::getMaxSlope() const
913 {
914 return m_maxSlopeRadians;
915 }
916
setMaxPenetrationDepth(btScalar d)917 void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
918 {
919 m_maxPenetrationDepth = d;
920 }
921
getMaxPenetrationDepth() const922 btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
923 {
924 return m_maxPenetrationDepth;
925 }
926
onGround() const927 bool btKinematicCharacterController::onGround() const
928 {
929 return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
930 }
931
setStepHeight(btScalar h)932 void btKinematicCharacterController::setStepHeight(btScalar h)
933 {
934 m_stepHeight = h;
935 }
936
getUpAxisDirections()937 btVector3* btKinematicCharacterController::getUpAxisDirections()
938 {
939 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)};
940
941 return sUpAxisDirection;
942 }
943
debugDraw(btIDebugDraw * debugDrawer)944 void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
945 {
946 }
947
setUpInterpolate(bool value)948 void btKinematicCharacterController::setUpInterpolate(bool value)
949 {
950 m_interpolateUp = value;
951 }
952
setUp(const btVector3 & up)953 void btKinematicCharacterController::setUp(const btVector3& up)
954 {
955 if (up.length2() > 0 && m_gravity > 0.0f)
956 {
957 setGravity(-m_gravity * up.normalized());
958 return;
959 }
960
961 setUpVector(up);
962 }
963
setUpVector(const btVector3 & up)964 void btKinematicCharacterController::setUpVector(const btVector3& up)
965 {
966 if (m_up == up)
967 return;
968
969 btVector3 u = m_up;
970
971 if (up.length2() > 0)
972 m_up = up.normalized();
973 else
974 m_up = btVector3(0.0, 0.0, 0.0);
975
976 if (!m_ghostObject) return;
977 btQuaternion rot = getRotation(m_up, u);
978
979 //set orientation with new up
980 btTransform xform;
981 xform = m_ghostObject->getWorldTransform();
982 btQuaternion orn = rot.inverse() * xform.getRotation();
983 xform.setRotation(orn);
984 m_ghostObject->setWorldTransform(xform);
985 }
986
getRotation(btVector3 & v0,btVector3 & v1) const987 btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const
988 {
989 if (v0.length2() == 0.0f || v1.length2() == 0.0f)
990 {
991 btQuaternion q;
992 return q;
993 }
994
995 return shortestArcQuatNormalize2(v0, v1);
996 }
997