b2FrictionJoint.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2006-2011 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/b2FrictionJoint.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 b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
00036 {
00037         bodyA = bA;
00038         bodyB = bB;
00039         localAnchorA = bodyA->GetLocalPoint(anchor);
00040         localAnchorB = bodyB->GetLocalPoint(anchor);
00041 }
00042 
00043 b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def)
00044 : b2Joint(def)
00045 {
00046         m_localAnchorA = def->localAnchorA;
00047         m_localAnchorB = def->localAnchorB;
00048 
00049         m_linearImpulse.SetZero();
00050         m_angularImpulse = 0.0f;
00051 
00052         m_maxForce = def->maxForce;
00053         m_maxTorque = def->maxTorque;
00054 }
00055 
00056 void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)
00057 {
00058         m_indexA = m_bodyA->m_islandIndex;
00059         m_indexB = m_bodyB->m_islandIndex;
00060         m_localCenterA = m_bodyA->m_sweep.localCenter;
00061         m_localCenterB = m_bodyB->m_sweep.localCenter;
00062         m_invMassA = m_bodyA->m_invMass;
00063         m_invMassB = m_bodyB->m_invMass;
00064         m_invIA = m_bodyA->m_invI;
00065         m_invIB = m_bodyB->m_invI;
00066 
00067         float32 aA = data.positions[m_indexA].a;
00068         b2Vec2 vA = data.velocities[m_indexA].v;
00069         float32 wA = data.velocities[m_indexA].w;
00070 
00071         float32 aB = data.positions[m_indexB].a;
00072         b2Vec2 vB = data.velocities[m_indexB].v;
00073         float32 wB = data.velocities[m_indexB].w;
00074 
00075         b2Rot qA(aA), qB(aB);
00076 
00077         // Compute the effective mass matrix.
00078         m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
00079         m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
00080 
00081         // J = [-I -r1_skew I r2_skew]
00082         //     [ 0       -1 0       1]
00083         // r_skew = [-ry; rx]
00084 
00085         // Matlab
00086         // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
00087         //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
00088         //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
00089 
00090         float32 mA = m_invMassA, mB = m_invMassB;
00091         float32 iA = m_invIA, iB = m_invIB;
00092 
00093         b2Mat22 K;
00094         K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
00095         K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
00096         K.ey.x = K.ex.y;
00097         K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
00098 
00099         m_linearMass = K.GetInverse();
00100 
00101         m_angularMass = iA + iB;
00102         if (m_angularMass > 0.0f)
00103         {
00104                 m_angularMass = 1.0f / m_angularMass;
00105         }
00106 
00107         if (data.step.warmStarting)
00108         {
00109                 // Scale impulses to support a variable time step.
00110                 m_linearImpulse *= data.step.dtRatio;
00111                 m_angularImpulse *= data.step.dtRatio;
00112 
00113                 b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
00114                 vA -= mA * P;
00115                 wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);
00116                 vB += mB * P;
00117                 wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);
00118         }
00119         else
00120         {
00121                 m_linearImpulse.SetZero();
00122                 m_angularImpulse = 0.0f;
00123         }
00124 
00125         data.velocities[m_indexA].v = vA;
00126         data.velocities[m_indexA].w = wA;
00127         data.velocities[m_indexB].v = vB;
00128         data.velocities[m_indexB].w = wB;
00129 }
00130 
00131 void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data)
00132 {
00133         b2Vec2 vA = data.velocities[m_indexA].v;
00134         float32 wA = data.velocities[m_indexA].w;
00135         b2Vec2 vB = data.velocities[m_indexB].v;
00136         float32 wB = data.velocities[m_indexB].w;
00137 
00138         float32 mA = m_invMassA, mB = m_invMassB;
00139         float32 iA = m_invIA, iB = m_invIB;
00140 
00141         float32 h = data.step.dt;
00142 
00143         // Solve angular friction
00144         {
00145                 float32 Cdot = wB - wA;
00146                 float32 impulse = -m_angularMass * Cdot;
00147 
00148                 float32 oldImpulse = m_angularImpulse;
00149                 float32 maxImpulse = h * m_maxTorque;
00150                 m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
00151                 impulse = m_angularImpulse - oldImpulse;
00152 
00153                 wA -= iA * impulse;
00154                 wB += iB * impulse;
00155         }
00156 
00157         // Solve linear friction
00158         {
00159                 b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
00160 
00161                 b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
00162                 b2Vec2 oldImpulse = m_linearImpulse;
00163                 m_linearImpulse += impulse;
00164 
00165                 float32 maxImpulse = h * m_maxForce;
00166 
00167                 if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
00168                 {
00169                         m_linearImpulse.Normalize();
00170                         m_linearImpulse *= maxImpulse;
00171                 }
00172 
00173                 impulse = m_linearImpulse - oldImpulse;
00174 
00175                 vA -= mA * impulse;
00176                 wA -= iA * b2Cross(m_rA, impulse);
00177 
00178                 vB += mB * impulse;
00179                 wB += iB * b2Cross(m_rB, impulse);
00180         }
00181 
00182         data.velocities[m_indexA].v = vA;
00183         data.velocities[m_indexA].w = wA;
00184         data.velocities[m_indexB].v = vB;
00185         data.velocities[m_indexB].w = wB;
00186 }
00187 
00188 bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data)
00189 {
00190         B2_NOT_USED(data);
00191 
00192         return true;
00193 }
00194 
00195 b2Vec2 b2FrictionJoint::GetAnchorA() const
00196 {
00197         return m_bodyA->GetWorldPoint(m_localAnchorA);
00198 }
00199 
00200 b2Vec2 b2FrictionJoint::GetAnchorB() const
00201 {
00202         return m_bodyB->GetWorldPoint(m_localAnchorB);
00203 }
00204 
00205 b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const
00206 {
00207         return inv_dt * m_linearImpulse;
00208 }
00209 
00210 float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const
00211 {
00212         return inv_dt * m_angularImpulse;
00213 }
00214 
00215 void b2FrictionJoint::SetMaxForce(float32 force)
00216 {
00217         b2Assert(b2IsValid(force) && force >= 0.0f);
00218         m_maxForce = force;
00219 }
00220 
00221 float32 b2FrictionJoint::GetMaxForce() const
00222 {
00223         return m_maxForce;
00224 }
00225 
00226 void b2FrictionJoint::SetMaxTorque(float32 torque)
00227 {
00228         b2Assert(b2IsValid(torque) && torque >= 0.0f);
00229         m_maxTorque = torque;
00230 }
00231 
00232 float32 b2FrictionJoint::GetMaxTorque() const
00233 {
00234         return m_maxTorque;
00235 }
00236 
00237 void b2FrictionJoint::Dump()
00238 {
00239         int32 indexA = m_bodyA->m_islandIndex;
00240         int32 indexB = m_bodyB->m_islandIndex;
00241 
00242         b2Log("  b2FrictionJointDef jd;\n");
00243         b2Log("  jd.bodyA = bodies[%d];\n", indexA);
00244         b2Log("  jd.bodyB = bodies[%d];\n", indexB);
00245         b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
00246         b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
00247         b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
00248         b2Log("  jd.maxForce = %.15lef;\n", m_maxForce);
00249         b2Log("  jd.maxTorque = %.15lef;\n", m_maxTorque);
00250         b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
00251 }


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