b2PulleyJoint.cpp
Go to the documentation of this file.
1 /*
2 * Copyright (c) 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 // Pulley:
24 // length1 = norm(p1 - s1)
25 // length2 = norm(p2 - s2)
26 // C0 = (length1 + ratio * length2)_initial
27 // C = C0 - (length1 + ratio * length2)
28 // u1 = (p1 - s1) / norm(p1 - s1)
29 // u2 = (p2 - s2) / norm(p2 - s2)
30 // Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
31 // J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]
32 // K = J * invM * JT
33 // = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
34 
36  const b2Vec2& groundA, const b2Vec2& groundB,
37  const b2Vec2& anchorA, const b2Vec2& anchorB,
38  float32 r)
39 {
40  bodyA = bA;
41  bodyB = bB;
42  groundAnchorA = groundA;
43  groundAnchorB = groundB;
44  localAnchorA = bodyA->GetLocalPoint(anchorA);
45  localAnchorB = bodyB->GetLocalPoint(anchorB);
46  b2Vec2 dA = anchorA - groundA;
47  lengthA = dA.Length();
48  b2Vec2 dB = anchorB - groundB;
49  lengthB = dB.Length();
50  ratio = r;
52 }
53 
55 : b2Joint(def)
56 {
61 
62  m_lengthA = def->lengthA;
63  m_lengthB = def->lengthB;
64 
65  b2Assert(def->ratio != 0.0f);
66  m_ratio = def->ratio;
67 
68  m_constant = def->lengthA + m_ratio * def->lengthB;
69 
70  m_impulse = 0.0f;
71 }
72 
74 {
83 
84  b2Vec2 cA = data.positions[m_indexA].c;
85  float32 aA = data.positions[m_indexA].a;
86  b2Vec2 vA = data.velocities[m_indexA].v;
87  float32 wA = data.velocities[m_indexA].w;
88 
89  b2Vec2 cB = data.positions[m_indexB].c;
90  float32 aB = data.positions[m_indexB].a;
91  b2Vec2 vB = data.velocities[m_indexB].v;
92  float32 wB = data.velocities[m_indexB].w;
93 
94  b2Rot qA(aA), qB(aB);
95 
98 
99  // Get the pulley axes.
100  m_uA = cA + m_rA - m_groundAnchorA;
101  m_uB = cB + m_rB - m_groundAnchorB;
102 
103  float32 lengthA = m_uA.Length();
104  float32 lengthB = m_uB.Length();
105 
106  if (lengthA > 10.0f * b2_linearSlop)
107  {
108  m_uA *= 1.0f / lengthA;
109  }
110  else
111  {
112  m_uA.SetZero();
113  }
114 
115  if (lengthB > 10.0f * b2_linearSlop)
116  {
117  m_uB *= 1.0f / lengthB;
118  }
119  else
120  {
121  m_uB.SetZero();
122  }
123 
124  // Compute effective mass.
125  float32 ruA = b2Cross(m_rA, m_uA);
126  float32 ruB = b2Cross(m_rB, m_uB);
127 
128  float32 mA = m_invMassA + m_invIA * ruA * ruA;
129  float32 mB = m_invMassB + m_invIB * ruB * ruB;
130 
131  m_mass = mA + m_ratio * m_ratio * mB;
132 
133  if (m_mass > 0.0f)
134  {
135  m_mass = 1.0f / m_mass;
136  }
137 
138  if (data.step.warmStarting)
139  {
140  // Scale impulses to support variable time steps.
141  m_impulse *= data.step.dtRatio;
142 
143  // Warm starting.
144  b2Vec2 PA = -(m_impulse) * m_uA;
145  b2Vec2 PB = (-m_ratio * m_impulse) * m_uB;
146 
147  vA += m_invMassA * PA;
148  wA += m_invIA * b2Cross(m_rA, PA);
149  vB += m_invMassB * PB;
150  wB += m_invIB * b2Cross(m_rB, PB);
151  }
152  else
153  {
154  m_impulse = 0.0f;
155  }
156 
157  data.velocities[m_indexA].v = vA;
158  data.velocities[m_indexA].w = wA;
159  data.velocities[m_indexB].v = vB;
160  data.velocities[m_indexB].w = wB;
161 }
162 
164 {
165  b2Vec2 vA = data.velocities[m_indexA].v;
166  float32 wA = data.velocities[m_indexA].w;
167  b2Vec2 vB = data.velocities[m_indexB].v;
168  float32 wB = data.velocities[m_indexB].w;
169 
170  b2Vec2 vpA = vA + b2Cross(wA, m_rA);
171  b2Vec2 vpB = vB + b2Cross(wB, m_rB);
172 
173  float32 Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB);
174  float32 impulse = -m_mass * Cdot;
175  m_impulse += impulse;
176 
177  b2Vec2 PA = -impulse * m_uA;
178  b2Vec2 PB = -m_ratio * impulse * m_uB;
179  vA += m_invMassA * PA;
180  wA += m_invIA * b2Cross(m_rA, PA);
181  vB += m_invMassB * PB;
182  wB += m_invIB * b2Cross(m_rB, PB);
183 
184  data.velocities[m_indexA].v = vA;
185  data.velocities[m_indexA].w = wA;
186  data.velocities[m_indexB].v = vB;
187  data.velocities[m_indexB].w = wB;
188 }
189 
191 {
192  b2Vec2 cA = data.positions[m_indexA].c;
193  float32 aA = data.positions[m_indexA].a;
194  b2Vec2 cB = data.positions[m_indexB].c;
195  float32 aB = data.positions[m_indexB].a;
196 
197  b2Rot qA(aA), qB(aB);
198 
201 
202  // Get the pulley axes.
203  b2Vec2 uA = cA + rA - m_groundAnchorA;
204  b2Vec2 uB = cB + rB - m_groundAnchorB;
205 
206  float32 lengthA = uA.Length();
207  float32 lengthB = uB.Length();
208 
209  if (lengthA > 10.0f * b2_linearSlop)
210  {
211  uA *= 1.0f / lengthA;
212  }
213  else
214  {
215  uA.SetZero();
216  }
217 
218  if (lengthB > 10.0f * b2_linearSlop)
219  {
220  uB *= 1.0f / lengthB;
221  }
222  else
223  {
224  uB.SetZero();
225  }
226 
227  // Compute effective mass.
228  float32 ruA = b2Cross(rA, uA);
229  float32 ruB = b2Cross(rB, uB);
230 
231  float32 mA = m_invMassA + m_invIA * ruA * ruA;
232  float32 mB = m_invMassB + m_invIB * ruB * ruB;
233 
234  float32 mass = mA + m_ratio * m_ratio * mB;
235 
236  if (mass > 0.0f)
237  {
238  mass = 1.0f / mass;
239  }
240 
241  float32 C = m_constant - lengthA - m_ratio * lengthB;
242  float32 linearError = b2Abs(C);
243 
244  float32 impulse = -mass * C;
245 
246  b2Vec2 PA = -impulse * uA;
247  b2Vec2 PB = -m_ratio * impulse * uB;
248 
249  cA += m_invMassA * PA;
250  aA += m_invIA * b2Cross(rA, PA);
251  cB += m_invMassB * PB;
252  aB += m_invIB * b2Cross(rB, PB);
253 
254  data.positions[m_indexA].c = cA;
255  data.positions[m_indexA].a = aA;
256  data.positions[m_indexB].c = cB;
257  data.positions[m_indexB].a = aB;
258 
259  return linearError < b2_linearSlop;
260 }
261 
263 {
265 }
266 
268 {
270 }
271 
273 {
274  b2Vec2 P = m_impulse * m_uB;
275  return inv_dt * P;
276 }
277 
279 {
280  B2_NOT_USED(inv_dt);
281  return 0.0f;
282 }
283 
285 {
286  return m_groundAnchorA;
287 }
288 
290 {
291  return m_groundAnchorB;
292 }
293 
295 {
296  return m_lengthA;
297 }
298 
300 {
301  return m_lengthB;
302 }
303 
305 {
306  return m_ratio;
307 }
308 
310 {
313  b2Vec2 d = p - s;
314  return d.Length();
315 }
316 
318 {
321  b2Vec2 d = p - s;
322  return d.Length();
323 }
324 
326 {
327  int32 indexA = m_bodyA->m_islandIndex;
328  int32 indexB = m_bodyB->m_islandIndex;
329 
330  b2Log(" b2PulleyJointDef jd;\n");
331  b2Log(" jd.bodyA = bodies[%d];\n", indexA);
332  b2Log(" jd.bodyB = bodies[%d];\n", indexB);
333  b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
334  b2Log(" jd.groundAnchorA.Set(%.15lef, %.15lef);\n", m_groundAnchorA.x, m_groundAnchorA.y);
335  b2Log(" jd.groundAnchorB.Set(%.15lef, %.15lef);\n", m_groundAnchorB.x, m_groundAnchorB.y);
336  b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
337  b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
338  b2Log(" jd.lengthA = %.15lef;\n", m_lengthA);
339  b2Log(" jd.lengthB = %.15lef;\n", m_lengthB);
340  b2Log(" jd.ratio = %.15lef;\n", m_ratio);
341  b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
342 }
343 
344 void b2PulleyJoint::ShiftOrigin(const b2Vec2& newOrigin)
345 {
346  m_groundAnchorA -= newOrigin;
347  m_groundAnchorB -= newOrigin;
348 }
d
b2Vec2 m_localAnchorA
float32 b2Dot(const b2Vec2 &a, const b2Vec2 &b)
Perform the dot product on two vectors.
Definition: b2Math.h:405
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:432
b2Vec2 GetReactionForce(float32 inv_dt) const
Get the reaction force on bodyB at the joint anchor in Newtons.
b2Vec2 m_localCenterB
void b2Log(const char *string,...)
Logging function.
Definition: b2Settings.cpp:38
void InitVelocityConstraints(const b2SolverData &data)
float32 GetLengthA() const
Get the current length of the segment attached to bodyA.
float32 a
Definition: b2TimeStep.h:52
#define b2_linearSlop
Definition: b2Settings.h:68
b2TimeStep step
Definition: b2TimeStep.h:65
float32 lengthA
The a reference length for the segment attached to bodyA.
Definition: b2PulleyJoint.h:62
b2Vec2 GetGroundAnchorB() const
Get the second ground anchor.
b2Vec2 c
Definition: b2TimeStep.h:51
float32 w
Definition: b2TimeStep.h:59
b2PulleyJoint(const b2PulleyJointDef *data)
bool m_collideConnected
Definition: b2Joint.h:181
#define B2_NOT_USED(x)
Definition: b2Settings.h:26
float32 dtRatio
Definition: b2TimeStep.h:42
b2Vec2 GetAnchorA() const
Get the anchor point on bodyA in world coordinates.
#define b2_epsilon
Definition: b2Settings.h:39
b2Vec2 localAnchorA
The local anchor point relative to bodyA&#39;s origin.
Definition: b2PulleyJoint.h:56
b2Vec2 GetWorldPoint(const b2Vec2 &localPoint) const
Definition: b2Body.h:556
float32 GetCurrentLengthB() const
Get the current length of the segment attached to bodyB.
b2Vec2 localAnchorB
The local anchor point relative to bodyB&#39;s origin.
Definition: b2PulleyJoint.h:59
float32 GetReactionTorque(float32 inv_dt) const
Get the reaction torque on bodyB in N*m.
void ShiftOrigin(const b2Vec2 &newOrigin)
Implement b2Joint::ShiftOrigin.
Solver Data.
Definition: b2TimeStep.h:63
void SetZero()
Set this vector to all zeros.
Definition: b2Math.h:61
float32 ratio
The pulley ratio, used to simulate a block-and-tackle.
Definition: b2PulleyJoint.h:68
int32 m_index
Definition: b2Joint.h:178
A 2D column vector.
Definition: b2Math.h:52
float32 m_lengthA
float32 GetRatio() const
Get the pulley ratio.
GLdouble s
signed int int32
Definition: b2Settings.h:31
b2Vec2 localCenter
local center of mass position
Definition: b2Math.h:392
float32 b2Cross(const b2Vec2 &a, const b2Vec2 &b)
Perform the cross product on two vectors. In 2D this produces a scalar.
Definition: b2Math.h:411
b2Vec2 groundAnchorB
The second ground anchor in world coordinates. This point never moves.
Definition: b2PulleyJoint.h:53
A rigid body. These are created via b2World::CreateBody.
Definition: b2Body.h:126
b2Vec2 v
Definition: b2TimeStep.h:58
void SolveVelocityConstraints(const b2SolverData &data)
float32 m_invMassB
b2Vec2 m_localCenterA
b2Vec2 m_localAnchorB
float32 m_invI
Definition: b2Body.h:458
GLint GLenum GLsizei GLint GLsizei const GLvoid * data
GLfloat GLfloat p
b2Vec2 groundAnchorA
The first ground anchor in world coordinates. This point never moves.
Definition: b2PulleyJoint.h:50
bool SolvePositionConstraints(const b2SolverData &data)
b2Body * m_bodyA
Definition: b2Joint.h:175
void Initialize(b2Body *bodyA, b2Body *bodyB, const b2Vec2 &groundAnchorA, const b2Vec2 &groundAnchorB, const b2Vec2 &anchorA, const b2Vec2 &anchorB, float32 ratio)
Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
b2Vec2 m_groundAnchorA
float32 y
Definition: b2Math.h:139
b2Vec2 GetLocalPoint(const b2Vec2 &worldPoint) const
Definition: b2Body.h:566
float32 GetLengthB() const
Get the current length of the segment attached to bodyB.
b2Vec2 GetAnchorB() const
Get the anchor point on bodyB in world coordinates.
b2Position * positions
Definition: b2TimeStep.h:66
#define b2Assert(A)
Definition: b2Settings.h:27
float32 m_constant
GLdouble GLdouble GLdouble r
T b2Abs(T a)
Definition: b2Math.h:615
void Dump()
Dump joint to dmLog.
bool warmStarting
Definition: b2TimeStep.h:45
Rotation.
Definition: b2Math.h:298
float32 m_impulse
float32 x
Definition: b2Math.h:139
b2Vec2 m_groundAnchorB
b2Body * bodyA
The first attached body.
Definition: b2Joint.h:92
b2Vec2 GetGroundAnchorA() const
Get the first ground anchor.
float32 Length() const
Get the length of this vector (the norm).
Definition: b2Math.h:100
float32 m_lengthB
float32 m_invMassA
b2Body * bodyB
The second attached body.
Definition: b2Joint.h:95
b2Body * m_bodyB
Definition: b2Joint.h:176
b2Sweep m_sweep
Definition: b2Body.h:437
float float32
Definition: b2Settings.h:35
CArrayDouble< 6 > C
float32 lengthB
The a reference length for the segment attached to bodyB.
Definition: b2PulleyJoint.h:65
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
float32 GetCurrentLengthA() const
Get the current length of the segment attached to bodyA.


mvsim
Author(s):
autogenerated on Fri May 7 2021 03:05:51