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 http://continuousphysics.com/Bullet/
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
18
19
20
21
22
23btScalar 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
40btScalar 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) {
71 int swapi = btRandInt2(j+1);
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];
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
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];
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;
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];
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];
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 }
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];
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];
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
@ SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
@ SOLVER_RANDMIZE_ORDER
@ SOLVER_USE_2_FRICTION_DIRECTIONS
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
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.
Definition: btIDebugDraw.h:30
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...
btSimdScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
btSimdScalar 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.
virtual void solveConstraintObsolete(btSolverBody &, btSolverBody &, btScalar)
internal method used by the constraint solver, don't use them directly
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:109
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:255
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:223
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btSimdScalar m_appliedImpulse