Bullet Collision Detection & Physics Library
btNNCGConstraintSolver.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
17
19{
21
26
31
32 return val;
33}
34
35btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
36{
40
41 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
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 {
59 int swapi = btRandInt2(j + 1);
62 }
63
64 for (int j = 0; j < numFrictionPool; ++j)
65 {
67 int swapi = btRandInt2(j + 1);
70 }
71 }
72 }
73 }
74
76 {
77 for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
78 {
80 if (iteration < constraint.m_overrideNumSolverIterations)
81 {
85 }
86 }
87 }
88
90 {
91 if (iteration == 0)
92 {
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
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 {
111 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
112 m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
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 }
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 {
132 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
133 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
136 constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
137 }
138 }
139
142 {
144 int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
145
146 for (int c = 0; c < numPoolConstraints; c++)
147 {
149
150 {
153 m_deltafC[c] = 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;
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;
186 m_deltafCF[c * multiplier + 1] = 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
201 int j;
202
203 for (j = 0; j < numPoolConstraints; j++)
204 {
207 m_deltafC[j] = deltaf;
209 }
210
212
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
227 }
228 else
229 {
230 m_deltafCF[j] = 0;
231 }
232 }
233 }
234
235 {
237 for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
238 {
241 if (totalImpulse > btScalar(0))
242 {
246
249
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];
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
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;
282 }
283 else
284 {
285 for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
286 {
288 if (iteration < constraint.m_overrideNumSolverIterations)
289 {
291 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
292 m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
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 {
306 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
307 m_pC[j] = beta * m_pC[j] + m_deltafC[j];
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 }
316 {
318 if (iteration < infoGlobal.m_numIterations)
319 {
321 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
322 m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
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 {
332 {
334 if (iteration < infoGlobal.m_numIterations)
335 {
337 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
338 m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
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 }
350 }
351
352 return deltaflengthsqr;
353}
354
356{
361
366
368}
@ SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
@ SOLVER_RANDMIZE_ORDER
@ SOLVER_USE_2_FRICTION_DIRECTIONS
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:27
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
int size() const
return the number of elements in the array
btCollisionObject can be used to manage collision detection objects.
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
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
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< btScalar > m_deltafCF
btAlignedObjectArray< btScalar > m_deltafC
btAlignedObjectArray< btScalar > m_deltafNC
btAlignedObjectArray< btScalar > m_deltafCRF
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btAlignedObjectArray< btScalar > m_pCRF
btAlignedObjectArray< btScalar > m_pC
btAlignedObjectArray< btScalar > m_pNC
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
TypedConstraint is the baseclass for Bullet constraints and vehicles.
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...