1/*
2* Copyright (c) 2006-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
19#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
20#include <Box2D/Dynamics/b2Body.h>
21#include <Box2D/Dynamics/b2TimeStep.h>
22
23// 1-D constrained system
24// m (v2 - v1) = lambda
25// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
26// x2 = x1 + h * v2
27
28// 1-D mass-damper-spring system
29// m (v2 - v1) + h * d * v2 + h * k *
30
31// C = norm(p2 - p1) - L
32// u = (p2 - p1) / norm(p2 - p1)
33// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
34// J = [-u -cross(r1, u) u cross(r2, u)]
35// K = J * invM * JT
36// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
37
38void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2,
39 const b2Vec2& anchor1, const b2Vec2& anchor2)
40{
41 bodyA = b1;
42 bodyB = b2;
43 localAnchorA = bodyA->GetLocalPoint(anchor1);
44 localAnchorB = bodyB->GetLocalPoint(anchor2);
45 b2Vec2 d = anchor2 - anchor1;
46 length = d.Length();
47}
48
49b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
50: b2Joint(def)
51{
52 m_localAnchorA = def->localAnchorA;
53 m_localAnchorB = def->localAnchorB;
54 m_length = def->length;
55 m_frequencyHz = def->frequencyHz;
56 m_dampingRatio = def->dampingRatio;
57 m_impulse = 0.0f;
58 m_gamma = 0.0f;
59 m_bias = 0.0f;
60}
61
62void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
63{
64 m_indexA = m_bodyA->m_islandIndex;
65 m_indexB = m_bodyB->m_islandIndex;
66 m_localCenterA = m_bodyA->m_sweep.localCenter;
67 m_localCenterB = m_bodyB->m_sweep.localCenter;
68 m_invMassA = m_bodyA->m_invMass;
69 m_invMassB = m_bodyB->m_invMass;
70 m_invIA = m_bodyA->m_invI;
71 m_invIB = m_bodyB->m_invI;
72
73 b2Vec2 cA = data.positions[m_indexA].c;
74 float32 aA = data.positions[m_indexA].a;
75 b2Vec2 vA = data.velocities[m_indexA].v;
76 float32 wA = data.velocities[m_indexA].w;
77
78 b2Vec2 cB = data.positions[m_indexB].c;
79 float32 aB = data.positions[m_indexB].a;
80 b2Vec2 vB = data.velocities[m_indexB].v;
81 float32 wB = data.velocities[m_indexB].w;
82
83 b2Rot qA(aA), qB(aB);
84
85 m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
86 m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
87 m_u = cB + m_rB - cA - m_rA;
88
89 // Handle singularity.
90 float32 length = m_u.Length();
91 if (length > b2_linearSlop)
92 {
93 m_u *= 1.0f / length;
94 }
95 else
96 {
97 m_u.Set(0.0f, 0.0f);
98 }
99
100 float32 crAu = b2Cross(m_rA, m_u);
101 float32 crBu = b2Cross(m_rB, m_u);
102 float32 invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;
103
104 // Compute the effective mass matrix.
105 m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
106
107 if (m_frequencyHz > 0.0f)
108 {
109 float32 C = length - m_length;
110
111 // Frequency
112 float32 omega = 2.0f * b2_pi * m_frequencyHz;
113
114 // Damping coefficient
115 float32 d = 2.0f * m_mass * m_dampingRatio * omega;
116
117 // Spring stiffness
118 float32 k = m_mass * omega * omega;
119
120 // magic formulas
121 float32 h = data.step.dt;
122 m_gamma = h * (d + h * k);
123 m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
124 m_bias = C * h * k * m_gamma;
125
126 invMass += m_gamma;
127 m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
128 }
129 else
130 {
131 m_gamma = 0.0f;
132 m_bias = 0.0f;
133 }
134
135 if (data.step.warmStarting)
136 {
137 // Scale the impulse to support a variable time step.
138 m_impulse *= data.step.dtRatio;
139
140 b2Vec2 P = m_impulse * m_u;
141 vA -= m_invMassA * P;
142 wA -= m_invIA * b2Cross(m_rA, P);
143 vB += m_invMassB * P;
144 wB += m_invIB * b2Cross(m_rB, P);
145 }
146 else
147 {
148 m_impulse = 0.0f;
149 }
150
151 data.velocities[m_indexA].v = vA;
152 data.velocities[m_indexA].w = wA;
153 data.velocities[m_indexB].v = vB;
154 data.velocities[m_indexB].w = wB;
155}
156
157void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data)
158{
159 b2Vec2 vA = data.velocities[m_indexA].v;
160 float32 wA = data.velocities[m_indexA].w;
161 b2Vec2 vB = data.velocities[m_indexB].v;
162 float32 wB = data.velocities[m_indexB].w;
163
164 // Cdot = dot(u, v + cross(w, r))
165 b2Vec2 vpA = vA + b2Cross(wA, m_rA);
166 b2Vec2 vpB = vB + b2Cross(wB, m_rB);
167 float32 Cdot = b2Dot(m_u, vpB - vpA);
168
169 float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
170 m_impulse += impulse;
171
172 b2Vec2 P = impulse * m_u;
173 vA -= m_invMassA * P;
174 wA -= m_invIA * b2Cross(m_rA, P);
175 vB += m_invMassB * P;
176 wB += m_invIB * b2Cross(m_rB, P);
177
178 data.velocities[m_indexA].v = vA;
179 data.velocities[m_indexA].w = wA;
180 data.velocities[m_indexB].v = vB;
181 data.velocities[m_indexB].w = wB;
182}
183
184bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data)
185{
186 if (m_frequencyHz > 0.0f)
187 {
188 // There is no position correction for soft distance constraints.
189 return true;
190 }
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
199 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
200 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
201 b2Vec2 u = cB + rB - cA - rA;
202
203 float32 length = u.Normalize();
204 float32 C = length - m_length;
205 C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
206
207 float32 impulse = -m_mass * C;
208 b2Vec2 P = impulse * u;
209
210 cA -= m_invMassA * P;
211 aA -= m_invIA * b2Cross(rA, P);
212 cB += m_invMassB * P;
213 aB += m_invIB * b2Cross(rB, P);
214
215 data.positions[m_indexA].c = cA;
216 data.positions[m_indexA].a = aA;
217 data.positions[m_indexB].c = cB;
218 data.positions[m_indexB].a = aB;
219
220 return b2Abs(C) < b2_linearSlop;
221}
222
223b2Vec2 b2DistanceJoint::GetAnchorA() const
224{
225 return m_bodyA->GetWorldPoint(m_localAnchorA);
226}
227
228b2Vec2 b2DistanceJoint::GetAnchorB() const
229{
230 return m_bodyB->GetWorldPoint(m_localAnchorB);
231}
232
233b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const
234{
235 b2Vec2 F = (inv_dt * m_impulse) * m_u;
236 return F;
237}
238
239float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const
240{
241 B2_NOT_USED(inv_dt);
242 return 0.0f;
243}
244
245void b2DistanceJoint::Dump()
246{
247 int32 indexA = m_bodyA->m_islandIndex;
248 int32 indexB = m_bodyB->m_islandIndex;
249
250 b2Log(" b2DistanceJointDef jd;\n");
251 b2Log(" jd.bodyA = bodies[%d];\n", indexA);
252 b2Log(" jd.bodyB = bodies[%d];\n", indexB);
253 b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
254 b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
255 b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
256 b2Log(" jd.length = %.15lef;\n", m_length);
257 b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz);
258 b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio);
259 b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
260}
261