Updated to Box2D 2.3.0
[qml-box2d:qml-box2d-folibis.git] / Box2D / Dynamics / Joints / b2WheelJoint.cpp
1 /*
2 * Copyright (c) 2006-2007 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/b2WheelJoint.h>
20 #include <Box2D/Dynamics/b2Body.h>
21 #include <Box2D/Dynamics/b2TimeStep.h>
22
23 // Linear constraint (point-to-line)
24 // d = pB - pA = xB + rB - xA - rA
25 // C = dot(ay, d)
26 // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
27 //      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
28 // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
29
30 // Spring linear constraint
31 // C = dot(ax, d)
32 // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
33 // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
34
35 // Motor rotational constraint
36 // Cdot = wB - wA
37 // J = [0 0 -1 0 0 1]
38
39 void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)
40 {
41         bodyA = bA;
42         bodyB = bB;
43         localAnchorA = bodyA->GetLocalPoint(anchor);
44         localAnchorB = bodyB->GetLocalPoint(anchor);
45         localAxisA = bodyA->GetLocalVector(axis);
46 }
47
48 b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def)
49 : b2Joint(def)
50 {
51         m_localAnchorA = def->localAnchorA;
52         m_localAnchorB = def->localAnchorB;
53         m_localXAxisA = def->localAxisA;
54         m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
55
56         m_mass = 0.0f;
57         m_impulse = 0.0f;
58         m_motorMass = 0.0f;
59         m_motorImpulse = 0.0f;
60         m_springMass = 0.0f;
61         m_springImpulse = 0.0f;
62
63         m_maxMotorTorque = def->maxMotorTorque;
64         m_motorSpeed = def->motorSpeed;
65         m_enableMotor = def->enableMotor;
66
67         m_frequencyHz = def->frequencyHz;
68         m_dampingRatio = def->dampingRatio;
69
70         m_bias = 0.0f;
71         m_gamma = 0.0f;
72
73         m_ax.SetZero();
74         m_ay.SetZero();
75 }
76
77 void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data)
78 {
79         m_indexA = m_bodyA->m_islandIndex;
80         m_indexB = m_bodyB->m_islandIndex;
81         m_localCenterA = m_bodyA->m_sweep.localCenter;
82         m_localCenterB = m_bodyB->m_sweep.localCenter;
83         m_invMassA = m_bodyA->m_invMass;
84         m_invMassB = m_bodyB->m_invMass;
85         m_invIA = m_bodyA->m_invI;
86         m_invIB = m_bodyB->m_invI;
87
88         float32 mA = m_invMassA, mB = m_invMassB;
89         float32 iA = m_invIA, iB = m_invIB;
90
91         b2Vec2 cA = data.positions[m_indexA].c;
92         float32 aA = data.positions[m_indexA].a;
93         b2Vec2 vA = data.velocities[m_indexA].v;
94         float32 wA = data.velocities[m_indexA].w;
95
96         b2Vec2 cB = data.positions[m_indexB].c;
97         float32 aB = data.positions[m_indexB].a;
98         b2Vec2 vB = data.velocities[m_indexB].v;
99         float32 wB = data.velocities[m_indexB].w;
100
101         b2Rot qA(aA), qB(aB);
102
103         // Compute the effective masses.
104         b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
105         b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
106         b2Vec2 d = cB + rB - cA - rA;
107
108         // Point to line constraint
109         {
110                 m_ay = b2Mul(qA, m_localYAxisA);
111                 m_sAy = b2Cross(d + rA, m_ay);
112                 m_sBy = b2Cross(rB, m_ay);
113
114                 m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;
115
116                 if (m_mass > 0.0f)
117                 {
118                         m_mass = 1.0f / m_mass;
119                 }
120         }
121
122         // Spring constraint
123         m_springMass = 0.0f;
124         m_bias = 0.0f;
125         m_gamma = 0.0f;
126         if (m_frequencyHz > 0.0f)
127         {
128                 m_ax = b2Mul(qA, m_localXAxisA);
129                 m_sAx = b2Cross(d + rA, m_ax);
130                 m_sBx = b2Cross(rB, m_ax);
131
132                 float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
133
134                 if (invMass > 0.0f)
135                 {
136                         m_springMass = 1.0f / invMass;
137
138                         float32 C = b2Dot(d, m_ax);
139
140                         // Frequency
141                         float32 omega = 2.0f * b2_pi * m_frequencyHz;
142
143                         // Damping coefficient
144                         float32 d = 2.0f * m_springMass * m_dampingRatio * omega;
145
146                         // Spring stiffness
147                         float32 k = m_springMass * omega * omega;
148
149                         // magic formulas
150                         float32 h = data.step.dt;
151                         m_gamma = h * (d + h * k);
152                         if (m_gamma > 0.0f)
153                         {
154                                 m_gamma = 1.0f / m_gamma;
155                         }
156
157                         m_bias = C * h * k * m_gamma;
158
159                         m_springMass = invMass + m_gamma;
160                         if (m_springMass > 0.0f)
161                         {
162                                 m_springMass = 1.0f / m_springMass;
163                         }
164                 }
165         }
166         else
167         {
168                 m_springImpulse = 0.0f;
169         }
170
171         // Rotational motor
172         if (m_enableMotor)
173         {
174                 m_motorMass = iA + iB;
175                 if (m_motorMass > 0.0f)
176                 {
177                         m_motorMass = 1.0f / m_motorMass;
178                 }
179         }
180         else
181         {
182                 m_motorMass = 0.0f;
183                 m_motorImpulse = 0.0f;
184         }
185
186         if (data.step.warmStarting)
187         {
188                 // Account for variable time step.
189                 m_impulse *= data.step.dtRatio;
190                 m_springImpulse *= data.step.dtRatio;
191                 m_motorImpulse *= data.step.dtRatio;
192
193                 b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax;
194                 float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
195                 float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;
196
197                 vA -= m_invMassA * P;
198                 wA -= m_invIA * LA;
199
200                 vB += m_invMassB * P;
201                 wB += m_invIB * LB;
202         }
203         else
204         {
205                 m_impulse = 0.0f;
206                 m_springImpulse = 0.0f;
207                 m_motorImpulse = 0.0f;
208         }
209
210         data.velocities[m_indexA].v = vA;
211         data.velocities[m_indexA].w = wA;
212         data.velocities[m_indexB].v = vB;
213         data.velocities[m_indexB].w = wB;
214 }
215
216 void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data)
217 {
218         float32 mA = m_invMassA, mB = m_invMassB;
219         float32 iA = m_invIA, iB = m_invIB;
220
221         b2Vec2 vA = data.velocities[m_indexA].v;
222         float32 wA = data.velocities[m_indexA].w;
223         b2Vec2 vB = data.velocities[m_indexB].v;
224         float32 wB = data.velocities[m_indexB].w;
225
226         // Solve spring constraint
227         {
228                 float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
229                 float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);
230                 m_springImpulse += impulse;
231
232                 b2Vec2 P = impulse * m_ax;
233                 float32 LA = impulse * m_sAx;
234                 float32 LB = impulse * m_sBx;
235
236                 vA -= mA * P;
237                 wA -= iA * LA;
238
239                 vB += mB * P;
240                 wB += iB * LB;
241         }
242
243         // Solve rotational motor constraint
244         {
245                 float32 Cdot = wB - wA - m_motorSpeed;
246                 float32 impulse = -m_motorMass * Cdot;
247
248                 float32 oldImpulse = m_motorImpulse;
249                 float32 maxImpulse = data.step.dt * m_maxMotorTorque;
250                 m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
251                 impulse = m_motorImpulse - oldImpulse;
252
253                 wA -= iA * impulse;
254                 wB += iB * impulse;
255         }
256
257         // Solve point to line constraint
258         {
259                 float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;
260                 float32 impulse = -m_mass * Cdot;
261                 m_impulse += impulse;
262
263                 b2Vec2 P = impulse * m_ay;
264                 float32 LA = impulse * m_sAy;
265                 float32 LB = impulse * m_sBy;
266
267                 vA -= mA * P;
268                 wA -= iA * LA;
269
270                 vB += mB * P;
271                 wB += iB * LB;
272         }
273
274         data.velocities[m_indexA].v = vA;
275         data.velocities[m_indexA].w = wA;
276         data.velocities[m_indexB].v = vB;
277         data.velocities[m_indexB].w = wB;
278 }
279
280 bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data)
281 {
282         b2Vec2 cA = data.positions[m_indexA].c;
283         float32 aA = data.positions[m_indexA].a;
284         b2Vec2 cB = data.positions[m_indexB].c;
285         float32 aB = data.positions[m_indexB].a;
286
287         b2Rot qA(aA), qB(aB);
288
289         b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
290         b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
291         b2Vec2 d = (cB - cA) + rB - rA;
292
293         b2Vec2 ay = b2Mul(qA, m_localYAxisA);
294
295         float32 sAy = b2Cross(d + rA, ay);
296         float32 sBy = b2Cross(rB, ay);
297
298         float32 C = b2Dot(d, ay);
299
300         float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
301
302         float32 impulse;
303         if (k != 0.0f)
304         {
305                 impulse = - C / k;
306         }
307         else
308         {
309                 impulse = 0.0f;
310         }
311
312         b2Vec2 P = impulse * ay;
313         float32 LA = impulse * sAy;
314         float32 LB = impulse * sBy;
315
316         cA -= m_invMassA * P;
317         aA -= m_invIA * LA;
318         cB += m_invMassB * P;
319         aB += m_invIB * LB;
320
321         data.positions[m_indexA].c = cA;
322         data.positions[m_indexA].a = aA;
323         data.positions[m_indexB].c = cB;
324         data.positions[m_indexB].a = aB;
325
326         return b2Abs(C) <= b2_linearSlop;
327 }
328
329 b2Vec2 b2WheelJoint::GetAnchorA() const
330 {
331         return m_bodyA->GetWorldPoint(m_localAnchorA);
332 }
333
334 b2Vec2 b2WheelJoint::GetAnchorB() const
335 {
336         return m_bodyB->GetWorldPoint(m_localAnchorB);
337 }
338
339 b2Vec2 b2WheelJoint::GetReactionForce(float32 inv_dt) const
340 {
341         return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax);
342 }
343
344 float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const
345 {
346         return inv_dt * m_motorImpulse;
347 }
348
349 float32 b2WheelJoint::GetJointTranslation() const
350 {
351         b2Body* bA = m_bodyA;
352         b2Body* bB = m_bodyB;
353
354         b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA);
355         b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB);
356         b2Vec2 d = pB - pA;
357         b2Vec2 axis = bA->GetWorldVector(m_localXAxisA);
358
359         float32 translation = b2Dot(d, axis);
360         return translation;
361 }
362
363 float32 b2WheelJoint::GetJointSpeed() const
364 {
365         float32 wA = m_bodyA->m_angularVelocity;
366         float32 wB = m_bodyB->m_angularVelocity;
367         return wB - wA;
368 }
369
370 bool b2WheelJoint::IsMotorEnabled() const
371 {
372         return m_enableMotor;
373 }
374
375 void b2WheelJoint::EnableMotor(bool flag)
376 {
377         m_bodyA->SetAwake(true);
378         m_bodyB->SetAwake(true);
379         m_enableMotor = flag;
380 }
381
382 void b2WheelJoint::SetMotorSpeed(float32 speed)
383 {
384         m_bodyA->SetAwake(true);
385         m_bodyB->SetAwake(true);
386         m_motorSpeed = speed;
387 }
388
389 void b2WheelJoint::SetMaxMotorTorque(float32 torque)
390 {
391         m_bodyA->SetAwake(true);
392         m_bodyB->SetAwake(true);
393         m_maxMotorTorque = torque;
394 }
395
396 float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const
397 {
398         return inv_dt * m_motorImpulse;
399 }
400
401 void b2WheelJoint::Dump()
402 {
403         int32 indexA = m_bodyA->m_islandIndex;
404         int32 indexB = m_bodyB->m_islandIndex;
405
406         b2Log("  b2WheelJointDef jd;\n");
407         b2Log("  jd.bodyA = bodies[%d];\n", indexA);
408         b2Log("  jd.bodyB = bodies[%d];\n", indexB);
409         b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
410         b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
411         b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
412         b2Log("  jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y);
413         b2Log("  jd.enableMotor = bool(%d);\n", m_enableMotor);
414         b2Log("  jd.motorSpeed = %.15lef;\n", m_motorSpeed);
415         b2Log("  jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);
416         b2Log("  jd.frequencyHz = %.15lef;\n", m_frequencyHz);
417         b2Log("  jd.dampingRatio = %.15lef;\n", m_dampingRatio);
418         b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
419 }