125
void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody)
127
/* int size = sizeof(btSolverBody);
128
int sizeofrb = sizeof(btRigidBody);
129
int sizemanifold = sizeof(btPersistentManifold);
130
int sizeofmp = sizeof(btManifoldPoint);
131
int sizeofPersistData = sizeof (btConstraintPersistentData);
134
solverBody->m_angularVelocity = rigidbody->getAngularVelocity();
135
solverBody->m_centerOfMassPosition = rigidbody->getCenterOfMassPosition();
136
solverBody->m_friction = rigidbody->getFriction();
137
// solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld();
138
solverBody->m_invMass = rigidbody->getInvMass();
139
solverBody->m_linearVelocity = rigidbody->getLinearVelocity();
140
solverBody->m_originalBody = rigidbody;
141
solverBody->m_angularFactor = rigidbody->getAngularFactor();
144
btScalar penetrationResolveFactor = btScalar(0.9);
124
btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
129
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
130
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
132
btRigidBody* rb = btRigidBody::upcast(collisionObject);
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();
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;
153
solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
154
solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
158
int gNumSplitImpulseRecoveries = 0;
160
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
145
161
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
147
163
btScalar rest = restitution * -rel_vel;
168
void resolveSplitPenetrationImpulseCacheFriendly(
171
const btSolverConstraint& contactConstraint,
172
const btContactSolverInfo& solverInfo);
175
void resolveSplitPenetrationImpulseCacheFriendly(
178
const btSolverConstraint& contactConstraint,
179
const btContactSolverInfo& solverInfo)
183
if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
186
gNumSplitImpulseRecoveries++;
187
btScalar normalImpulse;
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);
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);
201
rel_vel = vel1Dotn-vel2Dotn;
204
btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
205
// btScalar positionalError = contactConstraint.m_penetration;
207
btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
209
btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
210
btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
211
normalImpulse = penetrationImpulse+velocityImpulse;
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;
218
normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
220
body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
222
body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
156
229
//velocity + friction
157
230
//response between two dynamic objects with friction
158
SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly(
161
btSolverConstraint& contactConstraint,
232
btScalar resolveSingleCollisionCombinedCacheFriendly(
235
const btSolverConstraint& contactConstraint,
236
const btContactSolverInfo& solverInfo);
239
btScalar resolveSingleCollisionCombinedCacheFriendly(
242
const btSolverConstraint& contactConstraint,
162
243
const btContactSolverInfo& solverInfo)
164
245
(void)solverInfo;
166
btScalar normalImpulse(0.f);
247
btScalar normalImpulse;
168
if (contactConstraint.m_penetration < 0.f)
171
// Optimized version of projected relative velocity, use precomputed cross products with normal
172
// body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
173
// body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
174
// btVector3 vel = vel1 - vel2;
175
// btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel);
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);
177
258
btScalar rel_vel;
178
259
btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
183
264
rel_vel = vel1Dotn-vel2Dotn;
266
btScalar positionalError = 0.f;
267
if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
269
positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
186
btScalar positionalError = contactConstraint.m_penetration;
187
272
btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
189
274
btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
190
275
btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
191
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
276
normalImpulse = penetrationImpulse+velocityImpulse;
193
279
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
194
280
btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
195
281
btScalar sum = oldNormalImpulse + normalImpulse;
196
282
contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
198
btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse;
199
btScalar velocitySum = oldVelocityImpulse + velocityImpulse;
200
contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum;
202
284
normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
206
body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
286
body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
207
287
contactConstraint.m_angularComponentA,normalImpulse);
211
body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
289
body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
212
290
contactConstraint.m_angularComponentB,-normalImpulse);
219
293
return normalImpulse;
296
//#define NO_FRICTION_TANGENTIALS 1
223
297
#ifndef NO_FRICTION_TANGENTIALS
225
SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly(
228
btSolverConstraint& contactConstraint,
299
btScalar resolveSingleFrictionCacheFriendly(
302
const btSolverConstraint& contactConstraint,
303
const btContactSolverInfo& solverInfo,
304
btScalar appliedNormalImpulse);
307
btScalar resolveSingleFrictionCacheFriendly(
310
const btSolverConstraint& contactConstraint,
229
311
const btContactSolverInfo& solverInfo,
230
312
btScalar appliedNormalImpulse)
334
435
#endif //NO_FRICTION_TANGENTIALS
336
btAlignedObjectArray<btSolverBody> tmpSolverBodyPool;
337
btAlignedObjectArray<btSolverConstraint> tmpSolverConstraintPool;
338
btAlignedObjectArray<btSolverConstraint> tmpSolverFrictionConstraintPool;
341
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
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)
444
btRigidBody* body0=btRigidBody::upcast(colObj0);
445
btRigidBody* body1=btRigidBody::upcast(colObj1);
447
btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
448
solverConstraint.m_contactNormal = normalAxis;
450
solverConstraint.m_solverBodyIdA = solverBodyIdA;
451
solverConstraint.m_solverBodyIdB = solverBodyIdB;
452
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
453
solverConstraint.m_frictionIndex = frictionIndex;
455
solverConstraint.m_friction = cp.m_combinedFriction;
456
solverConstraint.m_originalContactPoint = 0;
458
solverConstraint.m_appliedImpulse = btScalar(0.);
459
solverConstraint.m_appliedPushImpulse = 0.f;
460
solverConstraint.m_penetration = 0.f;
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);
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);
472
#ifdef COMPUTE_IMPULSE_DENOM
473
btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
474
btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
477
btScalar denom0 = 0.f;
478
btScalar denom1 = 0.f;
481
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
482
denom0 = body0->getInvMass() + normalAxis.dot(vec);
486
vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
487
denom1 = body1->getInvMass() + normalAxis.dot(vec);
491
#endif //COMPUTE_IMPULSE_DENOM
492
btScalar denom = relaxation/(denom0+denom1);
493
solverConstraint.m_jacDiagABInv = denom;
499
void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
500
void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
502
if (colObj && colObj->hasAnisotropicFriction())
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;
517
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
519
BT_PROFILE("solveGroupCacheFriendlySetup");
343
520
(void)stackAlloc;
344
521
(void)debugDrawer;
346
524
if (!(numConstraints + numManifolds))
348
526
// printf("empty\n");
529
btPersistentManifold* manifold = 0;
530
btCollisionObject* colObj0=0,*colObj1=0;
532
//btRigidBody* rb0=0,*rb1=0;
535
#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
352
537
BEGIN_PROFILE("refreshManifolds");
355
543
for (i=0;i<numManifolds;i++)
357
btPersistentManifold* manifold = manifoldPtr[i];
358
btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
359
btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
545
manifold = manifoldPtr[i];
546
rb1 = (btRigidBody*)manifold->getBody1();
547
rb0 = (btRigidBody*)manifold->getBody0();
361
549
manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
365
553
END_PROFILE("refreshManifolds");
368
BEGIN_PROFILE("gatherSolverData");
554
#endif //FORCE_REFESH_CONTACT_MANIFOLDS
370
560
//int sizeofSB = sizeof(btSolverBody);
371
561
//int sizeofSC = sizeof(btSolverConstraint);
425
if (rb0->getIslandTag() >= 0)
617
if (colObj0->getIslandTag() >= 0)
427
solverBodyIdA = rb0->getCompanionId();
619
if (colObj0->getCompanionId() >= 0)
621
//body has already been converted
622
solverBodyIdA = colObj0->getCompanionId();
625
solverBodyIdA = m_tmpSolverBodyPool.size();
626
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
627
initSolverBody(&solverBody,colObj0);
628
colObj0->setCompanionId(solverBodyIdA);
430
632
//create a static body
431
solverBodyIdA = tmpSolverBodyPool.size();
432
btSolverBody& solverBody = tmpSolverBodyPool.expand();
433
initSolverBody(&solverBody,rb0);
633
solverBodyIdA = m_tmpSolverBodyPool.size();
634
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
635
initSolverBody(&solverBody,colObj0);
436
if (rb1->getIslandTag() >= 0)
638
if (colObj1->getIslandTag() >= 0)
438
solverBodyIdB = rb1->getCompanionId();
640
if (colObj1->getCompanionId() >= 0)
642
solverBodyIdB = colObj1->getCompanionId();
645
solverBodyIdB = m_tmpSolverBodyPool.size();
646
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
647
initSolverBody(&solverBody,colObj1);
648
colObj1->setCompanionId(solverBodyIdB);
441
652
//create a static body
442
solverBodyIdB = tmpSolverBodyPool.size();
443
btSolverBody& solverBody = tmpSolverBodyPool.expand();
444
initSolverBody(&solverBody,rb1);
653
solverBodyIdB = m_tmpSolverBodyPool.size();
654
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
655
initSolverBody(&solverBody,colObj1);
448
663
for (int j=0;j<manifold->getNumContacts();j++)
451
666
btManifoldPoint& cp = manifold->getContactPoint(j);
453
int frictionIndex = tmpSolverConstraintPool.size();
455
668
if (cp.getDistance() <= btScalar(0.))
458
671
const btVector3& pos1 = cp.getPositionWorldOnA();
459
672
const btVector3& pos2 = cp.getPositionWorldOnB();
461
btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition();
462
btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition();
674
rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
675
rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
465
btScalar relaxation = 1.f;
682
int frictionIndex = m_tmpSolverConstraintPool.size();
468
btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand();
685
btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
686
btRigidBody* rb0 = btRigidBody::upcast(colObj0);
687
btRigidBody* rb1 = btRigidBody::upcast(colObj1);
470
689
solverConstraint.m_solverBodyIdA = solverBodyIdA;
471
690
solverConstraint.m_solverBodyIdB = solverBodyIdB;
472
691
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
693
solverConstraint.m_originalContactPoint = &cp;
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);
477
//can be optimized, the cross products are already calculated
700
#ifdef COMPUTE_IMPULSE_DENOM
478
701
btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
479
702
btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
705
btScalar denom0 = 0.f;
706
btScalar denom1 = 0.f;
709
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
710
denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
714
vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
715
denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
717
#endif //COMPUTE_IMPULSE_DENOM
480
719
btScalar denom = relaxation/(denom0+denom1);
481
720
solverConstraint.m_jacDiagABInv = denom;
486
725
solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
489
btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1);
490
btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2);
728
btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
729
btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
492
btVector3 vel = vel1 - vel2;
494
733
rel_vel = cp.m_normalWorldOnB.dot(vel);
735
solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
736
//solverConstraint.m_penetration = cp.getDistance();
497
solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
498
738
solverConstraint.m_friction = cp.m_combinedFriction;
499
btScalar rest = restitutionCurve(rel_vel, cp.m_combinedRestitution);
500
if (rest <= btScalar(0.))
739
solverConstraint.m_restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
740
if (solverConstraint.m_restitution <= btScalar(0.))
742
solverConstraint.m_restitution = 0.f;
505
746
btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
510
solverConstraint.m_restitution = rest;
512
solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep);
514
solverConstraint.m_appliedImpulse = 0.f;
515
solverConstraint.m_appliedVelocityImpulse = 0.f;
518
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
519
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0;
520
btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
521
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1;
524
//create 2 '1d axis' constraints for 2 tangential friction directions
526
//re-calculate friction direction every frame, todo: check if this is really needed
527
btVector3 frictionTangential0a, frictionTangential1b;
529
btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b);
532
btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
533
solverConstraint.m_contactNormal = frictionTangential0a;
535
solverConstraint.m_solverBodyIdA = solverBodyIdA;
536
solverConstraint.m_solverBodyIdB = solverBodyIdB;
537
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
538
solverConstraint.m_frictionIndex = frictionIndex;
540
solverConstraint.m_friction = cp.m_combinedFriction;
542
solverConstraint.m_appliedImpulse = btScalar(0.);
543
solverConstraint.m_appliedVelocityImpulse = 0.f;
545
btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
546
btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
547
btScalar denom = relaxation/(denom0+denom1);
548
solverConstraint.m_jacDiagABInv = denom;
551
btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal);
552
solverConstraint.m_relpos1CrossNormal = ftorqueAxis0;
553
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0;
556
btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal);
557
solverConstraint.m_relpos2CrossNormal = ftorqueAxis0;
558
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0;
566
btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
567
solverConstraint.m_contactNormal = frictionTangential1b;
569
solverConstraint.m_solverBodyIdA = solverBodyIdA;
570
solverConstraint.m_solverBodyIdB = solverBodyIdB;
571
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
572
solverConstraint.m_frictionIndex = frictionIndex;
574
solverConstraint.m_friction = cp.m_combinedFriction;
576
solverConstraint.m_appliedImpulse = btScalar(0.);
577
solverConstraint.m_appliedVelocityImpulse = 0.f;
579
btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
580
btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
581
btScalar denom = relaxation/(denom0+denom1);
582
solverConstraint.m_jacDiagABInv = denom;
584
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
585
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
586
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1;
589
btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
590
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
591
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1;
750
if (solverConstraint.m_restitution > penVel)
752
solverConstraint.m_penetration = btScalar(0.);
757
///warm starting (or zero if disabled)
758
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
760
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
762
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
764
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
767
solverConstraint.m_appliedImpulse = 0.f;
770
solverConstraint.m_appliedPushImpulse = 0.f;
772
solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
773
if (!cp.m_lateralFrictionInitialized)
775
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
777
//scale anisotropic friction
779
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
780
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
782
btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
785
if (lat_rel_vel > SIMD_EPSILON)//0.0f)
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);
794
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
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);
804
cp.m_lateralFrictionInitialized = true;
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);
813
btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
814
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
816
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
818
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
820
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
823
frictionConstraint1.m_appliedImpulse = 0.f;
827
btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
828
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
830
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
832
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
834
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
837
frictionConstraint2.m_appliedImpulse = 0.f;
600
END_PROFILE("gatherSolverData");
602
BEGIN_PROFILE("prepareConstraints");
604
849
btContactSolverInfo info = infoGlobal;
615
btAlignedObjectArray<int> gOrderTmpConstraintPool;
616
btAlignedObjectArray<int> gOrderFrictionConstraintPool;
618
int numConstraintPool = tmpSolverConstraintPool.size();
619
int numFrictionPool = tmpSolverFrictionConstraintPool.size();
862
int numConstraintPool = m_tmpSolverConstraintPool.size();
863
int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
621
865
///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
622
gOrderTmpConstraintPool.resize(numConstraintPool);
623
gOrderFrictionConstraintPool.resize(numFrictionPool);
866
m_orderTmpConstraintPool.resize(numConstraintPool);
867
m_orderFrictionConstraintPool.resize(numFrictionPool);
626
870
for (i=0;i<numConstraintPool;i++)
628
gOrderTmpConstraintPool[i] = i;
872
m_orderTmpConstraintPool[i] = i;
630
874
for (i=0;i<numFrictionPool;i++)
632
gOrderFrictionConstraintPool[i] = i;
876
m_orderFrictionConstraintPool[i] = i;
639
END_PROFILE("prepareConstraints");
642
BEGIN_PROFILE("solveConstraints");
886
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
888
BT_PROFILE("solveGroupCacheFriendlyIterations");
889
int numConstraintPool = m_tmpSolverConstraintPool.size();
890
int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
644
892
//should traverse the contacts random order...
647
for ( iteration = 0;iteration<info.m_numIterations;iteration++)
895
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
651
if (m_solverMode & SOLVER_RANDMIZE_ORDER)
899
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
653
901
if ((iteration & 7) == 0) {
654
902
for (j=0; j<numConstraintPool; ++j) {
655
int tmp = gOrderTmpConstraintPool[j];
903
int tmp = m_orderTmpConstraintPool[j];
656
904
int swapi = btRandInt2(j+1);
657
gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi];
658
gOrderTmpConstraintPool[swapi] = tmp;
905
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
906
m_orderTmpConstraintPool[swapi] = tmp;
661
909
for (j=0; j<numFrictionPool; ++j) {
662
int tmp = gOrderFrictionConstraintPool[j];
910
int tmp = m_orderFrictionConstraintPool[j];
663
911
int swapi = btRandInt2(j+1);
664
gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi];
665
gOrderFrictionConstraintPool[swapi] = tmp;
912
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
913
m_orderFrictionConstraintPool[swapi] = tmp;
675
923
if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
677
tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
925
m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
679
927
if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
681
tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
929
m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
684
constraint->solveConstraint(info.m_timeStep);
932
constraint->solveConstraint(infoGlobal.m_timeStep);
686
934
if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
688
tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
936
m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
690
938
if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
692
tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
940
m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
698
int numPoolConstraints = tmpSolverConstraintPool.size();
946
int numPoolConstraints = m_tmpSolverConstraintPool.size();
699
947
for (j=0;j<numPoolConstraints;j++)
701
btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
702
resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
703
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info);
708
int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
709
for (j=0;j<numFrictionPoolConstraints;j++)
711
btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
712
btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
714
resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
715
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse);
950
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
951
resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
952
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
957
int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
959
for (j=0;j<numFrictionPoolConstraints;j++)
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;
965
resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
966
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
975
if (infoGlobal.m_splitImpulse)
978
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
981
int numPoolConstraints = m_tmpSolverConstraintPool.size();
983
for (j=0;j<numPoolConstraints;j++)
985
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
987
resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
988
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
1001
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1005
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1006
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1008
int numPoolConstraints = m_tmpSolverConstraintPool.size();
1010
for (j=0;j<numPoolConstraints;j++)
724
for ( i=0;i<tmpSolverBodyPool.size();i++)
1013
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
1014
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
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;
1020
//do a callback here?
1024
if (infoGlobal.m_splitImpulse)
1026
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1028
m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
726
tmpSolverBodyPool[i].writebackVelocity();
1032
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1034
m_tmpSolverBodyPool[i].writebackVelocity();
729
END_PROFILE("solveConstraints");
731
// printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
1038
// printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
734
printf("tmpSolverBodyPool.size() = %i\n",tmpSolverBodyPool.size());
735
printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
736
printf("tmpSolverFrictionConstraintPool.size() = %i\n",tmpSolverFrictionConstraintPool.size());
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());
739
printf("tmpSolverBodyPool.capacity() = %i\n",tmpSolverBodyPool.capacity());
740
printf("tmpSolverConstraintPool.capacity() = %i\n",tmpSolverConstraintPool.capacity());
741
printf("tmpSolverFrictionConstraintPool.capacity() = %i\n",tmpSolverFrictionConstraintPool.capacity());
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());
744
tmpSolverBodyPool.resize(0);
745
tmpSolverConstraintPool.resize(0);
746
tmpSolverFrictionConstraintPool.resize(0);
1051
m_tmpSolverBodyPool.resize(0);
1052
m_tmpSolverConstraintPool.resize(0);
1053
m_tmpSolverFrictionConstraintPool.resize(0);
752
1059
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
753
btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
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*/)
756
if (getSolverMode() & SOLVER_CACHE_FRIENDLY)
1062
BT_PROFILE("solveGroup");
1063
if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
758
1065
//you need to provide at least some bodies
759
1066
//btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
added
removed
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
