dali-physics/third-party/bullet3/src/BulletDynamics/Character/btKinematicCharacterController.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2008 Erwin Coumans  http://bulletphysics.com
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #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
  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:
  48         btKinematicClosestNotMeRayResultCallback(btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
  49         {
  50                 m_me = me;
  51         }
  52
  53         virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
  54         {
  55                 if (rayResult.m_collisionObject == m_me)
  56                         return 1.0;
  57
  58                 return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
  59         }
  60
  61 protected:
  62         btCollisionObject* m_me;
  63 };
  64
  65 class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
  66 {
  67 public:
  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
  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  */
 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  */
 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  */
 129 btVector3 btKinematicCharacterController::perpindicularComponent(const btVector3& direction, const btVector3& normal)
 130 {
 131         return direction - parallelComponent(direction, normal);
 132 }
 133
 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
 168 btKinematicCharacterController::~btKinematicCharacterController()
 169 {
 170 }
 171
 172 btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
 173 {
 174         return m_ghostObject;
 175 }
 176
 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
 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
 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
 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
 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
 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
 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
 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
 633 void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity)
 634 {
 635         m_AngVel = velocity;
 636 }
 637
 638 const btVector3& btKinematicCharacterController::getAngularVelocity() const
 639 {
 640         return m_AngVel;
 641 }
 642
 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
 670 btVector3 btKinematicCharacterController::getLinearVelocity() const
 671 {
 672         return m_walkDirection + (m_verticalVelocity * m_up);
 673 }
 674
 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
 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
 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
 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
 852 void btKinematicCharacterController::setFallSpeed(btScalar fallSpeed)
 853 {
 854         m_fallSpeed = fallSpeed;
 855 }
 856
 857 void btKinematicCharacterController::setJumpSpeed(btScalar jumpSpeed)
 858 {
 859         m_jumpSpeed = jumpSpeed;
 860         m_SetjumpSpeed = m_jumpSpeed;
 861 }
 862
 863 void btKinematicCharacterController::setMaxJumpHeight(btScalar maxJumpHeight)
 864 {
 865         m_maxJumpHeight = maxJumpHeight;
 866 }
 867
 868 bool btKinematicCharacterController::canJump() const
 869 {
 870         return onGround();
 871 }
 872
 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
 894 void btKinematicCharacterController::setGravity(const btVector3& gravity)
 895 {
 896         if (gravity.length2() > 0) setUpVector(-gravity);
 897
 898         m_gravity = gravity.length();
 899 }
 900
 901 btVector3 btKinematicCharacterController::getGravity() const
 902 {
 903         return -m_gravity * m_up;
 904 }
 905
 906 void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
 907 {
 908         m_maxSlopeRadians = slopeRadians;
 909         m_maxSlopeCosine = btCos(slopeRadians);
 910 }
 911
 912 btScalar btKinematicCharacterController::getMaxSlope() const
 913 {
 914         return m_maxSlopeRadians;
 915 }
 916
 917 void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
 918 {
 919         m_maxPenetrationDepth = d;
 920 }
 921
 922 btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
 923 {
 924         return m_maxPenetrationDepth;
 925 }
 926
 927 bool btKinematicCharacterController::onGround() const
 928 {
 929         return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
 930 }
 931
 932 void btKinematicCharacterController::setStepHeight(btScalar h)
 933 {
 934         m_stepHeight = h;
 935 }
 936
 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
 944 void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
 945 {
 946 }
 947
 948 void btKinematicCharacterController::setUpInterpolate(bool value)
 949 {
 950         m_interpolateUp = value;
 951 }
 952
 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
 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
 987 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 }