internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2GearJointDef obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public Gears()
{
b2Body ground = null;
{
b2BodyDef bd = new b2BodyDef();
ground = m_world.CreateBody(bd);
b2EdgeShape shape = new b2EdgeShape();
shape.Set(new b2Vec2(50.0f, 0.0f), new b2Vec2(-50.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
// Gears co
{
b2CircleShape circle1 = new b2CircleShape();
circle1.Radius = 1.0f;
b2PolygonShape box = new b2PolygonShape();
box.SetAsBox(0.5f, 5.0f);
b2CircleShape circle2 = new b2CircleShape();
circle2.Radius = 2.0f;
b2BodyDef bd1 = new b2BodyDef();
bd1.type = b2BodyType.b2_staticBody;
bd1.position.Set(10.0f, 9.0f);
b2Body body1 = m_world.CreateBody(bd1);
body1.CreateFixture(circle1, 0.0f);
b2BodyDef bd2 = new b2BodyDef();
bd2.type = b2BodyType.b2_dynamicBody;
bd2.position.Set(10.0f, 8.0f);
b2Body body2 = m_world.CreateBody(bd2);
body2.CreateFixture(box, 5.0f);
b2BodyDef bd3 = new b2BodyDef();
bd3.type = b2BodyType.b2_dynamicBody;
bd3.position.Set(10.0f, 6.0f);
b2Body body3 = m_world.CreateBody(bd3);
body3.CreateFixture(circle2, 5.0f);
b2RevoluteJointDef jd1 = new b2RevoluteJointDef();
jd1.Initialize(body2, body1, bd1.position);
b2Joint joint1 = m_world.CreateJoint(jd1);
b2RevoluteJointDef jd2 = new b2RevoluteJointDef();
jd2.Initialize(body2, body3, bd3.position);
b2Joint joint2 = m_world.CreateJoint(jd2);
b2GearJointDef jd4 = new b2GearJointDef();
jd4.BodyA = body1;
jd4.BodyB = body3;
jd4.joint1 = joint1;
jd4.joint2 = joint2;
jd4.ratio = circle2.Radius / circle1.Radius;
m_world.CreateJoint(jd4);
}
{
b2CircleShape circle1 = new b2CircleShape();
circle1.Radius = 1.0f;
b2CircleShape circle2 = new b2CircleShape();
circle2.Radius = 2.0f;
b2PolygonShape box = new b2PolygonShape();
box.SetAsBox(0.5f, 5.0f);
b2BodyDef bd1 = new b2BodyDef();
bd1.type = b2BodyType.b2_dynamicBody;
bd1.position.Set(-3.0f, 12.0f);
b2Body body1 = m_world.CreateBody(bd1);
body1.CreateFixture(circle1, 5.0f);
b2RevoluteJointDef jd1 = new b2RevoluteJointDef();
jd1.BodyA = ground;
jd1.BodyB = body1;
jd1.localAnchorA = ground.GetLocalPoint(bd1.position);
jd1.localAnchorB = body1.GetLocalPoint(bd1.position);
jd1.referenceAngle = body1.Angle - ground.Angle;
m_joint1 = (b2RevoluteJoint)m_world.CreateJoint(jd1);
b2BodyDef bd2 = new b2BodyDef();
bd2.type = b2BodyType.b2_dynamicBody;
bd2.position.Set(0.0f, 12.0f);
b2Body body2 = m_world.CreateBody(bd2);
body2.CreateFixture(circle2, 5.0f);
b2RevoluteJointDef jd2 = new b2RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.position);
m_joint2 = (b2RevoluteJoint)m_world.CreateJoint(jd2);
b2BodyDef bd3 = new b2BodyDef();
bd3.type = b2BodyType.b2_dynamicBody;
bd3.position.Set(2.5f, 12.0f);
b2Body body3 = m_world.CreateBody(bd3);
body3.CreateFixture(box, 5.0f);
b2PrismaticJointDef jd3 = new b2PrismaticJointDef();
jd3.Initialize(ground, body3, bd3.position, new b2Vec2(0.0f, 1.0f));
jd3.lowerTranslation = -5.0f;
jd3.upperTranslation = 5.0f;
jd3.enableLimit = true;
m_joint3 = (b2PrismaticJoint)m_world.CreateJoint(jd3);
b2GearJointDef jd4 = new b2GearJointDef();
jd4.BodyA = body1;
jd4.BodyB = body2;
jd4.joint1 = m_joint1;
jd4.joint2 = m_joint2;
jd4.ratio = circle2.Radius / circle1.Radius;
m_joint4 = (b2GearJoint)m_world.CreateJoint(jd4);
b2GearJointDef jd5 = new b2GearJointDef();
jd5.BodyA = body2;
jd5.BodyB = body3;
jd5.joint1 = m_joint2;
jd5.joint2 = m_joint3;
jd5.ratio = -1.0f / circle2.Radius;
m_joint5 = (b2GearJoint)m_world.CreateJoint(jd5);
}
}
// friend class b2Joint;
// Gear Joint:
// C0 = (coordinate1 + ratio * coordinate2)_initial
// C = (coordinate1 + ratio * coordinate2) - C0 = 0
// 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
internal b2GearJoint(b2GearJointDef def) : base(def)
{
m_joint1 = def.joint1;
m_joint2 = def.joint2;
m_typeA = m_joint1.GetType();
m_typeB = m_joint2.GetType();
Debug.Assert(m_typeA == b2JointType.e_revoluteJoint || m_typeA == b2JointType.e_prismaticJoint);
Debug.Assert(m_typeB == b2JointType.e_revoluteJoint || m_typeB == b2JointType.e_prismaticJoint);
float coordinateA;
float coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
m_bodyC = m_joint1.GetBodyA();
m_bodyA = m_joint1.GetBodyB();
// Get geometry of joint1
b2Transform xfA = new b2Transform(m_bodyA.m_xf);
float aA = m_bodyA.m_sweep.a;
b2Transform xfC = new b2Transform(m_bodyC.m_xf);
float aC = m_bodyC.m_sweep.a;
if (m_typeA == b2JointType.e_revoluteJoint)
{
b2RevoluteJoint revolute = (b2RevoluteJoint)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
{
b2PrismaticJoint prismatic = (b2PrismaticJoint)def.joint1;
m_localAnchorC = prismatic.m_localAnchorA;
m_localAnchorA = prismatic.m_localAnchorB;
m_referenceAngleA = prismatic.m_referenceAngle;
m_localAxisC = prismatic.m_localXAxisA;
b2Vec2 pC = new b2Vec2(m_localAnchorC);
b2Vec2 pA = Utils.b2MulT(xfC.q, Utils.b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = Utils.b2Dot(pA - pC, m_localAxisC);
}
m_bodyD = m_joint2.GetBodyA();
m_bodyB = m_joint2.GetBodyB();
// Get geometry of joint2
b2Transform xfB = new b2Transform(m_bodyB.m_xf);
float aB = m_bodyB.m_sweep.a;
b2Transform xfD = new b2Transform(m_bodyD.m_xf);
float aD = m_bodyD.m_sweep.a;
if (m_typeB == b2JointType.e_revoluteJoint)
{
b2RevoluteJoint revolute = (b2RevoluteJoint)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
{
b2PrismaticJoint prismatic = (b2PrismaticJoint)def.joint2;
m_localAnchorD = prismatic.m_localAnchorA;
m_localAnchorB = prismatic.m_localAnchorB;
m_referenceAngleB = prismatic.m_referenceAngle;
m_localAxisD = prismatic.m_localXAxisA;
b2Vec2 pD = new b2Vec2(m_localAnchorD);
b2Vec2 pB = Utils.b2MulT(xfD.q, Utils.b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = Utils.b2Dot(pB - pD, m_localAxisD);
}
m_ratio = def.ratio;
m_constant = coordinateA + m_ratio * coordinateB;
m_impulse = 0.0f;
}