2 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
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.
22 #include "Contacts/b2Contact.h"
23 #include "Contacts/b2ContactSolver.h"
24 #include "Joints/b2Joint.h"
25 #include "../Common/b2StackAllocator.h"
28 Position Correction Notes
29 =========================
30 I tried the several algorithms for position correction of the 2D revolute joint.
31 I looked at these systems:
32 - simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
33 - suspension bridge with 30 1m long planks of length 1m.
34 - multi-link chain with 30 1m long links.
36 Here are the algorithms:
38 Baumgarte - A fraction of the position error is added to the velocity error. There is no
39 separate position solver.
41 Pseudo Velocities - After the velocity solver and position integration,
42 the position error, Jacobian, and effective mass are recomputed. Then
43 the velocity constraints are solved with pseudo velocities and a fraction
44 of the position error is added to the pseudo velocity error. The pseudo
45 velocities are initialized to zero and there is no warm-starting. After
46 the position solver, the pseudo velocities are added to the positions.
47 This is also called the First Order World method or the Position LCP method.
49 Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
50 position error is re-computed for each constraint and the positions are updated
51 after the constraint is solved. The radius vectors (aka Jacobians) are
52 re-computed too (otherwise the algorithm has horrible instability). The pseudo
53 velocity states are not needed because they are effectively zero at the beginning
54 of each iteration. Since we have the current position error, we allow the
55 iterations to terminate early if the error becomes smaller than b2_linearSlop.
57 Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
58 each time a constraint is solved.
61 Baumgarte - this is the cheapest algorithm but it has some stability problems,
62 especially with the bridge. The chain links separate easily close to the root
63 and they jitter as they struggle to pull together. This is one of the most common
64 methods in the field. The big drawback is that the position correction artificially
65 affects the momentum, thus leading to instabilities and false bounce. I used a
66 bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
67 factor makes joints and contacts more spongy.
69 Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
70 stable. However, joints still separate with large angular velocities. Drag the
71 simple pendulum in a circle quickly and the joint will separate. The chain separates
72 easily and does not recover. I used a bias factor of 0.2. A larger value lead to
73 the bridge collapsing when a heavy cube drops on it.
75 Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
76 Velocities, but in other ways it is worse. The bridge and chain are much more
77 stable, but the simple pendulum goes unstable at high angular velocities.
79 Full NGS - stable in all tests. The joints display good stiffness. The bridge
80 still sags, but this is better than infinite forces.
83 Pseudo Velocities are not really worthwhile because the bridge and chain cannot
84 recover from joint separation. In other cases the benefit over Baumgarte is small.
86 Modified NGS is not a robust method for the revolute joint due to the violent
87 instability seen in the simple pendulum. Perhaps it is viable with other constraint
88 types, especially scalar constraints where the effective mass is a scalar.
90 This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
91 and is very fast. I don't think we can escape Baumgarte, especially in highly
92 demanding cases where high constraint fidelity is not needed.
94 Full NGS is robust and easy on the eyes. I recommend this as an option for
95 higher fidelity simulation and certainly for suspension bridges and long chains.
96 Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
97 joint separation can be problematic. The number of NGS iterations can be reduced
98 for better performance without harming robustness much.
100 Each joint in a can be handled differently in the position solver. So I recommend
101 a system where the user can select the algorithm on a per joint basis. I would
102 probably default to the slower Full NGS and let the user select the faster
103 Baumgarte method in performance critical scenarios.
108 int32 contactCapacity,
110 b2StackAllocator* allocator,
111 b2ContactListener* listener)
113 m_bodyCapacity = bodyCapacity;
114 m_contactCapacity = contactCapacity;
115 m_jointCapacity = jointCapacity;
120 m_allocator = allocator;
121 m_listener = listener;
123 m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*));
124 m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity * sizeof(b2Contact*));
125 m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*));
127 m_positionIterationCount = 0;
130 b2Island::~b2Island()
132 // Warning: the order should reverse the constructor order.
133 m_allocator->Free(m_joints);
134 m_allocator->Free(m_contacts);
135 m_allocator->Free(m_bodies);
138 void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool correctPositions, bool allowSleep)
140 // Integrate velocities and apply damping.
141 for (int32 i = 0; i < m_bodyCount; ++i)
143 b2Body* b = m_bodies[i];
148 // Integrate velocities.
149 b->m_linearVelocity += step.dt * (gravity + b->m_invMass * b->m_force);
150 b->m_angularVelocity += step.dt * b->m_invI * b->m_torque;
153 b->m_force.Set(0.0f, 0.0f);
157 // ODE: dv/dt + c * v = 0
158 // Solution: v(t) = v0 * exp(-c * t)
159 // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
160 // v2 = exp(-c * dt) * v1
162 // v2 = (1.0f - c * dt) * v1
163 b->m_linearVelocity *= b2Clamp(1.0f - step.dt * b->m_linearDamping, 0.0f, 1.0f);
164 b->m_angularVelocity *= b2Clamp(1.0f - step.dt * b->m_angularDamping, 0.0f, 1.0f);
166 // Check for large velocities.
167 #ifdef TARGET_FLOAT32_IS_FIXED
168 // Fixed point code written this way to prevent
169 // overflows, float code is optimized for speed
171 float32 vMagnitude = b->m_linearVelocity.Length();
172 if(vMagnitude > b2_maxLinearVelocity) {
173 b->m_linearVelocity *= b2_maxLinearVelocity/vMagnitude;
175 b->m_angularVelocity = b2Clamp(b->m_angularVelocity,
176 -b2_maxAngularVelocity, b2_maxAngularVelocity);
180 if (b2Dot(b->m_linearVelocity, b->m_linearVelocity) > b2_maxLinearVelocitySquared)
182 b->m_linearVelocity.Normalize();
183 b->m_linearVelocity *= b2_maxLinearVelocity;
185 if (b->m_angularVelocity * b->m_angularVelocity > b2_maxAngularVelocitySquared)
187 if (b->m_angularVelocity < 0.0f)
189 b->m_angularVelocity = -b2_maxAngularVelocity;
193 b->m_angularVelocity = b2_maxAngularVelocity;
200 b2ContactSolver contactSolver(step, m_contacts, m_contactCount, m_allocator);
202 // Initialize velocity constraints.
203 contactSolver.InitVelocityConstraints(step);
205 for (int32 i = 0; i < m_jointCount; ++i)
207 m_joints[i]->InitVelocityConstraints(step);
210 // Solve velocity constraints.
211 for (int32 i = 0; i < step.maxIterations; ++i)
213 contactSolver.SolveVelocityConstraints();
215 for (int32 j = 0; j < m_jointCount; ++j)
217 m_joints[j]->SolveVelocityConstraints(step);
221 // Post-solve (store impulses for warm starting).
222 contactSolver.FinalizeVelocityConstraints();
224 // Integrate positions.
225 for (int32 i = 0; i < m_bodyCount; ++i)
227 b2Body* b = m_bodies[i];
232 // Store positions for continuous collision.
233 b->m_sweep.c0 = b->m_sweep.c;
234 b->m_sweep.a0 = b->m_sweep.a;
237 b->m_sweep.c += step.dt * b->m_linearVelocity;
238 b->m_sweep.a += step.dt * b->m_angularVelocity;
240 // Compute new transform
241 b->SynchronizeTransform();
243 // Note: shapes are synchronized later.
246 if (correctPositions)
248 // Initialize position constraints.
249 // Contacts don't need initialization.
250 for (int32 i = 0; i < m_jointCount; ++i)
252 m_joints[i]->InitPositionConstraints();
255 // Iterate over constraints.
256 for (m_positionIterationCount = 0; m_positionIterationCount < step.maxIterations; ++m_positionIterationCount)
258 bool contactsOkay = contactSolver.SolvePositionConstraints(b2_contactBaumgarte);
260 bool jointsOkay = true;
261 for (int i = 0; i < m_jointCount; ++i)
263 bool jointOkay = m_joints[i]->SolvePositionConstraints();
264 jointsOkay = jointsOkay && jointOkay;
267 if (contactsOkay && jointsOkay)
274 Report(contactSolver.m_constraints);
278 float32 minSleepTime = B2_FLT_MAX;
280 #ifndef TARGET_FLOAT32_IS_FIXED
281 const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
282 const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
285 for (int32 i = 0; i < m_bodyCount; ++i)
287 b2Body* b = m_bodies[i];
288 if (b->m_invMass == 0.0f)
293 if ((b->m_flags & b2Body::e_allowSleepFlag) == 0)
295 b->m_sleepTime = 0.0f;
299 if ((b->m_flags & b2Body::e_allowSleepFlag) == 0 ||
300 #ifdef TARGET_FLOAT32_IS_FIXED
301 b2Abs(b->m_angularVelocity) > b2_angularSleepTolerance ||
302 b2Abs(b->m_linearVelocity.x) > b2_linearSleepTolerance ||
303 b2Abs(b->m_linearVelocity.y) > b2_linearSleepTolerance)
305 b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
306 b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
309 b->m_sleepTime = 0.0f;
314 b->m_sleepTime += step.dt;
315 minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
319 if (minSleepTime >= b2_timeToSleep)
321 for (int32 i = 0; i < m_bodyCount; ++i)
323 b2Body* b = m_bodies[i];
324 b->m_flags |= b2Body::e_sleepFlag;
325 b->m_linearVelocity = b2Vec2_zero;
326 b->m_angularVelocity = 0.0f;
332 void b2Island::SolveTOI(const b2TimeStep& subStep)
334 b2ContactSolver contactSolver(subStep, m_contacts, m_contactCount, m_allocator);
336 // No warm starting needed for TOI events.
338 // Solve velocity constraints.
339 for (int32 i = 0; i < subStep.maxIterations; ++i)
341 contactSolver.SolveVelocityConstraints();
344 // Don't store the TOI contact forces for warm starting
345 // because they can be quite large.
347 // Integrate positions.
348 for (int32 i = 0; i < m_bodyCount; ++i)
350 b2Body* b = m_bodies[i];
355 // Store positions for continuous collision.
356 b->m_sweep.c0 = b->m_sweep.c;
357 b->m_sweep.a0 = b->m_sweep.a;
360 b->m_sweep.c += subStep.dt * b->m_linearVelocity;
361 b->m_sweep.a += subStep.dt * b->m_angularVelocity;
363 // Compute new transform
364 b->SynchronizeTransform();
366 // Note: shapes are synchronized later.
369 // Solve position constraints.
370 const float32 k_toiBaumgarte = 0.75f;
371 for (int32 i = 0; i < subStep.maxIterations; ++i)
373 bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte);
380 Report(contactSolver.m_constraints);
383 void b2Island::Report(b2ContactConstraint* constraints)
385 if (m_listener == NULL)
390 for (int32 i = 0; i < m_contactCount; ++i)
392 b2Contact* c = m_contacts[i];
393 b2ContactConstraint* cc = constraints + i;
395 cr.shape1 = c->GetShape1();
396 cr.shape2 = c->GetShape2();
397 b2Body* b1 = cr.shape1->GetBody();
398 int32 manifoldCount = c->GetManifoldCount();
399 b2Manifold* manifolds = c->GetManifolds();
400 for (int32 j = 0; j < manifoldCount; ++j)
402 b2Manifold* manifold = manifolds + j;
403 cr.normal = manifold->normal;
404 for (int32 k = 0; k < manifold->pointCount; ++k)
406 b2ManifoldPoint* point = manifold->points + k;
407 b2ContactConstraintPoint* ccp = cc->points + k;
408 cr.position = b1->GetWorldPoint(point->localPoint1);
410 // TOI constraint results are not stored, so get
411 // the result from the constraint.
412 cr.normalImpulse = ccp->normalImpulse;
413 cr.tangentImpulse = ccp->tangentImpulse;
416 m_listener->Result(&cr);