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 <Box2D/Dynamics/Joints/b2LineJoint.h>
20 #include <Box2D/Dynamics/b2Body.h>
21 #include <Box2D/Dynamics/b2TimeStep.h>
22
23 // Linear constraint (point-to-line)
24 // d = p2 - p1 = x2 + r2 - x1 - r1
25 // C = dot(perp, d)
26 // Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
27 // = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
28 // J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
29 //
30 // K = J * invM * JT
31 //
32 // J = [-a -s1 a s2]
33 // a = perp
34 // s1 = cross(d + r1, a) = cross(p2 - x1, a)
35 // s2 = cross(r2, a) = cross(p2 - x2, a)
36
37
38 // Motor/Limit linear constraint
39 // C = dot(ax1, d)
40 // Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
41 // J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
42
43 // Block Solver
44 // We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
45 // when the mass has poor distribution (leading to large torques about the joint anchor points).
46 //
47 // The Jacobian has 3 rows:
48 // J = [-uT -s1 uT s2] // linear
49 // [-vT -a1 vT a2] // limit
50 //
51 // u = perp
52 // v = axis
53 // s1 = cross(d + r1, u), s2 = cross(r2, u)
54 // a1 = cross(d + r1, v), a2 = cross(r2, v)
55
56 // M * (v2 - v1) = JT * df
57 // J * v2 = bias
58 //
59 // v2 = v1 + invM * JT * df
60 // J * (v1 + invM * JT * df) = bias
61 // K * df = bias - J * v1 = -Cdot
62 // K = J * invM * JT
63 // Cdot = J * v1 - bias
64 //
65 // Now solve for f2.
66 // df = f2 - f1
67 // K * (f2 - f1) = -Cdot
68 // f2 = invK * (-Cdot) + f1
69 //
70 // Clamp accumulated limit impulse.
71 // lower: f2(2) = max(f2(2), 0)
72 // upper: f2(2) = min(f2(2), 0)
73 //
74 // Solve for correct f2(1)
75 // K(1,1) * f2(1) = -Cdot(1) - K(1,2) * f2(2) + K(1,1:2) * f1
76 // = -Cdot(1) - K(1,2) * f2(2) + K(1,1) * f1(1) + K(1,2) * f1(2)
77 // K(1,1) * f2(1) = -Cdot(1) - K(1,2) * (f2(2) - f1(2)) + K(1,1) * f1(1)
78 // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
79 //
80 // Now compute impulse to be applied:
81 // df = f2 - f1
82
Initialize(b2Body * b1,b2Body * b2,const b2Vec2 & anchor,const b2Vec2 & axis)83 void b2LineJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis)
84 {
85 bodyA = b1;
86 bodyB = b2;
87 localAnchorA = bodyA->GetLocalPoint(anchor);
88 localAnchorB = bodyB->GetLocalPoint(anchor);
89 localAxisA = bodyA->GetLocalVector(axis);
90 }
91
b2LineJoint(const b2LineJointDef * def)92 b2LineJoint::b2LineJoint(const b2LineJointDef* def)
93 : b2Joint(def)
94 {
95 m_localAnchor1 = def->localAnchorA;
96 m_localAnchor2 = def->localAnchorB;
97 m_localXAxis1 = def->localAxisA;
98 m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);
99
100 m_impulse.SetZero();
101 m_motorMass = 0.0;
102 m_motorImpulse = 0.0f;
103
104 m_lowerTranslation = def->lowerTranslation;
105 m_upperTranslation = def->upperTranslation;
106 m_maxMotorForce = def->maxMotorForce;
107 m_motorSpeed = def->motorSpeed;
108 m_enableLimit = def->enableLimit;
109 m_enableMotor = def->enableMotor;
110 m_limitState = e_inactiveLimit;
111
112 m_axis.SetZero();
113 m_perp.SetZero();
114 }
115
InitVelocityConstraints(const b2TimeStep & step)116 void b2LineJoint::InitVelocityConstraints(const b2TimeStep& step)
117 {
118 b2Body* b1 = m_bodyA;
119 b2Body* b2 = m_bodyB;
120
121 m_localCenterA = b1->GetLocalCenter();
122 m_localCenterB = b2->GetLocalCenter();
123
124 b2Transform xf1 = b1->GetTransform();
125 b2Transform xf2 = b2->GetTransform();
126
127 // Compute the effective masses.
128 b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA);
129 b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB);
130 b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
131
132 m_invMassA = b1->m_invMass;
133 m_invIA = b1->m_invI;
134 m_invMassB = b2->m_invMass;
135 m_invIB = b2->m_invI;
136
137 // Compute motor Jacobian and effective mass.
138 {
139 m_axis = b2Mul(xf1.R, m_localXAxis1);
140 m_a1 = b2Cross(d + r1, m_axis);
141 m_a2 = b2Cross(r2, m_axis);
142
143 m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;
144 if (m_motorMass > b2_epsilon)
145 {
146 m_motorMass = 1.0f / m_motorMass;
147 }
148 else
149 {
150 m_motorMass = 0.0f;
151 }
152 }
153
154 // Prismatic constraint.
155 {
156 m_perp = b2Mul(xf1.R, m_localYAxis1);
157
158 m_s1 = b2Cross(d + r1, m_perp);
159 m_s2 = b2Cross(r2, m_perp);
160
161 qreal m1 = m_invMassA, m2 = m_invMassB;
162 qreal i1 = m_invIA, i2 = m_invIB;
163
164 qreal k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
165 qreal k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
166 qreal k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
167
168 m_K.col1.Set(k11, k12);
169 m_K.col2.Set(k12, k22);
170 }
171
172 // Compute motor and limit terms.
173 if (m_enableLimit)
174 {
175 qreal jointTranslation = b2Dot(m_axis, d);
176 if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
177 {
178 m_limitState = e_equalLimits;
179 }
180 else if (jointTranslation <= m_lowerTranslation)
181 {
182 if (m_limitState != e_atLowerLimit)
183 {
184 m_limitState = e_atLowerLimit;
185 m_impulse.y = 0.0f;
186 }
187 }
188 else if (jointTranslation >= m_upperTranslation)
189 {
190 if (m_limitState != e_atUpperLimit)
191 {
192 m_limitState = e_atUpperLimit;
193 m_impulse.y = 0.0f;
194 }
195 }
196 else
197 {
198 m_limitState = e_inactiveLimit;
199 m_impulse.y = 0.0f;
200 }
201 }
202 else
203 {
204 m_limitState = e_inactiveLimit;
205 }
206
207 if (m_enableMotor == false)
208 {
209 m_motorImpulse = 0.0f;
210 }
211
212 if (step.warmStarting)
213 {
214 // Account for variable time step.
215 m_impulse *= step.dtRatio;
216 m_motorImpulse *= step.dtRatio;
217
218 b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis;
219 qreal L1 = m_impulse.x * m_s1 + (m_motorImpulse + m_impulse.y) * m_a1;
220 qreal L2 = m_impulse.x * m_s2 + (m_motorImpulse + m_impulse.y) * m_a2;
221
222 b1->m_linearVelocity -= m_invMassA * P;
223 b1->m_angularVelocity -= m_invIA * L1;
224
225 b2->m_linearVelocity += m_invMassB * P;
226 b2->m_angularVelocity += m_invIB * L2;
227 }
228 else
229 {
230 m_impulse.SetZero();
231 m_motorImpulse = 0.0f;
232 }
233 }
234
SolveVelocityConstraints(const b2TimeStep & step)235 void b2LineJoint::SolveVelocityConstraints(const b2TimeStep& step)
236 {
237 b2Body* b1 = m_bodyA;
238 b2Body* b2 = m_bodyB;
239
240 b2Vec2 v1 = b1->m_linearVelocity;
241 qreal w1 = b1->m_angularVelocity;
242 b2Vec2 v2 = b2->m_linearVelocity;
243 qreal w2 = b2->m_angularVelocity;
244
245 // Solve linear motor constraint.
246 if (m_enableMotor && m_limitState != e_equalLimits)
247 {
248 qreal Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
249 qreal impulse = m_motorMass * (m_motorSpeed - Cdot);
250 qreal oldImpulse = m_motorImpulse;
251 qreal maxImpulse = step.dt * m_maxMotorForce;
252 m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
253 impulse = m_motorImpulse - oldImpulse;
254
255 b2Vec2 P = impulse * m_axis;
256 qreal L1 = impulse * m_a1;
257 qreal L2 = impulse * m_a2;
258
259 v1 -= m_invMassA * P;
260 w1 -= m_invIA * L1;
261
262 v2 += m_invMassB * P;
263 w2 += m_invIB * L2;
264 }
265
266 qreal Cdot1 = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;
267
268 if (m_enableLimit && m_limitState != e_inactiveLimit)
269 {
270 // Solve prismatic and limit constraint in block form.
271 qreal Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
272 b2Vec2 Cdot(Cdot1, Cdot2);
273
274 b2Vec2 f1 = m_impulse;
275 b2Vec2 df = m_K.Solve(-Cdot);
276 m_impulse += df;
277
278 if (m_limitState == e_atLowerLimit)
279 {
280 m_impulse.y = b2Max(m_impulse.y, 0.0f);
281 }
282 else if (m_limitState == e_atUpperLimit)
283 {
284 m_impulse.y = b2Min(m_impulse.y, 0.0f);
285 }
286
287 // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
288 qreal b = -Cdot1 - (m_impulse.y - f1.y) * m_K.col2.x;
289 qreal f2r;
290 if (m_K.col1.x != 0.0f)
291 {
292 f2r = b / m_K.col1.x + f1.x;
293 }
294 else
295 {
296 f2r = f1.x;
297 }
298
299 m_impulse.x = f2r;
300
301 df = m_impulse - f1;
302
303 b2Vec2 P = df.x * m_perp + df.y * m_axis;
304 qreal L1 = df.x * m_s1 + df.y * m_a1;
305 qreal L2 = df.x * m_s2 + df.y * m_a2;
306
307 v1 -= m_invMassA * P;
308 w1 -= m_invIA * L1;
309
310 v2 += m_invMassB * P;
311 w2 += m_invIB * L2;
312 }
313 else
314 {
315 // Limit is inactive, just solve the prismatic constraint in block form.
316 qreal df;
317 if (m_K.col1.x != 0.0f)
318 {
319 df = - Cdot1 / m_K.col1.x;
320 }
321 else
322 {
323 df = 0.0f;
324 }
325 m_impulse.x += df;
326
327 b2Vec2 P = df * m_perp;
328 qreal L1 = df * m_s1;
329 qreal L2 = df * m_s2;
330
331 v1 -= m_invMassA * P;
332 w1 -= m_invIA * L1;
333
334 v2 += m_invMassB * P;
335 w2 += m_invIB * L2;
336 }
337
338 b1->m_linearVelocity = v1;
339 b1->m_angularVelocity = w1;
340 b2->m_linearVelocity = v2;
341 b2->m_angularVelocity = w2;
342 }
343
SolvePositionConstraints(qreal baumgarte)344 bool b2LineJoint::SolvePositionConstraints(qreal baumgarte)
345 {
346 B2_NOT_USED(baumgarte);
347
348 b2Body* b1 = m_bodyA;
349 b2Body* b2 = m_bodyB;
350
351 b2Vec2 c1 = b1->m_sweep.c;
352 qreal a1 = b1->m_sweep.a;
353
354 b2Vec2 c2 = b2->m_sweep.c;
355 qreal a2 = b2->m_sweep.a;
356
357 // Solve linear limit constraint.
358 qreal linearError = 0.0f, angularError = 0.0f;
359 bool active = false;
360 qreal C2 = 0.0f;
361
362 b2Mat22 R1(a1), R2(a2);
363
364 b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA);
365 b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB);
366 b2Vec2 d = c2 + r2 - c1 - r1;
367
368 if (m_enableLimit)
369 {
370 m_axis = b2Mul(R1, m_localXAxis1);
371
372 m_a1 = b2Cross(d + r1, m_axis);
373 m_a2 = b2Cross(r2, m_axis);
374
375 qreal translation = b2Dot(m_axis, d);
376 if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
377 {
378 // Prevent large angular corrections
379 C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
380 linearError = b2Abs(translation);
381 active = true;
382 }
383 else if (translation <= m_lowerTranslation)
384 {
385 // Prevent large linear corrections and allow some slop.
386 C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
387 linearError = m_lowerTranslation - translation;
388 active = true;
389 }
390 else if (translation >= m_upperTranslation)
391 {
392 // Prevent large linear corrections and allow some slop.
393 C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
394 linearError = translation - m_upperTranslation;
395 active = true;
396 }
397 }
398
399 m_perp = b2Mul(R1, m_localYAxis1);
400
401 m_s1 = b2Cross(d + r1, m_perp);
402 m_s2 = b2Cross(r2, m_perp);
403
404 b2Vec2 impulse;
405 qreal C1;
406 C1 = b2Dot(m_perp, d);
407
408 linearError = b2Max(linearError, b2Abs(C1));
409 angularError = 0.0f;
410
411 if (active)
412 {
413 qreal m1 = m_invMassA, m2 = m_invMassB;
414 qreal i1 = m_invIA, i2 = m_invIB;
415
416 qreal k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
417 qreal k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
418 qreal k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
419
420 m_K.col1.Set(k11, k12);
421 m_K.col2.Set(k12, k22);
422
423 b2Vec2 C;
424 C.x = C1;
425 C.y = C2;
426
427 impulse = m_K.Solve(-C);
428 }
429 else
430 {
431 qreal m1 = m_invMassA, m2 = m_invMassB;
432 qreal i1 = m_invIA, i2 = m_invIB;
433
434 qreal k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
435
436 qreal impulse1;
437 if (k11 != 0.0f)
438 {
439 impulse1 = - C1 / k11;
440 }
441 else
442 {
443 impulse1 = 0.0f;
444 }
445
446 impulse.x = impulse1;
447 impulse.y = 0.0f;
448 }
449
450 b2Vec2 P = impulse.x * m_perp + impulse.y * m_axis;
451 qreal L1 = impulse.x * m_s1 + impulse.y * m_a1;
452 qreal L2 = impulse.x * m_s2 + impulse.y * m_a2;
453
454 c1 -= m_invMassA * P;
455 a1 -= m_invIA * L1;
456 c2 += m_invMassB * P;
457 a2 += m_invIB * L2;
458
459 // TODO_ERIN remove need for this.
460 b1->m_sweep.c = c1;
461 b1->m_sweep.a = a1;
462 b2->m_sweep.c = c2;
463 b2->m_sweep.a = a2;
464 b1->SynchronizeTransform();
465 b2->SynchronizeTransform();
466
467 return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
468 }
469
GetAnchorA() const470 b2Vec2 b2LineJoint::GetAnchorA() const
471 {
472 return m_bodyA->GetWorldPoint(m_localAnchor1);
473 }
474
GetAnchorB() const475 b2Vec2 b2LineJoint::GetAnchorB() const
476 {
477 return m_bodyB->GetWorldPoint(m_localAnchor2);
478 }
479
GetReactionForce(qreal inv_dt) const480 b2Vec2 b2LineJoint::GetReactionForce(qreal inv_dt) const
481 {
482 return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis);
483 }
484
GetReactionTorque(qreal inv_dt) const485 qreal b2LineJoint::GetReactionTorque(qreal inv_dt) const
486 {
487 B2_NOT_USED(inv_dt);
488 return 0.0f;
489 }
490
GetJointTranslation() const491 qreal b2LineJoint::GetJointTranslation() const
492 {
493 b2Body* b1 = m_bodyA;
494 b2Body* b2 = m_bodyB;
495
496 b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1);
497 b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2);
498 b2Vec2 d = p2 - p1;
499 b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
500
501 qreal translation = b2Dot(d, axis);
502 return translation;
503 }
504
GetJointSpeed() const505 qreal b2LineJoint::GetJointSpeed() const
506 {
507 b2Body* b1 = m_bodyA;
508 b2Body* b2 = m_bodyB;
509
510 b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
511 b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
512 b2Vec2 p1 = b1->m_sweep.c + r1;
513 b2Vec2 p2 = b2->m_sweep.c + r2;
514 b2Vec2 d = p2 - p1;
515 b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
516
517 b2Vec2 v1 = b1->m_linearVelocity;
518 b2Vec2 v2 = b2->m_linearVelocity;
519 qreal w1 = b1->m_angularVelocity;
520 qreal w2 = b2->m_angularVelocity;
521
522 qreal speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1));
523 return speed;
524 }
525
IsLimitEnabled() const526 bool b2LineJoint::IsLimitEnabled() const
527 {
528 return m_enableLimit;
529 }
530
EnableLimit(bool flag)531 void b2LineJoint::EnableLimit(bool flag)
532 {
533 m_bodyA->SetAwake(true);
534 m_bodyB->SetAwake(true);
535 m_enableLimit = flag;
536 }
537
GetLowerLimit() const538 qreal b2LineJoint::GetLowerLimit() const
539 {
540 return m_lowerTranslation;
541 }
542
GetUpperLimit() const543 qreal b2LineJoint::GetUpperLimit() const
544 {
545 return m_upperTranslation;
546 }
547
SetLimits(qreal lower,qreal upper)548 void b2LineJoint::SetLimits(qreal lower, qreal upper)
549 {
550 b2Assert(lower <= upper);
551 m_bodyA->SetAwake(true);
552 m_bodyB->SetAwake(true);
553 m_lowerTranslation = lower;
554 m_upperTranslation = upper;
555 }
556
IsMotorEnabled() const557 bool b2LineJoint::IsMotorEnabled() const
558 {
559 return m_enableMotor;
560 }
561
EnableMotor(bool flag)562 void b2LineJoint::EnableMotor(bool flag)
563 {
564 m_bodyA->SetAwake(true);
565 m_bodyB->SetAwake(true);
566 m_enableMotor = flag;
567 }
568
SetMotorSpeed(qreal speed)569 void b2LineJoint::SetMotorSpeed(qreal speed)
570 {
571 m_bodyA->SetAwake(true);
572 m_bodyB->SetAwake(true);
573 m_motorSpeed = speed;
574 }
575
SetMaxMotorForce(qreal force)576 void b2LineJoint::SetMaxMotorForce(qreal force)
577 {
578 m_bodyA->SetAwake(true);
579 m_bodyB->SetAwake(true);
580 m_maxMotorForce = force;
581 }
582
GetMotorForce(qreal inv_dt) const583 qreal b2LineJoint::GetMotorForce(qreal inv_dt) const
584 {
585 return inv_dt * m_motorImpulse;
586 }
587
588
589
590
591
592