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; }
/// <summary> /// Requires two existing revolute or prismatic joints (any combination will work). The provided joints must /// attach a dynamic body to a static body. /// </summary> /// <param name="jointA">The first joint.</param> /// <param name="jointB">The second joint.</param> /// <param name="ratio">The ratio.</param> /// <param name="bodyA">The first body</param> /// <param name="bodyB">The second body</param> public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio = 1f) { JointType = JointType.Gear; BodyA = bodyA; BodyB = bodyB; JointA = jointA; JointB = jointB; Ratio = ratio; _typeA = jointA.JointType; _typeB = jointB.JointType; Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic); Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic); float coordinateA, coordinateB; // TODO_ERIN there might be some problem with the joint edges in b2Joint. _bodyC = JointA.BodyA; _bodyA = JointA.BodyB; // 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)jointA; _localAnchorC = revolute.LocalAnchorA; _localAnchorA = revolute.LocalAnchorB; _referenceAngleA = revolute.ReferenceAngle; _localAxisC = Vector2.Zero; coordinateA = aA - aC - _referenceAngleA; } else { PrismaticJoint prismatic = (PrismaticJoint)jointA; _localAnchorC = prismatic.LocalAnchorA; _localAnchorA = prismatic.LocalAnchorB; _referenceAngleA = prismatic.ReferenceAngle; _localAxisC = prismatic.LocalXAxis; Vector2 pC = _localAnchorC; Vector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p)); coordinateA = Vector2.Dot(pA - pC, _localAxisC); } _bodyD = JointB.BodyA; _bodyB = JointB.BodyB; // 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)jointB; _localAnchorD = revolute.LocalAnchorA; _localAnchorB = revolute.LocalAnchorB; _referenceAngleB = revolute.ReferenceAngle; _localAxisD = Vector2.Zero; coordinateB = aB - aD - _referenceAngleB; } else { PrismaticJoint prismatic = (PrismaticJoint)jointB; _localAnchorD = prismatic.LocalAnchorA; _localAnchorB = prismatic.LocalAnchorB; _referenceAngleB = prismatic.ReferenceAngle; _localAxisD = prismatic.LocalXAxis; Vector2 pD = _localAnchorD; Vector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p)); coordinateB = Vector2.Dot(pB - pD, _localAxisD); } _ratio = ratio; _constant = coordinateA + _ratio * coordinateB; _impulse = 0.0f; }