internal GearJoint(GearJointDef def)
: base(def)
{
JointType type1 = def.joint1.JointType;
JointType type2 = def.joint2.JointType;
//Debug.Assert(type1 == JointType.Revolute || type1 == JointType.Prismatic);
//Debug.Assert(type2 == JointType.Revolute || type2 == JointType.Prismatic);
//Debug.Assert(def.joint1.GetBodyA().GetType() == BodyType.Static);
//Debug.Assert(def.joint2.GetBodyA().GetType() == BodyType.Static);
_revolute1 = null;
_prismatic1 = null;
_revolute2 = null;
_prismatic2 = null;
float coordinate1, coordinate2;
_ground1 = def.joint1.GetBodyA();
_bodyA = def.joint1.GetBodyB();
if (type1 == JointType.Revolute)
{
_revolute1 = (RevoluteJoint)def.joint1;
_groundAnchor1 = _revolute1._localAnchor1;
_localAnchor1 = _revolute1._localAnchor2;
coordinate1 = _revolute1.GetJointAngle();
}
else
{
_prismatic1 = (PrismaticJoint)def.joint1;
_groundAnchor1 = _prismatic1._localAnchor1;
_localAnchor1 = _prismatic1._localAnchor2;
coordinate1 = _prismatic1.GetJointTranslation();
}
_ground2 = def.joint2.GetBodyA();
_bodyB = def.joint2.GetBodyB();
if (type2 == JointType.Revolute)
{
_revolute2 = (RevoluteJoint)def.joint2;
_groundAnchor2 = _revolute2._localAnchor1;
_localAnchor2 = _revolute2._localAnchor2;
coordinate2 = _revolute2.GetJointAngle();
}
else
{
_prismatic2 = (PrismaticJoint)def.joint2;
_groundAnchor2 = _prismatic2._localAnchor1;
_localAnchor2 = _prismatic2._localAnchor2;
coordinate2 = _prismatic2.GetJointTranslation();
}
_ratio = def.ratio;
_ant = coordinate1 + _ratio * coordinate2;
_impulse = 0.0f;
}
public Gears()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(50.0f, 0.0f), new Vec2(-50.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
{
CircleShape circle1 = new CircleShape();
circle1.m_radius = 1.0f;
circle1.Density = 5;
PolygonShape box = new PolygonShape();
box.SetAsBox(0.5f, 5.0f);
box.Density = 5;
CircleShape circle2 = new CircleShape();
circle2.m_radius = 2.0f;
circle2.Density = 5;
BodyDef bd1 = new BodyDef();
bd1.type = BodyType._staticBody;
bd1.Position.Set(10.0f, 9.0f);
Body body1 = m_world.CreateBody(bd1);
body1.CreateFixture(circle1);
BodyDef bd2 = new BodyDef();
bd2.type = BodyType._dynamicBody;
bd2.Position.Set(10.0f, 8.0f);
Body body2 = m_world.CreateBody(bd2);
body2.CreateFixture(box);
BodyDef bd3 = new BodyDef();
bd3.type = BodyType._dynamicBody;
bd3.Position.Set(10.0f, 6.0f);
Body body3 = m_world.CreateBody(bd3);
body3.CreateFixture(circle2);
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;
circle1.Density = 5;
CircleShape circle2 = new CircleShape();
circle2.m_radius = 2.0f;
circle2.Density = 5;
PolygonShape box = new PolygonShape();
box.SetAsBox(0.5f, 5.0f);
BodyDef bd1 = new BodyDef();
bd1.type = BodyType._dynamicBody;
bd1.Position.Set(-3.0f, 12.0f);
Body body1 = m_world.CreateBody(bd1);
body1.CreateFixture(circle1);
RevoluteJointDef jd1 = new RevoluteJointDef();
jd1.bodyA = ground;
jd1.bodyB = body1;
jd1.localAnchorA = ground.GetLocalPoint(bd1.Position);
jd1.localAnchorB = body1.GetLocalPoint(bd1.Position);
jd1.referenceAngle = body1.GetAngle() - ground.GetAngle();
m_joint1 = (RevoluteJoint)m_world.CreateJoint(jd1);
BodyDef bd2 = new BodyDef();
bd2.type = BodyType._dynamicBody;
bd2.Position.Set(0.0f, 12.0f);
Body body2 = m_world.CreateBody(bd2);
body2.CreateFixture(circle2);
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.Position);
m_joint2 = (RevoluteJoint)m_world.CreateJoint(jd2);
BodyDef bd3 = new BodyDef();
bd3.type = BodyType._dynamicBody;
bd3.Position.Set(2.5f, 12.0f);
Body body3 = m_world.CreateBody(bd3);
box.Density = 5;
body3.CreateFixture(box);
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 = -1.0f / circle2.m_radius;
m_joint5 = (GearJoint)m_world.CreateJoint(jd5);
}
}
internal GearJoint(GearJointDef def): base(def)
{
m_joint1 = def.joint1;
m_joint2 = def.joint2;
m_typeA = m_joint1.GetJointType();
m_typeB = m_joint2.GetJointType();
Utilities.Assert(m_typeA == JointType.e_revoluteJoint || m_typeA == JointType.e_prismaticJoint);
Utilities.Assert(m_typeB == JointType.e_revoluteJoint || m_typeB == JointType.e_prismaticJoint);
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.e_revoluteJoint)
{
RevoluteJoint revolute = (RevoluteJoint)def.joint1;
m_localAnchorC = revolute.m_localAnchorA;
m_localAnchorA = revolute.m_localAnchorB;
m_referenceAngleA = revolute.m_referenceAngle;
m_localAxisC.SetZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)def.joint1;
m_localAnchorC = prismatic.m_localAnchorA;
m_localAnchorA = prismatic.m_localAnchorB;
m_referenceAngleA = prismatic.m_referenceAngle;
m_localAxisC = prismatic.m_localXAxisA;
Vec2 pC = m_localAnchorC;
Vec2 pA = Utilities.MulT(xfC.q, Utilities.Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = Utilities.Dot(pA - pC, m_localAxisC);
}
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.e_revoluteJoint)
{
RevoluteJoint revolute = (RevoluteJoint)def.joint2;
m_localAnchorD = revolute.m_localAnchorA;
m_localAnchorB = revolute.m_localAnchorB;
m_referenceAngleB = revolute.m_referenceAngle;
m_localAxisD.SetZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)def.joint2;
m_localAnchorD = prismatic.m_localAnchorA;
m_localAnchorB = prismatic.m_localAnchorB;
m_referenceAngleB = prismatic.m_referenceAngle;
m_localAxisD = prismatic.m_localXAxisA;
Vec2 pD = m_localAnchorD;
Vec2 pB = Utilities.MulT(xfD.q, Utilities.Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = Utilities.Dot(pB - pD, m_localAxisD);
}
m_ratio = def.ratio;
m_constant = coordinateA + m_ratio * coordinateB;
m_impulse = 0.0f;
}
internal GearJoint(GearJointDef def) : base(def)
{
m_joint1 = def.joint1;
m_joint2 = def.joint2;
m_typeA = m_joint1.GetJointType();
m_typeB = m_joint2.GetJointType();
Utilities.Assert(m_typeA == JointType.e_revoluteJoint || m_typeA == JointType.e_prismaticJoint);
Utilities.Assert(m_typeB == JointType.e_revoluteJoint || m_typeB == JointType.e_prismaticJoint);
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.e_revoluteJoint)
{
RevoluteJoint revolute = (RevoluteJoint)def.joint1;
m_localAnchorC = revolute.m_localAnchorA;
m_localAnchorA = revolute.m_localAnchorB;
m_referenceAngleA = revolute.m_referenceAngle;
m_localAxisC.SetZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)def.joint1;
m_localAnchorC = prismatic.m_localAnchorA;
m_localAnchorA = prismatic.m_localAnchorB;
m_referenceAngleA = prismatic.m_referenceAngle;
m_localAxisC = prismatic.m_localXAxisA;
Vec2 pC = m_localAnchorC;
Vec2 pA = Utilities.MulT(xfC.q, Utilities.Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = Utilities.Dot(pA - pC, m_localAxisC);
}
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.e_revoluteJoint)
{
RevoluteJoint revolute = (RevoluteJoint)def.joint2;
m_localAnchorD = revolute.m_localAnchorA;
m_localAnchorB = revolute.m_localAnchorB;
m_referenceAngleB = revolute.m_referenceAngle;
m_localAxisD.SetZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)def.joint2;
m_localAnchorD = prismatic.m_localAnchorA;
m_localAnchorB = prismatic.m_localAnchorB;
m_referenceAngleB = prismatic.m_referenceAngle;
m_localAxisD = prismatic.m_localXAxisA;
Vec2 pD = m_localAnchorD;
Vec2 pB = Utilities.MulT(xfD.q, Utilities.Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = Utilities.Dot(pB - pD, m_localAxisD);
}
m_ratio = def.ratio;
m_constant = coordinateA + m_ratio * coordinateB;
m_impulse = 0.0f;
}
