Bullet Collision Detection & Physics Library: btNNCGConstraintSolver.cpp Source File

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
27 m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
28 m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
29 m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
30 m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
31
32 m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
33 m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
34 m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
35 m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
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) {
53 int tmp = m_orderNonContactConstraintPool[j];
54 int swapi = btRandInt2(j+1);
55 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
56 m_orderNonContactConstraintPool[swapi] = tmp;
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);
65 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
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);
72 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
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 {
86 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
87 if (iteration < constraint.m_overrideNumSolverIterations)
88 {
89 btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
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 {
98 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
99 if (iteration < constraint.m_overrideNumSolverIterations)
100 {
101 btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
102 m_deltafNC[j] = deltaf;
103 deltaflengthsqr += deltaf * deltaf;
104 }
105 }
106 }
107
108
109 if (m_onlyForNoneContact)
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 {
124 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
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
162 if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
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 {
172 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
173 btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
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
183 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
184
185 if (totalImpulse>btScalar(0))
186 {
187 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
188 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
189 btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
190 m_deltafCF[c*multiplier] = deltaf;
191 deltaflengthsqr += deltaf*deltaf;
192 } else {
193 m_deltafCF[c*multiplier] = 0;
194 }
195 }
196
197 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
198 {
199
200 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
201
202 if (totalImpulse>btScalar(0))
203 {
204 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
205 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
206 btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
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 {
225 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
226 //resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
227 btScalar deltaf = resolveSingleConstraintRowLowerLimit(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 {
239 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
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);
248 btScalar deltaf = resolveSingleConstraintRowGeneric(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
261 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
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 {
306 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
307 btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
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 {
315 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
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
323 btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
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 {
334 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
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 {
367 for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
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;
377 if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 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 {
383 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
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 {
397 btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
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 {
411 btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
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 ) {
424 for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
425 {
426 btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
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
447 btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
448 {
449 m_pNC.resizeNoInitialize(0);
450 m_pC.resizeNoInitialize(0);
451 m_pCF.resizeNoInitialize(0);
452 m_pCRF.resizeNoInitialize(0);
453
454 m_deltafNC.resizeNoInitialize(0);
455 m_deltafC.resizeNoInitialize(0);
456 m_deltafCF.resizeNoInitialize(0);
457 m_deltafCRF.resizeNoInitialize(0);
458
459 return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
460 }
461
462
463
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:54
btSolverConstraint::m_overrideNumSolverIterations
int m_overrideNumSolverIterations
Definition: btSolverConstraint.h:61
btSequentialImpulseConstraintSolver::m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactFrictionConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:39
btSequentialImpulseConstraintSolver::m_tmpSolverContactRollingFrictionConstraintPool
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:40
btSolverBody::internalApplyImpulse
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:255
btSolverConstraint::m_friction
btScalar m_friction
Definition: btSolverConstraint.h:46
btNNCGConstraintSolver::m_pCRF
btAlignedObjectArray< btScalar > m_pCRF
Definition: btNNCGConstraintSolver.h:30
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1839
btNNCGConstraintSolver.h
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btSolverConstraint.h:30
btAlignedObjectArray::resizeNoInitialize
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
Definition: btAlignedObjectArray.h:209
btNNCGConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btNNCGConstraintSolver.cpp:447
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:223
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit
btSimdScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:295
btNNCGConstraintSolver::m_pNC
btAlignedObjectArray< btScalar > m_pNC
Definition: btNNCGConstraintSolver.h:27
btSequentialImpulseConstraintSolver::m_orderTmpConstraintPool
btAlignedObjectArray< int > m_orderTmpConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:42
btNNCGConstraintSolver::m_deltafLengthSqrPrev
btScalar m_deltafLengthSqrPrev
Definition: btNNCGConstraintSolver.h:25
btNNCGConstraintSolver::m_onlyForNoneContact
bool m_onlyForNoneContact
Definition: btNNCGConstraintSolver.h:57
btSequentialImpulseConstraintSolver::getOrInitSolverBody
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
Definition: btSequentialImpulseConstraintSolver.cpp:717
btSolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btSolverConstraint.h:40
btSequentialImpulseConstraintSolver::m_tmpSolverBodyPool
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
Definition: btSequentialImpulseConstraintSolver.h:36
btNNCGConstraintSolver::m_deltafCRF
btAlignedObjectArray< btScalar > m_deltafCRF
Definition: btNNCGConstraintSolver.h:36
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric
btSimdScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:280
btSequentialImpulseConstraintSolver::btRandInt2
int btRandInt2(int n)
Definition: btSequentialImpulseConstraintSolver.cpp:433
SOLVER_RANDMIZE_ORDER
Definition: btContactSolverInfo.h:23
btSequentialImpulseConstraintSolver::m_tmpSolverContactConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:37
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:49
btNNCGConstraintSolver::m_deltafC
btAlignedObjectArray< btScalar > m_deltafC
Definition: btNNCGConstraintSolver.h:34
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
btSolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btSolverConstraint.h:41
btSolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btSolverConstraint.h:64
SOLVER_USE_2_FRICTION_DIRECTIONS
Definition: btContactSolverInfo.h:26
btSolverConstraint::m_upperLimit
btScalar m_upperLimit
Definition: btSolverConstraint.h:52
btContactSolverInfoData::m_numIterations
int m_numIterations
Definition: btContactSolverInfo.h:44
SOLVER_SIMD
Definition: btContactSolverInfo.h:30
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD
btSimdScalar resolveSingleConstraintRowGenericSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:270
SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
Definition: btContactSolverInfo.h:31
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:155
btTypedConstraint::solveConstraintObsolete
virtual void solveConstraintObsolete(btSolverBody &, btSolverBody &, btScalar)
internal method used by the constraint solver, don&#39;t use them directly
Definition: btTypedConstraint.h:219
btContactSolverInfo
Definition: btContactSolverInfo.h:65
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:108
btNNCGConstraintSolver::solveSingleIteration
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btNNCGConstraintSolver.cpp:40
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:78
btSolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btSolverConstraint.h:63
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btSequentialImpulseConstraintSolver.cpp:1184
btNNCGConstraintSolver::m_deltafCF
btAlignedObjectArray< btScalar > m_deltafCF
Definition: btNNCGConstraintSolver.h:35
btNNCGConstraintSolver::m_deltafNC
btAlignedObjectArray< btScalar > m_deltafNC
Definition: btNNCGConstraintSolver.h:33
btSolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btSolverConstraint.h:35
btSequentialImpulseConstraintSolver::m_orderFrictionConstraintPool
btAlignedObjectArray< int > m_orderFrictionConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:44
btNNCGConstraintSolver::m_pC
btAlignedObjectArray< btScalar > m_pC
Definition: btNNCGConstraintSolver.h:28
btSequentialImpulseConstraintSolver::m_orderNonContactConstraintPool
btAlignedObjectArray< int > m_orderNonContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:43
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD
btSimdScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:285
btNNCGConstraintSolver::m_pCF
btAlignedObjectArray< btScalar > m_pCF
Definition: btNNCGConstraintSolver.h:29
btSolverConstraint::m_lowerLimit
btScalar m_lowerLimit
Definition: btSolverConstraint.h:51
btSolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition: btSolverConstraint.h:44
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btNNCGConstraintSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btNNCGConstraintSolver.cpp:23
btSolverConstraint::m_frictionIndex
int m_frictionIndex
Definition: btSolverConstraint.h:62
btSequentialImpulseConstraintSolver::m_tmpSolverNonContactConstraintPool
btConstraintArray m_tmpSolverNonContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:38
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
btSolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btSolverConstraint.h:38
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:56