Bullet Collision Detection & Physics Library
btNNCGConstraintSolver.cpp
Go to the documentation of this file.
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 #include "btNNCGConstraintSolver.h"
17 
18 
19 
20 
21 
22 
23 btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
24 {
25  btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
26 
31 
36 
37  return val;
38 }
39 
40 btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
41 {
42 
43  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
44  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
45  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
46 
47  if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
48  {
49  if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
50  {
51 
52  for (int j=0; j<numNonContactPool; ++j) {
54  int swapi = btRandInt2(j+1);
57  }
58 
59  //contact/friction constraints are not solved more than
60  if (iteration< infoGlobal.m_numIterations)
61  {
62  for (int j=0; j<numConstraintPool; ++j) {
63  int tmp = m_orderTmpConstraintPool[j];
64  int swapi = btRandInt2(j+1);
66  m_orderTmpConstraintPool[swapi] = tmp;
67  }
68 
69  for (int j=0; j<numFrictionPool; ++j) {
70  int tmp = m_orderFrictionConstraintPool[j];
71  int swapi = btRandInt2(j+1);
73  m_orderFrictionConstraintPool[swapi] = tmp;
74  }
75  }
76  }
77  }
78 
79 
80  btScalar deltaflengthsqr = 0;
81 
82  if (infoGlobal.m_solverMode & SOLVER_SIMD)
83  {
84  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
85  {
87  if (iteration < constraint.m_overrideNumSolverIterations)
88  {
90  m_deltafNC[j] = deltaf;
91  deltaflengthsqr += deltaf * deltaf;
92  }
93  }
94  } else
95  {
96  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
97  {
99  if (iteration < constraint.m_overrideNumSolverIterations)
100  {
102  m_deltafNC[j] = deltaf;
103  deltaflengthsqr += deltaf * deltaf;
104  }
105  }
106  }
107 
108 
110  {
111  if (iteration==0)
112  {
113  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
114  } else {
115  // deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
116  btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
117  if (beta>1)
118  {
119  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
120  } else
121  {
122  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
123  {
125  if (iteration < constraint.m_overrideNumSolverIterations)
126  {
127  btScalar additionaldeltaimpulse = beta * m_pNC[j];
128  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
129  m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
130  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
131  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
132  const btSolverConstraint& c = constraint;
133  body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
134  body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
135  }
136  }
137  }
138  }
139  m_deltafLengthSqrPrev = deltaflengthsqr;
140  }
141 
142 
143 
144  if (infoGlobal.m_solverMode & SOLVER_SIMD)
145  {
146 
147  if (iteration< infoGlobal.m_numIterations)
148  {
149  for (int j=0;j<numConstraints;j++)
150  {
151  if (constraints[j]->isEnabled())
152  {
153  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
154  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
155  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
156  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
157  constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
158  }
159  }
160 
163  {
164  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
165  int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
166 
167  for (int c=0;c<numPoolConstraints;c++)
168  {
169  btScalar totalImpulse =0;
170 
171  {
174  m_deltafC[c] = deltaf;
175  deltaflengthsqr += deltaf*deltaf;
176  totalImpulse = solveManifold.m_appliedImpulse;
177  }
178  bool applyFriction = true;
179  if (applyFriction)
180  {
181  {
182 
184 
185  if (totalImpulse>btScalar(0))
186  {
187  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
188  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
190  m_deltafCF[c*multiplier] = deltaf;
191  deltaflengthsqr += deltaf*deltaf;
192  } else {
193  m_deltafCF[c*multiplier] = 0;
194  }
195  }
196 
198  {
199 
201 
202  if (totalImpulse>btScalar(0))
203  {
204  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
205  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
207  m_deltafCF[c*multiplier+1] = deltaf;
208  deltaflengthsqr += deltaf*deltaf;
209  } else {
210  m_deltafCF[c*multiplier+1] = 0;
211  }
212  }
213  }
214  }
215 
216  }
217  else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
218  {
219  //solve the friction constraints after all contact constraints, don't interleave them
220  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
221  int j;
222 
223  for (j=0;j<numPoolConstraints;j++)
224  {
226  //resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
228  m_deltafC[j] = deltaf;
229  deltaflengthsqr += deltaf*deltaf;
230  }
231 
232 
233 
235 
236  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
237  for (j=0;j<numFrictionPoolConstraints;j++)
238  {
240  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
241 
242  if (totalImpulse>btScalar(0))
243  {
244  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
245  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
246 
247  //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
249  m_deltafCF[j] = deltaf;
250  deltaflengthsqr += deltaf*deltaf;
251  } else {
252  m_deltafCF[j] = 0;
253  }
254  }
255 
256 
257  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
258  for (j=0;j<numRollingFrictionPoolConstraints;j++)
259  {
260 
262  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
263  if (totalImpulse>btScalar(0))
264  {
265  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
266  if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
267  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
268 
269  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
270  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
271 
272  btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
273  m_deltafCRF[j] = deltaf;
274  deltaflengthsqr += deltaf*deltaf;
275  } else {
276  m_deltafCRF[j] = 0;
277  }
278  }
279 
280 
281  }
282  }
283 
284 
285 
286  } else
287  {
288 
289  if (iteration< infoGlobal.m_numIterations)
290  {
291  for (int j=0;j<numConstraints;j++)
292  {
293  if (constraints[j]->isEnabled())
294  {
295  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
296  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
297  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
298  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
299  constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
300  }
301  }
303  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
304  for (int j=0;j<numPoolConstraints;j++)
305  {
308  m_deltafC[j] = deltaf;
309  deltaflengthsqr += deltaf*deltaf;
310  }
312  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
313  for (int j=0;j<numFrictionPoolConstraints;j++)
314  {
316  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
317 
318  if (totalImpulse>btScalar(0))
319  {
320  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
321  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
322 
324  m_deltafCF[j] = deltaf;
325  deltaflengthsqr += deltaf*deltaf;
326  } else {
327  m_deltafCF[j] = 0;
328  }
329  }
330 
331  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
332  for (int j=0;j<numRollingFrictionPoolConstraints;j++)
333  {
335  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
336  if (totalImpulse>btScalar(0))
337  {
338  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
339  if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
340  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
341 
342  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
343  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
344 
345  btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
346  m_deltafCRF[j] = deltaf;
347  deltaflengthsqr += deltaf*deltaf;
348  } else {
349  m_deltafCRF[j] = 0;
350  }
351  }
352  }
353  }
354 
355 
356 
357 
358  if (!m_onlyForNoneContact)
359  {
360  if (iteration==0)
361  {
362  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
363  for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
364  for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
365  if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 )
366  {
368  }
369  } else
370  {
371  // deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
372  btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
373  if (beta>1) {
374  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
375  for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
376  for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
378  for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
379  }
380  } else {
381  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
382  {
384  if (iteration < constraint.m_overrideNumSolverIterations) {
385  btScalar additionaldeltaimpulse = beta * m_pNC[j];
386  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
387  m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
388  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
389  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
390  const btSolverConstraint& c = constraint;
391  body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
392  body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
393  }
394  }
395  for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
396  {
398  if (iteration< infoGlobal.m_numIterations) {
399  btScalar additionaldeltaimpulse = beta * m_pC[j];
400  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
401  m_pC[j] = beta * m_pC[j] + m_deltafC[j];
402  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
403  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
404  const btSolverConstraint& c = constraint;
405  body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
406  body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
407  }
408  }
409  for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
410  {
412  if (iteration< infoGlobal.m_numIterations) {
413  btScalar additionaldeltaimpulse = beta * m_pCF[j];
414  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
415  m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
416  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
417  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
418  const btSolverConstraint& c = constraint;
419  body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
420  body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
421  }
422  }
423  if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
425  {
427  if (iteration< infoGlobal.m_numIterations) {
428  btScalar additionaldeltaimpulse = beta * m_pCRF[j];
429  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
430  m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
431  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
432  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
433  const btSolverConstraint& c = constraint;
434  body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
435  body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
436  }
437  }
438  }
439  }
440  }
441  m_deltafLengthSqrPrev = deltaflengthsqr;
442  }
443 
444  return deltaflengthsqr;
445 }
446 
448 {
453 
458 
459  return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
460 }
461 
462 
463 
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:255
btAlignedObjectArray< btScalar > m_pCRF
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
btSimdScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btAlignedObjectArray< btScalar > m_pNC
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
btAlignedObjectArray< btScalar > m_deltafCRF
int size() const
return the number of elements in the array
btSimdScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btCollisionObject can be used to manage collision detection objects.
btAlignedObjectArray< btScalar > m_deltafC
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:28
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:223
btSimdScalar resolveSingleConstraintRowGenericSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
virtual void solveConstraintObsolete(btSolverBody &, btSolverBody &, btScalar)
internal method used by the constraint solver, don&#39;t use them directly
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:108
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
TypedConstraint is the baseclass for Bullet constraints and vehicles.
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btAlignedObjectArray< btScalar > m_deltafCF
btAlignedObjectArray< btScalar > m_deltafNC
btAlignedObjectArray< btScalar > m_pC
btSimdScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btAlignedObjectArray< btScalar > m_pCF
btSimdScalar m_appliedImpulse
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278