1 /*
2 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
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 "b2ContactSolver.h"
20 #include "b2Contact.h"
21 #include "../b2Body.h"
22 #include "../b2World.h"
23 #include "../../Common/b2StackAllocator.h"
24
b2ContactSolver(const b2TimeStep & step,b2Contact ** contacts,int32 contactCount,b2StackAllocator * allocator)25 b2ContactSolver::b2ContactSolver(const b2TimeStep& step, b2Contact** contacts, int32 contactCount, b2StackAllocator* allocator)
26 {
27 m_step = step;
28 m_allocator = allocator;
29
30 m_constraintCount = 0;
31 for (int32 i = 0; i < contactCount; ++i)
32 {
33 b2Assert(contacts[i]->IsSolid());
34 m_constraintCount += contacts[i]->GetManifoldCount();
35 }
36
37 m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint));
38
39 int32 count = 0;
40 for (int32 i = 0; i < contactCount; ++i)
41 {
42 b2Contact* contact = contacts[i];
43
44 b2Body* b1 = contact->m_shape1->GetBody();
45 b2Body* b2 = contact->m_shape2->GetBody();
46 int32 manifoldCount = contact->GetManifoldCount();
47 b2Manifold* manifolds = contact->GetManifolds();
48 float32 friction = contact->m_friction;
49 float32 restitution = contact->m_restitution;
50
51 b2Vec2 v1 = b1->m_linearVelocity;
52 b2Vec2 v2 = b2->m_linearVelocity;
53 float32 w1 = b1->m_angularVelocity;
54 float32 w2 = b2->m_angularVelocity;
55
56 for (int32 j = 0; j < manifoldCount; ++j)
57 {
58 b2Manifold* manifold = manifolds + j;
59
60 b2Assert(manifold->pointCount > 0);
61
62 const b2Vec2 normal = manifold->normal;
63
64 b2Assert(count < m_constraintCount);
65 b2ContactConstraint* c = m_constraints + count;
66 c->body1 = b1;
67 c->body2 = b2;
68 c->manifold = manifold;
69 c->normal = normal;
70 c->pointCount = manifold->pointCount;
71 c->friction = friction;
72 c->restitution = restitution;
73
74 for (int32 k = 0; k < c->pointCount; ++k)
75 {
76 b2ManifoldPoint* cp = manifold->points + k;
77 b2ContactConstraintPoint* ccp = c->points + k;
78
79 ccp->normalImpulse = cp->normalImpulse;
80 ccp->tangentImpulse = cp->tangentImpulse;
81 ccp->separation = cp->separation;
82 ccp->positionImpulse = 0.0f;
83
84 ccp->localAnchor1 = cp->localPoint1;
85 ccp->localAnchor2 = cp->localPoint2;
86 ccp->r1 = b2Mul(b1->GetXForm().R, cp->localPoint1 - b1->GetLocalCenter());
87 ccp->r2 = b2Mul(b2->GetXForm().R, cp->localPoint2 - b2->GetLocalCenter());
88
89 float32 r1Sqr = b2Dot(ccp->r1, ccp->r1);
90 float32 r2Sqr = b2Dot(ccp->r2, ccp->r2);
91 float32 rn1 = b2Dot(ccp->r1, normal);
92 float32 rn2 = b2Dot(ccp->r2, normal);
93
94 float32 kNormal = b1->m_invMass + b2->m_invMass;
95 kNormal += b1->m_invI * (r1Sqr - rn1 * rn1) + b2->m_invI * (r2Sqr - rn2 * rn2);
96
97 b2Assert(kNormal > B2_FLT_EPSILON);
98 ccp->normalMass = 1.0f / kNormal;
99
100 float32 kEqualized = b1->m_mass * b1->m_invMass + b2->m_mass * b2->m_invMass;
101 kEqualized += b1->m_mass * b1->m_invI * (r1Sqr - rn1 * rn1) + b2->m_mass * b2->m_invI * (r2Sqr - rn2 * rn2);
102
103 b2Assert(kEqualized > B2_FLT_EPSILON);
104 ccp->equalizedMass = 1.0f / kEqualized;
105
106 b2Vec2 tangent = b2Cross(normal, 1.0f);
107
108 float32 rt1 = b2Dot(ccp->r1, tangent);
109 float32 rt2 = b2Dot(ccp->r2, tangent);
110 float32 kTangent = b1->m_invMass + b2->m_invMass;
111 kTangent += b1->m_invI * (r1Sqr - rt1 * rt1) + b2->m_invI * (r2Sqr - rt2 * rt2);
112
113 b2Assert(kTangent > B2_FLT_EPSILON);
114 ccp->tangentMass = 1.0f / kTangent;
115
116 // Setup a velocity bias for restitution.
117 ccp->velocityBias = 0.0f;
118 if (ccp->separation > 0.0f)
119 {
120 ccp->velocityBias = -60.0f * ccp->separation; // TODO_ERIN b2TimeStep
121 }
122
123 float32 vRel = b2Dot(c->normal, v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1));
124 if (vRel < -b2_velocityThreshold)
125 {
126 ccp->velocityBias += -c->restitution * vRel;
127 }
128 }
129
130 ++count;
131 }
132 }
133
134 b2Assert(count == m_constraintCount);
135 }
136
~b2ContactSolver()137 b2ContactSolver::~b2ContactSolver()
138 {
139 m_allocator->Free(m_constraints);
140 }
141
InitVelocityConstraints(const b2TimeStep & step)142 void b2ContactSolver::InitVelocityConstraints(const b2TimeStep& step)
143 {
144 // Warm start.
145 for (int32 i = 0; i < m_constraintCount; ++i)
146 {
147 b2ContactConstraint* c = m_constraints + i;
148
149 b2Body* b1 = c->body1;
150 b2Body* b2 = c->body2;
151 float32 invMass1 = b1->m_invMass;
152 float32 invI1 = b1->m_invI;
153 float32 invMass2 = b2->m_invMass;
154 float32 invI2 = b2->m_invI;
155 b2Vec2 normal = c->normal;
156 b2Vec2 tangent = b2Cross(normal, 1.0f);
157
158 if (step.warmStarting)
159 {
160 for (int32 j = 0; j < c->pointCount; ++j)
161 {
162 b2ContactConstraintPoint* ccp = c->points + j;
163 ccp->normalImpulse *= step.dtRatio;
164 ccp->tangentImpulse *= step.dtRatio;
165 b2Vec2 P = ccp->normalImpulse * normal + ccp->tangentImpulse * tangent;
166 b1->m_angularVelocity -= invI1 * b2Cross(ccp->r1, P);
167 b1->m_linearVelocity -= invMass1 * P;
168 b2->m_angularVelocity += invI2 * b2Cross(ccp->r2, P);
169 b2->m_linearVelocity += invMass2 * P;
170 }
171 }
172 else
173 {
174 for (int32 j = 0; j < c->pointCount; ++j)
175 {
176 b2ContactConstraintPoint* ccp = c->points + j;
177 ccp->normalImpulse = 0.0f;
178 ccp->tangentImpulse = 0.0f;
179 }
180 }
181 }
182 }
183
SolveVelocityConstraints()184 void b2ContactSolver::SolveVelocityConstraints()
185 {
186 for (int32 i = 0; i < m_constraintCount; ++i)
187 {
188 b2ContactConstraint* c = m_constraints + i;
189 b2Body* b1 = c->body1;
190 b2Body* b2 = c->body2;
191 float32 w1 = b1->m_angularVelocity;
192 float32 w2 = b2->m_angularVelocity;
193 b2Vec2 v1 = b1->m_linearVelocity;
194 b2Vec2 v2 = b2->m_linearVelocity;
195 float32 invMass1 = b1->m_invMass;
196 float32 invI1 = b1->m_invI;
197 float32 invMass2 = b2->m_invMass;
198 float32 invI2 = b2->m_invI;
199 b2Vec2 normal = c->normal;
200 b2Vec2 tangent = b2Cross(normal, 1.0f);
201 float32 friction = c->friction;
202 //#define DEFERRED_UPDATE
203 #ifdef DEFERRED_UPDATE
204 b2Vec2 b1_linearVelocity = b1->m_linearVelocity;
205 float32 b1_angularVelocity = b1->m_angularVelocity;
206 b2Vec2 b2_linearVelocity = b2->m_linearVelocity;
207 float32 b2_angularVelocity = b2->m_angularVelocity;
208 #endif
209 // Solve normal constraints
210 for (int32 j = 0; j < c->pointCount; ++j)
211 {
212 b2ContactConstraintPoint* ccp = c->points + j;
213
214 // Relative velocity at contact
215 b2Vec2 dv = v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1);
216
217 // Compute normal impulse
218 float32 vn = b2Dot(dv, normal);
219 float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias);
220
221 // b2Clamp the accumulated impulse
222 float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f);
223 lambda = newImpulse - ccp->normalImpulse;
224
225 // Apply contact impulse
226 b2Vec2 P = lambda * normal;
227 #ifdef DEFERRED_UPDATE
228 b1_linearVelocity -= invMass1 * P;
229 b1_angularVelocity -= invI1 * b2Cross(r1, P);
230
231 b2_linearVelocity += invMass2 * P;
232 b2_angularVelocity += invI2 * b2Cross(r2, P);
233 #else
234 v1 -= invMass1 * P;
235 w1 -= invI1 * b2Cross(ccp->r1, P);
236
237 v2 += invMass2 * P;
238 w2 += invI2 * b2Cross(ccp->r2, P);
239 #endif
240 ccp->normalImpulse = newImpulse;
241 }
242
243 #ifdef DEFERRED_UPDATE
244 b1->m_linearVelocity = b1_linearVelocity;
245 b1->m_angularVelocity = b1_angularVelocity;
246 b2->m_linearVelocity = b2_linearVelocity;
247 b2->m_angularVelocity = b2_angularVelocity;
248 #endif
249 // Solve tangent constraints
250 for (int32 j = 0; j < c->pointCount; ++j)
251 {
252 b2ContactConstraintPoint* ccp = c->points + j;
253
254 // Relative velocity at contact
255 b2Vec2 dv = v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1);
256
257 // Compute tangent force
258 float32 vt = b2Dot(dv, tangent);
259 float32 lambda = ccp->tangentMass * (-vt);
260
261 // b2Clamp the accumulated force
262 float32 maxFriction = friction * ccp->normalImpulse;
263 float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction);
264 lambda = newImpulse - ccp->tangentImpulse;
265
266 // Apply contact impulse
267 b2Vec2 P = lambda * tangent;
268
269 v1 -= invMass1 * P;
270 w1 -= invI1 * b2Cross(ccp->r1, P);
271
272 v2 += invMass2 * P;
273 w2 += invI2 * b2Cross(ccp->r2, P);
274
275 ccp->tangentImpulse = newImpulse;
276 }
277
278 b1->m_linearVelocity = v1;
279 b1->m_angularVelocity = w1;
280 b2->m_linearVelocity = v2;
281 b2->m_angularVelocity = w2;
282 }
283 }
284
FinalizeVelocityConstraints()285 void b2ContactSolver::FinalizeVelocityConstraints()
286 {
287 for (int32 i = 0; i < m_constraintCount; ++i)
288 {
289 b2ContactConstraint* c = m_constraints + i;
290 b2Manifold* m = c->manifold;
291
292 for (int32 j = 0; j < c->pointCount; ++j)
293 {
294 m->points[j].normalImpulse = c->points[j].normalImpulse;
295 m->points[j].tangentImpulse = c->points[j].tangentImpulse;
296 }
297 }
298 }
299
SolvePositionConstraints(float32 baumgarte)300 bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte)
301 {
302 float32 minSeparation = 0.0f;
303
304 for (int32 i = 0; i < m_constraintCount; ++i)
305 {
306 b2ContactConstraint* c = m_constraints + i;
307 b2Body* b1 = c->body1;
308 b2Body* b2 = c->body2;
309 float32 invMass1 = b1->m_mass * b1->m_invMass;
310 float32 invI1 = b1->m_mass * b1->m_invI;
311 float32 invMass2 = b2->m_mass * b2->m_invMass;
312 float32 invI2 = b2->m_mass * b2->m_invI;
313
314 b2Vec2 normal = c->normal;
315
316 // Solver normal constraints
317 for (int32 j = 0; j < c->pointCount; ++j)
318 {
319 b2ContactConstraintPoint* ccp = c->points + j;
320
321 b2Vec2 r1 = b2Mul(b1->GetXForm().R, ccp->localAnchor1 - b1->GetLocalCenter());
322 b2Vec2 r2 = b2Mul(b2->GetXForm().R, ccp->localAnchor2 - b2->GetLocalCenter());
323
324 b2Vec2 p1 = b1->m_sweep.c + r1;
325 b2Vec2 p2 = b2->m_sweep.c + r2;
326 b2Vec2 dp = p2 - p1;
327
328 // Approximate the current separation.
329 float32 separation = b2Dot(dp, normal) + ccp->separation;
330
331 // Track max constraint error.
332 minSeparation = b2Min(minSeparation, separation);
333
334 // Prevent large corrections and allow slop.
335 float32 C = baumgarte * b2Clamp(separation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
336
337 // Compute normal impulse
338 float32 dImpulse = -ccp->equalizedMass * C;
339
340 // b2Clamp the accumulated impulse
341 float32 impulse0 = ccp->positionImpulse;
342 ccp->positionImpulse = b2Max(impulse0 + dImpulse, 0.0f);
343 dImpulse = ccp->positionImpulse - impulse0;
344
345 b2Vec2 impulse = dImpulse * normal;
346
347 b1->m_sweep.c -= invMass1 * impulse;
348 b1->m_sweep.a -= invI1 * b2Cross(r1, impulse);
349 b1->SynchronizeTransform();
350
351 b2->m_sweep.c += invMass2 * impulse;
352 b2->m_sweep.a += invI2 * b2Cross(r2, impulse);
353 b2->SynchronizeTransform();
354 }
355 }
356
357 // We can't expect minSpeparation >= -b2_linearSlop because we don't
358 // push the separation above -b2_linearSlop.
359 return minSeparation >= -1.5f * b2_linearSlop;
360 }
361