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; }