mirror of https://github.com/axmolengine/axmol.git
260 lines
7.1 KiB
C++
260 lines
7.1 KiB
C++
/*
|
|
* 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;
|
|
}
|