File indexing completed on 2025-08-03 03:49:58

 /*
 * Copyright (c) 2007 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
 #include <Box2D/Dynamics/Joints/b2GearJoint.h>
 #include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
 #include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
 #include <Box2D/Dynamics/b2Body.h>
 #include <Box2D/Dynamics/b2TimeStep.h>
 
 // Gear Joint:
 // C0 = (coordinate1 + ratio * coordinate2)_initial
 // C = C0 - (cordinate1 + ratio * coordinate2) = 0
 // Cdot = -(Cdot1 + ratio * Cdot2)
 // J = -[J1 ratio * J2]
 // K = J * invM * JT
 //   = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
 //
 // Revolute:
 // coordinate = rotation
 // Cdot = angularVelocity
 // J = [0 0 1]
 // K = J * invM * JT = invI
 //
 // Prismatic:
 // coordinate = dot(p - pg, ug)
 // Cdot = dot(v + cross(w, r), ug)
 // J = [ug cross(r, ug)]
 // K = J * invM * JT = invMass + invI * cross(r, ug)^2
 
 b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 : b2Joint(def)
 {
     b2JointType type1 = def->joint1->GetType();
     b2JointType type2 = def->joint2->GetType();
 
     b2Assert(type1 == e_revoluteJoint || type1 == e_prismaticJoint);
     b2Assert(type2 == e_revoluteJoint || type2 == e_prismaticJoint);
     b2Assert(def->joint1->GetBodyA()->GetType() == b2_staticBody);
     b2Assert(def->joint2->GetBodyA()->GetType() == b2_staticBody);
 
     m_revolute1 = NULL;
     m_prismatic1 = NULL;
     m_revolute2 = NULL;
     m_prismatic2 = NULL;
 
     qreal coordinate1, coordinate2;
 
     m_ground1 = def->joint1->GetBodyA();
     m_bodyA = def->joint1->GetBodyB();
     if (type1 == e_revoluteJoint)
     {
         m_revolute1 = (b2RevoluteJoint*)def->joint1;
         m_groundAnchor1 = m_revolute1->m_localAnchor1;
         m_localAnchor1 = m_revolute1->m_localAnchor2;
         coordinate1 = m_revolute1->GetJointAngle();
     }
     else
     {
         m_prismatic1 = (b2PrismaticJoint*)def->joint1;
         m_groundAnchor1 = m_prismatic1->m_localAnchor1;
         m_localAnchor1 = m_prismatic1->m_localAnchor2;
         coordinate1 = m_prismatic1->GetJointTranslation();
     }
 
     m_ground2 = def->joint2->GetBodyA();
     m_bodyB = def->joint2->GetBodyB();
     if (type2 == e_revoluteJoint)
     {
         m_revolute2 = (b2RevoluteJoint*)def->joint2;
         m_groundAnchor2 = m_revolute2->m_localAnchor1;
         m_localAnchor2 = m_revolute2->m_localAnchor2;
         coordinate2 = m_revolute2->GetJointAngle();
     }
     else
     {
         m_prismatic2 = (b2PrismaticJoint*)def->joint2;
         m_groundAnchor2 = m_prismatic2->m_localAnchor1;
         m_localAnchor2 = m_prismatic2->m_localAnchor2;
         coordinate2 = m_prismatic2->GetJointTranslation();
     }
 
     m_ratio = def->ratio;
 
     m_constant = coordinate1 + m_ratio * coordinate2;
 
     m_impulse = 0.0f;
 }
 
 void b2GearJoint::InitVelocityConstraints(const b2TimeStep& step)
 {
     b2Body* g1 = m_ground1;
     b2Body* g2 = m_ground2;
     b2Body* b1 = m_bodyA;
     b2Body* b2 = m_bodyB;
 
     qreal K = 0.0f;
     m_J.SetZero();
 
     if (m_revolute1)
     {
         m_J.angularA = -1.0f;
         K += b1->m_invI;
     }
     else
     {
         b2Vec2 ug = b2Mul(g1->GetTransform().R, m_prismatic1->m_localXAxis1);
         b2Vec2 r = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
         qreal crug = b2Cross(r, ug);
         m_J.linearA = -ug;
         m_J.angularA = -crug;
         K += b1->m_invMass + b1->m_invI * crug * crug;
     }
 
     if (m_revolute2)
     {
         m_J.angularB = -m_ratio;
         K += m_ratio * m_ratio * b2->m_invI;
     }
     else
     {
         b2Vec2 ug = b2Mul(g2->GetTransform().R, m_prismatic2->m_localXAxis1);
         b2Vec2 r = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
         qreal crug = b2Cross(r, ug);
         m_J.linearB = -m_ratio * ug;
         m_J.angularB = -m_ratio * crug;
         K += m_ratio * m_ratio * (b2->m_invMass + b2->m_invI * crug * crug);
     }
 
     // Compute effective mass.
     m_mass = K > 0.0f ? 1.0f / K : 0.0f;
 
     if (step.warmStarting)
     {
         // Warm starting.
         b1->m_linearVelocity += b1->m_invMass * m_impulse * m_J.linearA;
         b1->m_angularVelocity += b1->m_invI * m_impulse * m_J.angularA;
         b2->m_linearVelocity += b2->m_invMass * m_impulse * m_J.linearB;
         b2->m_angularVelocity += b2->m_invI * m_impulse * m_J.angularB;
     }
     else
     {
         m_impulse = 0.0f;
     }
 }
 
 void b2GearJoint::SolveVelocityConstraints(const b2TimeStep& step)
 {
     B2_NOT_USED(step);
 
     b2Body* b1 = m_bodyA;
     b2Body* b2 = m_bodyB;
 
     qreal Cdot = m_J.Compute(   b1->m_linearVelocity, b1->m_angularVelocity,
                                 b2->m_linearVelocity, b2->m_angularVelocity);
 
     qreal impulse = m_mass * (-Cdot);
     m_impulse += impulse;
 
     b1->m_linearVelocity += b1->m_invMass * impulse * m_J.linearA;
     b1->m_angularVelocity += b1->m_invI * impulse * m_J.angularA;
     b2->m_linearVelocity += b2->m_invMass * impulse * m_J.linearB;
     b2->m_angularVelocity += b2->m_invI * impulse * m_J.angularB;
 }
 
 bool b2GearJoint::SolvePositionConstraints(qreal baumgarte)
 {
     B2_NOT_USED(baumgarte);
     
     qreal linearError = 0.0f;
 
     b2Body* b1 = m_bodyA;
     b2Body* b2 = m_bodyB;
 
     qreal coordinate1, coordinate2;
     if (m_revolute1)
     {
         coordinate1 = m_revolute1->GetJointAngle();
     }
     else
     {
         coordinate1 = m_prismatic1->GetJointTranslation();
     }
 
     if (m_revolute2)
     {
         coordinate2 = m_revolute2->GetJointAngle();
     }
     else
     {
         coordinate2 = m_prismatic2->GetJointTranslation();
     }
 
     qreal C = m_constant - (coordinate1 + m_ratio * coordinate2);
 
     qreal impulse = m_mass * (-C);
 
     b1->m_sweep.c += b1->m_invMass * impulse * m_J.linearA;
     b1->m_sweep.a += b1->m_invI * impulse * m_J.angularA;
     b2->m_sweep.c += b2->m_invMass * impulse * m_J.linearB;
     b2->m_sweep.a += b2->m_invI * impulse * m_J.angularB;
 
     b1->SynchronizeTransform();
     b2->SynchronizeTransform();
 
     // TODO_ERIN not implemented
     return linearError < b2_linearSlop;
 }
 
 b2Vec2 b2GearJoint::GetAnchorA() const
 {
     return m_bodyA->GetWorldPoint(m_localAnchor1);
 }
 
 b2Vec2 b2GearJoint::GetAnchorB() const
 {
     return m_bodyB->GetWorldPoint(m_localAnchor2);
 }
 
 b2Vec2 b2GearJoint::GetReactionForce(qreal inv_dt) const
 {
     // TODO_ERIN not tested
     b2Vec2 P = m_impulse * m_J.linearB;
     return inv_dt * P;
 }
 
 qreal b2GearJoint::GetReactionTorque(qreal inv_dt) const
 {
     // TODO_ERIN not tested
     b2Vec2 r = b2Mul(m_bodyB->GetTransform().R, m_localAnchor2 - m_bodyB->GetLocalCenter());
     b2Vec2 P = m_impulse * m_J.linearB;
     qreal L = m_impulse * m_J.angularB - b2Cross(r, P);
     return inv_dt * L;
 }
 
 void b2GearJoint::SetRatio(qreal ratio)
 {
     b2Assert(b2IsValid(ratio));
     m_ratio = ratio;
 }
 
 qreal b2GearJoint::GetRatio() const
 {
     return m_ratio;
 }