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