1/*
2* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
3*
4* This software is provided 'as-is', without any express or implied
5* warranty. In no event will the authors be held liable for any damages
6* 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
9* freely, subject to the following restrictions:
10* 1. The origin of this software must not be misrepresented; you must not
11* claim that you wrote the original software. If you use this software
12* in a product, an acknowledgment in the product documentation would be
13* appreciated but is not required.
14* 2. Altered source versions must be plainly marked as such, and must not be
15* misrepresented as being the original software.
16* 3. This notice may not be removed or altered from any source distribution.
17*/
18
19#include <Box2D/Collision/b2Distance.h>
20#include <Box2D/Dynamics/b2Island.h>
21#include <Box2D/Dynamics/b2Body.h>
22#include <Box2D/Dynamics/b2Fixture.h>
23#include <Box2D/Dynamics/b2World.h>
24#include <Box2D/Dynamics/Contacts/b2Contact.h>
25#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
26#include <Box2D/Dynamics/Joints/b2Joint.h>
27#include <Box2D/Common/b2StackAllocator.h>
28#include <Box2D/Common/b2Timer.h>
29
30/*
31Position Correction Notes
32=========================
33I tried the several algorithms for position correction of the 2D revolute joint.
34I looked at these systems:
35- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
36- suspension bridge with 30 1m long planks of length 1m.
37- multi-link chain with 30 1m long links.
38
39Here are the algorithms:
40
41Baumgarte - A fraction of the position error is added to the velocity error. There is no
42separate position solver.
43
44Pseudo Velocities - After the velocity solver and position integration,
45the position error, Jacobian, and effective mass are recomputed. Then
46the velocity constraints are solved with pseudo velocities and a fraction
47of the position error is added to the pseudo velocity error. The pseudo
48velocities are initialized to zero and there is no warm-starting. After
49the position solver, the pseudo velocities are added to the positions.
50This is also called the First Order World method or the Position LCP method.
51
52Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
53position error is re-computed for each constraint and the positions are updated
54after the constraint is solved. The radius vectors (aka Jacobians) are
55re-computed too (otherwise the algorithm has horrible instability). The pseudo
56velocity states are not needed because they are effectively zero at the beginning
57of each iteration. Since we have the current position error, we allow the
58iterations to terminate early if the error becomes smaller than b2_linearSlop.
59
60Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
61each time a constraint is solved.
62
63Here are the results:
64Baumgarte - this is the cheapest algorithm but it has some stability problems,
65especially with the bridge. The chain links separate easily close to the root
66and they jitter as they struggle to pull together. This is one of the most common
67methods in the field. The big drawback is that the position correction artificially
68affects the momentum, thus leading to instabilities and false bounce. I used a
69bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
70factor makes joints and contacts more spongy.
71
72Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
73stable. However, joints still separate with large angular velocities. Drag the
74simple pendulum in a circle quickly and the joint will separate. The chain separates
75easily and does not recover. I used a bias factor of 0.2. A larger value lead to
76the bridge collapsing when a heavy cube drops on it.
77
78Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
79Velocities, but in other ways it is worse. The bridge and chain are much more
80stable, but the simple pendulum goes unstable at high angular velocities.
81
82Full NGS - stable in all tests. The joints display good stiffness. The bridge
83still sags, but this is better than infinite forces.
84
85Recommendations
86Pseudo Velocities are not really worthwhile because the bridge and chain cannot
87recover from joint separation. In other cases the benefit over Baumgarte is small.
88
89Modified NGS is not a robust method for the revolute joint due to the violent
90instability seen in the simple pendulum. Perhaps it is viable with other constraint
91types, especially scalar constraints where the effective mass is a scalar.
92
93This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
94and is very fast. I don't think we can escape Baumgarte, especially in highly
95demanding cases where high constraint fidelity is not needed.
96
97Full NGS is robust and easy on the eyes. I recommend this as an option for
98higher fidelity simulation and certainly for suspension bridges and long chains.
99Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
100joint separation can be problematic. The number of NGS iterations can be reduced
101for better performance without harming robustness much.
102
103Each joint in a can be handled differently in the position solver. So I recommend
104a system where the user can select the algorithm on a per joint basis. I would
105probably default to the slower Full NGS and let the user select the faster
106Baumgarte method in performance critical scenarios.
107*/
108
109/*
110Cache Performance
111
112The Box2D solvers are dominated by cache misses. Data structures are designed
113to increase the number of cache hits. Much of misses are due to random access
114to body data. The constraint structures are iterated over linearly, which leads
115to few cache misses.
116
117The bodies are not accessed during iteration. Instead read only data, such as
118the mass values are stored with the constraints. The mutable data are the constraint
119impulses and the bodies velocities/positions. The impulses are held inside the
120constraint structures. The body velocities/positions are held in compact, temporary
121arrays to increase the number of cache hits. Linear and angular velocity are
122stored in a single array since multiple arrays lead to multiple misses.
123*/
124
125/*
1262D Rotation
127
128R = [cos(theta) -sin(theta)]
129 [sin(theta) cos(theta) ]
130
131thetaDot = omega
132
133Let q1 = cos(theta), q2 = sin(theta).
134R = [q1 -q2]
135 [q2 q1]
136
137q1Dot = -thetaDot * q2
138q2Dot = thetaDot * q1
139
140q1_new = q1_old - dt * w * q2
141q2_new = q2_old + dt * w * q1
142then normalize.
143
144This might be faster than computing sin+cos.
145However, we can compute sin+cos of the same angle fast.
146*/
147
148b2Island::b2Island(
149 int32 bodyCapacity,
150 int32 contactCapacity,
151 int32 jointCapacity,
152 b2StackAllocator* allocator,
153 b2ContactListener* listener)
154{
155 m_bodyCapacity = bodyCapacity;
156 m_contactCapacity = contactCapacity;
157 m_jointCapacity = jointCapacity;
158 m_bodyCount = 0;
159 m_contactCount = 0;
160 m_jointCount = 0;
161
162 m_allocator = allocator;
163 m_listener = listener;
164
165 m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*));
166 m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity * sizeof(b2Contact*));
167 m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*));
168
169 m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity));
170 m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position));
171}
172
173b2Island::~b2Island()
174{
175 // Warning: the order should reverse the constructor order.
176 m_allocator->Free(m_positions);
177 m_allocator->Free(m_velocities);
178 m_allocator->Free(m_joints);
179 m_allocator->Free(m_contacts);
180 m_allocator->Free(m_bodies);
181}
182
183void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
184{
185 b2Timer timer;
186
187 float32 h = step.dt;
188
189 // Integrate velocities and apply damping. Initialize the body state.
190 for (int32 i = 0; i < m_bodyCount; ++i)
191 {
192 b2Body* b = m_bodies[i];
193
194 b2Vec2 c = b->m_sweep.c;
195 float32 a = b->m_sweep.a;
196 b2Vec2 v = b->m_linearVelocity;
197 float32 w = b->m_angularVelocity;
198
199 // Store positions for continuous collision.
200 b->m_sweep.c0 = b->m_sweep.c;
201 b->m_sweep.a0 = b->m_sweep.a;
202
203 if (b->m_type == b2_dynamicBody)
204 {
205 // Integrate velocities.
206 v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force);
207 w += h * b->m_invI * b->m_torque;
208
209 // Apply damping.
210 // ODE: dv/dt + c * v = 0
211 // Solution: v(t) = v0 * exp(-c * t)
212 // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
213 // v2 = exp(-c * dt) * v1
214 // Pade approximation:
215 // v2 = v1 * 1 / (1 + c * dt)
216 v *= 1.0f / (1.0f + h * b->m_linearDamping);
217 w *= 1.0f / (1.0f + h * b->m_angularDamping);
218 }
219
220 m_positions[i].c = c;
221 m_positions[i].a = a;
222 m_velocities[i].v = v;
223 m_velocities[i].w = w;
224 }
225
226 timer.Reset();
227
228 // Solver data
229 b2SolverData solverData;
230 solverData.step = step;
231 solverData.positions = m_positions;
232 solverData.velocities = m_velocities;
233
234 // Initialize velocity constraints.
235 b2ContactSolverDef contactSolverDef;
236 contactSolverDef.step = step;
237 contactSolverDef.contacts = m_contacts;
238 contactSolverDef.count = m_contactCount;
239 contactSolverDef.positions = m_positions;
240 contactSolverDef.velocities = m_velocities;
241 contactSolverDef.allocator = m_allocator;
242
243 b2ContactSolver contactSolver(&contactSolverDef);
244 contactSolver.InitializeVelocityConstraints();
245
246 if (step.warmStarting)
247 {
248 contactSolver.WarmStart();
249 }
250
251 for (int32 i = 0; i < m_jointCount; ++i)
252 {
253 m_joints[i]->InitVelocityConstraints(solverData);
254 }
255
256 profile->solveInit = timer.GetMilliseconds();
257
258 // Solve velocity constraints
259 timer.Reset();
260 for (int32 i = 0; i < step.velocityIterations; ++i)
261 {
262 for (int32 j = 0; j < m_jointCount; ++j)
263 {
264 m_joints[j]->SolveVelocityConstraints(solverData);
265 }
266
267 contactSolver.SolveVelocityConstraints();
268 }
269
270 // Store impulses for warm starting
271 contactSolver.StoreImpulses();
272 profile->solveVelocity = timer.GetMilliseconds();
273
274 // Integrate positions
275 for (int32 i = 0; i < m_bodyCount; ++i)
276 {
277 b2Vec2 c = m_positions[i].c;
278 float32 a = m_positions[i].a;
279 b2Vec2 v = m_velocities[i].v;
280 float32 w = m_velocities[i].w;
281
282 // Check for large velocities
283 b2Vec2 translation = h * v;
284 if (b2Dot(translation, translation) > b2_maxTranslationSquared)
285 {
286 float32 ratio = b2_maxTranslation / translation.Length();
287 v *= ratio;
288 }
289
290 float32 rotation = h * w;
291 if (rotation * rotation > b2_maxRotationSquared)
292 {
293 float32 ratio = b2_maxRotation / b2Abs(rotation);
294 w *= ratio;
295 }
296
297 // Integrate
298 c += h * v;
299 a += h * w;
300
301 m_positions[i].c = c;
302 m_positions[i].a = a;
303 m_velocities[i].v = v;
304 m_velocities[i].w = w;
305 }
306
307 // Solve position constraints
308 timer.Reset();
309 bool positionSolved = false;
310 for (int32 i = 0; i < step.positionIterations; ++i)
311 {
312 bool contactsOkay = contactSolver.SolvePositionConstraints();
313
314 bool jointsOkay = true;
315 for (int32 i = 0; i < m_jointCount; ++i)
316 {
317 bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData);
318 jointsOkay = jointsOkay && jointOkay;
319 }
320
321 if (contactsOkay && jointsOkay)
322 {
323 // Exit early if the position errors are small.
324 positionSolved = true;
325 break;
326 }
327 }
328
329 // Copy state buffers back to the bodies
330 for (int32 i = 0; i < m_bodyCount; ++i)
331 {
332 b2Body* body = m_bodies[i];
333 body->m_sweep.c = m_positions[i].c;
334 body->m_sweep.a = m_positions[i].a;
335 body->m_linearVelocity = m_velocities[i].v;
336 body->m_angularVelocity = m_velocities[i].w;
337 body->SynchronizeTransform();
338 }
339
340 profile->solvePosition = timer.GetMilliseconds();
341
342 Report(contactSolver.m_velocityConstraints);
343
344 if (allowSleep)
345 {
346 float32 minSleepTime = b2_maxFloat;
347
348 const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
349 const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
350
351 for (int32 i = 0; i < m_bodyCount; ++i)
352 {
353 b2Body* b = m_bodies[i];
354 if (b->GetType() == b2_staticBody)
355 {
356 continue;
357 }
358
359 if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
360 b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
361 b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
362 {
363 b->m_sleepTime = 0.0f;
364 minSleepTime = 0.0f;
365 }
366 else
367 {
368 b->m_sleepTime += h;
369 minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
370 }
371 }
372
373 if (minSleepTime >= b2_timeToSleep && positionSolved)
374 {
375 for (int32 i = 0; i < m_bodyCount; ++i)
376 {
377 b2Body* b = m_bodies[i];
378 b->SetAwake(false);
379 }
380 }
381 }
382}
383
384void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB)
385{
386 b2Assert(toiIndexA < m_bodyCount);
387 b2Assert(toiIndexB < m_bodyCount);
388
389 // Initialize the body state.
390 for (int32 i = 0; i < m_bodyCount; ++i)
391 {
392 b2Body* b = m_bodies[i];
393 m_positions[i].c = b->m_sweep.c;
394 m_positions[i].a = b->m_sweep.a;
395 m_velocities[i].v = b->m_linearVelocity;
396 m_velocities[i].w = b->m_angularVelocity;
397 }
398
399 b2ContactSolverDef contactSolverDef;
400 contactSolverDef.contacts = m_contacts;
401 contactSolverDef.count = m_contactCount;
402 contactSolverDef.allocator = m_allocator;
403 contactSolverDef.step = subStep;
404 contactSolverDef.positions = m_positions;
405 contactSolverDef.velocities = m_velocities;
406 b2ContactSolver contactSolver(&contactSolverDef);
407
408 // Solve position constraints.
409 for (int32 i = 0; i < subStep.positionIterations; ++i)
410 {
411 bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
412 if (contactsOkay)
413 {
414 break;
415 }
416 }
417
418#if 0
419 // Is the new position really safe?
420 for (int32 i = 0; i < m_contactCount; ++i)
421 {
422 b2Contact* c = m_contacts[i];
423 b2Fixture* fA = c->GetFixtureA();
424 b2Fixture* fB = c->GetFixtureB();
425
426 b2Body* bA = fA->GetBody();
427 b2Body* bB = fB->GetBody();
428
429 int32 indexA = c->GetChildIndexA();
430 int32 indexB = c->GetChildIndexB();
431
432 b2DistanceInput input;
433 input.proxyA.Set(fA->GetShape(), indexA);
434 input.proxyB.Set(fB->GetShape(), indexB);
435 input.transformA = bA->GetTransform();
436 input.transformB = bB->GetTransform();
437 input.useRadii = false;
438
439 b2DistanceOutput output;
440 b2SimplexCache cache;
441 cache.count = 0;
442 b2Distance(&output, &cache, &input);
443
444 if (output.distance == 0 || cache.count == 3)
445 {
446 cache.count += 0;
447 }
448 }
449#endif
450
451 // Leap of faith to new safe state.
452 m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;
453 m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;
454 m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;
455 m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;
456
457 // No warm starting is needed for TOI events because warm
458 // starting impulses were applied in the discrete solver.
459 contactSolver.InitializeVelocityConstraints();
460
461 // Solve velocity constraints.
462 for (int32 i = 0; i < subStep.velocityIterations; ++i)
463 {
464 contactSolver.SolveVelocityConstraints();
465 }
466
467 // Don't store the TOI contact forces for warm starting
468 // because they can be quite large.
469
470 float32 h = subStep.dt;
471
472 // Integrate positions
473 for (int32 i = 0; i < m_bodyCount; ++i)
474 {
475 b2Vec2 c = m_positions[i].c;
476 float32 a = m_positions[i].a;
477 b2Vec2 v = m_velocities[i].v;
478 float32 w = m_velocities[i].w;
479
480 // Check for large velocities
481 b2Vec2 translation = h * v;
482 if (b2Dot(translation, translation) > b2_maxTranslationSquared)
483 {
484 float32 ratio = b2_maxTranslation / translation.Length();
485 v *= ratio;
486 }
487
488 float32 rotation = h * w;
489 if (rotation * rotation > b2_maxRotationSquared)
490 {
491 float32 ratio = b2_maxRotation / b2Abs(rotation);
492 w *= ratio;
493 }
494
495 // Integrate
496 c += h * v;
497 a += h * w;
498
499 m_positions[i].c = c;
500 m_positions[i].a = a;
501 m_velocities[i].v = v;
502 m_velocities[i].w = w;
503
504 // Sync bodies
505 b2Body* body = m_bodies[i];
506 body->m_sweep.c = c;
507 body->m_sweep.a = a;
508 body->m_linearVelocity = v;
509 body->m_angularVelocity = w;
510 body->SynchronizeTransform();
511 }
512
513 Report(contactSolver.m_velocityConstraints);
514}
515
516void b2Island::Report(const b2ContactVelocityConstraint* constraints)
517{
518 if (m_listener == NULL)
519 {
520 return;
521 }
522
523 for (int32 i = 0; i < m_contactCount; ++i)
524 {
525 b2Contact* c = m_contacts[i];
526
527 const b2ContactVelocityConstraint* vc = constraints + i;
528
529 b2ContactImpulse impulse;
530 impulse.count = vc->pointCount;
531 for (int32 j = 0; j < vc->pointCount; ++j)
532 {
533 impulse.normalImpulses[j] = vc->points[j].normalImpulse;
534 impulse.tangentImpulses[j] = vc->points[j].tangentImpulse;
535 }
536
537 m_listener->PostSolve(c, &impulse);
538 }
539}
540