b2GearJoint.cpp
上传用户:gb3593
上传日期:2022-01-07
资源大小:3028k
文件大小:7k
- /*
- * 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;
- float32 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;
- float32 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());
- float32 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());
- float32 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;
- float32 Cdot = m_J.Compute( b1->m_linearVelocity, b1->m_angularVelocity,
- b2->m_linearVelocity, b2->m_angularVelocity);
- float32 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(float32 baumgarte)
- {
- B2_NOT_USED(baumgarte);
-
- float32 linearError = 0.0f;
- b2Body* b1 = m_bodyA;
- b2Body* b2 = m_bodyB;
- float32 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();
- }
- float32 C = m_constant - (coordinate1 + m_ratio * coordinate2);
- float32 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(float32 inv_dt) const
- {
- // TODO_ERIN not tested
- b2Vec2 P = m_impulse * m_J.linearB;
- return inv_dt * P;
- }
- float32 b2GearJoint::GetReactionTorque(float32 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;
- float32 L = m_impulse * m_J.angularB - b2Cross(r, P);
- return inv_dt * L;
- }
- void b2GearJoint::SetRatio(float32 ratio)
- {
- b2Assert(b2IsValid(ratio));
- m_ratio = ratio;
- }
- float32 b2GearJoint::GetRatio() const
- {
- return m_ratio;
- }