b2MotorJoint.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2006-2012 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/b2MotorJoint.h>
00020 #include <Box2D/Dynamics/b2Body.h>
00021 #include <Box2D/Dynamics/b2TimeStep.h>
00022 
00023 // Point-to-point constraint
00024 // Cdot = v2 - v1
00025 //      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
00026 // J = [-I -r1_skew I r2_skew ]
00027 // Identity used:
00028 // w k % (rx i + ry j) = w * (-ry i + rx j)
00029 
00030 // Angle constraint
00031 // Cdot = w2 - w1
00032 // J = [0 0 -1 0 0 1]
00033 // K = invI1 + invI2
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         // Compute the effective mass matrix.
00085         m_rA = b2Mul(qA, -m_localCenterA);
00086         m_rB = b2Mul(qB, -m_localCenterB);
00087 
00088         // J = [-I -r1_skew I r2_skew]
00089         //     [ 0       -1 0       1]
00090         // r_skew = [-ry; rx]
00091 
00092         // Matlab
00093         // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
00094         //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
00095         //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
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                 // Scale impulses to support a variable time step.
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         // Solve angular friction
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         // Solve linear friction
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 }


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