b2WheelJoint.cpp
Go to the documentation of this file.
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 
20 #include <Box2D/Dynamics/b2Body.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);
46 }
47 
49 : b2Joint(def)
50 {
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 
64  m_motorSpeed = def->motorSpeed;
66 
69 
70  m_bias = 0.0f;
71  m_gamma = 0.0f;
72 
73  m_ax.SetZero();
74  m_ay.SetZero();
75 }
76 
78 {
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.
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 
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 
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 
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 
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 
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 
330 {
332 }
333 
335 {
337 }
338 
340 {
341  return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax);
342 }
343 
345 {
346  return inv_dt * m_motorImpulse;
347 }
348 
350 {
351  b2Body* bA = m_bodyA;
352  b2Body* bB = m_bodyB;
353 
356  b2Vec2 d = pB - pA;
357  b2Vec2 axis = bA->GetWorldVector(m_localXAxisA);
358 
359  float32 translation = b2Dot(d, axis);
360  return translation;
361 }
362 
364 {
367  return wB - wA;
368 }
369 
371 {
372  return m_enableMotor;
373 }
374 
376 {
377  m_bodyA->SetAwake(true);
378  m_bodyB->SetAwake(true);
379  m_enableMotor = flag;
380 }
381 
383 {
384  m_bodyA->SetAwake(true);
385  m_bodyB->SetAwake(true);
386  m_motorSpeed = speed;
387 }
388 
390 {
391  m_bodyA->SetAwake(true);
392  m_bodyB->SetAwake(true);
393  m_maxMotorTorque = torque;
394 }
395 
397 {
398  return inv_dt * m_motorImpulse;
399 }
400 
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 }
float32 m_invIB
Definition: b2WheelJoint.h:166
d
float32 GetJointSpeed() const
Get the current joint translation speed, usually in meters per second.
float32 b2Dot(const b2Vec2 &a, const b2Vec2 &b)
Perform the dot product on two vectors.
Definition: b2Math.h:406
b2WheelJoint(const b2WheelJointDef *def)
b2Velocity * velocities
Definition: b2TimeStep.h:67
float32 m_invMass
Definition: b2Body.h:455
int32 m_islandIndex
Definition: b2Body.h:434
b2Vec2 b2Mul(const b2Mat22 &A, const b2Vec2 &v)
Definition: b2Math.h:433
float32 m_dampingRatio
Definition: b2WheelJoint.h:142
bool SolvePositionConstraints(const b2SolverData &data)
float32 m_maxMotorTorque
Definition: b2WheelJoint.h:154
b2Vec2 GetAnchorA() const
Get the anchor point on bodyA in world coordinates.
float32 m_sAx
Definition: b2WheelJoint.h:169
void b2Log(const char *string,...)
Logging function.
Definition: b2Settings.cpp:38
float32 a
Definition: b2TimeStep.h:52
#define b2_linearSlop
Definition: b2Settings.h:68
#define b2_pi
Definition: b2Settings.h:40
f
b2TimeStep step
Definition: b2TimeStep.h:65
void SolveVelocityConstraints(const b2SolverData &data)
float32 m_sBy
Definition: b2WheelJoint.h:170
b2Vec2 GetAnchorB() const
Get the anchor point on bodyB in world coordinates.
b2Vec2 c
Definition: b2TimeStep.h:51
float32 m_springImpulse
Definition: b2WheelJoint.h:152
float32 w
Definition: b2TimeStep.h:59
float32 m_motorSpeed
Definition: b2WheelJoint.h:155
bool m_collideConnected
Definition: b2Joint.h:181
float32 dtRatio
Definition: b2TimeStep.h:42
void Initialize(b2Body *bodyA, b2Body *bodyB, const b2Vec2 &anchor, const b2Vec2 &axis)
b2Vec2 GetWorldPoint(const b2Vec2 &localPoint) const
Definition: b2Body.h:556
Solver Data.
Definition: b2TimeStep.h:63
void SetZero()
Set this vector to all zeros.
Definition: b2Math.h:62
float32 m_motorMass
Definition: b2WheelJoint.h:173
b2Vec2 m_localAnchorB
Definition: b2WheelJoint.h:146
int32 m_index
Definition: b2Joint.h:178
A 2D column vector.
Definition: b2Math.h:53
signed int int32
Definition: b2Settings.h:31
b2Vec2 localCenter
local center of mass position
Definition: b2Math.h:393
float32 b2Cross(const b2Vec2 &a, const b2Vec2 &b)
Perform the cross product on two vectors. In 2D this produces a scalar.
Definition: b2Math.h:412
float32 frequencyHz
Suspension frequency, zero indicates no suspension.
Definition: b2WheelJoint.h:68
float32 m_invIA
Definition: b2WheelJoint.h:165
void SetMotorSpeed(float32 speed)
Set the motor speed, usually in radians per second.
A rigid body. These are created via b2World::CreateBody.
Definition: b2Body.h:126
float32 motorSpeed
The desired motor speed in radians per second.
Definition: b2WheelJoint.h:65
b2Vec2 localAnchorA
The local anchor point relative to bodyA&#39;s origin.
Definition: b2WheelJoint.h:50
b2Vec2 v
Definition: b2TimeStep.h:58
void SetMaxMotorTorque(float32 torque)
Set/Get the maximum motor force, usually in N-m.
float32 m_invMassB
Definition: b2WheelJoint.h:164
float32 m_motorImpulse
Definition: b2WheelJoint.h:151
b2Vec2 localAxisA
The local translation axis in bodyA.
Definition: b2WheelJoint.h:56
b2Vec2 GetWorldVector(const b2Vec2 &localVector) const
Definition: b2Body.h:561
b2Vec2 m_localCenterA
Definition: b2WheelJoint.h:161
float32 m_bias
Definition: b2WheelJoint.h:176
float32 m_invI
Definition: b2Body.h:458
bool enableMotor
Enable/disable the joint motor.
Definition: b2WheelJoint.h:59
void Dump()
Dump to b2Log.
b2Body * m_bodyA
Definition: b2Joint.h:175
float32 maxMotorTorque
The maximum motor torque, usually in N-m.
Definition: b2WheelJoint.h:62
b2Vec2 m_localYAxisA
Definition: b2WheelJoint.h:148
b2Vec2 m_localAnchorA
Definition: b2WheelJoint.h:145
float32 GetJointTranslation() const
Get the current joint translation, usually in meters.
float32 m_sAy
Definition: b2WheelJoint.h:170
float32 y
Definition: b2Math.h:140
b2Vec2 GetLocalPoint(const b2Vec2 &worldPoint) const
Definition: b2Body.h:566
float32 m_gamma
Definition: b2WheelJoint.h:177
b2Vec2 GetLocalVector(const b2Vec2 &worldVector) const
Definition: b2Body.h:571
b2Position * positions
Definition: b2TimeStep.h:66
T b2Clamp(T a, T low, T high)
Definition: b2Math.h:654
float32 m_mass
Definition: b2WheelJoint.h:172
float32 m_sBx
Definition: b2WheelJoint.h:169
void InitVelocityConstraints(const b2SolverData &data)
float32 m_frequencyHz
Definition: b2WheelJoint.h:141
float32 m_impulse
Definition: b2WheelJoint.h:150
T b2Abs(T a)
Definition: b2Math.h:616
b2Vec2 localAnchorB
The local anchor point relative to bodyB&#39;s origin.
Definition: b2WheelJoint.h:53
void EnableMotor(bool flag)
Enable/disable the joint motor.
float32 m_angularVelocity
Definition: b2Body.h:440
bool warmStarting
Definition: b2TimeStep.h:45
b2Vec2 GetReactionForce(float32 inv_dt) const
Get the reaction force on bodyB at the joint anchor in Newtons.
Rotation.
Definition: b2Math.h:299
bool IsMotorEnabled() const
Is the joint motor enabled?
float32 GetMotorTorque(float32 inv_dt) const
Get the current motor torque given the inverse time step, usually in N-m.
float32 GetReactionTorque(float32 inv_dt) const
Get the reaction torque on bodyB in N*m.
b2Vec2 m_localCenterB
Definition: b2WheelJoint.h:162
float32 x
Definition: b2Math.h:140
b2Body * bodyA
The first attached body.
Definition: b2Joint.h:92
float32 m_invMassA
Definition: b2WheelJoint.h:163
float32 dt
Definition: b2TimeStep.h:40
float32 m_springMass
Definition: b2WheelJoint.h:174
void SetAwake(bool flag)
Definition: b2Body.h:633
b2Vec2 m_localXAxisA
Definition: b2WheelJoint.h:147
b2Body * bodyB
The second attached body.
Definition: b2Joint.h:95
b2Body * m_bodyB
Definition: b2Joint.h:176
float32 dampingRatio
Suspension damping ratio, one indicates critical damping.
Definition: b2WheelJoint.h:71
b2Sweep m_sweep
Definition: b2Body.h:437
float float32
Definition: b2Settings.h:35
bool m_enableMotor
Definition: b2WheelJoint.h:156


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 19:36:40