public override void initTest(bool argDeserialized)
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = getWorld().createBody(bd);
EdgeShape shape = new EdgeShape();
shape.set(new Vec2(50.0f, 0.0f), new Vec2(-50.0f, 0.0f));
ground.createFixture(shape, 0.0f);
}
{
CircleShape circle1 = new CircleShape();
circle1.m_radius = 1.0f;
PolygonShape box = new PolygonShape();
box.setAsBox(0.5f, 5.0f);
CircleShape circle2 = new CircleShape();
circle2.m_radius = 2.0f;
BodyDef bd1 = new BodyDef();
bd1.type = BodyType.STATIC;
bd1.position.set(10.0f, 9.0f);
Body body1 = m_world.createBody(bd1);
body1.createFixture(circle1, 5.0f);
BodyDef bd2 = new BodyDef();
bd2.type = BodyType.DYNAMIC;
bd2.position.set(10.0f, 8.0f);
Body body2 = m_world.createBody(bd2);
body2.createFixture(box, 5.0f);
BodyDef bd3 = new BodyDef();
bd3.type = BodyType.DYNAMIC;
bd3.position.set(10.0f, 6.0f);
Body body3 = m_world.createBody(bd3);
body3.createFixture(circle2, 5.0f);
RevoluteJointDef jd1 = new RevoluteJointDef();
jd1.initialize(body2, body1, bd1.position);
Joint joint1 = m_world.createJoint(jd1);
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.initialize(body2, body3, bd3.position);
Joint joint2 = m_world.createJoint(jd2);
GearJointDef jd4 = new GearJointDef();
jd4.bodyA = body1;
jd4.bodyB = body3;
jd4.joint1 = joint1;
jd4.joint2 = joint2;
jd4.ratio = circle2.m_radius/circle1.m_radius;
m_world.createJoint(jd4);
}
{
CircleShape circle1 = new CircleShape();
circle1.m_radius = 1.0f;
CircleShape circle2 = new CircleShape();
circle2.m_radius = 2.0f;
PolygonShape box = new PolygonShape();
box.setAsBox(0.5f, 5.0f);
BodyDef bd1 = new BodyDef();
bd1.type = BodyType.DYNAMIC;
bd1.position.set(-3.0f, 12.0f);
Body body1 = m_world.createBody(bd1);
body1.createFixture(circle1, 5.0f);
RevoluteJointDef jd1 = new RevoluteJointDef();
jd1.bodyA = ground;
jd1.bodyB = body1;
ground.getLocalPointToOut(bd1.position, ref jd1.localAnchorA);
body1.getLocalPointToOut(bd1.position, ref jd1.localAnchorB);
jd1.referenceAngle = body1.getAngle() - ground.getAngle();
m_joint1 = (RevoluteJoint) m_world.createJoint(jd1);
BodyDef bd2 = new BodyDef();
bd2.type = BodyType.DYNAMIC;
bd2.position.set(0.0f, 12.0f);
Body body2 = m_world.createBody(bd2);
body2.createFixture(circle2, 5.0f);
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.initialize(ground, body2, bd2.position);
m_joint2 = (RevoluteJoint) m_world.createJoint(jd2);
BodyDef bd3 = new BodyDef();
bd3.type = BodyType.DYNAMIC;
bd3.position.set(2.5f, 12.0f);
Body body3 = m_world.createBody(bd3);
body3.createFixture(box, 5.0f);
PrismaticJointDef jd3 = new PrismaticJointDef();
jd3.initialize(ground, body3, bd3.position, new Vec2(0.0f, 1.0f));
jd3.lowerTranslation = -5.0f;
jd3.upperTranslation = 5.0f;
jd3.enableLimit = true;
m_joint3 = (PrismaticJoint) m_world.createJoint(jd3);
GearJointDef jd4 = new GearJointDef();
jd4.bodyA = body1;
jd4.bodyB = body2;
jd4.joint1 = m_joint1;
jd4.joint2 = m_joint2;
jd4.ratio = circle2.m_radius/circle1.m_radius;
m_joint4 = (GearJoint) m_world.createJoint(jd4);
GearJointDef jd5 = new GearJointDef();
jd5.bodyA = body2;
jd5.bodyB = body3;
jd5.joint1 = m_joint2;
jd5.joint2 = m_joint3;
jd5.ratio = 1f/circle2.m_radius;
m_joint5 = (GearJoint) m_world.createJoint(jd5);
}
}
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;
}