b2WheelJoint.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
00003 *
00004 * This software is provided 'as-is', without any express or implied
00005 * warranty.  In no event will the authors be held liable for any damages
00006 * arising from the use of this software.
00007 * Permission is granted to anyone to use this software for any purpose,
00008 * including commercial applications, and to alter it and redistribute it
00009 * freely, subject to the following restrictions:
00010 * 1. The origin of this software must not be misrepresented; you must not
00011 * claim that you wrote the original software. If you use this software
00012 * in a product, an acknowledgment in the product documentation would be
00013 * appreciated but is not required.
00014 * 2. Altered source versions must be plainly marked as such, and must not be
00015 * misrepresented as being the original software.
00016 * 3. This notice may not be removed or altered from any source distribution.
00017 */
00018 
00019 #include <Box2D/Dynamics/Joints/b2WheelJoint.h>
00020 #include <Box2D/Dynamics/b2Body.h>
00021 #include <Box2D/Dynamics/b2TimeStep.h>
00022 
00023 // Linear constraint (point-to-line)
00024 // d = pB - pA = xB + rB - xA - rA
00025 // C = dot(ay, d)
00026 // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
00027 //      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
00028 // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
00029 
00030 // Spring linear constraint
00031 // C = dot(ax, d)
00032 // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
00033 // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
00034 
00035 // Motor rotational constraint
00036 // Cdot = wB - wA
00037 // J = [0 0 -1 0 0 1]
00038 
00039 void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)
00040 {
00041         bodyA = bA;
00042         bodyB = bB;
00043         localAnchorA = bodyA->GetLocalPoint(anchor);
00044         localAnchorB = bodyB->GetLocalPoint(anchor);
00045         localAxisA = bodyA->GetLocalVector(axis);
00046 }
00047 
00048 b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def)
00049 : b2Joint(def)
00050 {
00051         m_localAnchorA = def->localAnchorA;
00052         m_localAnchorB = def->localAnchorB;
00053         m_localXAxisA = def->localAxisA;
00054         m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
00055 
00056         m_mass = 0.0f;
00057         m_impulse = 0.0f;
00058         m_motorMass = 0.0f;
00059         m_motorImpulse = 0.0f;
00060         m_springMass = 0.0f;
00061         m_springImpulse = 0.0f;
00062 
00063         m_maxMotorTorque = def->maxMotorTorque;
00064         m_motorSpeed = def->motorSpeed;
00065         m_enableMotor = def->enableMotor;
00066 
00067         m_frequencyHz = def->frequencyHz;
00068         m_dampingRatio = def->dampingRatio;
00069 
00070         m_bias = 0.0f;
00071         m_gamma = 0.0f;
00072 
00073         m_ax.SetZero();
00074         m_ay.SetZero();
00075 }
00076 
00077 void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data)
00078 {
00079         m_indexA = m_bodyA->m_islandIndex;
00080         m_indexB = m_bodyB->m_islandIndex;
00081         m_localCenterA = m_bodyA->m_sweep.localCenter;
00082         m_localCenterB = m_bodyB->m_sweep.localCenter;
00083         m_invMassA = m_bodyA->m_invMass;
00084         m_invMassB = m_bodyB->m_invMass;
00085         m_invIA = m_bodyA->m_invI;
00086         m_invIB = m_bodyB->m_invI;
00087 
00088         float32 mA = m_invMassA, mB = m_invMassB;
00089         float32 iA = m_invIA, iB = m_invIB;
00090 
00091         b2Vec2 cA = data.positions[m_indexA].c;
00092         float32 aA = data.positions[m_indexA].a;
00093         b2Vec2 vA = data.velocities[m_indexA].v;
00094         float32 wA = data.velocities[m_indexA].w;
00095 
00096         b2Vec2 cB = data.positions[m_indexB].c;
00097         float32 aB = data.positions[m_indexB].a;
00098         b2Vec2 vB = data.velocities[m_indexB].v;
00099         float32 wB = data.velocities[m_indexB].w;
00100 
00101         b2Rot qA(aA), qB(aB);
00102 
00103         // Compute the effective masses.
00104         b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
00105         b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
00106         b2Vec2 d = cB + rB - cA - rA;
00107 
00108         // Point to line constraint
00109         {
00110                 m_ay = b2Mul(qA, m_localYAxisA);
00111                 m_sAy = b2Cross(d + rA, m_ay);
00112                 m_sBy = b2Cross(rB, m_ay);
00113 
00114                 m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;
00115 
00116                 if (m_mass > 0.0f)
00117                 {
00118                         m_mass = 1.0f / m_mass;
00119                 }
00120         }
00121 
00122         // Spring constraint
00123         m_springMass = 0.0f;
00124         m_bias = 0.0f;
00125         m_gamma = 0.0f;
00126         if (m_frequencyHz > 0.0f)
00127         {
00128                 m_ax = b2Mul(qA, m_localXAxisA);
00129                 m_sAx = b2Cross(d + rA, m_ax);
00130                 m_sBx = b2Cross(rB, m_ax);
00131 
00132                 float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
00133 
00134                 if (invMass > 0.0f)
00135                 {
00136                         m_springMass = 1.0f / invMass;
00137 
00138                         float32 C = b2Dot(d, m_ax);
00139 
00140                         // Frequency
00141                         float32 omega = 2.0f * b2_pi * m_frequencyHz;
00142 
00143                         // Damping coefficient
00144                         float32 d = 2.0f * m_springMass * m_dampingRatio * omega;
00145 
00146                         // Spring stiffness
00147                         float32 k = m_springMass * omega * omega;
00148 
00149                         // magic formulas
00150                         float32 h = data.step.dt;
00151                         m_gamma = h * (d + h * k);
00152                         if (m_gamma > 0.0f)
00153                         {
00154                                 m_gamma = 1.0f / m_gamma;
00155                         }
00156 
00157                         m_bias = C * h * k * m_gamma;
00158 
00159                         m_springMass = invMass + m_gamma;
00160                         if (m_springMass > 0.0f)
00161                         {
00162                                 m_springMass = 1.0f / m_springMass;
00163                         }
00164                 }
00165         }
00166         else
00167         {
00168                 m_springImpulse = 0.0f;
00169         }
00170 
00171         // Rotational motor
00172         if (m_enableMotor)
00173         {
00174                 m_motorMass = iA + iB;
00175                 if (m_motorMass > 0.0f)
00176                 {
00177                         m_motorMass = 1.0f / m_motorMass;
00178                 }
00179         }
00180         else
00181         {
00182                 m_motorMass = 0.0f;
00183                 m_motorImpulse = 0.0f;
00184         }
00185 
00186         if (data.step.warmStarting)
00187         {
00188                 // Account for variable time step.
00189                 m_impulse *= data.step.dtRatio;
00190                 m_springImpulse *= data.step.dtRatio;
00191                 m_motorImpulse *= data.step.dtRatio;
00192 
00193                 b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax;
00194                 float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
00195                 float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;
00196 
00197                 vA -= m_invMassA * P;
00198                 wA -= m_invIA * LA;
00199 
00200                 vB += m_invMassB * P;
00201                 wB += m_invIB * LB;
00202         }
00203         else
00204         {
00205                 m_impulse = 0.0f;
00206                 m_springImpulse = 0.0f;
00207                 m_motorImpulse = 0.0f;
00208         }
00209 
00210         data.velocities[m_indexA].v = vA;
00211         data.velocities[m_indexA].w = wA;
00212         data.velocities[m_indexB].v = vB;
00213         data.velocities[m_indexB].w = wB;
00214 }
00215 
00216 void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data)
00217 {
00218         float32 mA = m_invMassA, mB = m_invMassB;
00219         float32 iA = m_invIA, iB = m_invIB;
00220 
00221         b2Vec2 vA = data.velocities[m_indexA].v;
00222         float32 wA = data.velocities[m_indexA].w;
00223         b2Vec2 vB = data.velocities[m_indexB].v;
00224         float32 wB = data.velocities[m_indexB].w;
00225 
00226         // Solve spring constraint
00227         {
00228                 float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
00229                 float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);
00230                 m_springImpulse += impulse;
00231 
00232                 b2Vec2 P = impulse * m_ax;
00233                 float32 LA = impulse * m_sAx;
00234                 float32 LB = impulse * m_sBx;
00235 
00236                 vA -= mA * P;
00237                 wA -= iA * LA;
00238 
00239                 vB += mB * P;
00240                 wB += iB * LB;
00241         }
00242 
00243         // Solve rotational motor constraint
00244         {
00245                 float32 Cdot = wB - wA - m_motorSpeed;
00246                 float32 impulse = -m_motorMass * Cdot;
00247 
00248                 float32 oldImpulse = m_motorImpulse;
00249                 float32 maxImpulse = data.step.dt * m_maxMotorTorque;
00250                 m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
00251                 impulse = m_motorImpulse - oldImpulse;
00252 
00253                 wA -= iA * impulse;
00254                 wB += iB * impulse;
00255         }
00256 
00257         // Solve point to line constraint
00258         {
00259                 float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;
00260                 float32 impulse = -m_mass * Cdot;
00261                 m_impulse += impulse;
00262 
00263                 b2Vec2 P = impulse * m_ay;
00264                 float32 LA = impulse * m_sAy;
00265                 float32 LB = impulse * m_sBy;
00266 
00267                 vA -= mA * P;
00268                 wA -= iA * LA;
00269 
00270                 vB += mB * P;
00271                 wB += iB * LB;
00272         }
00273 
00274         data.velocities[m_indexA].v = vA;
00275         data.velocities[m_indexA].w = wA;
00276         data.velocities[m_indexB].v = vB;
00277         data.velocities[m_indexB].w = wB;
00278 }
00279 
00280 bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data)
00281 {
00282         b2Vec2 cA = data.positions[m_indexA].c;
00283         float32 aA = data.positions[m_indexA].a;
00284         b2Vec2 cB = data.positions[m_indexB].c;
00285         float32 aB = data.positions[m_indexB].a;
00286 
00287         b2Rot qA(aA), qB(aB);
00288 
00289         b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
00290         b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
00291         b2Vec2 d = (cB - cA) + rB - rA;
00292 
00293         b2Vec2 ay = b2Mul(qA, m_localYAxisA);
00294 
00295         float32 sAy = b2Cross(d + rA, ay);
00296         float32 sBy = b2Cross(rB, ay);
00297 
00298         float32 C = b2Dot(d, ay);
00299 
00300         float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
00301 
00302         float32 impulse;
00303         if (k != 0.0f)
00304         {
00305                 impulse = - C / k;
00306         }
00307         else
00308         {
00309                 impulse = 0.0f;
00310         }
00311 
00312         b2Vec2 P = impulse * ay;
00313         float32 LA = impulse * sAy;
00314         float32 LB = impulse * sBy;
00315 
00316         cA -= m_invMassA * P;
00317         aA -= m_invIA * LA;
00318         cB += m_invMassB * P;
00319         aB += m_invIB * LB;
00320 
00321         data.positions[m_indexA].c = cA;
00322         data.positions[m_indexA].a = aA;
00323         data.positions[m_indexB].c = cB;
00324         data.positions[m_indexB].a = aB;
00325 
00326         return b2Abs(C) <= b2_linearSlop;
00327 }
00328 
00329 b2Vec2 b2WheelJoint::GetAnchorA() const
00330 {
00331         return m_bodyA->GetWorldPoint(m_localAnchorA);
00332 }
00333 
00334 b2Vec2 b2WheelJoint::GetAnchorB() const
00335 {
00336         return m_bodyB->GetWorldPoint(m_localAnchorB);
00337 }
00338 
00339 b2Vec2 b2WheelJoint::GetReactionForce(float32 inv_dt) const
00340 {
00341         return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax);
00342 }
00343 
00344 float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const
00345 {
00346         return inv_dt * m_motorImpulse;
00347 }
00348 
00349 float32 b2WheelJoint::GetJointTranslation() const
00350 {
00351         b2Body* bA = m_bodyA;
00352         b2Body* bB = m_bodyB;
00353 
00354         b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA);
00355         b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB);
00356         b2Vec2 d = pB - pA;
00357         b2Vec2 axis = bA->GetWorldVector(m_localXAxisA);
00358 
00359         float32 translation = b2Dot(d, axis);
00360         return translation;
00361 }
00362 
00363 float32 b2WheelJoint::GetJointSpeed() const
00364 {
00365         float32 wA = m_bodyA->m_angularVelocity;
00366         float32 wB = m_bodyB->m_angularVelocity;
00367         return wB - wA;
00368 }
00369 
00370 bool b2WheelJoint::IsMotorEnabled() const
00371 {
00372         return m_enableMotor;
00373 }
00374 
00375 void b2WheelJoint::EnableMotor(bool flag)
00376 {
00377         m_bodyA->SetAwake(true);
00378         m_bodyB->SetAwake(true);
00379         m_enableMotor = flag;
00380 }
00381 
00382 void b2WheelJoint::SetMotorSpeed(float32 speed)
00383 {
00384         m_bodyA->SetAwake(true);
00385         m_bodyB->SetAwake(true);
00386         m_motorSpeed = speed;
00387 }
00388 
00389 void b2WheelJoint::SetMaxMotorTorque(float32 torque)
00390 {
00391         m_bodyA->SetAwake(true);
00392         m_bodyB->SetAwake(true);
00393         m_maxMotorTorque = torque;
00394 }
00395 
00396 float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const
00397 {
00398         return inv_dt * m_motorImpulse;
00399 }
00400 
00401 void b2WheelJoint::Dump()
00402 {
00403         int32 indexA = m_bodyA->m_islandIndex;
00404         int32 indexB = m_bodyB->m_islandIndex;
00405 
00406         b2Log("  b2WheelJointDef jd;\n");
00407         b2Log("  jd.bodyA = bodies[%d];\n", indexA);
00408         b2Log("  jd.bodyB = bodies[%d];\n", indexB);
00409         b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
00410         b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
00411         b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
00412         b2Log("  jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y);
00413         b2Log("  jd.enableMotor = bool(%d);\n", m_enableMotor);
00414         b2Log("  jd.motorSpeed = %.15lef;\n", m_motorSpeed);
00415         b2Log("  jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);
00416         b2Log("  jd.frequencyHz = %.15lef;\n", m_frequencyHz);
00417         b2Log("  jd.dampingRatio = %.15lef;\n", m_dampingRatio);
00418         b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
00419 }


mvsim
Author(s):
autogenerated on Thu Sep 7 2017 09:27:48