← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

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