extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   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 //#define COMPUTE_IMPULSE_DENOM 1
  17 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
  18 //#define FORCE_REFESH_CONTACT_MANIFOLDS 1
  19
  20 #include "btSequentialImpulseConstraintSolver.h"
  21 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
  22 #include "BulletDynamics/Dynamics/btRigidBody.h"
  23 #include "btContactConstraint.h"
  24 #include "btSolve2LinearConstraint.h"
  25 #include "btContactSolverInfo.h"
  26 #include "LinearMath/btIDebugDraw.h"
  27 #include "btJacobianEntry.h"
  28 #include "LinearMath/btMinMax.h"
  29 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
  30 #include <new>
  31 #include "LinearMath/btStackAlloc.h"
  32 #include "LinearMath/btQuickprof.h"
  33 #include "btSolverBody.h"
  34 #include "btSolverConstraint.h"
  35
  36
  37 #include "LinearMath/btAlignedObjectArray.h"
  38
  39
  40 int totalCpd = 0;
  41
  42 int     gTotalContactPoints = 0;
  43
  44 struct  btOrderIndex
  45 {
  46         int     m_manifoldIndex;
  47         int     m_pointIndex;
  48 };
  49
  50
  51
  52 #define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
  53 static btOrderIndex     gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
  54
  55
  56 unsigned long btSequentialImpulseConstraintSolver::btRand2()
  57 {
  58   m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
  59   return m_btSeed2;
  60 }
  61
  62
  63
  64 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
  65 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
  66 {
  67   // seems good; xor-fold and modulus
  68   const unsigned long un = static_cast<unsigned long>(n);
  69   unsigned long r = btRand2();
  70
  71   // note: probably more aggressive than it needs to be -- might be
  72   //       able to get away without one or two of the innermost branches.
  73   if (un <= 0x00010000UL) {
  74     r ^= (r >> 16);
  75     if (un <= 0x00000100UL) {
  76       r ^= (r >> 8);
  77       if (un <= 0x00000010UL) {
  78         r ^= (r >> 4);
  79         if (un <= 0x00000004UL) {
  80           r ^= (r >> 2);
  81           if (un <= 0x00000002UL) {
  82             r ^= (r >> 1);
  83           }
  84         }
  85      }
  86     }
  87    }
  88
  89   return (int) (r % un);
  90 }
  91
  92
  93
  94
  95 bool  MyContactDestroyedCallback(void* userPersistentData);
  96 bool  MyContactDestroyedCallback(void* userPersistentData)
  97 {
  98         assert (userPersistentData);
  99         btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
 100         btAlignedFree(cpd);
 101         totalCpd--;
 102         //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
 103         return true;
 104 }
 105
 106
 107
 108 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
 109 :m_btSeed2(0)
 110 {
 111         gContactDestroyedCallback = &MyContactDestroyedCallback;
 112
 113         //initialize default friction/contact funcs
 114         int i,j;
 115         for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
 116                 for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
 117                 {
 118
 119                         m_contactDispatch[i][j] = resolveSingleCollision;
 120                         m_frictionDispatch[i][j] = resolveSingleFriction;
 121                 }
 122 }
 123
 124 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
 125 {
 126
 127 }
 128
 129 void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
 130 void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
 131 {
 132         btRigidBody* rb = btRigidBody::upcast(collisionObject);
 133         if (rb)
 134         {
 135                 solverBody->m_angularVelocity = rb->getAngularVelocity() ;
 136                 solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
 137                 solverBody->m_friction = collisionObject->getFriction();
 138                 solverBody->m_invMass = rb->getInvMass();
 139                 solverBody->m_linearVelocity = rb->getLinearVelocity();
 140                 solverBody->m_originalBody = rb;
 141                 solverBody->m_angularFactor = rb->getAngularFactor();
 142         } else
 143         {
 144                 solverBody->m_angularVelocity.setValue(0,0,0);
 145                 solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
 146                 solverBody->m_friction = collisionObject->getFriction();
 147                 solverBody->m_invMass = 0.f;
 148                 solverBody->m_linearVelocity.setValue(0,0,0);
 149                 solverBody->m_originalBody = 0;
 150                 solverBody->m_angularFactor = 1.f;
 151         }
 152
 153         solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
 154         solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
 155 }
 156
 157
 158 int             gNumSplitImpulseRecoveries = 0;
 159
 160 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
 161 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
 162 {
 163         btScalar rest = restitution * -rel_vel;
 164         return rest;
 165 }
 166
 167
 168 void    resolveSplitPenetrationImpulseCacheFriendly(
 169         btSolverBody& body1,
 170         btSolverBody& body2,
 171         const btSolverConstraint& contactConstraint,
 172         const btContactSolverInfo& solverInfo);
 173
 174 //SIMD_FORCE_INLINE
 175 void    resolveSplitPenetrationImpulseCacheFriendly(
 176         btSolverBody& body1,
 177         btSolverBody& body2,
 178         const btSolverConstraint& contactConstraint,
 179         const btContactSolverInfo& solverInfo)
 180 {
 181         (void)solverInfo;
 182
 183                 if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
 184         {
 185
 186                                 gNumSplitImpulseRecoveries++;
 187                 btScalar normalImpulse;
 188
 189                 //  Optimized version of projected relative velocity, use precomputed cross products with normal
 190                 //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
 191                 //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
 192                 //      btVector3 vel = vel1 - vel2;
 193                 //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
 194
 195                 btScalar rel_vel;
 196                 btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
 197                 + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
 198                 btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
 199                 + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
 200
 201                 rel_vel = vel1Dotn-vel2Dotn;
 202
 203
 204                                 btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
 205                 //      btScalar positionalError = contactConstraint.m_penetration;
 206
 207                 btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
 208
 209                 btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
 210                 btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
 211                 normalImpulse = penetrationImpulse+velocityImpulse;
 212
 213                 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
 214                 btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
 215                 btScalar sum = oldNormalImpulse + normalImpulse;
 216                 contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
 217
 218                 normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
 219
 220                                 body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
 221
 222                                 body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
 223
 224         }
 225
 226 }
 227
 228
 229 //velocity + friction
 230 //response  between two dynamic objects with friction
 231
 232 btScalar resolveSingleCollisionCombinedCacheFriendly(
 233         btSolverBody& body1,
 234         btSolverBody& body2,
 235         const btSolverConstraint& contactConstraint,
 236         const btContactSolverInfo& solverInfo);
 237
 238 //SIMD_FORCE_INLINE
 239 btScalar resolveSingleCollisionCombinedCacheFriendly(
 240         btSolverBody& body1,
 241         btSolverBody& body2,
 242         const btSolverConstraint& contactConstraint,
 243         const btContactSolverInfo& solverInfo)
 244 {
 245         (void)solverInfo;
 246
 247         btScalar normalImpulse;
 248
 249         {
 250
 251
 252                 //  Optimized version of projected relative velocity, use precomputed cross products with normal
 253                 //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
 254                 //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
 255                 //      btVector3 vel = vel1 - vel2;
 256                 //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
 257
 258                 btScalar rel_vel;
 259                 btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
 260                                         + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
 261                 btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
 262                                         + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
 263
 264                 rel_vel = vel1Dotn-vel2Dotn;
 265
 266                 btScalar positionalError = 0.f;
 267                 if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
 268                 {
 269                         positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
 270                 }
 271
 272                 btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
 273
 274                 btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
 275                 btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
 276                 normalImpulse = penetrationImpulse+velocityImpulse;
 277
 278
 279                 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
 280                 btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
 281                 btScalar sum = oldNormalImpulse + normalImpulse;
 282                 contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
 283
 284                 normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
 285
 286                 body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
 287                                 contactConstraint.m_angularComponentA,normalImpulse);
 288
 289                 body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
 290                                 contactConstraint.m_angularComponentB,-normalImpulse);
 291         }
 292
 293         return normalImpulse;
 294 }
 295
 296 //#define NO_FRICTION_TANGENTIALS 1
 297 #ifndef NO_FRICTION_TANGENTIALS
 298
 299 btScalar resolveSingleFrictionCacheFriendly(
 300         btSolverBody& body1,
 301         btSolverBody& body2,
 302         const btSolverConstraint& contactConstraint,
 303         const btContactSolverInfo& solverInfo,
 304         btScalar appliedNormalImpulse);
 305
 306 //SIMD_FORCE_INLINE
 307 btScalar resolveSingleFrictionCacheFriendly(
 308         btSolverBody& body1,
 309         btSolverBody& body2,
 310         const btSolverConstraint& contactConstraint,
 311         const btContactSolverInfo& solverInfo,
 312         btScalar appliedNormalImpulse)
 313 {
 314         (void)solverInfo;
 315
 316
 317         const btScalar combinedFriction = contactConstraint.m_friction;
 318
 319         const btScalar limit = appliedNormalImpulse * combinedFriction;
 320
 321         if (appliedNormalImpulse>btScalar(0.))
 322         //friction
 323         {
 324
 325                 btScalar j1;
 326                 {
 327
 328                         btScalar rel_vel;
 329                         const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
 330                                                 + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
 331                         const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
 332                                 + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
 333                         rel_vel = vel1Dotn-vel2Dotn;
 334
 335                         // calculate j that moves us to zero relative velocity
 336                         j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
 337 #define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
 338 #ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
 339                         btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
 340                         contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
 341
 342                         if (limit < contactConstraint.m_appliedImpulse)
 343                         {
 344                                 contactConstraint.m_appliedImpulse = limit;
 345                         } else
 346                         {
 347                                 if (contactConstraint.m_appliedImpulse < -limit)
 348                                         contactConstraint.m_appliedImpulse = -limit;
 349                         }
 350                         j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
 351 #else
 352                         if (limit < j1)
 353                         {
 354                                 j1 = limit;
 355                         } else
 356                         {
 357                                 if (j1 < -limit)
 358                                         j1 = -limit;
 359                         }
 360
 361 #endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
 362
 363                         //GEN_set_min(contactConstraint.m_appliedImpulse, limit);
 364                         //GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
 365
 366
 367
 368                 }
 369
 370                 body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
 371
 372                 body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
 373
 374         }
 375         return 0.f;
 376 }
 377
 378
 379 #else
 380
 381 //velocity + friction
 382 //response  between two dynamic objects with friction
 383 btScalar resolveSingleFrictionCacheFriendly(
 384         btSolverBody& body1,
 385         btSolverBody& body2,
 386         btSolverConstraint& contactConstraint,
 387         const btContactSolverInfo& solverInfo)
 388 {
 389
 390         btVector3 vel1;
 391         btVector3 vel2;
 392         btScalar normalImpulse(0.f);
 393
 394         {
 395                 const btVector3& normal = contactConstraint.m_contactNormal;
 396                 if (contactConstraint.m_penetration < 0.f)
 397                         return 0.f;
 398
 399
 400                 body1.getVelocityInLocalPoint(contactConstraint.m_relpos1CrossNormal,vel1);
 401                 body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
 402                 btVector3 vel = vel1 - vel2;
 403                 btScalar rel_vel;
 404                 rel_vel = normal.dot(vel);
 405
 406                 btVector3 lat_vel = vel - normal * rel_vel;
 407                 btScalar lat_rel_vel = lat_vel.length2();
 408
 409                 btScalar combinedFriction = contactConstraint.m_friction;
 410                 const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
 411                 const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
 412
 413
 414                 if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
 415                 {
 416                         lat_rel_vel = btSqrt(lat_rel_vel);
 417
 418                         lat_vel /= lat_rel_vel;
 419                         btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
 420                         btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
 421                         btScalar friction_impulse = lat_rel_vel /
 422                                 (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
 423                         btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
 424
 425                         btSetMin(friction_impulse, normal_impulse);
 426                         btSetMin(friction_impulse, -normal_impulse);
 427                         body1.internalApplyImpulse(lat_vel * -friction_impulse, rel_pos1);
 428                         body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
 429                 }
 430         }
 431
 432         return normalImpulse;
 433 }
 434
 435 #endif //NO_FRICTION_TANGENTIALS
 436
 437
 438
 439
 440
 441 void    btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
 442 {
 443
 444         btRigidBody* body0=btRigidBody::upcast(colObj0);
 445         btRigidBody* body1=btRigidBody::upcast(colObj1);
 446
 447         btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
 448         solverConstraint.m_contactNormal = normalAxis;
 449
 450         solverConstraint.m_solverBodyIdA = solverBodyIdA;
 451         solverConstraint.m_solverBodyIdB = solverBodyIdB;
 452         solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
 453         solverConstraint.m_frictionIndex = frictionIndex;
 454
 455         solverConstraint.m_friction = cp.m_combinedFriction;
 456         solverConstraint.m_originalContactPoint = 0;
 457
 458         solverConstraint.m_appliedImpulse = btScalar(0.);
 459         solverConstraint.m_appliedPushImpulse = 0.f;
 460         solverConstraint.m_penetration = 0.f;
 461         {
 462                 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
 463                 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
 464                 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
 465         }
 466         {
 467                 btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
 468                 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
 469                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
 470         }
 471
 472 #ifdef COMPUTE_IMPULSE_DENOM
 473         btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
 474         btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
 475 #else
 476         btVector3 vec;
 477         btScalar denom0 = 0.f;
 478         btScalar denom1 = 0.f;
 479         if (body0)
 480         {
 481                 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
 482                 denom0 = body0->getInvMass() + normalAxis.dot(vec);
 483         }
 484         if (body1)
 485         {
 486                 vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
 487                 denom1 = body1->getInvMass() + normalAxis.dot(vec);
 488         }
 489
 490
 491 #endif //COMPUTE_IMPULSE_DENOM
 492         btScalar denom = relaxation/(denom0+denom1);
 493         solverConstraint.m_jacDiagABInv = denom;
 494
 495
 496 }
 497
 498
 499 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
 500 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
 501 {
 502         if (colObj && colObj->hasAnisotropicFriction())
 503         {
 504                 // transform to local coordinates
 505                 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
 506                 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
 507                 //apply anisotropic friction
 508                 loc_lateral *= friction_scaling;
 509                 // ... and transform it back to global coordinates
 510                 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
 511         }
 512 }
 513
 514
 515
 516
 517 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
 518 {
 519         BT_PROFILE("solveGroupCacheFriendlySetup");
 520         (void)stackAlloc;
 521         (void)debugDrawer;
 522
 523
 524         if (!(numConstraints + numManifolds))
 525         {
 526 //              printf("empty\n");
 527                 return 0.f;
 528         }
 529         btPersistentManifold* manifold = 0;
 530         btCollisionObject* colObj0=0,*colObj1=0;
 531
 532         //btRigidBody* rb0=0,*rb1=0;
 533
 534
 535 #ifdef FORCE_REFESH_CONTACT_MANIFOLDS
 536
 537         BEGIN_PROFILE("refreshManifolds");
 538
 539         int i;
 540
 541
 542
 543         for (i=0;i<numManifolds;i++)
 544         {
 545                 manifold = manifoldPtr[i];
 546                 rb1 = (btRigidBody*)manifold->getBody1();
 547                 rb0 = (btRigidBody*)manifold->getBody0();
 548
 549                 manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
 550
 551         }
 552
 553         END_PROFILE("refreshManifolds");
 554 #endif //FORCE_REFESH_CONTACT_MANIFOLDS
 555
 556
 557
 558
 559
 560         //int sizeofSB = sizeof(btSolverBody);
 561         //int sizeofSC = sizeof(btSolverConstraint);
 562
 563
 564         //if (1)
 565         {
 566                 //if m_stackAlloc, try to pack bodies/constraints to speed up solving
 567 //              btBlock*                                        sablock;
 568 //              sablock = stackAlloc->beginBlock();
 569
 570         //      int memsize = 16;
 571 //              unsigned char* stackMemory = stackAlloc->allocate(memsize);
 572
 573
 574                 //todo: use stack allocator for this temp memory
 575 //              int minReservation = numManifolds*2;
 576
 577                 //m_tmpSolverBodyPool.reserve(minReservation);
 578
 579                 //don't convert all bodies, only the one we need so solver the constraints
 580 /*
 581                 {
 582                         for (int i=0;i<numBodies;i++)
 583                         {
 584                                 btRigidBody* rb = btRigidBody::upcast(bodies[i]);
 585                                 if (rb &&       (rb->getIslandTag() >= 0))
 586                                 {
 587                                         btAssert(rb->getCompanionId() < 0);
 588                                         int solverBodyId = m_tmpSolverBodyPool.size();
 589                                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
 590                                         initSolverBody(&solverBody,rb);
 591                                         rb->setCompanionId(solverBodyId);
 592                                 }
 593                         }
 594                 }
 595 */
 596
 597                 //m_tmpSolverConstraintPool.reserve(minReservation);
 598                 //m_tmpSolverFrictionConstraintPool.reserve(minReservation);
 599
 600                 {
 601                         int i;
 602
 603                         for (i=0;i<numManifolds;i++)
 604                         {
 605                                 manifold = manifoldPtr[i];
 606                                 colObj0 = (btCollisionObject*)manifold->getBody0();
 607                                 colObj1 = (btCollisionObject*)manifold->getBody1();
 608
 609                                 int solverBodyIdA=-1;
 610                                 int solverBodyIdB=-1;
 611
 612                                 if (manifold->getNumContacts())
 613                                 {
 614
 615
 616
 617                                         if (colObj0->getIslandTag() >= 0)
 618                                         {
 619                                                 if (colObj0->getCompanionId() >= 0)
 620                                                 {
 621                                                         //body has already been converted
 622                                                         solverBodyIdA = colObj0->getCompanionId();
 623                                                 } else
 624                                                 {
 625                                                         solverBodyIdA = m_tmpSolverBodyPool.size();
 626                                                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
 627                                                         initSolverBody(&solverBody,colObj0);
 628                                                         colObj0->setCompanionId(solverBodyIdA);
 629                                                 }
 630                                         } else
 631                                         {
 632                                                 //create a static body
 633                                                 solverBodyIdA = m_tmpSolverBodyPool.size();
 634                                                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
 635                                                 initSolverBody(&solverBody,colObj0);
 636                                         }
 637
 638                                         if (colObj1->getIslandTag() >= 0)
 639                                         {
 640                                                 if (colObj1->getCompanionId() >= 0)
 641                                                 {
 642                                                         solverBodyIdB = colObj1->getCompanionId();
 643                                                 } else
 644                                                 {
 645                                                         solverBodyIdB = m_tmpSolverBodyPool.size();
 646                                                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
 647                                                         initSolverBody(&solverBody,colObj1);
 648                                                         colObj1->setCompanionId(solverBodyIdB);
 649                                                 }
 650                                         } else
 651                                         {
 652                                                 //create a static body
 653                                                 solverBodyIdB = m_tmpSolverBodyPool.size();
 654                                                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
 655                                                 initSolverBody(&solverBody,colObj1);
 656                                         }
 657                                 }
 658
 659                                 btVector3 rel_pos1;
 660                                 btVector3 rel_pos2;
 661                                 btScalar relaxation;
 662
 663                                 for (int j=0;j<manifold->getNumContacts();j++)
 664                                 {
 665
 666                                         btManifoldPoint& cp = manifold->getContactPoint(j);
 667
 668                                         if (cp.getDistance() <= btScalar(0.))
 669                                         {
 670
 671                                                 const btVector3& pos1 = cp.getPositionWorldOnA();
 672                                                 const btVector3& pos2 = cp.getPositionWorldOnB();
 673
 674                                                  rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
 675                                                  rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
 676
 677
 678                                                 relaxation = 1.f;
 679                                                 btScalar rel_vel;
 680                                                 btVector3 vel;
 681
 682                                                 int frictionIndex = m_tmpSolverConstraintPool.size();
 683
 684                                                 {
 685                                                         btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
 686                                                         btRigidBody* rb0 = btRigidBody::upcast(colObj0);
 687                                                         btRigidBody* rb1 = btRigidBody::upcast(colObj1);
 688
 689                                                         solverConstraint.m_solverBodyIdA = solverBodyIdA;
 690                                                         solverConstraint.m_solverBodyIdB = solverBodyIdB;
 691                                                         solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
 692
 693                                                         solverConstraint.m_originalContactPoint = &cp;
 694
 695                                                         btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
 696                                                         solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
 697                                                         btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
 698                                                         solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
 699                                                         {
 700 #ifdef COMPUTE_IMPULSE_DENOM
 701                                                                 btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
 702                                                                 btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
 703 #else
 704                                                                 btVector3 vec;
 705                                                                 btScalar denom0 = 0.f;
 706                                                                 btScalar denom1 = 0.f;
 707                                                                 if (rb0)
 708                                                                 {
 709                                                                         vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
 710                                                                         denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
 711                                                                 }
 712                                                                 if (rb1)
 713                                                                 {
 714                                                                         vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
 715                                                                         denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
 716                                                                 }
 717 #endif //COMPUTE_IMPULSE_DENOM
 718
 719                                                                 btScalar denom = relaxation/(denom0+denom1);
 720                                                                 solverConstraint.m_jacDiagABInv = denom;
 721                                                         }
 722
 723                                                         solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
 724                                                         solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
 725                                                         solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
 726
 727
 728                                                         btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
 729                                                         btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
 730
 731                                                         vel  = vel1 - vel2;
 732
 733                                                         rel_vel = cp.m_normalWorldOnB.dot(vel);
 734
 735                                                         solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
 736                                                         //solverConstraint.m_penetration = cp.getDistance();
 737
 738                                                         solverConstraint.m_friction = cp.m_combinedFriction;
 739                                                         solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
 740                                                         if (solverConstraint.m_restitution <= btScalar(0.))
 741                                                         {
 742                                                                 solverConstraint.m_restitution = 0.f;
 743                                                         };
 744
 745
 746                                                         btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
 747
 748
 749
 750                                                         if (solverConstraint.m_restitution > penVel)
 751                                                         {
 752                                                                 solverConstraint.m_penetration = btScalar(0.);
 753                                                         }
 754
 755
 756
 757                                                         ///warm starting (or zero if disabled)
 758                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 759                                                         {
 760                                                                 solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
 761                                                                 if (rb0)
 762                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
 763                                                                 if (rb1)
 764                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
 765                                                         } else
 766                                                         {
 767                                                                 solverConstraint.m_appliedImpulse = 0.f;
 768                                                         }
 769
 770                                                         solverConstraint.m_appliedPushImpulse = 0.f;
 771
 772                                                         solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
 773                                                         if (!cp.m_lateralFrictionInitialized)
 774                                                         {
 775                                                                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
 776
 777                                                                 //scale anisotropic friction
 778
 779                                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
 780                                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
 781
 782                                                                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
 783
 784
 785                                                                 if (lat_rel_vel > SIMD_EPSILON)//0.0f)
 786                                                                 {
 787                                                                         cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
 788                                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 789                                                                         cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
 790                                                                         cp.m_lateralFrictionDir2.normalize();
 791                                                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
 792                                                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
 793
 794                                                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 795                                                                 } else
 796                                                                 {
 797                                                                         //re-calculate friction direction every frame, todo: check if this is really needed
 798                                                                         btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
 799                                                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
 800                                                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
 801                                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 802                                                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 803                                                                 }
 804                                                                 cp.m_lateralFrictionInitialized = true;
 805
 806                                                         } else
 807                                                         {
 808                                                                 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 809                                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 810                                                         }
 811
 812                                                         {
 813                                                                 btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
 814                                                                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 815                                                                 {
 816                                                                         frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
 817                                                                         if (rb0)
 818                                                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
 819                                                                         if (rb1)
 820                                                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
 821                                                                 } else
 822                                                                 {
 823                                                                         frictionConstraint1.m_appliedImpulse = 0.f;
 824                                                                 }
 825                                                         }
 826                                                         {
 827                                                                 btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
 828                                                                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 829                                                                 {
 830                                                                         frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
 831                                                                         if (rb0)
 832                                                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
 833                                                                         if (rb1)
 834                                                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
 835                                                                 } else
 836                                                                 {
 837                                                                         frictionConstraint2.m_appliedImpulse = 0.f;
 838                                                                 }
 839                                                         }
 840                                                 }
 841
 842
 843                                         }
 844                                 }
 845                         }
 846                 }
 847         }
 848
 849         btContactSolverInfo info = infoGlobal;
 850
 851         {
 852                 int j;
 853                 for (j=0;j<numConstraints;j++)
 854                 {
 855                         btTypedConstraint* constraint = constraints[j];
 856                         constraint->buildJacobian();
 857                 }
 858         }
 859
 860
 861
 862         int numConstraintPool = m_tmpSolverConstraintPool.size();
 863         int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
 864
 865         ///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
 866         m_orderTmpConstraintPool.resize(numConstraintPool);
 867         m_orderFrictionConstraintPool.resize(numFrictionPool);
 868         {
 869                 int i;
 870                 for (i=0;i<numConstraintPool;i++)
 871                 {
 872                         m_orderTmpConstraintPool[i] = i;
 873                 }
 874                 for (i=0;i<numFrictionPool;i++)
 875                 {
 876                         m_orderFrictionConstraintPool[i] = i;
 877                 }
 878         }
 879
 880
 881
 882         return 0.f;
 883
 884 }
 885
 886 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
 887 {
 888         BT_PROFILE("solveGroupCacheFriendlyIterations");
 889         int numConstraintPool = m_tmpSolverConstraintPool.size();
 890         int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
 891
 892         //should traverse the contacts random order...
 893         int iteration;
 894         {
 895                 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
 896                 {
 897
 898                         int j;
 899                         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 900                         {
 901                                 if ((iteration & 7) == 0) {
 902                                         for (j=0; j<numConstraintPool; ++j) {
 903                                                 int tmp = m_orderTmpConstraintPool[j];
 904                                                 int swapi = btRandInt2(j+1);
 905                                                 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
 906                                                 m_orderTmpConstraintPool[swapi] = tmp;
 907                                         }
 908
 909                                         for (j=0; j<numFrictionPool; ++j) {
 910                                                 int tmp = m_orderFrictionConstraintPool[j];
 911                                                 int swapi = btRandInt2(j+1);
 912                                                 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
 913                                                 m_orderFrictionConstraintPool[swapi] = tmp;
 914                                         }
 915                                 }
 916                         }
 917
 918                         for (j=0;j<numConstraints;j++)
 919                         {
 920                                 btTypedConstraint* constraint = constraints[j];
 921                                 ///todo: use solver bodies, so we don't need to copy from/to btRigidBody
 922
 923                                 if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
 924                                 {
 925                                         m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
 926                                 }
 927                                 if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
 928                                 {
 929                                         m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
 930                                 }
 931
 932                                 constraint->solveConstraint(infoGlobal.m_timeStep);
 933
 934                                 if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
 935                                 {
 936                                         m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
 937                                 }
 938                                 if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
 939                                 {
 940                                         m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
 941                                 }
 942
 943                         }
 944
 945                         {
 946                                 int numPoolConstraints = m_tmpSolverConstraintPool.size();
 947                                 for (j=0;j<numPoolConstraints;j++)
 948                                 {
 949
 950                                         const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
 951                                         resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
 952                                                 m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
 953                                 }
 954                         }
 955
 956                         {
 957                                  int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
 958
 959                                  for (j=0;j<numFrictionPoolConstraints;j++)
 960                                 {
 961                                         const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
 962                                         btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
 963                                                                 m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;
 964
 965                                                 resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
 966                                                         m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
 967                                                         totalImpulse);
 968                                 }
 969                         }
 970
 971
 972
 973                 }
 974
 975                 if (infoGlobal.m_splitImpulse)
 976                 {
 977
 978                         for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
 979                         {
 980                                 {
 981                                         int numPoolConstraints = m_tmpSolverConstraintPool.size();
 982                                         int j;
 983                                         for (j=0;j<numPoolConstraints;j++)
 984                                         {
 985                                                 const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
 986
 987                                                 resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
 988                                                         m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
 989                                         }
 990                                 }
 991                         }
 992
 993                 }
 994
 995         }
 996
 997         return 0.f;
 998 }
 999
1000
1001 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1002 {
1003         int i;
1004
1005         solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1006         solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1007
1008         int numPoolConstraints = m_tmpSolverConstraintPool.size();
1009         int j;
1010         for (j=0;j<numPoolConstraints;j++)
1011         {
1012
1013                 const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
1014                 btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1015                 btAssert(pt);
1016                 pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1017                 pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1018                 pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1019
1020                 //do a callback here?
1021
1022         }
1023
1024         if (infoGlobal.m_splitImpulse)
1025         {
1026                 for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1027                 {
1028                         m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
1029                 }
1030         } else
1031         {
1032                 for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1033         {
1034                 m_tmpSolverBodyPool[i].writebackVelocity();
1035         }
1036         }
1037
1038 //      printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
1039
1040 /*
1041         printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
1042         printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
1043         printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
1044
1045
1046         printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
1047         printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
1048         printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
1049 */
1050
1051         m_tmpSolverBodyPool.resize(0);
1052         m_tmpSolverConstraintPool.resize(0);
1053         m_tmpSolverFrictionConstraintPool.resize(0);
1054
1055
1056         return 0.f;
1057 }
1058
1059 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
1060 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
1061 {
1062         BT_PROFILE("solveGroup");
1063         if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
1064         {
1065                 //you need to provide at least some bodies
1066                 //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
1067                 btAssert(bodies);
1068                 btAssert(numBodies);
1069                 return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1070         }
1071
1072
1073
1074         btContactSolverInfo info = infoGlobal;
1075
1076         int numiter = infoGlobal.m_numIterations;
1077
1078         int totalPoints = 0;
1079
1080
1081         {
1082                 short j;
1083                 for (j=0;j<numManifolds;j++)
1084                 {
1085                         btPersistentManifold* manifold = manifoldPtr[j];
1086                         prepareConstraints(manifold,info,debugDrawer);
1087
1088                         for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
1089                         {
1090                                 gOrder[totalPoints].m_manifoldIndex = j;
1091                                 gOrder[totalPoints].m_pointIndex = p;
1092                                 totalPoints++;
1093                         }
1094                 }
1095         }
1096
1097         {
1098                 int j;
1099                 for (j=0;j<numConstraints;j++)
1100                 {
1101                         btTypedConstraint* constraint = constraints[j];
1102                         constraint->buildJacobian();
1103                 }
1104         }
1105
1106
1107         //should traverse the contacts random order...
1108         int iteration;
1109
1110         {
1111                 for ( iteration = 0;iteration<numiter;iteration++)
1112                 {
1113                         int j;
1114                         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1115                         {
1116                                 if ((iteration & 7) == 0) {
1117                                         for (j=0; j<totalPoints; ++j) {
1118                                                 btOrderIndex tmp = gOrder[j];
1119                                                 int swapi = btRandInt2(j+1);
1120                                                 gOrder[j] = gOrder[swapi];
1121                                                 gOrder[swapi] = tmp;
1122                                         }
1123                                 }
1124                         }
1125
1126                         for (j=0;j<numConstraints;j++)
1127                         {
1128                                 btTypedConstraint* constraint = constraints[j];
1129                                 constraint->solveConstraint(info.m_timeStep);
1130                         }
1131
1132                         for (j=0;j<totalPoints;j++)
1133                         {
1134                                 btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
1135                                 solve( (btRigidBody*)manifold->getBody0(),
1136                                                                         (btRigidBody*)manifold->getBody1()
1137                                 ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
1138                         }
1139
1140                         for (j=0;j<totalPoints;j++)
1141                         {
1142                                 btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
1143                                 solveFriction((btRigidBody*)manifold->getBody0(),
1144                                         (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
1145                         }
1146
1147                 }
1148         }
1149
1150
1151
1152
1153         return btScalar(0.);
1154 }
1155
1156
1157
1158
1159
1160
1161
1162 void    btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
1163 {
1164
1165         (void)debugDrawer;
1166
1167         btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
1168         btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
1169
1170
1171         //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
1172         {
1173 #ifdef FORCE_REFESH_CONTACT_MANIFOLDS
1174                 manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
1175 #endif //FORCE_REFESH_CONTACT_MANIFOLDS
1176                 int numpoints = manifoldPtr->getNumContacts();
1177
1178                 gTotalContactPoints += numpoints;
1179
1180
1181                 for (int i=0;i<numpoints ;i++)
1182                 {
1183                         btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
1184                         if (cp.getDistance() <= btScalar(0.))
1185                         {
1186                                 const btVector3& pos1 = cp.getPositionWorldOnA();
1187                                 const btVector3& pos2 = cp.getPositionWorldOnB();
1188
1189                                 btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition();
1190                                 btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
1191
1192
1193                                 //this jacobian entry is re-used for all iterations
1194                                 btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
1195                                         body1->getCenterOfMassTransform().getBasis().transpose(),
1196                                         rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
1197                                         body1->getInvInertiaDiagLocal(),body1->getInvMass());
1198
1199
1200                                 btScalar jacDiagAB = jac.getDiagonal();
1201
1202                                 btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1203                                 if (cpd)
1204                                 {
1205                                         //might be invalid
1206                                         cpd->m_persistentLifeTime++;
1207                                         if (cpd->m_persistentLifeTime != cp.getLifeTime())
1208                                         {
1209                                                 //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
1210                                                 new (cpd) btConstraintPersistentData;
1211                                                 cpd->m_persistentLifeTime = cp.getLifeTime();
1212
1213                                         } else
1214                                         {
1215                                                 //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
1216
1217                                         }
1218                                 } else
1219                                 {
1220
1221                                         //todo: should this be in a pool?
1222                                         void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
1223                                         cpd = new (mem)btConstraintPersistentData;
1224                                         assert(cpd);
1225
1226                                         totalCpd ++;
1227                                         //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
1228                                         cp.m_userPersistentData = cpd;
1229                                         cpd->m_persistentLifeTime = cp.getLifeTime();
1230                                         //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
1231
1232                                 }
1233                                 assert(cpd);
1234
1235                                 cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
1236
1237                                 //Dependent on Rigidbody A and B types, fetch the contact/friction response func
1238                                 //perhaps do a similar thing for friction/restutution combiner funcs...
1239
1240                                 cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
1241                                 cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
1242
1243                                 btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
1244                                 btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
1245                                 btVector3 vel = vel1 - vel2;
1246                                 btScalar rel_vel;
1247                                 rel_vel = cp.m_normalWorldOnB.dot(vel);
1248
1249                                 btScalar combinedRestitution = cp.m_combinedRestitution;
1250
1251                                 cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
1252                                 cpd->m_friction = cp.m_combinedFriction;
1253                                 cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
1254                                 if (cpd->m_restitution <= btScalar(0.))
1255                                 {
1256                                         cpd->m_restitution = btScalar(0.0);
1257
1258                                 };
1259
1260                                 //restitution and penetration work in same direction so
1261                                 //rel_vel
1262
1263                                 btScalar penVel = -cpd->m_penetration/info.m_timeStep;
1264
1265                                 if (cpd->m_restitution > penVel)
1266                                 {
1267                                         cpd->m_penetration = btScalar(0.);
1268                                 }
1269
1270
1271                                 btScalar relaxation = info.m_damping;
1272                                 if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
1273                                 {
1274                                         cpd->m_appliedImpulse *= relaxation;
1275                                 } else
1276                                 {
1277                                         cpd->m_appliedImpulse =btScalar(0.);
1278                                 }
1279
1280                                 //for friction
1281                                 cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
1282
1283                                 //re-calculate friction direction every frame, todo: check if this is really needed
1284                                 btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
1285
1286
1287 #define NO_FRICTION_WARMSTART 1
1288
1289         #ifdef NO_FRICTION_WARMSTART
1290                                 cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
1291                                 cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
1292         #endif //NO_FRICTION_WARMSTART
1293                                 btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
1294                                 btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
1295                                 btScalar denom = relaxation/(denom0+denom1);
1296                                 cpd->m_jacDiagABInvTangent0 = denom;
1297
1298
1299                                 denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
1300                                 denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
1301                                 denom = relaxation/(denom0+denom1);
1302                                 cpd->m_jacDiagABInvTangent1 = denom;
1303
1304                                 btVector3 totalImpulse =
1305         #ifndef NO_FRICTION_WARMSTART
1306                                         cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
1307                                         cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
1308         #endif //NO_FRICTION_WARMSTART
1309                                         cp.m_normalWorldOnB*cpd->m_appliedImpulse;
1310
1311
1312
1313                                 ///
1314                                 {
1315                                 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
1316                                 cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
1317                                 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
1318                                 cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
1319                                 }
1320                                 {
1321                                         btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
1322                                         cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
1323                                 }
1324                                 {
1325                                         btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
1326                                         cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
1327                                 }
1328                                 {
1329                                         btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
1330                                         cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
1331                                 }
1332                                 {
1333                                         btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
1334                                         cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
1335                                 }
1336
1337                                 ///
1338
1339
1340
1341                                 //apply previous frames impulse on both bodies
1342                                 body0->applyImpulse(totalImpulse, rel_pos1);
1343                                 body1->applyImpulse(-totalImpulse, rel_pos2);
1344                         }
1345
1346                 }
1347         }
1348 }
1349
1350
1351 btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
1352 {
1353         btScalar maxImpulse = btScalar(0.);
1354
1355         {
1356
1357
1358                 {
1359                         if (cp.getDistance() <= btScalar(0.))
1360                         {
1361
1362
1363
1364                                 {
1365
1366                                         //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1367                                         btScalar impulse = resolveSingleCollisionCombined(
1368                                                 *body0,*body1,
1369                                                 cp,
1370                                                 info);
1371
1372                                         if (maxImpulse < impulse)
1373                                                 maxImpulse  = impulse;
1374
1375                                 }
1376                         }
1377                 }
1378         }
1379         return maxImpulse;
1380 }
1381
1382
1383
1384 btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
1385 {
1386
1387         btScalar maxImpulse = btScalar(0.);
1388
1389         {
1390
1391
1392                 {
1393                         if (cp.getDistance() <= btScalar(0.))
1394                         {
1395
1396
1397
1398                                 {
1399
1400                                         btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1401                                         btScalar impulse = cpd->m_contactSolverFunc(
1402                                                 *body0,*body1,
1403                                                 cp,
1404                                                 info);
1405
1406                                         if (maxImpulse < impulse)
1407                                                 maxImpulse  = impulse;
1408
1409                                 }
1410                         }
1411                 }
1412         }
1413         return maxImpulse;
1414 }
1415
1416 btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
1417 {
1418
1419         (void)debugDrawer;
1420         (void)iter;
1421
1422
1423         {
1424
1425
1426                 {
1427
1428                         if (cp.getDistance() <= btScalar(0.))
1429                         {
1430
1431                                 btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1432                                 cpd->m_frictionSolverFunc(
1433                                         *body0,*body1,
1434                                         cp,
1435                                         info);
1436
1437
1438                         }
1439                 }
1440
1441
1442         }
1443         return btScalar(0.);
1444 }
1445
1446
1447 void    btSequentialImpulseConstraintSolver::reset()
1448 {
1449         m_btSeed2 = 0;
1450 }
1451
1452