b2RevoluteJoint.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/b2RevoluteJoint.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 // Motor constraint
00032 // Cdot = w2 - w1
00033 // J = [0 0 -1 0 0 1]
00034 // K = invI1 + invI2
00035 
00036 void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
00037 {
00038         bodyA = bA;
00039         bodyB = bB;
00040         localAnchorA = bodyA->GetLocalPoint(anchor);
00041         localAnchorB = bodyB->GetLocalPoint(anchor);
00042         referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
00043 }
00044 
00045 b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
00046 : b2Joint(def)
00047 {
00048         m_localAnchorA = def->localAnchorA;
00049         m_localAnchorB = def->localAnchorB;
00050         m_referenceAngle = def->referenceAngle;
00051 
00052         m_impulse.SetZero();
00053         m_motorImpulse = 0.0f;
00054 
00055         m_lowerAngle = def->lowerAngle;
00056         m_upperAngle = def->upperAngle;
00057         m_maxMotorTorque = def->maxMotorTorque;
00058         m_motorSpeed = def->motorSpeed;
00059         m_enableLimit = def->enableLimit;
00060         m_enableMotor = def->enableMotor;
00061         m_limitState = e_inactiveLimit;
00062 }
00063 
00064 void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)
00065 {
00066         m_indexA = m_bodyA->m_islandIndex;
00067         m_indexB = m_bodyB->m_islandIndex;
00068         m_localCenterA = m_bodyA->m_sweep.localCenter;
00069         m_localCenterB = m_bodyB->m_sweep.localCenter;
00070         m_invMassA = m_bodyA->m_invMass;
00071         m_invMassB = m_bodyB->m_invMass;
00072         m_invIA = m_bodyA->m_invI;
00073         m_invIB = m_bodyB->m_invI;
00074 
00075         float32 aA = data.positions[m_indexA].a;
00076         b2Vec2 vA = data.velocities[m_indexA].v;
00077         float32 wA = data.velocities[m_indexA].w;
00078 
00079         float32 aB = data.positions[m_indexB].a;
00080         b2Vec2 vB = data.velocities[m_indexB].v;
00081         float32 wB = data.velocities[m_indexB].w;
00082 
00083         b2Rot qA(aA), qB(aB);
00084 
00085         m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
00086         m_rB = b2Mul(qB, m_localAnchorB - 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         bool fixedRotation = (iA + iB == 0.0f);
00101 
00102         m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
00103         m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
00104         m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
00105         m_mass.ex.y = m_mass.ey.x;
00106         m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
00107         m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
00108         m_mass.ex.z = m_mass.ez.x;
00109         m_mass.ey.z = m_mass.ez.y;
00110         m_mass.ez.z = iA + iB;
00111 
00112         m_motorMass = iA + iB;
00113         if (m_motorMass > 0.0f)
00114         {
00115                 m_motorMass = 1.0f / m_motorMass;
00116         }
00117 
00118         if (m_enableMotor == false || fixedRotation)
00119         {
00120                 m_motorImpulse = 0.0f;
00121         }
00122 
00123         if (m_enableLimit && fixedRotation == false)
00124         {
00125                 float32 jointAngle = aB - aA - m_referenceAngle;
00126                 if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
00127                 {
00128                         m_limitState = e_equalLimits;
00129                 }
00130                 else if (jointAngle <= m_lowerAngle)
00131                 {
00132                         if (m_limitState != e_atLowerLimit)
00133                         {
00134                                 m_impulse.z = 0.0f;
00135                         }
00136                         m_limitState = e_atLowerLimit;
00137                 }
00138                 else if (jointAngle >= m_upperAngle)
00139                 {
00140                         if (m_limitState != e_atUpperLimit)
00141                         {
00142                                 m_impulse.z = 0.0f;
00143                         }
00144                         m_limitState = e_atUpperLimit;
00145                 }
00146                 else
00147                 {
00148                         m_limitState = e_inactiveLimit;
00149                         m_impulse.z = 0.0f;
00150                 }
00151         }
00152         else
00153         {
00154                 m_limitState = e_inactiveLimit;
00155         }
00156 
00157         if (data.step.warmStarting)
00158         {
00159                 // Scale impulses to support a variable time step.
00160                 m_impulse *= data.step.dtRatio;
00161                 m_motorImpulse *= data.step.dtRatio;
00162 
00163                 b2Vec2 P(m_impulse.x, m_impulse.y);
00164 
00165                 vA -= mA * P;
00166                 wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);
00167 
00168                 vB += mB * P;
00169                 wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
00170         }
00171         else
00172         {
00173                 m_impulse.SetZero();
00174                 m_motorImpulse = 0.0f;
00175         }
00176 
00177         data.velocities[m_indexA].v = vA;
00178         data.velocities[m_indexA].w = wA;
00179         data.velocities[m_indexB].v = vB;
00180         data.velocities[m_indexB].w = wB;
00181 }
00182 
00183 void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data)
00184 {
00185         b2Vec2 vA = data.velocities[m_indexA].v;
00186         float32 wA = data.velocities[m_indexA].w;
00187         b2Vec2 vB = data.velocities[m_indexB].v;
00188         float32 wB = data.velocities[m_indexB].w;
00189 
00190         float32 mA = m_invMassA, mB = m_invMassB;
00191         float32 iA = m_invIA, iB = m_invIB;
00192 
00193         bool fixedRotation = (iA + iB == 0.0f);
00194 
00195         // Solve motor constraint.
00196         if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false)
00197         {
00198                 float32 Cdot = wB - wA - m_motorSpeed;
00199                 float32 impulse = -m_motorMass * Cdot;
00200                 float32 oldImpulse = m_motorImpulse;
00201                 float32 maxImpulse = data.step.dt * m_maxMotorTorque;
00202                 m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
00203                 impulse = m_motorImpulse - oldImpulse;
00204 
00205                 wA -= iA * impulse;
00206                 wB += iB * impulse;
00207         }
00208 
00209         // Solve limit constraint.
00210         if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
00211         {
00212                 b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
00213                 float32 Cdot2 = wB - wA;
00214                 b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
00215 
00216                 b2Vec3 impulse = -m_mass.Solve33(Cdot);
00217 
00218                 if (m_limitState == e_equalLimits)
00219                 {
00220                         m_impulse += impulse;
00221                 }
00222                 else if (m_limitState == e_atLowerLimit)
00223                 {
00224                         float32 newImpulse = m_impulse.z + impulse.z;
00225                         if (newImpulse < 0.0f)
00226                         {
00227                                 b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
00228                                 b2Vec2 reduced = m_mass.Solve22(rhs);
00229                                 impulse.x = reduced.x;
00230                                 impulse.y = reduced.y;
00231                                 impulse.z = -m_impulse.z;
00232                                 m_impulse.x += reduced.x;
00233                                 m_impulse.y += reduced.y;
00234                                 m_impulse.z = 0.0f;
00235                         }
00236                         else
00237                         {
00238                                 m_impulse += impulse;
00239                         }
00240                 }
00241                 else if (m_limitState == e_atUpperLimit)
00242                 {
00243                         float32 newImpulse = m_impulse.z + impulse.z;
00244                         if (newImpulse > 0.0f)
00245                         {
00246                                 b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
00247                                 b2Vec2 reduced = m_mass.Solve22(rhs);
00248                                 impulse.x = reduced.x;
00249                                 impulse.y = reduced.y;
00250                                 impulse.z = -m_impulse.z;
00251                                 m_impulse.x += reduced.x;
00252                                 m_impulse.y += reduced.y;
00253                                 m_impulse.z = 0.0f;
00254                         }
00255                         else
00256                         {
00257                                 m_impulse += impulse;
00258                         }
00259                 }
00260 
00261                 b2Vec2 P(impulse.x, impulse.y);
00262 
00263                 vA -= mA * P;
00264                 wA -= iA * (b2Cross(m_rA, P) + impulse.z);
00265 
00266                 vB += mB * P;
00267                 wB += iB * (b2Cross(m_rB, P) + impulse.z);
00268         }
00269         else
00270         {
00271                 // Solve point-to-point constraint
00272                 b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
00273                 b2Vec2 impulse = m_mass.Solve22(-Cdot);
00274 
00275                 m_impulse.x += impulse.x;
00276                 m_impulse.y += impulse.y;
00277 
00278                 vA -= mA * impulse;
00279                 wA -= iA * b2Cross(m_rA, impulse);
00280 
00281                 vB += mB * impulse;
00282                 wB += iB * b2Cross(m_rB, impulse);
00283         }
00284 
00285         data.velocities[m_indexA].v = vA;
00286         data.velocities[m_indexA].w = wA;
00287         data.velocities[m_indexB].v = vB;
00288         data.velocities[m_indexB].w = wB;
00289 }
00290 
00291 bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)
00292 {
00293         b2Vec2 cA = data.positions[m_indexA].c;
00294         float32 aA = data.positions[m_indexA].a;
00295         b2Vec2 cB = data.positions[m_indexB].c;
00296         float32 aB = data.positions[m_indexB].a;
00297 
00298         b2Rot qA(aA), qB(aB);
00299 
00300         float32 angularError = 0.0f;
00301         float32 positionError = 0.0f;
00302 
00303         bool fixedRotation = (m_invIA + m_invIB == 0.0f);
00304 
00305         // Solve angular limit constraint.
00306         if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
00307         {
00308                 float32 angle = aB - aA - m_referenceAngle;
00309                 float32 limitImpulse = 0.0f;
00310 
00311                 if (m_limitState == e_equalLimits)
00312                 {
00313                         // Prevent large angular corrections
00314                         float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
00315                         limitImpulse = -m_motorMass * C;
00316                         angularError = b2Abs(C);
00317                 }
00318                 else if (m_limitState == e_atLowerLimit)
00319                 {
00320                         float32 C = angle - m_lowerAngle;
00321                         angularError = -C;
00322 
00323                         // Prevent large angular corrections and allow some slop.
00324                         C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
00325                         limitImpulse = -m_motorMass * C;
00326                 }
00327                 else if (m_limitState == e_atUpperLimit)
00328                 {
00329                         float32 C = angle - m_upperAngle;
00330                         angularError = C;
00331 
00332                         // Prevent large angular corrections and allow some slop.
00333                         C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
00334                         limitImpulse = -m_motorMass * C;
00335                 }
00336 
00337                 aA -= m_invIA * limitImpulse;
00338                 aB += m_invIB * limitImpulse;
00339         }
00340 
00341         // Solve point-to-point constraint.
00342         {
00343                 qA.Set(aA);
00344                 qB.Set(aB);
00345                 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
00346                 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
00347 
00348                 b2Vec2 C = cB + rB - cA - rA;
00349                 positionError = C.Length();
00350 
00351                 float32 mA = m_invMassA, mB = m_invMassB;
00352                 float32 iA = m_invIA, iB = m_invIB;
00353 
00354                 b2Mat22 K;
00355                 K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
00356                 K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
00357                 K.ey.x = K.ex.y;
00358                 K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
00359 
00360                 b2Vec2 impulse = -K.Solve(C);
00361 
00362                 cA -= mA * impulse;
00363                 aA -= iA * b2Cross(rA, impulse);
00364 
00365                 cB += mB * impulse;
00366                 aB += iB * b2Cross(rB, impulse);
00367         }
00368 
00369         data.positions[m_indexA].c = cA;
00370         data.positions[m_indexA].a = aA;
00371         data.positions[m_indexB].c = cB;
00372         data.positions[m_indexB].a = aB;
00373         
00374         return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
00375 }
00376 
00377 b2Vec2 b2RevoluteJoint::GetAnchorA() const
00378 {
00379         return m_bodyA->GetWorldPoint(m_localAnchorA);
00380 }
00381 
00382 b2Vec2 b2RevoluteJoint::GetAnchorB() const
00383 {
00384         return m_bodyB->GetWorldPoint(m_localAnchorB);
00385 }
00386 
00387 b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const
00388 {
00389         b2Vec2 P(m_impulse.x, m_impulse.y);
00390         return inv_dt * P;
00391 }
00392 
00393 float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const
00394 {
00395         return inv_dt * m_impulse.z;
00396 }
00397 
00398 float32 b2RevoluteJoint::GetJointAngle() const
00399 {
00400         b2Body* bA = m_bodyA;
00401         b2Body* bB = m_bodyB;
00402         return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
00403 }
00404 
00405 float32 b2RevoluteJoint::GetJointSpeed() const
00406 {
00407         b2Body* bA = m_bodyA;
00408         b2Body* bB = m_bodyB;
00409         return bB->m_angularVelocity - bA->m_angularVelocity;
00410 }
00411 
00412 bool b2RevoluteJoint::IsMotorEnabled() const
00413 {
00414         return m_enableMotor;
00415 }
00416 
00417 void b2RevoluteJoint::EnableMotor(bool flag)
00418 {
00419         m_bodyA->SetAwake(true);
00420         m_bodyB->SetAwake(true);
00421         m_enableMotor = flag;
00422 }
00423 
00424 float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const
00425 {
00426         return inv_dt * m_motorImpulse;
00427 }
00428 
00429 void b2RevoluteJoint::SetMotorSpeed(float32 speed)
00430 {
00431         m_bodyA->SetAwake(true);
00432         m_bodyB->SetAwake(true);
00433         m_motorSpeed = speed;
00434 }
00435 
00436 void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)
00437 {
00438         m_bodyA->SetAwake(true);
00439         m_bodyB->SetAwake(true);
00440         m_maxMotorTorque = torque;
00441 }
00442 
00443 bool b2RevoluteJoint::IsLimitEnabled() const
00444 {
00445         return m_enableLimit;
00446 }
00447 
00448 void b2RevoluteJoint::EnableLimit(bool flag)
00449 {
00450         if (flag != m_enableLimit)
00451         {
00452                 m_bodyA->SetAwake(true);
00453                 m_bodyB->SetAwake(true);
00454                 m_enableLimit = flag;
00455                 m_impulse.z = 0.0f;
00456         }
00457 }
00458 
00459 float32 b2RevoluteJoint::GetLowerLimit() const
00460 {
00461         return m_lowerAngle;
00462 }
00463 
00464 float32 b2RevoluteJoint::GetUpperLimit() const
00465 {
00466         return m_upperAngle;
00467 }
00468 
00469 void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
00470 {
00471         b2Assert(lower <= upper);
00472         
00473         if (lower != m_lowerAngle || upper != m_upperAngle)
00474         {
00475                 m_bodyA->SetAwake(true);
00476                 m_bodyB->SetAwake(true);
00477                 m_impulse.z = 0.0f;
00478                 m_lowerAngle = lower;
00479                 m_upperAngle = upper;
00480         }
00481 }
00482 
00483 void b2RevoluteJoint::Dump()
00484 {
00485         int32 indexA = m_bodyA->m_islandIndex;
00486         int32 indexB = m_bodyB->m_islandIndex;
00487 
00488         b2Log("  b2RevoluteJointDef jd;\n");
00489         b2Log("  jd.bodyA = bodies[%d];\n", indexA);
00490         b2Log("  jd.bodyB = bodies[%d];\n", indexB);
00491         b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
00492         b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
00493         b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
00494         b2Log("  jd.referenceAngle = %.15lef;\n", m_referenceAngle);
00495         b2Log("  jd.enableLimit = bool(%d);\n", m_enableLimit);
00496         b2Log("  jd.lowerAngle = %.15lef;\n", m_lowerAngle);
00497         b2Log("  jd.upperAngle = %.15lef;\n", m_upperAngle);
00498         b2Log("  jd.enableMotor = bool(%d);\n", m_enableMotor);
00499         b2Log("  jd.motorSpeed = %.15lef;\n", m_motorSpeed);
00500         b2Log("  jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);
00501         b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
00502 }


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