00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <Box2D/Dynamics/Joints/b2WeldJoint.h>
00020 #include <Box2D/Dynamics/b2Body.h>
00021 #include <Box2D/Dynamics/b2TimeStep.h>
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
00038 {
00039 bodyA = bA;
00040 bodyB = bB;
00041 localAnchorA = bodyA->GetLocalPoint(anchor);
00042 localAnchorB = bodyB->GetLocalPoint(anchor);
00043 referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
00044 }
00045
00046 b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def)
00047 : b2Joint(def)
00048 {
00049 m_localAnchorA = def->localAnchorA;
00050 m_localAnchorB = def->localAnchorB;
00051 m_referenceAngle = def->referenceAngle;
00052 m_frequencyHz = def->frequencyHz;
00053 m_dampingRatio = def->dampingRatio;
00054
00055 m_impulse.SetZero();
00056 }
00057
00058 void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)
00059 {
00060 m_indexA = m_bodyA->m_islandIndex;
00061 m_indexB = m_bodyB->m_islandIndex;
00062 m_localCenterA = m_bodyA->m_sweep.localCenter;
00063 m_localCenterB = m_bodyB->m_sweep.localCenter;
00064 m_invMassA = m_bodyA->m_invMass;
00065 m_invMassB = m_bodyB->m_invMass;
00066 m_invIA = m_bodyA->m_invI;
00067 m_invIB = m_bodyB->m_invI;
00068
00069 float32 aA = data.positions[m_indexA].a;
00070 b2Vec2 vA = data.velocities[m_indexA].v;
00071 float32 wA = data.velocities[m_indexA].w;
00072
00073 float32 aB = data.positions[m_indexB].a;
00074 b2Vec2 vB = data.velocities[m_indexB].v;
00075 float32 wB = data.velocities[m_indexB].w;
00076
00077 b2Rot qA(aA), qB(aB);
00078
00079 m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
00080 m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091 float32 mA = m_invMassA, mB = m_invMassB;
00092 float32 iA = m_invIA, iB = m_invIB;
00093
00094 b2Mat33 K;
00095 K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
00096 K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
00097 K.ez.x = -m_rA.y * iA - m_rB.y * iB;
00098 K.ex.y = K.ey.x;
00099 K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
00100 K.ez.y = m_rA.x * iA + m_rB.x * iB;
00101 K.ex.z = K.ez.x;
00102 K.ey.z = K.ez.y;
00103 K.ez.z = iA + iB;
00104
00105 if (m_frequencyHz > 0.0f)
00106 {
00107 K.GetInverse22(&m_mass);
00108
00109 float32 invM = iA + iB;
00110 float32 m = invM > 0.0f ? 1.0f / invM : 0.0f;
00111
00112 float32 C = aB - aA - m_referenceAngle;
00113
00114
00115 float32 omega = 2.0f * b2_pi * m_frequencyHz;
00116
00117
00118 float32 d = 2.0f * m * m_dampingRatio * omega;
00119
00120
00121 float32 k = m * omega * omega;
00122
00123
00124 float32 h = data.step.dt;
00125 m_gamma = h * (d + h * k);
00126 m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
00127 m_bias = C * h * k * m_gamma;
00128
00129 invM += m_gamma;
00130 m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
00131 }
00132 else if (K.ez.z == 0.0f)
00133 {
00134 K.GetInverse22(&m_mass);
00135 m_gamma = 0.0f;
00136 m_bias = 0.0f;
00137 }
00138 else
00139 {
00140 K.GetSymInverse33(&m_mass);
00141 m_gamma = 0.0f;
00142 m_bias = 0.0f;
00143 }
00144
00145 if (data.step.warmStarting)
00146 {
00147
00148 m_impulse *= data.step.dtRatio;
00149
00150 b2Vec2 P(m_impulse.x, m_impulse.y);
00151
00152 vA -= mA * P;
00153 wA -= iA * (b2Cross(m_rA, P) + m_impulse.z);
00154
00155 vB += mB * P;
00156 wB += iB * (b2Cross(m_rB, P) + m_impulse.z);
00157 }
00158 else
00159 {
00160 m_impulse.SetZero();
00161 }
00162
00163 data.velocities[m_indexA].v = vA;
00164 data.velocities[m_indexA].w = wA;
00165 data.velocities[m_indexB].v = vB;
00166 data.velocities[m_indexB].w = wB;
00167 }
00168
00169 void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data)
00170 {
00171 b2Vec2 vA = data.velocities[m_indexA].v;
00172 float32 wA = data.velocities[m_indexA].w;
00173 b2Vec2 vB = data.velocities[m_indexB].v;
00174 float32 wB = data.velocities[m_indexB].w;
00175
00176 float32 mA = m_invMassA, mB = m_invMassB;
00177 float32 iA = m_invIA, iB = m_invIB;
00178
00179 if (m_frequencyHz > 0.0f)
00180 {
00181 float32 Cdot2 = wB - wA;
00182
00183 float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);
00184 m_impulse.z += impulse2;
00185
00186 wA -= iA * impulse2;
00187 wB += iB * impulse2;
00188
00189 b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
00190
00191 b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1);
00192 m_impulse.x += impulse1.x;
00193 m_impulse.y += impulse1.y;
00194
00195 b2Vec2 P = impulse1;
00196
00197 vA -= mA * P;
00198 wA -= iA * b2Cross(m_rA, P);
00199
00200 vB += mB * P;
00201 wB += iB * b2Cross(m_rB, P);
00202 }
00203 else
00204 {
00205 b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
00206 float32 Cdot2 = wB - wA;
00207 b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
00208
00209 b2Vec3 impulse = -b2Mul(m_mass, Cdot);
00210 m_impulse += impulse;
00211
00212 b2Vec2 P(impulse.x, impulse.y);
00213
00214 vA -= mA * P;
00215 wA -= iA * (b2Cross(m_rA, P) + impulse.z);
00216
00217 vB += mB * P;
00218 wB += iB * (b2Cross(m_rB, P) + impulse.z);
00219 }
00220
00221 data.velocities[m_indexA].v = vA;
00222 data.velocities[m_indexA].w = wA;
00223 data.velocities[m_indexB].v = vB;
00224 data.velocities[m_indexB].w = wB;
00225 }
00226
00227 bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)
00228 {
00229 b2Vec2 cA = data.positions[m_indexA].c;
00230 float32 aA = data.positions[m_indexA].a;
00231 b2Vec2 cB = data.positions[m_indexB].c;
00232 float32 aB = data.positions[m_indexB].a;
00233
00234 b2Rot qA(aA), qB(aB);
00235
00236 float32 mA = m_invMassA, mB = m_invMassB;
00237 float32 iA = m_invIA, iB = m_invIB;
00238
00239 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
00240 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
00241
00242 float32 positionError, angularError;
00243
00244 b2Mat33 K;
00245 K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
00246 K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
00247 K.ez.x = -rA.y * iA - rB.y * iB;
00248 K.ex.y = K.ey.x;
00249 K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
00250 K.ez.y = rA.x * iA + rB.x * iB;
00251 K.ex.z = K.ez.x;
00252 K.ey.z = K.ez.y;
00253 K.ez.z = iA + iB;
00254
00255 if (m_frequencyHz > 0.0f)
00256 {
00257 b2Vec2 C1 = cB + rB - cA - rA;
00258
00259 positionError = C1.Length();
00260 angularError = 0.0f;
00261
00262 b2Vec2 P = -K.Solve22(C1);
00263
00264 cA -= mA * P;
00265 aA -= iA * b2Cross(rA, P);
00266
00267 cB += mB * P;
00268 aB += iB * b2Cross(rB, P);
00269 }
00270 else
00271 {
00272 b2Vec2 C1 = cB + rB - cA - rA;
00273 float32 C2 = aB - aA - m_referenceAngle;
00274
00275 positionError = C1.Length();
00276 angularError = b2Abs(C2);
00277
00278 b2Vec3 C(C1.x, C1.y, C2);
00279
00280 b2Vec3 impulse;
00281 if (K.ez.z > 0.0f)
00282 {
00283 impulse = -K.Solve33(C);
00284 }
00285 else
00286 {
00287 b2Vec2 impulse2 = -K.Solve22(C1);
00288 impulse.Set(impulse2.x, impulse2.y, 0.0f);
00289 }
00290
00291 b2Vec2 P(impulse.x, impulse.y);
00292
00293 cA -= mA * P;
00294 aA -= iA * (b2Cross(rA, P) + impulse.z);
00295
00296 cB += mB * P;
00297 aB += iB * (b2Cross(rB, P) + impulse.z);
00298 }
00299
00300 data.positions[m_indexA].c = cA;
00301 data.positions[m_indexA].a = aA;
00302 data.positions[m_indexB].c = cB;
00303 data.positions[m_indexB].a = aB;
00304
00305 return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
00306 }
00307
00308 b2Vec2 b2WeldJoint::GetAnchorA() const
00309 {
00310 return m_bodyA->GetWorldPoint(m_localAnchorA);
00311 }
00312
00313 b2Vec2 b2WeldJoint::GetAnchorB() const
00314 {
00315 return m_bodyB->GetWorldPoint(m_localAnchorB);
00316 }
00317
00318 b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const
00319 {
00320 b2Vec2 P(m_impulse.x, m_impulse.y);
00321 return inv_dt * P;
00322 }
00323
00324 float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const
00325 {
00326 return inv_dt * m_impulse.z;
00327 }
00328
00329 void b2WeldJoint::Dump()
00330 {
00331 int32 indexA = m_bodyA->m_islandIndex;
00332 int32 indexB = m_bodyB->m_islandIndex;
00333
00334 b2Log(" b2WeldJointDef jd;\n");
00335 b2Log(" jd.bodyA = bodies[%d];\n", indexA);
00336 b2Log(" jd.bodyB = bodies[%d];\n", indexB);
00337 b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
00338 b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
00339 b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
00340 b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle);
00341 b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz);
00342 b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio);
00343 b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
00344 }