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
39void 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
48b2WheelJoint::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
77void 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
216void 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
280bool 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
329b2Vec2 b2WheelJoint::GetAnchorA() const
330{
331 return m_bodyA->GetWorldPoint(m_localAnchorA);
332}
333
334b2Vec2 b2WheelJoint::GetAnchorB() const
335{
336 return m_bodyB->GetWorldPoint(m_localAnchorB);
337}
338
339b2Vec2 b2WheelJoint::GetReactionForce(float32 inv_dt) const
340{
341 return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax);
342}
343
344float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const
345{
346 return inv_dt * m_motorImpulse;
347}
348
349float32 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
363float32 b2WheelJoint::GetJointSpeed() const
364{
365 float32 wA = m_bodyA->m_angularVelocity;
366 float32 wB = m_bodyB->m_angularVelocity;
367 return wB - wA;
368}
369
370bool b2WheelJoint::IsMotorEnabled() const
371{
372 return m_enableMotor;
373}
374
375void b2WheelJoint::EnableMotor(bool flag)
376{
377 m_bodyA->SetAwake(true);
378 m_bodyB->SetAwake(true);
379 m_enableMotor = flag;
380}
381
382void b2WheelJoint::SetMotorSpeed(float32 speed)
383{
384 m_bodyA->SetAwake(true);
385 m_bodyB->SetAwake(true);
386 m_motorSpeed = speed;
387}
388
389void b2WheelJoint::SetMaxMotorTorque(float32 torque)
390{
391 m_bodyA->SetAwake(true);
392 m_bodyB->SetAwake(true);
393 m_maxMotorTorque = torque;
394}
395
396float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const
397{
398 return inv_dt * m_motorImpulse;
399}
400
401void 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}
420