public void ApplyAttribtues(GearJointDef j)
{
j.collideConnected = collideConnected;
if (ratio != 1.0f)
{
j.ratio = ratio;
}
}
// Use this for initialization
protected override IntPtr Init()
{
if (!joint1.Started)
{
joint1.Start();
}
if (!joint2.Started)
{
joint2.Start();
}
GearJointDef jd = new GearJointDef(other.body, body.body);
jd.joint1 = joint1.joint;
jd.joint2 = joint2.joint;
jd.ratio = ratio;
return(API.CreateGearJoint(B2DWorld.instance.world, jd));
}
void Start()
{
//if (connectedBody == null) {
var jointDef = new GearJointDef();
jointDef.Body1 = GetComponent <Box2DBody>();
jointDef.Body2 = connectedBody;
jointDef.Joint1 = joint1;
jointDef.Joint2 = joint2;
// [CHRISK] TODO
//}
#if USING_BOX2DX
#else
// jointDef.Initialize(connectedBody, GetComponent<Box2DBody>(), anchor);
#endif
var world = Box2DWorld.Instance();
joint = (GearJoint)world.CreateJoint(jointDef);
}
public GearJoint()
{
Body ground;
{
var bd = new BodyDef();
ground = World.CreateBody(bd);
var shape = new EdgeShape();
shape.SetTwoSided(new Vector2(50.0f, 0.0f), new Vector2(-50.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
var circle1 = new CircleShape();
circle1.Radius = 1.0f;
var box = new PolygonShape();
box.SetAsBox(0.5f, 5.0f);
var circle2 = new CircleShape();
circle2.Radius = 2.0f;
var bd1 = new BodyDef();
bd1.BodyType = BodyType.StaticBody;
bd1.Position.Set(10.0f, 9.0f);
var body1 = World.CreateBody(bd1);
body1.CreateFixture(circle1, 5.0f);
var bd2 = new BodyDef();
bd2.BodyType = BodyType.DynamicBody;
bd2.Position.Set(10.0f, 8.0f);
var body2 = World.CreateBody(bd2);
body2.CreateFixture(box, 5.0f);
var bd3 = new BodyDef();
bd3.BodyType = BodyType.DynamicBody;
bd3.Position.Set(10.0f, 6.0f);
var body3 = World.CreateBody(bd3);
body3.CreateFixture(circle2, 5.0f);
var jd1 = new RevoluteJointDef();
jd1.Initialize(body1, body2, bd1.Position);
var joint1 = World.CreateJoint(jd1);
var jd2 = new RevoluteJointDef();
jd2.Initialize(body2, body3, bd3.Position);
var joint2 = World.CreateJoint(jd2);
var jd4 = new GearJointDef();
jd4.BodyA = body1;
jd4.BodyB = body3;
jd4.Joint1 = joint1;
jd4.Joint2 = joint2;
jd4.Ratio = circle2.Radius / circle1.Radius;
World.CreateJoint(jd4);
}
{
var circle1 = new CircleShape();
circle1.Radius = 1.0f;
var circle2 = new CircleShape();
circle2.Radius = 2.0f;
var box = new PolygonShape();
box.SetAsBox(0.5f, 5.0f);
var bd1 = new BodyDef();
bd1.BodyType = BodyType.DynamicBody;
bd1.Position.Set(-3.0f, 12.0f);
var body1 = World.CreateBody(bd1);
body1.CreateFixture(circle1, 5.0f);
var 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();
_joint1 = (RevoluteJoint)World.CreateJoint(jd1);
var bd2 = new BodyDef();
bd2.BodyType = BodyType.DynamicBody;
bd2.Position.Set(0.0f, 12.0f);
var body2 = World.CreateBody(bd2);
body2.CreateFixture(circle2, 5.0f);
var jd2 = new RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.Position);
_joint2 = (RevoluteJoint)World.CreateJoint(jd2);
var bd3 = new BodyDef();
bd3.BodyType = BodyType.DynamicBody;
bd3.Position.Set(2.5f, 12.0f);
var body3 = World.CreateBody(bd3);
body3.CreateFixture(box, 5.0f);
var jd3 = new PrismaticJointDef();
jd3.Initialize(ground, body3, bd3.Position, new Vector2(0.0f, 1.0f));
jd3.LowerTranslation = -5.0f;
jd3.UpperTranslation = 5.0f;
jd3.EnableLimit = true;
_joint3 = (PrismaticJoint)World.CreateJoint(jd3);
var jd4 = new GearJointDef();
jd4.BodyA = body1;
jd4.BodyB = body2;
jd4.Joint1 = _joint1;
jd4.Joint2 = _joint2;
jd4.Ratio = circle2.Radius / circle1.Radius;
_joint4 = (Box2DSharp.Dynamics.Joints.GearJoint)World.CreateJoint(jd4);
var jd5 = new GearJointDef();
jd5.BodyA = body2;
jd5.BodyB = body3;
jd5.Joint1 = _joint2;
jd5.Joint2 = _joint3;
jd5.Ratio = -1.0f / circle2.Radius;
_joint5 = (Box2DSharp.Dynamics.Joints.GearJoint)World.CreateJoint(jd5);
}
}
Gears()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vector2(50.0f, 0.0f), new Vector2(-50.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
CircleShape circle1 = new CircleShape();
circle1._radius = 1.0f;
CircleShape circle2 = new CircleShape();
circle2._radius = 2.0f;
PolygonShape box = new PolygonShape();
box.SetAsBox(0.5f, 5.0f);
BodyDef bd1 = new BodyDef();
bd1.type = BodyType.Dynamic;
bd1.position = new Vector2(-3.0f, 12.0f);
Body body1 = _world.CreateBody(bd1);
body1.CreateFixture(circle1, 5.0f);
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();
_joint1 = (RevoluteJoint)_world.CreateJoint(jd1);
BodyDef bd2 = new BodyDef();
bd2.type = BodyType.Dynamic;
bd2.position = new Vector2(0.0f, 12.0f);
Body body2 = _world.CreateBody(bd2);
body2.CreateFixture(circle2, 5.0f);
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.position);
_joint2 = (RevoluteJoint)_world.CreateJoint(jd2);
BodyDef bd3 = new BodyDef();
bd3.type = BodyType.Dynamic;
bd3.position = new Vector2(2.5f, 12.0f);
Body body3 = _world.CreateBody(bd3);
body3.CreateFixture(box, 5.0f);
PrismaticJointDef jd3 = new PrismaticJointDef();
jd3.Initialize(ground, body3, bd3.position, new Vector2(0.0f, 1.0f));
jd3.lowerTranslation = -5.0f;
jd3.upperTranslation = 5.0f;
jd3.enableLimit = true;
_joint3 = (PrismaticJoint)_world.CreateJoint(jd3);
GearJointDef jd4 = new GearJointDef();
jd4.bodyA = body1;
jd4.bodyB = body2;
jd4.joint1 = _joint1;
jd4.joint2 = _joint2;
jd4.ratio = circle2._radius / circle1._radius;
_joint4 = (GearJoint)_world.CreateJoint(jd4);
GearJointDef jd5 = new GearJointDef();
jd5.bodyA = body2;
jd5.bodyB = body3;
jd5.joint1 = _joint2;
jd5.joint2 = _joint3;
jd5.ratio = -1.0f / circle2._radius;
_joint5 = (GearJoint)_world.CreateJoint(jd5);
}
}
public GearJoint(GearJointDef def) : base(def)
{
_jointA = def.JointA;
_jointB = def.JointB;
_typeA = _jointA.JointType;
_typeB = _jointB.JointType;
Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic);
Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
_bodyC = _jointA.BodyA;
_bodyA = _jointA.BodyB;
// Body B on joint1 must be dynamic
Debug.Assert(_bodyA._type == BodyType.Dynamic);
// Get geometry of joint1
Transform xfA = _bodyA._xf;
float aA = _bodyA._sweep.A;
Transform xfC = _bodyC._xf;
float aC = _bodyC._sweep.A;
if (_typeA == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)def.JointA;
_localAnchorC = revolute._localAnchorA;
_localAnchorA = revolute._localAnchorB;
_referenceAngleA = revolute._referenceAngle;
_localAxisC = Vector2.Zero;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)def.JointA;
_localAnchorC = prismatic._localAnchorA;
_localAnchorA = prismatic._localAnchorB;
_referenceAngleA = prismatic._referenceAngle;
_localAxisC = prismatic._localXAxisA;
Vector2 pC = _localAnchorC;
Vector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
coordinateA = MathUtils.Dot(pA - pC, _localAxisC);
}
_bodyD = _jointB.BodyA;
_bodyB = _jointB.BodyB;
// Body B on joint2 must be dynamic
Debug.Assert(_bodyB._type == BodyType.Dynamic);
// Get geometry of joint2
Transform xfB = _bodyB._xf;
float aB = _bodyB._sweep.A;
Transform xfD = _bodyD._xf;
float aD = _bodyD._sweep.A;
if (_typeB == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)def.JointB;
_localAnchorD = revolute._localAnchorA;
_localAnchorB = revolute._localAnchorB;
_referenceAngleB = revolute._referenceAngle;
_localAxisD = Vector2.Zero;
coordinateB = aB - aD - _referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)def.JointB;
_localAnchorD = prismatic._localAnchorA;
_localAnchorB = prismatic._localAnchorB;
_referenceAngleB = prismatic._referenceAngle;
_localAxisD = prismatic._localXAxisA;
Vector2 pD = _localAnchorD;
Vector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
coordinateB = MathUtils.Dot(pB - pD, _localAxisD);
}
_ratio = def.Ratio;
_constant = coordinateA + _ratio * coordinateB;
_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);
}
}
public Gears()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(50.0f, 0.0f), new Vec2(-50.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
CircleShape circle1 = new CircleShape();
circle1.Radius = 1.0f;
CircleShape circle2 = new CircleShape();
circle2.Radius = 2.0f;
PolygonShape box = new PolygonShape();
box.SetAsBox(0.5f, 5.0f);
BodyDef bd1 = new BodyDef();
bd1.BodyType = BodyType.Dynamic;
bd1.Position = new Vec2(-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;
jd1.LocalAnchorA = ground.GetLocalPoint(bd1.Position);
jd1.LocalAnchorB = body1.GetLocalPoint(bd1.Position);
jd1.ReferenceAngle = body1.Angle - ground.Angle;
m_joint1 = (RevoluteJoint)m_world.CreateJoint(jd1);
BodyDef bd2 = new BodyDef();
bd2.BodyType = BodyType.Dynamic;
bd2.Position = new Vec2(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.BodyType = BodyType.Dynamic;
bd3.Position = new Vec2(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.JointA = m_joint1;
jd4.JointB = m_joint2;
jd4.Ratio = circle2.Radius / circle1.Radius;
m_joint4 = (GearJoint)m_world.CreateJoint(jd4);
GearJointDef jd5 = new GearJointDef();
jd5.BodyA = body2;
jd5.BodyB = body3;
jd5.JointA = m_joint2;
jd5.JointB = m_joint3;
jd5.Ratio = -1.0f / circle2.Radius;
m_joint5 = (GearJoint)m_world.CreateJoint(jd5);
}
}
public static Joint AddJoint(this IJointable ithis, V2DJoint joint, float offsetX, float offsetY)
{
Joint result = null;
JointDef jointDef = null;
//Body targ0 = ithis.VScreen.bodyMap[joint.Body1];
//Body targ1 = ithis.VScreen.bodyMap[joint.Body2];
Body targ0 = GetBody(ithis, joint.Body1);
Body targ1 = GetBody(ithis, joint.Body2);
// gears need the first body static
if (targ0 != null && targ1 != null && targ1.GetType() == BodyType.Static && targ0.GetType() != BodyType.Static)
{
Body temp = targ0;
targ0 = targ1;
targ1 = temp;
}
Vector2 pt0 = new Vector2(joint.X + offsetX, joint.Y + offsetY);
string name = joint.Name;
Vector2 anchor0 = new Vector2(pt0.X / V2DScreen.WorldScale, pt0.Y / V2DScreen.WorldScale);
Vector2 anchor1 = new Vector2();
switch (joint.Type)
{
case V2DJointKind.Distance:
Vector2 pt1 = new Vector2(joint.X2 + offsetX, joint.Y2 + offsetY);
anchor1 = new Vector2(pt1.X / V2DScreen.WorldScale, pt1.Y / V2DScreen.WorldScale);
DistanceJointDef dj = new DistanceJointDef();
dj.Initialize(targ0, targ1, anchor0, anchor1);
dj.collideConnected = joint.CollideConnected;
dj.dampingRatio = joint.DampingRatio;
dj.frequencyHz = joint.FrequencyHz;
if (joint.Length != -1)
{
dj.length = joint.Length / V2DScreen.WorldScale;
}
jointDef = dj;
break;
case V2DJointKind.Revolute:
float rot0 = joint.Min; //(typeof(joint["min"]) == "string") ? parseFloat(joint["min"]) / 180 * Math.PI : joint["min"];
float rot1 = joint.Max; //(typeof(joint["max"]) == "string") ? parseFloat(joint["max"]) / 180 * Math.PI : joint["max"];
RevoluteJointDef rj = new RevoluteJointDef();
rj.Initialize(targ0, targ1, anchor0);
rj.lowerAngle = rot0;
rj.upperAngle = rot1;
rj.enableLimit = rot0 != 0 && rot1 != 0;
rj.maxMotorTorque = joint.MaxMotorTorque;
rj.motorSpeed = joint.MotorSpeed;
rj.enableMotor = joint.EnableMotor;
jointDef = rj;
break;
case V2DJointKind.Prismatic:
float axisX = joint.AxisX;
float axisY = joint.AxisY;
float min = joint.Min;
float max = joint.Max;
PrismaticJointDef pj = new PrismaticJointDef();
Vector2 worldAxis = new Vector2(axisX, axisY);
pj.Initialize(targ0, targ1, anchor0, worldAxis);
pj.lowerTranslation = min / V2DScreen.WorldScale;
pj.upperTranslation = max / V2DScreen.WorldScale;
pj.enableLimit = joint.EnableLimit;
pj.maxMotorForce = joint.MaxMotorTorque;
pj.motorSpeed = joint.MotorSpeed;
pj.enableMotor = joint.EnableMotor;
jointDef = pj;
break;
case V2DJointKind.Pully:
Vector2 pt2 = new Vector2(joint.X2 + offsetX, joint.Y2 + offsetY);
anchor1 = new Vector2(pt2.X / V2DScreen.WorldScale, pt2.Y / V2DScreen.WorldScale);
Vector2 groundAnchor0 = new Vector2(joint.GroundAnchor1X / V2DScreen.WorldScale, joint.GroundAnchor1Y / V2DScreen.WorldScale);
Vector2 groundAnchor1 = new Vector2(joint.GroundAnchor2X / V2DScreen.WorldScale, joint.GroundAnchor2Y / V2DScreen.WorldScale);
float max0 = joint.MaxLength1;
float max1 = joint.MaxLength2;
float rat = joint.Ratio;
PulleyJointDef puj = new PulleyJointDef();
puj.Initialize(targ0, targ1, groundAnchor0, groundAnchor1, anchor0, anchor1, rat);
puj.maxLengthA = (max0 + max1) / V2DScreen.WorldScale;
puj.maxLengthB = (max0 + max1) / V2DScreen.WorldScale;
puj.collideConnected = joint.CollideConnected;
jointDef = puj;
break;
case V2DJointKind.Gear:
GearJointDef gj = new GearJointDef();
gj.bodyA = targ0;
gj.bodyB = targ1;
gj.joint1 = GetFirstGearableJoint(targ0.GetJointList());
gj.joint2 = GetFirstGearableJoint(targ1.GetJointList());
gj.ratio = joint.Ratio;
jointDef = gj;
break;
}
if (jointDef != null)
{
result = SetJointWithReflection(ithis, name, jointDef);
if (result != null)
{
Dictionary <string, string> dict = new Dictionary <string, string>();
dict["name"] = name;
result.SetUserData(dict);
}
}
return(result);
}
/// <summary>
/// Initializes a new instance of the <see cref="GearJoint" /> class
/// </summary>
/// <param name="def">The def</param>
public GearJoint(GearJointDef def) : base(def)
{
jointA = def.JointA;
jointB = def.JointB;
typeA = jointA.JointType;
typeB = jointB.JointType;
Debug.Assert(typeA == JointType.Revolute || typeA == JointType.Prismatic);
Debug.Assert(typeB == JointType.Revolute || typeB == JointType.Prismatic);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
bodyC = jointA.BodyA;
BodyA = jointA.BodyB;
// Body B on joint1 must be dynamic
Debug.Assert(BodyA.Type == BodyType.Dynamic);
// Get geometry of joint1
Transform xfA = BodyA.Xf;
float aA = BodyA.Sweep.A;
Transform xfC = bodyC.Xf;
float aC = bodyC.Sweep.A;
if (typeA == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)def.JointA;
localAnchorC = revolute.LocalAnchorA;
localAnchorA = revolute.LocalAnchorB;
referenceAngleA = revolute.ReferenceAngle;
localAxisC = Vector2.Zero;
coordinateA = aA - aC - referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)def.JointA;
localAnchorC = prismatic.LocalAnchorA;
localAnchorA = prismatic.LocalAnchorB;
referenceAngleA = prismatic.ReferenceAngle;
localAxisC = prismatic.LocalXAxisA;
Vector2 pC = localAnchorC;
Vector2 pA = MathUtils.MulT(xfC.Q, MathUtils.Mul(xfA.Q, localAnchorA) + (xfA.P - xfC.P));
coordinateA = MathUtils.Dot(pA - pC, localAxisC);
}
bodyD = jointB.BodyA;
BodyB = jointB.BodyB;
// Body B on joint2 must be dynamic
Debug.Assert(BodyB.Type == BodyType.Dynamic);
// Get geometry of joint2
Transform xfB = BodyB.Xf;
float aB = BodyB.Sweep.A;
Transform xfD = bodyD.Xf;
float aD = bodyD.Sweep.A;
if (typeB == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)def.JointB;
localAnchorD = revolute.LocalAnchorA;
localAnchorB = revolute.LocalAnchorB;
referenceAngleB = revolute.ReferenceAngle;
localAxisD = Vector2.Zero;
coordinateB = aB - aD - referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)def.JointB;
localAnchorD = prismatic.LocalAnchorA;
localAnchorB = prismatic.LocalAnchorB;
referenceAngleB = prismatic.ReferenceAngle;
localAxisD = prismatic.LocalXAxisA;
Vector2 pD = localAnchorD;
Vector2 pB = MathUtils.MulT(xfD.Q, MathUtils.Mul(xfB.Q, localAnchorB) + (xfB.P - xfD.P));
coordinateB = MathUtils.Dot(pB - pD, localAxisD);
}
ratio = def.Ratio;
constant = coordinateA + ratio * coordinateB;
impulse = 0.0f;
}
Gears()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vector2(50.0f, 0.0f), new Vector2(-50.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
CircleShape circle1 = new CircleShape();
circle1._radius = 1.0f;
CircleShape circle2 = new CircleShape();
circle2._radius = 2.0f;
PolygonShape box = new PolygonShape();
box.SetAsBox(0.5f, 5.0f);
BodyDef bd1 = new BodyDef();
bd1.type = BodyType.Dynamic;
bd1.position = new Vector2(-3.0f, 12.0f);
Body body1 = _world.CreateBody(bd1);
body1.CreateFixture(circle1, 5.0f);
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();
_joint1 = (RevoluteJoint)_world.CreateJoint(jd1);
BodyDef bd2 = new BodyDef();
bd2.type = BodyType.Dynamic;
bd2.position = new Vector2(0.0f, 12.0f);
Body body2 = _world.CreateBody(bd2);
body2.CreateFixture(circle2, 5.0f);
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.position);
_joint2 = (RevoluteJoint)_world.CreateJoint(jd2);
BodyDef bd3 = new BodyDef();
bd3.type = BodyType.Dynamic;
bd3.position = new Vector2(2.5f, 12.0f);
Body body3 = _world.CreateBody(bd3);
body3.CreateFixture(box, 5.0f);
PrismaticJointDef jd3 = new PrismaticJointDef();
jd3.Initialize(ground, body3, bd3.position, new Vector2(0.0f, 1.0f));
jd3.lowerTranslation = -5.0f;
jd3.upperTranslation = 5.0f;
jd3.enableLimit = true;
_joint3 = (PrismaticJoint)_world.CreateJoint(jd3);
GearJointDef jd4 = new GearJointDef();
jd4.bodyA = body1;
jd4.bodyB = body2;
jd4.joint1 = _joint1;
jd4.joint2 = _joint2;
jd4.ratio = circle2._radius / circle1._radius;
_joint4 = (GearJoint)_world.CreateJoint(jd4);
GearJointDef jd5 = new GearJointDef();
jd5.bodyA = body2;
jd5.bodyB = body3;
jd5.joint1 = _joint2;
jd5.joint2 = _joint3;
jd5.ratio = -1.0f / circle2._radius;
_joint5 = (GearJoint)_world.CreateJoint(jd5);
}
}
public Gears()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, -10.0f);
ground = _world.CreateBody(bd);
PolygonDef sd = new PolygonDef();
sd.SetAsBox(50.0f, 10.0f);
ground.CreateShape(sd);
}
{
CircleDef circle1 = new CircleDef();
circle1.Radius = 1.0f;
circle1.Density = 5.0f;
CircleDef circle2 = new CircleDef();
circle2.Radius = 2.0f;
circle2.Density = 5.0f;
PolygonDef box = new PolygonDef();
box.SetAsBox(0.5f, 5.0f);
box.Density = 5.0f;
BodyDef bd1 = new BodyDef();
bd1.Position.Set(-3.0f, 12.0f);
Body body1 = _world.CreateBody(bd1);
body1.CreateShape(circle1);
body1.SetMassFromShapes();
RevoluteJointDef jd1 = new RevoluteJointDef();
jd1.Body1 = ground;
jd1.Body2 = body1;
jd1.LocalAnchor1 = ground.GetLocalPoint(bd1.Position);
jd1.LocalAnchor2 = body1.GetLocalPoint(bd1.Position);
jd1.ReferenceAngle = body1.GetAngle() - ground.GetAngle();
_joint1 = (RevoluteJoint)_world.CreateJoint(jd1);
BodyDef bd2 = new BodyDef();
bd2.Position.Set(0.0f, 12.0f);
Body body2 = _world.CreateBody(bd2);
body2.CreateShape(circle2);
body2.SetMassFromShapes();
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.Position);
_joint2 = (RevoluteJoint)_world.CreateJoint(jd2);
BodyDef bd3 = new BodyDef();
bd3.Position.Set(2.5f, 12.0f);
Body body3 = _world.CreateBody(bd3);
body3.CreateShape(box);
body3.SetMassFromShapes();
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;
_joint3 = (PrismaticJoint)_world.CreateJoint(jd3);
GearJointDef jd4 = new GearJointDef();
jd4.Body1 = body1;
jd4.Body2 = body2;
jd4.Joint1 = _joint1;
jd4.Joint2 = _joint2;
jd4.Ratio = circle2.Radius / circle1.Radius;
_joint4 = (GearJoint)_world.CreateJoint(jd4);
GearJointDef jd5 = new GearJointDef();
jd5.Body1 = body2;
jd5.Body2 = body3;
jd5.Joint1 = _joint2;
jd5.Joint2 = _joint3;
jd5.Ratio = -1.0f / circle2.Radius;
_joint5 = (GearJoint)_world.CreateJoint(jd5);
}
}
private GearJointTest()
{
Body ground;
{
BodyDef bd = new BodyDef();
ground = BodyFactory.CreateFromDef(World, bd);
EdgeShape shape = new EdgeShape();
shape.SetTwoSided(new Vector2(50.0f, 0.0f), new Vector2(-50.0f, 0.0f));
ground.AddFixture(shape);
}
{
CircleShape circle1 = new CircleShape(5.0f);
circle1.Radius = 1.0f;
PolygonShape box = new PolygonShape(5.0f);
box.SetAsBox(0.5f, 5.0f);
CircleShape circle2 = new CircleShape(5.0f);
circle2.Radius = 2.0f;
BodyDef bd1 = new BodyDef();
bd1.Type = BodyType.Static;
bd1.Position = new Vector2(10.0f, 9.0f);
Body body1 = BodyFactory.CreateFromDef(World, bd1);
body1.AddFixture(circle1);
BodyDef bd2 = new BodyDef();
bd2.Type = BodyType.Dynamic;
bd2.Position = new Vector2(10.0f, 8.0f);
Body body2 = BodyFactory.CreateFromDef(World, bd2);
body2.AddFixture(box);
BodyDef bd3 = new BodyDef();
bd3.Type = BodyType.Dynamic;
bd3.Position = new Vector2(10.0f, 6.0f);
Body body3 = BodyFactory.CreateFromDef(World, bd3);
body3.AddFixture(circle2);
RevoluteJointDef jd1 = new RevoluteJointDef();
jd1.Initialize(body1, body2, bd1.Position);
Joint joint1 = JointFactory.CreateFromDef(World, jd1);
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.Initialize(body2, body3, bd3.Position);
Joint joint2 = JointFactory.CreateFromDef(World, jd2);
GearJointDef jd4 = new GearJointDef();
jd4.BodyA = body1;
jd4.BodyB = body3;
jd4.JointA = joint1;
jd4.JointB = joint2;
jd4.Ratio = circle2.Radius / circle1.Radius;
JointFactory.CreateFromDef(World, jd4);
}
{
CircleShape circle1 = new CircleShape(5.0f);
circle1.Radius = 1.0f;
CircleShape circle2 = new CircleShape(5.0f);
circle2.Radius = 2.0f;
PolygonShape box = new PolygonShape(5.0f);
box.SetAsBox(0.5f, 5.0f);
BodyDef bd1 = new BodyDef();
bd1.Type = BodyType.Dynamic;
bd1.Position = new Vector2(-3.0f, 12.0f);
Body body1 = BodyFactory.CreateFromDef(World, bd1);
body1.AddFixture(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.Rotation - ground.Rotation;
_joint1 = (RevoluteJoint)JointFactory.CreateFromDef(World, jd1);
BodyDef bd2 = new BodyDef();
bd2.Type = BodyType.Dynamic;
bd2.Position = new Vector2(0.0f, 12.0f);
Body body2 = BodyFactory.CreateFromDef(World, bd2);
body2.AddFixture(circle2);
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.Position);
_joint2 = (RevoluteJoint)JointFactory.CreateFromDef(World, jd2);
BodyDef bd3 = new BodyDef();
bd3.Type = BodyType.Dynamic;
bd3.Position = new Vector2(2.5f, 12.0f);
Body body3 = BodyFactory.CreateFromDef(World, bd3);
body3.AddFixture(box);
PrismaticJointDef jd3 = new PrismaticJointDef();
jd3.Initialize(ground, body3, bd3.Position, new Vector2(0.0f, 1.0f));
jd3.LowerTranslation = -5.0f;
jd3.UpperTranslation = 5.0f;
jd3.EnableLimit = true;
_joint3 = (PrismaticJoint)JointFactory.CreateFromDef(World, jd3);
GearJointDef jd4 = new GearJointDef();
jd4.BodyA = body1;
jd4.BodyB = body2;
jd4.JointA = _joint1;
jd4.JointB = _joint2;
jd4.Ratio = circle2.Radius / circle1.Radius;
_joint4 = (GearJoint)JointFactory.CreateFromDef(World, jd4);
GearJointDef jd5 = new GearJointDef();
jd5.BodyA = body2;
jd5.BodyB = body3;
jd5.JointA = _joint2;
jd5.JointB = _joint3;
jd5.Ratio = -1.0f / circle2.Radius;
_joint5 = (GearJoint)JointFactory.CreateFromDef(World, jd5);
}
}
