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/Dynamics/Joints/b2RevoluteJoint.h>
20#include <Box2D/Dynamics/b2Body.h>
21#include <Box2D/Dynamics/b2TimeStep.h>
22
23// Point-to-point constraint
24// C = p2 - p1
25// Cdot = v2 - v1
26// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
27// J = [-I -r1_skew I r2_skew ]
28// Identity used:
29// w k % (rx i + ry j) = w * (-ry i + rx j)
30
31// Motor constraint
32// Cdot = w2 - w1
33// J = [0 0 -1 0 0 1]
34// K = invI1 + invI2
35
36void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
37{
38 bodyA = bA;
39 bodyB = bB;
40 localAnchorA = bodyA->GetLocalPoint(anchor);
41 localAnchorB = bodyB->GetLocalPoint(anchor);
42 referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
43}
44
45b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
46: b2Joint(def)
47{
48 m_localAnchorA = def->localAnchorA;
49 m_localAnchorB = def->localAnchorB;
50 m_referenceAngle = def->referenceAngle;
51
52 m_impulse.SetZero();
53 m_motorImpulse = 0.0f;
54
55 m_lowerAngle = def->lowerAngle;
56 m_upperAngle = def->upperAngle;
57 m_maxMotorTorque = def->maxMotorTorque;
58 m_motorSpeed = def->motorSpeed;
59 m_enableLimit = def->enableLimit;
60 m_enableMotor = def->enableMotor;
61 m_limitState = e_inactiveLimit;
62}
63
64void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)
65{
66 m_indexA = m_bodyA->m_islandIndex;
67 m_indexB = m_bodyB->m_islandIndex;
68 m_localCenterA = m_bodyA->m_sweep.localCenter;
69 m_localCenterB = m_bodyB->m_sweep.localCenter;
70 m_invMassA = m_bodyA->m_invMass;
71 m_invMassB = m_bodyB->m_invMass;
72 m_invIA = m_bodyA->m_invI;
73 m_invIB = m_bodyB->m_invI;
74
75 float32 aA = data.positions[m_indexA].a;
76 b2Vec2 vA = data.velocities[m_indexA].v;
77 float32 wA = data.velocities[m_indexA].w;
78
79 float32 aB = data.positions[m_indexB].a;
80 b2Vec2 vB = data.velocities[m_indexB].v;
81 float32 wB = data.velocities[m_indexB].w;
82
83 b2Rot qA(aA), qB(aB);
84
85 m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
86 m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
87
88 // J = [-I -r1_skew I r2_skew]
89 // [ 0 -1 0 1]
90 // r_skew = [-ry; rx]
91
92 // Matlab
93 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
94 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
95 // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
96
97 float32 mA = m_invMassA, mB = m_invMassB;
98 float32 iA = m_invIA, iB = m_invIB;
99
100 bool fixedRotation = (iA + iB == 0.0f);
101
102 m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
103 m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
104 m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
105 m_mass.ex.y = m_mass.ey.x;
106 m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
107 m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
108 m_mass.ex.z = m_mass.ez.x;
109 m_mass.ey.z = m_mass.ez.y;
110 m_mass.ez.z = iA + iB;
111
112 m_motorMass = iA + iB;
113 if (m_motorMass > 0.0f)
114 {
115 m_motorMass = 1.0f / m_motorMass;
116 }
117
118 if (m_enableMotor == false || fixedRotation)
119 {
120 m_motorImpulse = 0.0f;
121 }
122
123 if (m_enableLimit && fixedRotation == false)
124 {
125 float32 jointAngle = aB - aA - m_referenceAngle;
126 if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
127 {
128 m_limitState = e_equalLimits;
129 }
130 else if (jointAngle <= m_lowerAngle)
131 {
132 if (m_limitState != e_atLowerLimit)
133 {
134 m_impulse.z = 0.0f;
135 }
136 m_limitState = e_atLowerLimit;
137 }
138 else if (jointAngle >= m_upperAngle)
139 {
140 if (m_limitState != e_atUpperLimit)
141 {
142 m_impulse.z = 0.0f;
143 }
144 m_limitState = e_atUpperLimit;
145 }
146 else
147 {
148 m_limitState = e_inactiveLimit;
149 m_impulse.z = 0.0f;
150 }
151 }
152 else
153 {
154 m_limitState = e_inactiveLimit;
155 }
156
157 if (data.step.warmStarting)
158 {
159 // Scale impulses to support a variable time step.
160 m_impulse *= data.step.dtRatio;
161 m_motorImpulse *= data.step.dtRatio;
162
163 b2Vec2 P(m_impulse.x, m_impulse.y);
164
165 vA -= mA * P;
166 wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);
167
168 vB += mB * P;
169 wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
170 }
171 else
172 {
173 m_impulse.SetZero();
174 m_motorImpulse = 0.0f;
175 }
176
177 data.velocities[m_indexA].v = vA;
178 data.velocities[m_indexA].w = wA;
179 data.velocities[m_indexB].v = vB;
180 data.velocities[m_indexB].w = wB;
181}
182
183void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data)
184{
185 b2Vec2 vA = data.velocities[m_indexA].v;
186 float32 wA = data.velocities[m_indexA].w;
187 b2Vec2 vB = data.velocities[m_indexB].v;
188 float32 wB = data.velocities[m_indexB].w;
189
190 float32 mA = m_invMassA, mB = m_invMassB;
191 float32 iA = m_invIA, iB = m_invIB;
192
193 bool fixedRotation = (iA + iB == 0.0f);
194
195 // Solve motor constraint.
196 if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false)
197 {
198 float32 Cdot = wB - wA - m_motorSpeed;
199 float32 impulse = -m_motorMass * Cdot;
200 float32 oldImpulse = m_motorImpulse;
201 float32 maxImpulse = data.step.dt * m_maxMotorTorque;
202 m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
203 impulse = m_motorImpulse - oldImpulse;
204
205 wA -= iA * impulse;
206 wB += iB * impulse;
207 }
208
209 // Solve limit constraint.
210 if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
211 {
212 b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
213 float32 Cdot2 = wB - wA;
214 b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
215
216 b2Vec3 impulse = -m_mass.Solve33(Cdot);
217
218 if (m_limitState == e_equalLimits)
219 {
220 m_impulse += impulse;
221 }
222 else if (m_limitState == e_atLowerLimit)
223 {
224 float32 newImpulse = m_impulse.z + impulse.z;
225 if (newImpulse < 0.0f)
226 {
227 b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
228 b2Vec2 reduced = m_mass.Solve22(rhs);
229 impulse.x = reduced.x;
230 impulse.y = reduced.y;
231 impulse.z = -m_impulse.z;
232 m_impulse.x += reduced.x;
233 m_impulse.y += reduced.y;
234 m_impulse.z = 0.0f;
235 }
236 else
237 {
238 m_impulse += impulse;
239 }
240 }
241 else if (m_limitState == e_atUpperLimit)
242 {
243 float32 newImpulse = m_impulse.z + impulse.z;
244 if (newImpulse > 0.0f)
245 {
246 b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
247 b2Vec2 reduced = m_mass.Solve22(rhs);
248 impulse.x = reduced.x;
249 impulse.y = reduced.y;
250 impulse.z = -m_impulse.z;
251 m_impulse.x += reduced.x;
252 m_impulse.y += reduced.y;
253 m_impulse.z = 0.0f;
254 }
255 else
256 {
257 m_impulse += impulse;
258 }
259 }
260
261 b2Vec2 P(impulse.x, impulse.y);
262
263 vA -= mA * P;
264 wA -= iA * (b2Cross(m_rA, P) + impulse.z);
265
266 vB += mB * P;
267 wB += iB * (b2Cross(m_rB, P) + impulse.z);
268 }
269 else
270 {
271 // Solve point-to-point constraint
272 b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
273 b2Vec2 impulse = m_mass.Solve22(-Cdot);
274
275 m_impulse.x += impulse.x;
276 m_impulse.y += impulse.y;
277
278 vA -= mA * impulse;
279 wA -= iA * b2Cross(m_rA, impulse);
280
281 vB += mB * impulse;
282 wB += iB * b2Cross(m_rB, impulse);
283 }
284
285 data.velocities[m_indexA].v = vA;
286 data.velocities[m_indexA].w = wA;
287 data.velocities[m_indexB].v = vB;
288 data.velocities[m_indexB].w = wB;
289}
290
291bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)
292{
293 b2Vec2 cA = data.positions[m_indexA].c;
294 float32 aA = data.positions[m_indexA].a;
295 b2Vec2 cB = data.positions[m_indexB].c;
296 float32 aB = data.positions[m_indexB].a;
297
298 b2Rot qA(aA), qB(aB);
299
300 float32 angularError = 0.0f;
301 float32 positionError = 0.0f;
302
303 bool fixedRotation = (m_invIA + m_invIB == 0.0f);
304
305 // Solve angular limit constraint.
306 if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
307 {
308 float32 angle = aB - aA - m_referenceAngle;
309 float32 limitImpulse = 0.0f;
310
311 if (m_limitState == e_equalLimits)
312 {
313 // Prevent large angular corrections
314 float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
315 limitImpulse = -m_motorMass * C;
316 angularError = b2Abs(C);
317 }
318 else if (m_limitState == e_atLowerLimit)
319 {
320 float32 C = angle - m_lowerAngle;
321 angularError = -C;
322
323 // Prevent large angular corrections and allow some slop.
324 C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
325 limitImpulse = -m_motorMass * C;
326 }
327 else if (m_limitState == e_atUpperLimit)
328 {
329 float32 C = angle - m_upperAngle;
330 angularError = C;
331
332 // Prevent large angular corrections and allow some slop.
333 C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
334 limitImpulse = -m_motorMass * C;
335 }
336
337 aA -= m_invIA * limitImpulse;
338 aB += m_invIB * limitImpulse;
339 }
340
341 // Solve point-to-point constraint.
342 {
343 qA.Set(aA);
344 qB.Set(aB);
345 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
346 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
347
348 b2Vec2 C = cB + rB - cA - rA;
349 positionError = C.Length();
350
351 float32 mA = m_invMassA, mB = m_invMassB;
352 float32 iA = m_invIA, iB = m_invIB;
353
354 b2Mat22 K;
355 K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
356 K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
357 K.ey.x = K.ex.y;
358 K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
359
360 b2Vec2 impulse = -K.Solve(C);
361
362 cA -= mA * impulse;
363 aA -= iA * b2Cross(rA, impulse);
364
365 cB += mB * impulse;
366 aB += iB * b2Cross(rB, impulse);
367 }
368
369 data.positions[m_indexA].c = cA;
370 data.positions[m_indexA].a = aA;
371 data.positions[m_indexB].c = cB;
372 data.positions[m_indexB].a = aB;
373
374 return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
375}
376
377b2Vec2 b2RevoluteJoint::GetAnchorA() const
378{
379 return m_bodyA->GetWorldPoint(m_localAnchorA);
380}
381
382b2Vec2 b2RevoluteJoint::GetAnchorB() const
383{
384 return m_bodyB->GetWorldPoint(m_localAnchorB);
385}
386
387b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const
388{
389 b2Vec2 P(m_impulse.x, m_impulse.y);
390 return inv_dt * P;
391}
392
393float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const
394{
395 return inv_dt * m_impulse.z;
396}
397
398float32 b2RevoluteJoint::GetJointAngle() const
399{
400 b2Body* bA = m_bodyA;
401 b2Body* bB = m_bodyB;
402 return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
403}
404
405float32 b2RevoluteJoint::GetJointSpeed() const
406{
407 b2Body* bA = m_bodyA;
408 b2Body* bB = m_bodyB;
409 return bB->m_angularVelocity - bA->m_angularVelocity;
410}
411
412bool b2RevoluteJoint::IsMotorEnabled() const
413{
414 return m_enableMotor;
415}
416
417void b2RevoluteJoint::EnableMotor(bool flag)
418{
419 m_bodyA->SetAwake(true);
420 m_bodyB->SetAwake(true);
421 m_enableMotor = flag;
422}
423
424float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const
425{
426 return inv_dt * m_motorImpulse;
427}
428
429void b2RevoluteJoint::SetMotorSpeed(float32 speed)
430{
431 m_bodyA->SetAwake(true);
432 m_bodyB->SetAwake(true);
433 m_motorSpeed = speed;
434}
435
436void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)
437{
438 m_bodyA->SetAwake(true);
439 m_bodyB->SetAwake(true);
440 m_maxMotorTorque = torque;
441}
442
443bool b2RevoluteJoint::IsLimitEnabled() const
444{
445 return m_enableLimit;
446}
447
448void b2RevoluteJoint::EnableLimit(bool flag)
449{
450 if (flag != m_enableLimit)
451 {
452 m_bodyA->SetAwake(true);
453 m_bodyB->SetAwake(true);
454 m_enableLimit = flag;
455 m_impulse.z = 0.0f;
456 }
457}
458
459float32 b2RevoluteJoint::GetLowerLimit() const
460{
461 return m_lowerAngle;
462}
463
464float32 b2RevoluteJoint::GetUpperLimit() const
465{
466 return m_upperAngle;
467}
468
469void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
470{
471 b2Assert(lower <= upper);
472
473 if (lower != m_lowerAngle || upper != m_upperAngle)
474 {
475 m_bodyA->SetAwake(true);
476 m_bodyB->SetAwake(true);
477 m_impulse.z = 0.0f;
478 m_lowerAngle = lower;
479 m_upperAngle = upper;
480 }
481}
482
483void b2RevoluteJoint::Dump()
484{
485 int32 indexA = m_bodyA->m_islandIndex;
486 int32 indexB = m_bodyB->m_islandIndex;
487
488 b2Log(" b2RevoluteJointDef jd;\n");
489 b2Log(" jd.bodyA = bodies[%d];\n", indexA);
490 b2Log(" jd.bodyB = bodies[%d];\n", indexB);
491 b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
492 b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
493 b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
494 b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle);
495 b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit);
496 b2Log(" jd.lowerAngle = %.15lef;\n", m_lowerAngle);
497 b2Log(" jd.upperAngle = %.15lef;\n", m_upperAngle);
498 b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor);
499 b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed);
500 b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);
501 b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
502}
503