internal GearJoint(IWorldPool argWorldPool, GearJointDef def)
: base(argWorldPool, def)
{
m_joint1 = def.joint1;
m_joint2 = def.joint2;
m_typeA = m_joint1.getType();
m_typeB = m_joint2.getType();
Debug.Assert(m_typeA == JointType.REVOLUTE || m_typeA == JointType.PRISMATIC);
Debug.Assert(m_typeB == JointType.REVOLUTE || m_typeB == JointType.PRISMATIC);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in Joint.
m_bodyC = m_joint1.getBodyA();
m_bodyA = m_joint1.getBodyB();
// Get geometry of joint1
Transform xfA = m_bodyA.m_xf;
float aA = m_bodyA.m_sweep.a;
Transform xfC = m_bodyC.m_xf;
float aC = m_bodyC.m_sweep.a;
if (m_typeA == JointType.REVOLUTE)
{
RevoluteJoint revolute = (RevoluteJoint) def.joint1;
m_localAnchorC.set(revolute.m_localAnchorA);
m_localAnchorA.set(revolute.m_localAnchorB);
m_referenceAngleA = revolute.m_referenceAngle;
m_localAxisC.setZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
Vec2 pA = pool.popVec2();
Vec2 temp = pool.popVec2();
PrismaticJoint prismatic = (PrismaticJoint) def.joint1;
m_localAnchorC.set(prismatic.m_localAnchorA);
m_localAnchorA.set(prismatic.m_localAnchorB);
m_referenceAngleA = prismatic.m_referenceAngle;
m_localAxisC.set(prismatic.m_localXAxisA);
Vec2 pC = m_localAnchorC;
Rot.mulToOutUnsafe(xfA.q, m_localAnchorA, ref temp);
temp.addLocal(xfA.p);
temp.subLocal(xfC.p);
Rot.mulTransUnsafe(xfC.q, temp, ref pA);
pA.subLocal(pC);
coordinateA = Vec2.dot(pA, m_localAxisC);
pool.pushVec2(2);
}
m_bodyD = m_joint2.getBodyA();
m_bodyB = m_joint2.getBodyB();
// Get geometry of joint2
Transform xfB = m_bodyB.m_xf;
float aB = m_bodyB.m_sweep.a;
Transform xfD = m_bodyD.m_xf;
float aD = m_bodyD.m_sweep.a;
if (m_typeB == JointType.REVOLUTE)
{
RevoluteJoint revolute = (RevoluteJoint) def.joint2;
m_localAnchorD.set(revolute.m_localAnchorA);
m_localAnchorB.set(revolute.m_localAnchorB);
m_referenceAngleB = revolute.m_referenceAngle;
m_localAxisD.setZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
Vec2 pB = pool.popVec2();
Vec2 temp = pool.popVec2();
PrismaticJoint prismatic = (PrismaticJoint) def.joint2;
m_localAnchorD.set(prismatic.m_localAnchorA);
m_localAnchorB.set(prismatic.m_localAnchorB);
m_referenceAngleB = prismatic.m_referenceAngle;
m_localAxisD.set(prismatic.m_localXAxisA);
Vec2 pD = m_localAnchorD;
Rot.mulToOutUnsafe(xfB.q, m_localAnchorB, ref temp);
temp.addLocal(xfB.p);
temp.subLocal(xfD.p);
Rot.mulTransUnsafe(xfD.q, temp, ref pB);
pB.subLocal(pD);
coordinateB = Vec2.dot(pB, m_localAxisD);
pool.pushVec2(2);
}
m_ratio = def.ratio;
m_constant = coordinateA + m_ratio*coordinateB;
m_impulse = 0.0f;
}
private void drawJoint(Joint joint)
{
Body bodyA = joint.getBodyA();
Body bodyB = joint.getBodyB();
Transform xf1 = bodyA.getTransform();
Transform xf2 = bodyB.getTransform();
Vec2 x1 = xf1.p;
Vec2 x2 = xf2.p;
Vec2 p1 = pool.popVec2();
Vec2 p2 = pool.popVec2();
joint.getAnchorA(ref p1);
joint.getAnchorB(ref p2);
color.set(0.5f, 0.8f, 0.8f);
switch (joint.getType())
{
// TODO djm write after writing joints
case JointType.DISTANCE:
m_debugDraw.drawSegment(p1, p2, color);
break;
case JointType.PULLEY:
{
PulleyJoint pulley = (PulleyJoint) joint;
Vec2 s1 = pulley.getGroundAnchorA();
Vec2 s2 = pulley.getGroundAnchorB();
m_debugDraw.drawSegment(s1, p1, color);
m_debugDraw.drawSegment(s2, p2, color);
m_debugDraw.drawSegment(s1, s2, color);
}
break;
case JointType.CONSTANT_VOLUME:
case JointType.MOUSE:
// don't draw this
break;
default:
m_debugDraw.drawSegment(x1, p1, color);
m_debugDraw.drawSegment(p1, p2, color);
m_debugDraw.drawSegment(x2, p2, color);
break;
}
pool.pushVec2(2);
}