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
solveGroupCacheFriendlySetup(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer)18 btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
19 {
20 btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
21
22 m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
23 m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
24 m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
25 m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
26
27 m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
28 m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
29 m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
30 m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
31
32 return val;
33 }
34
solveSingleIteration(int iteration,btCollisionObject **,int,btPersistentManifold **,int,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw *)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
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 {
47 int tmp = m_orderNonContactConstraintPool[j];
48 int swapi = btRandInt2(j + 1);
49 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
50 m_orderNonContactConstraintPool[swapi] = tmp;
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);
60 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
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);
68 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
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 {
79 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
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
89 if (m_onlyForNoneContact)
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 {
107 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
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 {
132 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
133 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
134 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
135 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
136 constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
137 }
138 }
139
140 ///solve all contact constraints
141 if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
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 {
151 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
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 {
161 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
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
177 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
178 {
179 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
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 {
205 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
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
211 ///solve all friction constraints
212
213 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
214 for (j = 0; j < numFrictionPoolConstraints; j++)
215 {
216 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
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 {
239 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
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
263 if (!m_onlyForNoneContact)
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 {
287 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
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 {
302 btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
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 {
317 btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
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 {
333 btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
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
solveGroupCacheFriendlyFinish(btCollisionObject ** bodies,int numBodies,const btContactSolverInfo & infoGlobal)355 btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
356 {
357 m_pNC.resizeNoInitialize(0);
358 m_pC.resizeNoInitialize(0);
359 m_pCF.resizeNoInitialize(0);
360 m_pCRF.resizeNoInitialize(0);
361
362 m_deltafNC.resizeNoInitialize(0);
363 m_deltafC.resizeNoInitialize(0);
364 m_deltafCF.resizeNoInitialize(0);
365 m_deltafCRF.resizeNoInitialize(0);
366
367 return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
368 }
369