Blender  V3.3
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 
19 {
21 
26 
31 
32  return val;
33 }
34 
35 btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
36 {
37  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
38  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
39  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
40 
42  {
43  if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
44  {
45  for (int j = 0; j < numNonContactPool; ++j)
46  {
48  int swapi = btRandInt2(j + 1);
51  }
52 
53  //contact/friction constraints are not solved more than
54  if (iteration < infoGlobal.m_numIterations)
55  {
56  for (int j = 0; j < numConstraintPool; ++j)
57  {
58  int tmp = m_orderTmpConstraintPool[j];
59  int swapi = btRandInt2(j + 1);
61  m_orderTmpConstraintPool[swapi] = tmp;
62  }
63 
64  for (int j = 0; j < numFrictionPool; ++j)
65  {
66  int tmp = m_orderFrictionConstraintPool[j];
67  int swapi = btRandInt2(j + 1);
69  m_orderFrictionConstraintPool[swapi] = tmp;
70  }
71  }
72  }
73  }
74 
75  btScalar deltaflengthsqr = 0;
76  {
77  for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
78  {
80  if (iteration < constraint.m_overrideNumSolverIterations)
81  {
82  btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
83  m_deltafNC[j] = deltaf;
84  deltaflengthsqr += deltaf * deltaf;
85  }
86  }
87  }
88 
90  {
91  if (iteration == 0)
92  {
93  for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = m_deltafNC[j];
94  }
95  else
96  {
97  // 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
98  btScalar beta = m_deltafLengthSqrPrev > 0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
99  if (beta > 1)
100  {
101  for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = 0;
102  }
103  else
104  {
105  for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
106  {
108  if (iteration < constraint.m_overrideNumSolverIterations)
109  {
110  btScalar additionaldeltaimpulse = beta * m_pNC[j];
111  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
112  m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
113  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
114  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
115  const btSolverConstraint& c = constraint;
116  body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
117  body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
118  }
119  }
120  }
121  }
122  m_deltafLengthSqrPrev = deltaflengthsqr;
123  }
124 
125  {
126  if (iteration < infoGlobal.m_numIterations)
127  {
128  for (int j = 0; j < numConstraints; j++)
129  {
130  if (constraints[j]->isEnabled())
131  {
134  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
135  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
136  constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
137  }
138  }
139 
142  {
143  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
144  int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
145 
146  for (int c = 0; c < numPoolConstraints; c++)
147  {
148  btScalar totalImpulse = 0;
149 
150  {
152  btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
153  m_deltafC[c] = deltaf;
154  deltaflengthsqr += deltaf * deltaf;
155  totalImpulse = solveManifold.m_appliedImpulse;
156  }
157  bool applyFriction = true;
158  if (applyFriction)
159  {
160  {
162 
163  if (totalImpulse > btScalar(0))
164  {
165  solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
166  solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
167  btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
168  m_deltafCF[c * multiplier] = deltaf;
169  deltaflengthsqr += deltaf * deltaf;
170  }
171  else
172  {
173  m_deltafCF[c * multiplier] = 0;
174  }
175  }
176 
178  {
180 
181  if (totalImpulse > btScalar(0))
182  {
183  solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
184  solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
185  btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
186  m_deltafCF[c * multiplier + 1] = deltaf;
187  deltaflengthsqr += deltaf * deltaf;
188  }
189  else
190  {
191  m_deltafCF[c * multiplier + 1] = 0;
192  }
193  }
194  }
195  }
196  }
197  else //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
198  {
199  //solve the friction constraints after all contact constraints, don't interleave them
200  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
201  int j;
202 
203  for (j = 0; j < numPoolConstraints; j++)
204  {
206  btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
207  m_deltafC[j] = deltaf;
208  deltaflengthsqr += deltaf * deltaf;
209  }
210 
212 
213  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
214  for (j = 0; j < numFrictionPoolConstraints; j++)
215  {
217  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
218 
219  if (totalImpulse > btScalar(0))
220  {
221  solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
222  solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
223 
224  btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
225  m_deltafCF[j] = deltaf;
226  deltaflengthsqr += deltaf * deltaf;
227  }
228  else
229  {
230  m_deltafCF[j] = 0;
231  }
232  }
233  }
234 
235  {
236  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
237  for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
238  {
240  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
241  if (totalImpulse > btScalar(0))
242  {
243  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
244  if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
245  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
246 
247  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
248  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
249 
250  btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
251  m_deltafCRF[j] = deltaf;
252  deltaflengthsqr += deltaf * deltaf;
253  }
254  else
255  {
256  m_deltafCRF[j] = 0;
257  }
258  }
259  }
260  }
261  }
262 
264  {
265  if (iteration == 0)
266  {
267  for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = m_deltafNC[j];
268  for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++) m_pC[j] = m_deltafC[j];
269  for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++) m_pCF[j] = m_deltafCF[j];
270  for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++) m_pCRF[j] = m_deltafCRF[j];
271  }
272  else
273  {
274  // 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
275  btScalar beta = m_deltafLengthSqrPrev > 0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
276  if (beta > 1)
277  {
278  for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = 0;
279  for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++) m_pC[j] = 0;
280  for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++) m_pCF[j] = 0;
281  for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++) m_pCRF[j] = 0;
282  }
283  else
284  {
285  for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
286  {
288  if (iteration < constraint.m_overrideNumSolverIterations)
289  {
290  btScalar additionaldeltaimpulse = beta * m_pNC[j];
291  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
292  m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
293  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
294  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
295  const btSolverConstraint& c = constraint;
296  body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
297  body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
298  }
299  }
300  for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++)
301  {
303  if (iteration < infoGlobal.m_numIterations)
304  {
305  btScalar additionaldeltaimpulse = beta * m_pC[j];
306  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
307  m_pC[j] = beta * m_pC[j] + m_deltafC[j];
308  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
309  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
310  const btSolverConstraint& c = constraint;
311  body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
312  body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
313  }
314  }
315  for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++)
316  {
318  if (iteration < infoGlobal.m_numIterations)
319  {
320  btScalar additionaldeltaimpulse = beta * m_pCF[j];
321  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
322  m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
323  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
324  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
325  const btSolverConstraint& c = constraint;
326  body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
327  body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
328  }
329  }
330  {
331  for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++)
332  {
334  if (iteration < infoGlobal.m_numIterations)
335  {
336  btScalar additionaldeltaimpulse = beta * m_pCRF[j];
337  constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
338  m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
339  btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
340  btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
341  const btSolverConstraint& c = constraint;
342  body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
343  body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
344  }
345  }
346  }
347  }
348  }
349  m_deltafLengthSqrPrev = deltaflengthsqr;
350  }
351 
352  return deltaflengthsqr;
353 }
354 
356 {
361 
366 
368 }
btCollisionObject
const btRigidBody & getRigidBodyA() const
const btRigidBody & getRigidBodyB() const
@ SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
@ SOLVER_RANDMIZE_ORDER
@ SOLVER_USE_2_FRICTION_DIRECTIONS
bool m_onlyForNoneContact
btAlignedObjectArray< btScalar > m_deltafNC
btAlignedObjectArray< btScalar > m_pCRF
btAlignedObjectArray< btScalar > m_deltafC
btAlignedObjectArray< btScalar > m_deltafCRF
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< btScalar > m_deltafCF
btAlignedObjectArray< btScalar > m_pC
btAlignedObjectArray< btScalar > m_pNC
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btAlignedObjectArray< btScalar > m_pCF
btPersistentManifold()
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint ** constraints
btSequentialImpulseConstraintSolverMt int btPersistentManifold ** manifoldPtr
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int numConstraints
btSequentialImpulseConstraintSolverMt int numBodies
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btSequentialImpulseConstraintSolverMt int btPersistentManifold int numManifolds
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool
btAlignedObjectArray< int > m_orderTmpConstraintPool
btAlignedObjectArray< int > m_orderNonContactConstraintPool
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btAlignedObjectArray< int > m_orderFrictionConstraintPool
btConstraintArray m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
btConstraintArray m_tmpSolverNonContactConstraintPool
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
bool isEnabled() const
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
SIMD_FORCE_INLINE void resizeNoInitialize(int newsize)
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
static unsigned c
Definition: RandGen.cpp:83
ccl_device_inline float beta(float x, float y)
Definition: util/math.h:775