b2WeldJoint.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/b2WeldJoint.h>
00020 #include <Box2D/Dynamics/b2Body.h>
00021 #include <Box2D/Dynamics/b2TimeStep.h>
00022 
00023 // Point-to-point constraint
00024 // C = p2 - p1
00025 // Cdot = v2 - v1
00026 //      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
00027 // J = [-I -r1_skew I r2_skew ]
00028 // Identity used:
00029 // w k % (rx i + ry j) = w * (-ry i + rx j)
00030 
00031 // Angle constraint
00032 // C = angle2 - angle1 - referenceAngle
00033 // Cdot = w2 - w1
00034 // J = [0 0 -1 0 0 1]
00035 // K = invI1 + invI2
00036 
00037 void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
00038 {
00039         bodyA = bA;
00040         bodyB = bB;
00041         localAnchorA = bodyA->GetLocalPoint(anchor);
00042         localAnchorB = bodyB->GetLocalPoint(anchor);
00043         referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
00044 }
00045 
00046 b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def)
00047 : b2Joint(def)
00048 {
00049         m_localAnchorA = def->localAnchorA;
00050         m_localAnchorB = def->localAnchorB;
00051         m_referenceAngle = def->referenceAngle;
00052         m_frequencyHz = def->frequencyHz;
00053         m_dampingRatio = def->dampingRatio;
00054 
00055         m_impulse.SetZero();
00056 }
00057 
00058 void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)
00059 {
00060         m_indexA = m_bodyA->m_islandIndex;
00061         m_indexB = m_bodyB->m_islandIndex;
00062         m_localCenterA = m_bodyA->m_sweep.localCenter;
00063         m_localCenterB = m_bodyB->m_sweep.localCenter;
00064         m_invMassA = m_bodyA->m_invMass;
00065         m_invMassB = m_bodyB->m_invMass;
00066         m_invIA = m_bodyA->m_invI;
00067         m_invIB = m_bodyB->m_invI;
00068 
00069         float32 aA = data.positions[m_indexA].a;
00070         b2Vec2 vA = data.velocities[m_indexA].v;
00071         float32 wA = data.velocities[m_indexA].w;
00072 
00073         float32 aB = data.positions[m_indexB].a;
00074         b2Vec2 vB = data.velocities[m_indexB].v;
00075         float32 wB = data.velocities[m_indexB].w;
00076 
00077         b2Rot qA(aA), qB(aB);
00078 
00079         m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
00080         m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
00081 
00082         // J = [-I -r1_skew I r2_skew]
00083         //     [ 0       -1 0       1]
00084         // r_skew = [-ry; rx]
00085 
00086         // Matlab
00087         // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
00088         //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
00089         //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
00090 
00091         float32 mA = m_invMassA, mB = m_invMassB;
00092         float32 iA = m_invIA, iB = m_invIB;
00093 
00094         b2Mat33 K;
00095         K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
00096         K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
00097         K.ez.x = -m_rA.y * iA - m_rB.y * iB;
00098         K.ex.y = K.ey.x;
00099         K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
00100         K.ez.y = m_rA.x * iA + m_rB.x * iB;
00101         K.ex.z = K.ez.x;
00102         K.ey.z = K.ez.y;
00103         K.ez.z = iA + iB;
00104 
00105         if (m_frequencyHz > 0.0f)
00106         {
00107                 K.GetInverse22(&m_mass);
00108 
00109                 float32 invM = iA + iB;
00110                 float32 m = invM > 0.0f ? 1.0f / invM : 0.0f;
00111 
00112                 float32 C = aB - aA - m_referenceAngle;
00113 
00114                 // Frequency
00115                 float32 omega = 2.0f * b2_pi * m_frequencyHz;
00116 
00117                 // Damping coefficient
00118                 float32 d = 2.0f * m * m_dampingRatio * omega;
00119 
00120                 // Spring stiffness
00121                 float32 k = m * omega * omega;
00122 
00123                 // magic formulas
00124                 float32 h = data.step.dt;
00125                 m_gamma = h * (d + h * k);
00126                 m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
00127                 m_bias = C * h * k * m_gamma;
00128 
00129                 invM += m_gamma;
00130                 m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
00131         }
00132         else if (K.ez.z == 0.0f)
00133         {
00134                 K.GetInverse22(&m_mass);
00135                 m_gamma = 0.0f;
00136                 m_bias = 0.0f;
00137         }
00138         else
00139         {
00140                 K.GetSymInverse33(&m_mass);
00141                 m_gamma = 0.0f;
00142                 m_bias = 0.0f;
00143         }
00144 
00145         if (data.step.warmStarting)
00146         {
00147                 // Scale impulses to support a variable time step.
00148                 m_impulse *= data.step.dtRatio;
00149 
00150                 b2Vec2 P(m_impulse.x, m_impulse.y);
00151 
00152                 vA -= mA * P;
00153                 wA -= iA * (b2Cross(m_rA, P) + m_impulse.z);
00154 
00155                 vB += mB * P;
00156                 wB += iB * (b2Cross(m_rB, P) + m_impulse.z);
00157         }
00158         else
00159         {
00160                 m_impulse.SetZero();
00161         }
00162 
00163         data.velocities[m_indexA].v = vA;
00164         data.velocities[m_indexA].w = wA;
00165         data.velocities[m_indexB].v = vB;
00166         data.velocities[m_indexB].w = wB;
00167 }
00168 
00169 void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data)
00170 {
00171         b2Vec2 vA = data.velocities[m_indexA].v;
00172         float32 wA = data.velocities[m_indexA].w;
00173         b2Vec2 vB = data.velocities[m_indexB].v;
00174         float32 wB = data.velocities[m_indexB].w;
00175 
00176         float32 mA = m_invMassA, mB = m_invMassB;
00177         float32 iA = m_invIA, iB = m_invIB;
00178 
00179         if (m_frequencyHz > 0.0f)
00180         {
00181                 float32 Cdot2 = wB - wA;
00182 
00183                 float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);
00184                 m_impulse.z += impulse2;
00185 
00186                 wA -= iA * impulse2;
00187                 wB += iB * impulse2;
00188 
00189                 b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
00190 
00191                 b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1);
00192                 m_impulse.x += impulse1.x;
00193                 m_impulse.y += impulse1.y;
00194 
00195                 b2Vec2 P = impulse1;
00196 
00197                 vA -= mA * P;
00198                 wA -= iA * b2Cross(m_rA, P);
00199 
00200                 vB += mB * P;
00201                 wB += iB * b2Cross(m_rB, P);
00202         }
00203         else
00204         {
00205                 b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
00206                 float32 Cdot2 = wB - wA;
00207                 b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
00208 
00209                 b2Vec3 impulse = -b2Mul(m_mass, Cdot);
00210                 m_impulse += impulse;
00211 
00212                 b2Vec2 P(impulse.x, impulse.y);
00213 
00214                 vA -= mA * P;
00215                 wA -= iA * (b2Cross(m_rA, P) + impulse.z);
00216 
00217                 vB += mB * P;
00218                 wB += iB * (b2Cross(m_rB, P) + impulse.z);
00219         }
00220 
00221         data.velocities[m_indexA].v = vA;
00222         data.velocities[m_indexA].w = wA;
00223         data.velocities[m_indexB].v = vB;
00224         data.velocities[m_indexB].w = wB;
00225 }
00226 
00227 bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)
00228 {
00229         b2Vec2 cA = data.positions[m_indexA].c;
00230         float32 aA = data.positions[m_indexA].a;
00231         b2Vec2 cB = data.positions[m_indexB].c;
00232         float32 aB = data.positions[m_indexB].a;
00233 
00234         b2Rot qA(aA), qB(aB);
00235 
00236         float32 mA = m_invMassA, mB = m_invMassB;
00237         float32 iA = m_invIA, iB = m_invIB;
00238 
00239         b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
00240         b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
00241 
00242         float32 positionError, angularError;
00243 
00244         b2Mat33 K;
00245         K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
00246         K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
00247         K.ez.x = -rA.y * iA - rB.y * iB;
00248         K.ex.y = K.ey.x;
00249         K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
00250         K.ez.y = rA.x * iA + rB.x * iB;
00251         K.ex.z = K.ez.x;
00252         K.ey.z = K.ez.y;
00253         K.ez.z = iA + iB;
00254 
00255         if (m_frequencyHz > 0.0f)
00256         {
00257                 b2Vec2 C1 =  cB + rB - cA - rA;
00258 
00259                 positionError = C1.Length();
00260                 angularError = 0.0f;
00261 
00262                 b2Vec2 P = -K.Solve22(C1);
00263 
00264                 cA -= mA * P;
00265                 aA -= iA * b2Cross(rA, P);
00266 
00267                 cB += mB * P;
00268                 aB += iB * b2Cross(rB, P);
00269         }
00270         else
00271         {
00272                 b2Vec2 C1 =  cB + rB - cA - rA;
00273                 float32 C2 = aB - aA - m_referenceAngle;
00274 
00275                 positionError = C1.Length();
00276                 angularError = b2Abs(C2);
00277 
00278                 b2Vec3 C(C1.x, C1.y, C2);
00279         
00280                 b2Vec3 impulse;
00281                 if (K.ez.z > 0.0f)
00282                 {
00283                         impulse = -K.Solve33(C);
00284                 }
00285                 else
00286                 {
00287                         b2Vec2 impulse2 = -K.Solve22(C1);
00288                         impulse.Set(impulse2.x, impulse2.y, 0.0f);
00289                 }
00290 
00291                 b2Vec2 P(impulse.x, impulse.y);
00292 
00293                 cA -= mA * P;
00294                 aA -= iA * (b2Cross(rA, P) + impulse.z);
00295 
00296                 cB += mB * P;
00297                 aB += iB * (b2Cross(rB, P) + impulse.z);
00298         }
00299 
00300         data.positions[m_indexA].c = cA;
00301         data.positions[m_indexA].a = aA;
00302         data.positions[m_indexB].c = cB;
00303         data.positions[m_indexB].a = aB;
00304 
00305         return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
00306 }
00307 
00308 b2Vec2 b2WeldJoint::GetAnchorA() const
00309 {
00310         return m_bodyA->GetWorldPoint(m_localAnchorA);
00311 }
00312 
00313 b2Vec2 b2WeldJoint::GetAnchorB() const
00314 {
00315         return m_bodyB->GetWorldPoint(m_localAnchorB);
00316 }
00317 
00318 b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const
00319 {
00320         b2Vec2 P(m_impulse.x, m_impulse.y);
00321         return inv_dt * P;
00322 }
00323 
00324 float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const
00325 {
00326         return inv_dt * m_impulse.z;
00327 }
00328 
00329 void b2WeldJoint::Dump()
00330 {
00331         int32 indexA = m_bodyA->m_islandIndex;
00332         int32 indexB = m_bodyB->m_islandIndex;
00333 
00334         b2Log("  b2WeldJointDef jd;\n");
00335         b2Log("  jd.bodyA = bodies[%d];\n", indexA);
00336         b2Log("  jd.bodyB = bodies[%d];\n", indexB);
00337         b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
00338         b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
00339         b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
00340         b2Log("  jd.referenceAngle = %.15lef;\n", m_referenceAngle);
00341         b2Log("  jd.frequencyHz = %.15lef;\n", m_frequencyHz);
00342         b2Log("  jd.dampingRatio = %.15lef;\n", m_dampingRatio);
00343         b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
00344 }


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