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
solveGroupCacheFriendlySetup(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer)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
solveSingleIteration(int iteration,btCollisionObject **,int,btPersistentManifold **,int,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw *)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
161 ///solve all contact constraints using SIMD, if available
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
234 ///solve all friction constraints, using SIMD, if available
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 }
302 ///solve all contact constraints
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 }
311 ///solve all friction constraints
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
solveGroupCacheFriendlyFinish(btCollisionObject ** bodies,int numBodies,const btContactSolverInfo & infoGlobal)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
464