b2GearJoint.cpp
Go to the documentation of this file.
1 /*
2 * Copyright (c) 2007-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 
22 #include <Box2D/Dynamics/b2Body.h>
24 
25 // Gear Joint:
26 // C0 = (coordinate1 + ratio * coordinate2)_initial
27 // C = (coordinate1 + ratio * coordinate2) - C0 = 0
28 // J = [J1 ratio * J2]
29 // K = J * invM * JT
30 // = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
31 //
32 // Revolute:
33 // coordinate = rotation
34 // Cdot = angularVelocity
35 // J = [0 0 1]
36 // K = J * invM * JT = invI
37 //
38 // Prismatic:
39 // coordinate = dot(p - pg, ug)
40 // Cdot = dot(v + cross(w, r), ug)
41 // J = [ug cross(r, ug)]
42 // K = J * invM * JT = invMass + invI * cross(r, ug)^2
43 
45 : b2Joint(def)
46 {
47  m_joint1 = def->joint1;
48  m_joint2 = def->joint2;
49 
52 
55 
56  float32 coordinateA, coordinateB;
57 
58  // TODO_ERIN there might be some problem with the joint edges in b2Joint.
59 
62 
63  // Get geometry of joint1
64  b2Transform xfA = m_bodyA->m_xf;
65  float32 aA = m_bodyA->m_sweep.a;
66  b2Transform xfC = m_bodyC->m_xf;
67  float32 aC = m_bodyC->m_sweep.a;
68 
69  if (m_typeA == e_revoluteJoint)
70  {
71  b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1;
72  m_localAnchorC = revolute->m_localAnchorA;
73  m_localAnchorA = revolute->m_localAnchorB;
76 
77  coordinateA = aA - aC - m_referenceAngleA;
78  }
79  else
80  {
81  b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1;
82  m_localAnchorC = prismatic->m_localAnchorA;
83  m_localAnchorA = prismatic->m_localAnchorB;
85  m_localAxisC = prismatic->m_localXAxisA;
86 
88  b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
89  coordinateA = b2Dot(pA - pC, m_localAxisC);
90  }
91 
94 
95  // Get geometry of joint2
96  b2Transform xfB = m_bodyB->m_xf;
97  float32 aB = m_bodyB->m_sweep.a;
98  b2Transform xfD = m_bodyD->m_xf;
99  float32 aD = m_bodyD->m_sweep.a;
100 
101  if (m_typeB == e_revoluteJoint)
102  {
103  b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2;
104  m_localAnchorD = revolute->m_localAnchorA;
105  m_localAnchorB = revolute->m_localAnchorB;
108 
109  coordinateB = aB - aD - m_referenceAngleB;
110  }
111  else
112  {
113  b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2;
114  m_localAnchorD = prismatic->m_localAnchorA;
115  m_localAnchorB = prismatic->m_localAnchorB;
116  m_referenceAngleB = prismatic->m_referenceAngle;
117  m_localAxisD = prismatic->m_localXAxisA;
118 
119  b2Vec2 pD = m_localAnchorD;
120  b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
121  coordinateB = b2Dot(pB - pD, m_localAxisD);
122  }
123 
124  m_ratio = def->ratio;
125 
126  m_constant = coordinateA + m_ratio * coordinateB;
127 
128  m_impulse = 0.0f;
129 }
130 
132 {
145  m_iA = m_bodyA->m_invI;
146  m_iB = m_bodyB->m_invI;
147  m_iC = m_bodyC->m_invI;
148  m_iD = m_bodyD->m_invI;
149 
150  float32 aA = data.positions[m_indexA].a;
151  b2Vec2 vA = data.velocities[m_indexA].v;
152  float32 wA = data.velocities[m_indexA].w;
153 
154  float32 aB = data.positions[m_indexB].a;
155  b2Vec2 vB = data.velocities[m_indexB].v;
156  float32 wB = data.velocities[m_indexB].w;
157 
158  float32 aC = data.positions[m_indexC].a;
159  b2Vec2 vC = data.velocities[m_indexC].v;
160  float32 wC = data.velocities[m_indexC].w;
161 
162  float32 aD = data.positions[m_indexD].a;
163  b2Vec2 vD = data.velocities[m_indexD].v;
164  float32 wD = data.velocities[m_indexD].w;
165 
166  b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
167 
168  m_mass = 0.0f;
169 
170  if (m_typeA == e_revoluteJoint)
171  {
172  m_JvAC.SetZero();
173  m_JwA = 1.0f;
174  m_JwC = 1.0f;
175  m_mass += m_iA + m_iC;
176  }
177  else
178  {
179  b2Vec2 u = b2Mul(qC, m_localAxisC);
180  b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
181  b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
182  m_JvAC = u;
183  m_JwC = b2Cross(rC, u);
184  m_JwA = b2Cross(rA, u);
185  m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA;
186  }
187 
188  if (m_typeB == e_revoluteJoint)
189  {
190  m_JvBD.SetZero();
191  m_JwB = m_ratio;
192  m_JwD = m_ratio;
193  m_mass += m_ratio * m_ratio * (m_iB + m_iD);
194  }
195  else
196  {
197  b2Vec2 u = b2Mul(qD, m_localAxisD);
198  b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
199  b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
200  m_JvBD = m_ratio * u;
201  m_JwD = m_ratio * b2Cross(rD, u);
202  m_JwB = m_ratio * b2Cross(rB, u);
203  m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB;
204  }
205 
206  // Compute effective mass.
207  m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f;
208 
209  if (data.step.warmStarting)
210  {
211  vA += (m_mA * m_impulse) * m_JvAC;
212  wA += m_iA * m_impulse * m_JwA;
213  vB += (m_mB * m_impulse) * m_JvBD;
214  wB += m_iB * m_impulse * m_JwB;
215  vC -= (m_mC * m_impulse) * m_JvAC;
216  wC -= m_iC * m_impulse * m_JwC;
217  vD -= (m_mD * m_impulse) * m_JvBD;
218  wD -= m_iD * m_impulse * m_JwD;
219  }
220  else
221  {
222  m_impulse = 0.0f;
223  }
224 
225  data.velocities[m_indexA].v = vA;
226  data.velocities[m_indexA].w = wA;
227  data.velocities[m_indexB].v = vB;
228  data.velocities[m_indexB].w = wB;
229  data.velocities[m_indexC].v = vC;
230  data.velocities[m_indexC].w = wC;
231  data.velocities[m_indexD].v = vD;
232  data.velocities[m_indexD].w = wD;
233 }
234 
236 {
237  b2Vec2 vA = data.velocities[m_indexA].v;
238  float32 wA = data.velocities[m_indexA].w;
239  b2Vec2 vB = data.velocities[m_indexB].v;
240  float32 wB = data.velocities[m_indexB].w;
241  b2Vec2 vC = data.velocities[m_indexC].v;
242  float32 wC = data.velocities[m_indexC].w;
243  b2Vec2 vD = data.velocities[m_indexD].v;
244  float32 wD = data.velocities[m_indexD].w;
245 
246  float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD);
247  Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD);
248 
249  float32 impulse = -m_mass * Cdot;
250  m_impulse += impulse;
251 
252  vA += (m_mA * impulse) * m_JvAC;
253  wA += m_iA * impulse * m_JwA;
254  vB += (m_mB * impulse) * m_JvBD;
255  wB += m_iB * impulse * m_JwB;
256  vC -= (m_mC * impulse) * m_JvAC;
257  wC -= m_iC * impulse * m_JwC;
258  vD -= (m_mD * impulse) * m_JvBD;
259  wD -= m_iD * impulse * m_JwD;
260 
261  data.velocities[m_indexA].v = vA;
262  data.velocities[m_indexA].w = wA;
263  data.velocities[m_indexB].v = vB;
264  data.velocities[m_indexB].w = wB;
265  data.velocities[m_indexC].v = vC;
266  data.velocities[m_indexC].w = wC;
267  data.velocities[m_indexD].v = vD;
268  data.velocities[m_indexD].w = wD;
269 }
270 
272 {
273  b2Vec2 cA = data.positions[m_indexA].c;
274  float32 aA = data.positions[m_indexA].a;
275  b2Vec2 cB = data.positions[m_indexB].c;
276  float32 aB = data.positions[m_indexB].a;
277  b2Vec2 cC = data.positions[m_indexC].c;
278  float32 aC = data.positions[m_indexC].a;
279  b2Vec2 cD = data.positions[m_indexD].c;
280  float32 aD = data.positions[m_indexD].a;
281 
282  b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
283 
284  float32 linearError = 0.0f;
285 
286  float32 coordinateA, coordinateB;
287 
288  b2Vec2 JvAC, JvBD;
289  float32 JwA, JwB, JwC, JwD;
290  float32 mass = 0.0f;
291 
292  if (m_typeA == e_revoluteJoint)
293  {
294  JvAC.SetZero();
295  JwA = 1.0f;
296  JwC = 1.0f;
297  mass += m_iA + m_iC;
298 
299  coordinateA = aA - aC - m_referenceAngleA;
300  }
301  else
302  {
303  b2Vec2 u = b2Mul(qC, m_localAxisC);
304  b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
305  b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
306  JvAC = u;
307  JwC = b2Cross(rC, u);
308  JwA = b2Cross(rA, u);
309  mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
310 
311  b2Vec2 pC = m_localAnchorC - m_lcC;
312  b2Vec2 pA = b2MulT(qC, rA + (cA - cC));
313  coordinateA = b2Dot(pA - pC, m_localAxisC);
314  }
315 
316  if (m_typeB == e_revoluteJoint)
317  {
318  JvBD.SetZero();
319  JwB = m_ratio;
320  JwD = m_ratio;
321  mass += m_ratio * m_ratio * (m_iB + m_iD);
322 
323  coordinateB = aB - aD - m_referenceAngleB;
324  }
325  else
326  {
327  b2Vec2 u = b2Mul(qD, m_localAxisD);
328  b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
329  b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
330  JvBD = m_ratio * u;
331  JwD = m_ratio * b2Cross(rD, u);
332  JwB = m_ratio * b2Cross(rB, u);
333  mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
334 
335  b2Vec2 pD = m_localAnchorD - m_lcD;
336  b2Vec2 pB = b2MulT(qD, rB + (cB - cD));
337  coordinateB = b2Dot(pB - pD, m_localAxisD);
338  }
339 
340  float32 C = (coordinateA + m_ratio * coordinateB) - m_constant;
341 
342  float32 impulse = 0.0f;
343  if (mass > 0.0f)
344  {
345  impulse = -C / mass;
346  }
347 
348  cA += m_mA * impulse * JvAC;
349  aA += m_iA * impulse * JwA;
350  cB += m_mB * impulse * JvBD;
351  aB += m_iB * impulse * JwB;
352  cC -= m_mC * impulse * JvAC;
353  aC -= m_iC * impulse * JwC;
354  cD -= m_mD * impulse * JvBD;
355  aD -= m_iD * impulse * JwD;
356 
357  data.positions[m_indexA].c = cA;
358  data.positions[m_indexA].a = aA;
359  data.positions[m_indexB].c = cB;
360  data.positions[m_indexB].a = aB;
361  data.positions[m_indexC].c = cC;
362  data.positions[m_indexC].a = aC;
363  data.positions[m_indexD].c = cD;
364  data.positions[m_indexD].a = aD;
365 
366  // TODO_ERIN not implemented
367  return linearError < b2_linearSlop;
368 }
369 
371 {
373 }
374 
376 {
378 }
379 
381 {
382  b2Vec2 P = m_impulse * m_JvAC;
383  return inv_dt * P;
384 }
385 
387 {
388  float32 L = m_impulse * m_JwA;
389  return inv_dt * L;
390 }
391 
393 {
394  b2Assert(b2IsValid(ratio));
395  m_ratio = ratio;
396 }
397 
399 {
400  return m_ratio;
401 }
402 
404 {
405  int32 indexA = m_bodyA->m_islandIndex;
406  int32 indexB = m_bodyB->m_islandIndex;
407 
408  int32 index1 = m_joint1->m_index;
409  int32 index2 = m_joint2->m_index;
410 
411  b2Log(" b2GearJointDef jd;\n");
412  b2Log(" jd.bodyA = bodies[%d];\n", indexA);
413  b2Log(" jd.bodyB = bodies[%d];\n", indexB);
414  b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
415  b2Log(" jd.joint1 = joints[%d];\n", index1);
416  b2Log(" jd.joint2 = joints[%d];\n", index2);
417  b2Log(" jd.ratio = %.15lef;\n", m_ratio);
418  b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
419 }
float32 b2Dot(const b2Vec2 &a, const b2Vec2 &b)
Perform the dot product on two vectors.
Definition: b2Math.h:406
b2Vec2 m_localAxisD
Definition: b2GearJoint.h:105
float32 m_referenceAngleA
Definition: b2GearJoint.h:107
b2Velocity * velocities
Definition: b2TimeStep.h:67
float32 m_invMass
Definition: b2Body.h:455
b2Vec2 m_JvBD
Definition: b2GearJoint.h:120
float32 m_iC
Definition: b2GearJoint.h:119
int32 m_islandIndex
Definition: b2Body.h:434
b2Vec2 b2Mul(const b2Mat22 &A, const b2Vec2 &v)
Definition: b2Math.h:433
float32 m_mA
Definition: b2GearJoint.h:118
b2Joint * joint2
The second revolute/prismatic joint attached to the gear joint.
Definition: b2GearJoint.h:40
void b2Log(const char *string,...)
Logging function.
Definition: b2Settings.cpp:38
b2Joint * m_joint2
Definition: b2GearJoint.h:88
float32 m_mB
Definition: b2GearJoint.h:118
b2Vec2 p
Definition: b2Math.h:372
float32 a
Definition: b2TimeStep.h:52
#define b2_linearSlop
Definition: b2Settings.h:68
float32 m_mass
Definition: b2GearJoint.h:122
b2Rot q
Definition: b2Math.h:373
f
b2Body * GetBodyA()
Get the first body attached to this joint.
Definition: b2Joint.h:191
float32 ratio
Definition: b2GearJoint.h:44
b2TimeStep step
Definition: b2TimeStep.h:65
bool SolvePositionConstraints(const b2SolverData &data)
b2Vec2 c
Definition: b2TimeStep.h:51
b2Body * m_bodyD
Definition: b2GearJoint.h:96
int32 m_indexD
Definition: b2GearJoint.h:116
float32 w
Definition: b2TimeStep.h:59
void SolveVelocityConstraints(const b2SolverData &data)
b2JointType GetType() const
Get the type of the concrete joint.
Definition: b2Joint.h:186
bool m_collideConnected
Definition: b2Joint.h:181
float32 m_constant
Definition: b2GearJoint.h:110
float32 m_ratio
Definition: b2GearJoint.h:111
int32 m_indexC
Definition: b2GearJoint.h:116
b2JointType m_typeA
Definition: b2GearJoint.h:90
b2Vec2 GetWorldPoint(const b2Vec2 &localPoint) const
Definition: b2Body.h:556
float32 GetReactionTorque(float32 inv_dt) const
Get the reaction torque on bodyB in N*m.
Solver Data.
Definition: b2TimeStep.h:63
void SetZero()
Set this vector to all zeros.
Definition: b2Math.h:62
b2Vec2 m_lcC
Definition: b2GearJoint.h:117
int32 m_index
Definition: b2Joint.h:178
A 2D column vector.
Definition: b2Math.h:53
b2Vec2 GetReactionForce(float32 inv_dt) const
Get the reaction force on bodyB at the joint anchor in Newtons.
b2GearJoint(const b2GearJointDef *data)
Definition: b2GearJoint.cpp:44
float32 m_referenceAngle
float32 m_iD
Definition: b2GearJoint.h:119
b2Body * GetBodyB()
Get the second body attached to this joint.
Definition: b2Joint.h:196
signed int int32
Definition: b2Settings.h:31
float32 m_iA
Definition: b2GearJoint.h:119
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
int32 m_indexB
Definition: b2GearJoint.h:116
float32 m_mD
Definition: b2GearJoint.h:118
b2Vec2 v
Definition: b2TimeStep.h:58
b2Vec2 m_localAnchorB
Definition: b2GearJoint.h:100
b2Body * m_bodyC
Definition: b2GearJoint.h:95
float32 m_JwA
Definition: b2GearJoint.h:121
bool b2IsValid(float32 x)
This function is used to ensure that a floating point number is not a NaN or infinity.
Definition: b2Math.h:26
b2Joint * m_joint1
Definition: b2GearJoint.h:87
b2Vec2 m_localAnchorC
Definition: b2GearJoint.h:101
float32 m_invI
Definition: b2Body.h:458
b2Joint * joint1
The first revolute/prismatic joint attached to the gear joint.
Definition: b2GearJoint.h:37
b2Vec2 m_lcB
Definition: b2GearJoint.h:117
float32 m_referenceAngleB
Definition: b2GearJoint.h:108
float32 m_mC
Definition: b2GearJoint.h:118
b2Vec2 GetAnchorB() const
Get the anchor point on bodyB in world coordinates.
b2Body * m_bodyA
Definition: b2Joint.h:175
b2Vec2 b2MulT(const b2Mat22 &A, const b2Vec2 &v)
Definition: b2Math.h:440
void SetRatio(float32 ratio)
Set/Get the gear ratio.
b2Vec2 m_lcA
Definition: b2GearJoint.h:117
b2Position * positions
Definition: b2TimeStep.h:66
b2Vec2 m_localAnchorA
Definition: b2GearJoint.h:99
#define b2Assert(A)
Definition: b2Settings.h:27
float32 m_impulse
Definition: b2GearJoint.h:113
b2Vec2 m_localAxisC
Definition: b2GearJoint.h:104
b2JointType m_typeB
Definition: b2GearJoint.h:91
b2Transform m_xf
Definition: b2Body.h:436
float32 m_JwB
Definition: b2GearJoint.h:121
float32 m_JwC
Definition: b2GearJoint.h:121
bool warmStarting
Definition: b2TimeStep.h:45
Rotation.
Definition: b2Math.h:299
b2Vec2 m_localAnchorD
Definition: b2GearJoint.h:102
float32 m_JwD
Definition: b2GearJoint.h:121
b2Vec2 GetAnchorA() const
Get the anchor point on bodyA in world coordinates.
float32 a
world angles
Definition: b2Math.h:395
void Dump()
Dump joint to dmLog.
float32 m_iB
Definition: b2GearJoint.h:119
float32 GetRatio() const
b2Body * m_bodyB
Definition: b2Joint.h:176
b2Vec2 m_lcD
Definition: b2GearJoint.h:117
b2Sweep m_sweep
Definition: b2Body.h:437
float float32
Definition: b2Settings.h:35
void InitVelocityConstraints(const b2SolverData &data)
b2Vec2 m_JvAC
Definition: b2GearJoint.h:120
int32 m_indexA
Definition: b2GearJoint.h:116


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