b2PulleyJoint.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 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/b2PulleyJoint.h>
00020 #include <Box2D/Dynamics/b2Body.h>
00021 #include <Box2D/Dynamics/b2TimeStep.h>
00022 
00023 // Pulley:
00024 // length1 = norm(p1 - s1)
00025 // length2 = norm(p2 - s2)
00026 // C0 = (length1 + ratio * length2)_initial
00027 // C = C0 - (length1 + ratio * length2)
00028 // u1 = (p1 - s1) / norm(p1 - s1)
00029 // u2 = (p2 - s2) / norm(p2 - s2)
00030 // Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
00031 // J = -[u1 cross(r1, u1) ratio * u2  ratio * cross(r2, u2)]
00032 // K = J * invM * JT
00033 //   = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
00034 
00035 void b2PulleyJointDef::Initialize(b2Body* bA, b2Body* bB,
00036                                 const b2Vec2& groundA, const b2Vec2& groundB,
00037                                 const b2Vec2& anchorA, const b2Vec2& anchorB,
00038                                 float32 r)
00039 {
00040         bodyA = bA;
00041         bodyB = bB;
00042         groundAnchorA = groundA;
00043         groundAnchorB = groundB;
00044         localAnchorA = bodyA->GetLocalPoint(anchorA);
00045         localAnchorB = bodyB->GetLocalPoint(anchorB);
00046         b2Vec2 dA = anchorA - groundA;
00047         lengthA = dA.Length();
00048         b2Vec2 dB = anchorB - groundB;
00049         lengthB = dB.Length();
00050         ratio = r;
00051         b2Assert(ratio > b2_epsilon);
00052 }
00053 
00054 b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def)
00055 : b2Joint(def)
00056 {
00057         m_groundAnchorA = def->groundAnchorA;
00058         m_groundAnchorB = def->groundAnchorB;
00059         m_localAnchorA = def->localAnchorA;
00060         m_localAnchorB = def->localAnchorB;
00061 
00062         m_lengthA = def->lengthA;
00063         m_lengthB = def->lengthB;
00064 
00065         b2Assert(def->ratio != 0.0f);
00066         m_ratio = def->ratio;
00067 
00068         m_constant = def->lengthA + m_ratio * def->lengthB;
00069 
00070         m_impulse = 0.0f;
00071 }
00072 
00073 void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data)
00074 {
00075         m_indexA = m_bodyA->m_islandIndex;
00076         m_indexB = m_bodyB->m_islandIndex;
00077         m_localCenterA = m_bodyA->m_sweep.localCenter;
00078         m_localCenterB = m_bodyB->m_sweep.localCenter;
00079         m_invMassA = m_bodyA->m_invMass;
00080         m_invMassB = m_bodyB->m_invMass;
00081         m_invIA = m_bodyA->m_invI;
00082         m_invIB = m_bodyB->m_invI;
00083 
00084         b2Vec2 cA = data.positions[m_indexA].c;
00085         float32 aA = data.positions[m_indexA].a;
00086         b2Vec2 vA = data.velocities[m_indexA].v;
00087         float32 wA = data.velocities[m_indexA].w;
00088 
00089         b2Vec2 cB = data.positions[m_indexB].c;
00090         float32 aB = data.positions[m_indexB].a;
00091         b2Vec2 vB = data.velocities[m_indexB].v;
00092         float32 wB = data.velocities[m_indexB].w;
00093 
00094         b2Rot qA(aA), qB(aB);
00095 
00096         m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
00097         m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
00098 
00099         // Get the pulley axes.
00100         m_uA = cA + m_rA - m_groundAnchorA;
00101         m_uB = cB + m_rB - m_groundAnchorB;
00102 
00103         float32 lengthA = m_uA.Length();
00104         float32 lengthB = m_uB.Length();
00105 
00106         if (lengthA > 10.0f * b2_linearSlop)
00107         {
00108                 m_uA *= 1.0f / lengthA;
00109         }
00110         else
00111         {
00112                 m_uA.SetZero();
00113         }
00114 
00115         if (lengthB > 10.0f * b2_linearSlop)
00116         {
00117                 m_uB *= 1.0f / lengthB;
00118         }
00119         else
00120         {
00121                 m_uB.SetZero();
00122         }
00123 
00124         // Compute effective mass.
00125         float32 ruA = b2Cross(m_rA, m_uA);
00126         float32 ruB = b2Cross(m_rB, m_uB);
00127 
00128         float32 mA = m_invMassA + m_invIA * ruA * ruA;
00129         float32 mB = m_invMassB + m_invIB * ruB * ruB;
00130 
00131         m_mass = mA + m_ratio * m_ratio * mB;
00132 
00133         if (m_mass > 0.0f)
00134         {
00135                 m_mass = 1.0f / m_mass;
00136         }
00137 
00138         if (data.step.warmStarting)
00139         {
00140                 // Scale impulses to support variable time steps.
00141                 m_impulse *= data.step.dtRatio;
00142 
00143                 // Warm starting.
00144                 b2Vec2 PA = -(m_impulse) * m_uA;
00145                 b2Vec2 PB = (-m_ratio * m_impulse) * m_uB;
00146 
00147                 vA += m_invMassA * PA;
00148                 wA += m_invIA * b2Cross(m_rA, PA);
00149                 vB += m_invMassB * PB;
00150                 wB += m_invIB * b2Cross(m_rB, PB);
00151         }
00152         else
00153         {
00154                 m_impulse = 0.0f;
00155         }
00156 
00157         data.velocities[m_indexA].v = vA;
00158         data.velocities[m_indexA].w = wA;
00159         data.velocities[m_indexB].v = vB;
00160         data.velocities[m_indexB].w = wB;
00161 }
00162 
00163 void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data)
00164 {
00165         b2Vec2 vA = data.velocities[m_indexA].v;
00166         float32 wA = data.velocities[m_indexA].w;
00167         b2Vec2 vB = data.velocities[m_indexB].v;
00168         float32 wB = data.velocities[m_indexB].w;
00169 
00170         b2Vec2 vpA = vA + b2Cross(wA, m_rA);
00171         b2Vec2 vpB = vB + b2Cross(wB, m_rB);
00172 
00173         float32 Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB);
00174         float32 impulse = -m_mass * Cdot;
00175         m_impulse += impulse;
00176 
00177         b2Vec2 PA = -impulse * m_uA;
00178         b2Vec2 PB = -m_ratio * impulse * m_uB;
00179         vA += m_invMassA * PA;
00180         wA += m_invIA * b2Cross(m_rA, PA);
00181         vB += m_invMassB * PB;
00182         wB += m_invIB * b2Cross(m_rB, PB);
00183 
00184         data.velocities[m_indexA].v = vA;
00185         data.velocities[m_indexA].w = wA;
00186         data.velocities[m_indexB].v = vB;
00187         data.velocities[m_indexB].w = wB;
00188 }
00189 
00190 bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data)
00191 {
00192         b2Vec2 cA = data.positions[m_indexA].c;
00193         float32 aA = data.positions[m_indexA].a;
00194         b2Vec2 cB = data.positions[m_indexB].c;
00195         float32 aB = data.positions[m_indexB].a;
00196 
00197         b2Rot qA(aA), qB(aB);
00198 
00199         b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
00200         b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
00201 
00202         // Get the pulley axes.
00203         b2Vec2 uA = cA + rA - m_groundAnchorA;
00204         b2Vec2 uB = cB + rB - m_groundAnchorB;
00205 
00206         float32 lengthA = uA.Length();
00207         float32 lengthB = uB.Length();
00208 
00209         if (lengthA > 10.0f * b2_linearSlop)
00210         {
00211                 uA *= 1.0f / lengthA;
00212         }
00213         else
00214         {
00215                 uA.SetZero();
00216         }
00217 
00218         if (lengthB > 10.0f * b2_linearSlop)
00219         {
00220                 uB *= 1.0f / lengthB;
00221         }
00222         else
00223         {
00224                 uB.SetZero();
00225         }
00226 
00227         // Compute effective mass.
00228         float32 ruA = b2Cross(rA, uA);
00229         float32 ruB = b2Cross(rB, uB);
00230 
00231         float32 mA = m_invMassA + m_invIA * ruA * ruA;
00232         float32 mB = m_invMassB + m_invIB * ruB * ruB;
00233 
00234         float32 mass = mA + m_ratio * m_ratio * mB;
00235 
00236         if (mass > 0.0f)
00237         {
00238                 mass = 1.0f / mass;
00239         }
00240 
00241         float32 C = m_constant - lengthA - m_ratio * lengthB;
00242         float32 linearError = b2Abs(C);
00243 
00244         float32 impulse = -mass * C;
00245 
00246         b2Vec2 PA = -impulse * uA;
00247         b2Vec2 PB = -m_ratio * impulse * uB;
00248 
00249         cA += m_invMassA * PA;
00250         aA += m_invIA * b2Cross(rA, PA);
00251         cB += m_invMassB * PB;
00252         aB += m_invIB * b2Cross(rB, PB);
00253 
00254         data.positions[m_indexA].c = cA;
00255         data.positions[m_indexA].a = aA;
00256         data.positions[m_indexB].c = cB;
00257         data.positions[m_indexB].a = aB;
00258 
00259         return linearError < b2_linearSlop;
00260 }
00261 
00262 b2Vec2 b2PulleyJoint::GetAnchorA() const
00263 {
00264         return m_bodyA->GetWorldPoint(m_localAnchorA);
00265 }
00266 
00267 b2Vec2 b2PulleyJoint::GetAnchorB() const
00268 {
00269         return m_bodyB->GetWorldPoint(m_localAnchorB);
00270 }
00271 
00272 b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const
00273 {
00274         b2Vec2 P = m_impulse * m_uB;
00275         return inv_dt * P;
00276 }
00277 
00278 float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const
00279 {
00280         B2_NOT_USED(inv_dt);
00281         return 0.0f;
00282 }
00283 
00284 b2Vec2 b2PulleyJoint::GetGroundAnchorA() const
00285 {
00286         return m_groundAnchorA;
00287 }
00288 
00289 b2Vec2 b2PulleyJoint::GetGroundAnchorB() const
00290 {
00291         return m_groundAnchorB;
00292 }
00293 
00294 float32 b2PulleyJoint::GetLengthA() const
00295 {
00296         return m_lengthA;
00297 }
00298 
00299 float32 b2PulleyJoint::GetLengthB() const
00300 {
00301         return m_lengthB;
00302 }
00303 
00304 float32 b2PulleyJoint::GetRatio() const
00305 {
00306         return m_ratio;
00307 }
00308 
00309 float32 b2PulleyJoint::GetCurrentLengthA() const
00310 {
00311         b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA);
00312         b2Vec2 s = m_groundAnchorA;
00313         b2Vec2 d = p - s;
00314         return d.Length();
00315 }
00316 
00317 float32 b2PulleyJoint::GetCurrentLengthB() const
00318 {
00319         b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB);
00320         b2Vec2 s = m_groundAnchorB;
00321         b2Vec2 d = p - s;
00322         return d.Length();
00323 }
00324 
00325 void b2PulleyJoint::Dump()
00326 {
00327         int32 indexA = m_bodyA->m_islandIndex;
00328         int32 indexB = m_bodyB->m_islandIndex;
00329 
00330         b2Log("  b2PulleyJointDef jd;\n");
00331         b2Log("  jd.bodyA = bodies[%d];\n", indexA);
00332         b2Log("  jd.bodyB = bodies[%d];\n", indexB);
00333         b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
00334         b2Log("  jd.groundAnchorA.Set(%.15lef, %.15lef);\n", m_groundAnchorA.x, m_groundAnchorA.y);
00335         b2Log("  jd.groundAnchorB.Set(%.15lef, %.15lef);\n", m_groundAnchorB.x, m_groundAnchorB.y);
00336         b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
00337         b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
00338         b2Log("  jd.lengthA = %.15lef;\n", m_lengthA);
00339         b2Log("  jd.lengthB = %.15lef;\n", m_lengthB);
00340         b2Log("  jd.ratio = %.15lef;\n", m_ratio);
00341         b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
00342 }
00343 
00344 void b2PulleyJoint::ShiftOrigin(const b2Vec2& newOrigin)
00345 {
00346         m_groundAnchorA -= newOrigin;
00347         m_groundAnchorB -= newOrigin;
00348 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:35