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  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
83  {
85  if (iteration < constraint.m_overrideNumSolverIterations)
86  {
88  m_deltafNC[j] = deltaf;
89  deltaflengthsqr += deltaf * deltaf;
90  }
91  }
92  }
93 
94 
96  {
97  if (iteration==0)
98  {
99  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
100  } else {
101  // 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
102  btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
103  if (beta>1)
104  {
105  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
106  } else
107  {
108  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
109  {
111  if (iteration < constraint.m_overrideNumSolverIterations)
112  {
113  btScalar additionaldeltaimpulse = beta * m_pNC[j];
114  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
115  m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
116  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
117  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
118  const btSolverConstraint& c = constraint;
119  body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
120  body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
121  }
122  }
123  }
124  }
125  m_deltafLengthSqrPrev = deltaflengthsqr;
126  }
127 
128 
129 
130  {
131 
132  if (iteration< infoGlobal.m_numIterations)
133  {
134  for (int j=0;j<numConstraints;j++)
135  {
136  if (constraints[j]->isEnabled())
137  {
138  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
139  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
140  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
141  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
142  constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
143  }
144  }
145 
148  {
149  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
150  int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
151 
152  for (int c=0;c<numPoolConstraints;c++)
153  {
154  btScalar totalImpulse =0;
155 
156  {
159  m_deltafC[c] = deltaf;
160  deltaflengthsqr += deltaf*deltaf;
161  totalImpulse = solveManifold.m_appliedImpulse;
162  }
163  bool applyFriction = true;
164  if (applyFriction)
165  {
166  {
167 
169 
170  if (totalImpulse>btScalar(0))
171  {
172  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
173  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
175  m_deltafCF[c*multiplier] = deltaf;
176  deltaflengthsqr += deltaf*deltaf;
177  } else {
178  m_deltafCF[c*multiplier] = 0;
179  }
180  }
181 
183  {
184 
186 
187  if (totalImpulse>btScalar(0))
188  {
189  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
190  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
192  m_deltafCF[c*multiplier+1] = deltaf;
193  deltaflengthsqr += deltaf*deltaf;
194  } else {
195  m_deltafCF[c*multiplier+1] = 0;
196  }
197  }
198  }
199  }
200 
201  }
202  else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
203  {
204  //solve the friction constraints after all contact constraints, don't interleave them
205  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
206  int j;
207 
208  for (j=0;j<numPoolConstraints;j++)
209  {
212  m_deltafC[j] = deltaf;
213  deltaflengthsqr += deltaf*deltaf;
214  }
215 
216 
217 
219 
220  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
221  for (j=0;j<numFrictionPoolConstraints;j++)
222  {
224  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
225 
226  if (totalImpulse>btScalar(0))
227  {
228  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
229  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
230 
232  m_deltafCF[j] = deltaf;
233  deltaflengthsqr += deltaf*deltaf;
234  } else {
235  m_deltafCF[j] = 0;
236  }
237  }
238  }
239 
240  {
241  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
242  for (int j=0;j<numRollingFrictionPoolConstraints;j++)
243  {
244 
246  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
247  if (totalImpulse>btScalar(0))
248  {
249  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
250  if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
251  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
252 
253  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
254  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
255 
256  btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
257  m_deltafCRF[j] = deltaf;
258  deltaflengthsqr += deltaf*deltaf;
259  } else {
260  m_deltafCRF[j] = 0;
261  }
262  }
263  }
264 
265  }
266 
267 
268 
269  }
270 
271 
272 
273 
274  if (!m_onlyForNoneContact)
275  {
276  if (iteration==0)
277  {
278  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
279  for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
280  for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
282  } else
283  {
284  // 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
285  btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
286  if (beta>1) {
287  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
288  for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
289  for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
290  for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
291  } else {
292  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
293  {
295  if (iteration < constraint.m_overrideNumSolverIterations) {
296  btScalar additionaldeltaimpulse = beta * m_pNC[j];
297  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
298  m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
299  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
300  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
301  const btSolverConstraint& c = constraint;
302  body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
303  body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
304  }
305  }
306  for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
307  {
309  if (iteration< infoGlobal.m_numIterations) {
310  btScalar additionaldeltaimpulse = beta * m_pC[j];
311  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
312  m_pC[j] = beta * m_pC[j] + m_deltafC[j];
313  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
314  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
315  const btSolverConstraint& c = constraint;
316  body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
317  body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
318  }
319  }
320  for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
321  {
323  if (iteration< infoGlobal.m_numIterations) {
324  btScalar additionaldeltaimpulse = beta * m_pCF[j];
325  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
326  m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
327  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
328  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
329  const btSolverConstraint& c = constraint;
330  body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
331  body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
332  }
333  }
334  {
336  {
338  if (iteration< infoGlobal.m_numIterations) {
339  btScalar additionaldeltaimpulse = beta * m_pCRF[j];
340  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
341  m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
342  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
343  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
344  const btSolverConstraint& c = constraint;
345  body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
346  body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
347  }
348  }
349  }
350  }
351  }
352  m_deltafLengthSqrPrev = deltaflengthsqr;
353  }
354 
355  return deltaflengthsqr;
356 }
357 
359 {
364 
369 
370  return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
371 }
372 
373 
374 
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)
btAlignedObjectArray< btScalar > m_pNC
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
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
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:29
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:223
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
btAlignedObjectArray< btScalar > m_pCF
btSimdScalar m_appliedImpulse
btScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
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:292