Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <Box2D/Dynamics/Joints/b2MotorJoint.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 void b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB)
00036 {
00037 bodyA = bA;
00038 bodyB = bB;
00039 b2Vec2 xB = bodyB->GetPosition();
00040 linearOffset = bodyA->GetLocalPoint(xB);
00041
00042 float32 angleA = bodyA->GetAngle();
00043 float32 angleB = bodyB->GetAngle();
00044 angularOffset = angleB - angleA;
00045 }
00046
00047 b2MotorJoint::b2MotorJoint(const b2MotorJointDef* def)
00048 : b2Joint(def)
00049 {
00050 m_linearOffset = def->linearOffset;
00051 m_angularOffset = def->angularOffset;
00052
00053 m_linearImpulse.SetZero();
00054 m_angularImpulse = 0.0f;
00055
00056 m_maxForce = def->maxForce;
00057 m_maxTorque = def->maxTorque;
00058 m_correctionFactor = def->correctionFactor;
00059 }
00060
00061 void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data)
00062 {
00063 m_indexA = m_bodyA->m_islandIndex;
00064 m_indexB = m_bodyB->m_islandIndex;
00065 m_localCenterA = m_bodyA->m_sweep.localCenter;
00066 m_localCenterB = m_bodyB->m_sweep.localCenter;
00067 m_invMassA = m_bodyA->m_invMass;
00068 m_invMassB = m_bodyB->m_invMass;
00069 m_invIA = m_bodyA->m_invI;
00070 m_invIB = m_bodyB->m_invI;
00071
00072 b2Vec2 cA = data.positions[m_indexA].c;
00073 float32 aA = data.positions[m_indexA].a;
00074 b2Vec2 vA = data.velocities[m_indexA].v;
00075 float32 wA = data.velocities[m_indexA].w;
00076
00077 b2Vec2 cB = data.positions[m_indexB].c;
00078 float32 aB = data.positions[m_indexB].a;
00079 b2Vec2 vB = data.velocities[m_indexB].v;
00080 float32 wB = data.velocities[m_indexB].w;
00081
00082 b2Rot qA(aA), qB(aB);
00083
00084
00085 m_rA = b2Mul(qA, -m_localCenterA);
00086 m_rB = b2Mul(qB, -m_localCenterB);
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097 float32 mA = m_invMassA, mB = m_invMassB;
00098 float32 iA = m_invIA, iB = m_invIB;
00099
00100 b2Mat22 K;
00101 K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
00102 K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
00103 K.ey.x = K.ex.y;
00104 K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
00105
00106 m_linearMass = K.GetInverse();
00107
00108 m_angularMass = iA + iB;
00109 if (m_angularMass > 0.0f)
00110 {
00111 m_angularMass = 1.0f / m_angularMass;
00112 }
00113
00114 m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset);
00115 m_angularError = aB - aA - m_angularOffset;
00116
00117 if (data.step.warmStarting)
00118 {
00119
00120 m_linearImpulse *= data.step.dtRatio;
00121 m_angularImpulse *= data.step.dtRatio;
00122
00123 b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
00124 vA -= mA * P;
00125 wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);
00126 vB += mB * P;
00127 wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);
00128 }
00129 else
00130 {
00131 m_linearImpulse.SetZero();
00132 m_angularImpulse = 0.0f;
00133 }
00134
00135 data.velocities[m_indexA].v = vA;
00136 data.velocities[m_indexA].w = wA;
00137 data.velocities[m_indexB].v = vB;
00138 data.velocities[m_indexB].w = wB;
00139 }
00140
00141 void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data)
00142 {
00143 b2Vec2 vA = data.velocities[m_indexA].v;
00144 float32 wA = data.velocities[m_indexA].w;
00145 b2Vec2 vB = data.velocities[m_indexB].v;
00146 float32 wB = data.velocities[m_indexB].w;
00147
00148 float32 mA = m_invMassA, mB = m_invMassB;
00149 float32 iA = m_invIA, iB = m_invIB;
00150
00151 float32 h = data.step.dt;
00152 float32 inv_h = data.step.inv_dt;
00153
00154
00155 {
00156 float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError;
00157 float32 impulse = -m_angularMass * Cdot;
00158
00159 float32 oldImpulse = m_angularImpulse;
00160 float32 maxImpulse = h * m_maxTorque;
00161 m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
00162 impulse = m_angularImpulse - oldImpulse;
00163
00164 wA -= iA * impulse;
00165 wB += iB * impulse;
00166 }
00167
00168
00169 {
00170 b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError;
00171
00172 b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
00173 b2Vec2 oldImpulse = m_linearImpulse;
00174 m_linearImpulse += impulse;
00175
00176 float32 maxImpulse = h * m_maxForce;
00177
00178 if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
00179 {
00180 m_linearImpulse.Normalize();
00181 m_linearImpulse *= maxImpulse;
00182 }
00183
00184 impulse = m_linearImpulse - oldImpulse;
00185
00186 vA -= mA * impulse;
00187 wA -= iA * b2Cross(m_rA, impulse);
00188
00189 vB += mB * impulse;
00190 wB += iB * b2Cross(m_rB, impulse);
00191 }
00192
00193 data.velocities[m_indexA].v = vA;
00194 data.velocities[m_indexA].w = wA;
00195 data.velocities[m_indexB].v = vB;
00196 data.velocities[m_indexB].w = wB;
00197 }
00198
00199 bool b2MotorJoint::SolvePositionConstraints(const b2SolverData& data)
00200 {
00201 B2_NOT_USED(data);
00202
00203 return true;
00204 }
00205
00206 b2Vec2 b2MotorJoint::GetAnchorA() const
00207 {
00208 return m_bodyA->GetPosition();
00209 }
00210
00211 b2Vec2 b2MotorJoint::GetAnchorB() const
00212 {
00213 return m_bodyB->GetPosition();
00214 }
00215
00216 b2Vec2 b2MotorJoint::GetReactionForce(float32 inv_dt) const
00217 {
00218 return inv_dt * m_linearImpulse;
00219 }
00220
00221 float32 b2MotorJoint::GetReactionTorque(float32 inv_dt) const
00222 {
00223 return inv_dt * m_angularImpulse;
00224 }
00225
00226 void b2MotorJoint::SetMaxForce(float32 force)
00227 {
00228 b2Assert(b2IsValid(force) && force >= 0.0f);
00229 m_maxForce = force;
00230 }
00231
00232 float32 b2MotorJoint::GetMaxForce() const
00233 {
00234 return m_maxForce;
00235 }
00236
00237 void b2MotorJoint::SetMaxTorque(float32 torque)
00238 {
00239 b2Assert(b2IsValid(torque) && torque >= 0.0f);
00240 m_maxTorque = torque;
00241 }
00242
00243 float32 b2MotorJoint::GetMaxTorque() const
00244 {
00245 return m_maxTorque;
00246 }
00247
00248 void b2MotorJoint::SetCorrectionFactor(float32 factor)
00249 {
00250 b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f);
00251 m_correctionFactor = factor;
00252 }
00253
00254 float32 b2MotorJoint::GetCorrectionFactor() const
00255 {
00256 return m_correctionFactor;
00257 }
00258
00259 void b2MotorJoint::SetLinearOffset(const b2Vec2& linearOffset)
00260 {
00261 if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y)
00262 {
00263 m_bodyA->SetAwake(true);
00264 m_bodyB->SetAwake(true);
00265 m_linearOffset = linearOffset;
00266 }
00267 }
00268
00269 const b2Vec2& b2MotorJoint::GetLinearOffset() const
00270 {
00271 return m_linearOffset;
00272 }
00273
00274 void b2MotorJoint::SetAngularOffset(float32 angularOffset)
00275 {
00276 if (angularOffset != m_angularOffset)
00277 {
00278 m_bodyA->SetAwake(true);
00279 m_bodyB->SetAwake(true);
00280 m_angularOffset = angularOffset;
00281 }
00282 }
00283
00284 float32 b2MotorJoint::GetAngularOffset() const
00285 {
00286 return m_angularOffset;
00287 }
00288
00289 void b2MotorJoint::Dump()
00290 {
00291 int32 indexA = m_bodyA->m_islandIndex;
00292 int32 indexB = m_bodyB->m_islandIndex;
00293
00294 b2Log(" b2MotorJointDef jd;\n");
00295 b2Log(" jd.bodyA = bodies[%d];\n", indexA);
00296 b2Log(" jd.bodyB = bodies[%d];\n", indexB);
00297 b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
00298 b2Log(" jd.linearOffset.Set(%.15lef, %.15lef);\n", m_linearOffset.x, m_linearOffset.y);
00299 b2Log(" jd.angularOffset = %.15lef;\n", m_angularOffset);
00300 b2Log(" jd.maxForce = %.15lef;\n", m_maxForce);
00301 b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque);
00302 b2Log(" jd.correctionFactor = %.15lef;\n", m_correctionFactor);
00303 b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
00304 }