public static PrismaticJoint CreatePrismaticJoint(World world, Body bodyA, Body bodyB, TSVector2 anchor, TSVector2 axis, bool useWorldCoordinates = false)
{
PrismaticJoint prismaticJoint = new PrismaticJoint(bodyA, bodyB, anchor, axis, useWorldCoordinates);
world.AddJoint(prismaticJoint);
return(prismaticJoint);
}
public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, FP ratio)
{
base.JointType = JointType.Gear;
base.BodyA = bodyA;
base.BodyB = bodyB;
this.JointA = jointA;
this.JointB = jointB;
this.Ratio = ratio;
this._typeA = jointA.JointType;
this._typeB = jointB.JointType;
Debug.Assert(this._typeA == JointType.Revolute || this._typeA == JointType.Prismatic || this._typeA == JointType.FixedRevolute || this._typeA == JointType.FixedPrismatic);
Debug.Assert(this._typeB == JointType.Revolute || this._typeB == JointType.Prismatic || this._typeB == JointType.FixedRevolute || this._typeB == JointType.FixedPrismatic);
this._bodyC = this.JointA.BodyA;
this._bodyA = this.JointA.BodyB;
Transform xf = this._bodyA._xf;
FP a = this._bodyA._sweep.A;
Transform xf2 = this._bodyC._xf;
FP a2 = this._bodyC._sweep.A;
bool flag = this._typeA == JointType.Revolute;
FP x;
if (flag)
{
RevoluteJoint revoluteJoint = (RevoluteJoint)jointA;
this._localAnchorC = revoluteJoint.LocalAnchorA;
this._localAnchorA = revoluteJoint.LocalAnchorB;
this._referenceAngleA = revoluteJoint.ReferenceAngle;
this._localAxisC = TSVector2.zero;
x = a - a2 - this._referenceAngleA;
}
else
{
PrismaticJoint prismaticJoint = (PrismaticJoint)jointA;
this._localAnchorC = prismaticJoint.LocalAnchorA;
this._localAnchorA = prismaticJoint.LocalAnchorB;
this._referenceAngleA = prismaticJoint.ReferenceAngle;
this._localAxisC = prismaticJoint.LocalXAxis;
TSVector2 localAnchorC = this._localAnchorC;
TSVector2 value = MathUtils.MulT(xf2.q, MathUtils.Mul(xf.q, this._localAnchorA) + (xf.p - xf2.p));
x = TSVector2.Dot(value - localAnchorC, this._localAxisC);
}
this._bodyD = this.JointB.BodyA;
this._bodyB = this.JointB.BodyB;
Transform xf3 = this._bodyB._xf;
FP a3 = this._bodyB._sweep.A;
Transform xf4 = this._bodyD._xf;
FP a4 = this._bodyD._sweep.A;
bool flag2 = this._typeB == JointType.Revolute;
FP y;
if (flag2)
{
RevoluteJoint revoluteJoint2 = (RevoluteJoint)jointB;
this._localAnchorD = revoluteJoint2.LocalAnchorA;
this._localAnchorB = revoluteJoint2.LocalAnchorB;
this._referenceAngleB = revoluteJoint2.ReferenceAngle;
this._localAxisD = TSVector2.zero;
y = a3 - a4 - this._referenceAngleB;
}
else
{
PrismaticJoint prismaticJoint2 = (PrismaticJoint)jointB;
this._localAnchorD = prismaticJoint2.LocalAnchorA;
this._localAnchorB = prismaticJoint2.LocalAnchorB;
this._referenceAngleB = prismaticJoint2.ReferenceAngle;
this._localAxisD = prismaticJoint2.LocalXAxis;
TSVector2 localAnchorD = this._localAnchorD;
TSVector2 value2 = MathUtils.MulT(xf4.q, MathUtils.Mul(xf3.q, this._localAnchorB) + (xf3.p - xf4.p));
y = TSVector2.Dot(value2 - localAnchorD, this._localAxisD);
}
this._ratio = ratio;
this._constant = x + this._ratio * y;
this._impulse = 0f;
}
private static void SerializeJoint(List <Body> bodies, Joint joint)
{
WorldXmlSerializer._writer.WriteStartElement("Joint");
WorldXmlSerializer._writer.WriteAttributeString("Type", joint.JointType.ToString());
WorldXmlSerializer.WriteElement("BodyA", WorldXmlSerializer.FindIndex(bodies, joint.BodyA));
WorldXmlSerializer.WriteElement("BodyB", WorldXmlSerializer.FindIndex(bodies, joint.BodyB));
WorldXmlSerializer.WriteElement("CollideConnected", joint.CollideConnected);
WorldXmlSerializer.WriteElement("Breakpoint", joint.Breakpoint);
bool flag = joint.UserData != null;
if (flag)
{
WorldXmlSerializer._writer.WriteStartElement("UserData");
WorldXmlSerializer.WriteDynamicType(joint.UserData.GetType(), joint.UserData);
WorldXmlSerializer._writer.WriteEndElement();
}
switch (joint.JointType)
{
case JointType.Revolute:
{
RevoluteJoint revoluteJoint = (RevoluteJoint)joint;
WorldXmlSerializer.WriteElement("EnableLimit", revoluteJoint.LimitEnabled);
WorldXmlSerializer.WriteElement("EnableMotor", revoluteJoint.MotorEnabled);
WorldXmlSerializer.WriteElement("LocalAnchorA", revoluteJoint.LocalAnchorA);
WorldXmlSerializer.WriteElement("LocalAnchorB", revoluteJoint.LocalAnchorB);
WorldXmlSerializer.WriteElement("LowerAngle", revoluteJoint.LowerLimit);
WorldXmlSerializer.WriteElement("MaxMotorTorque", revoluteJoint.MaxMotorTorque);
WorldXmlSerializer.WriteElement("MotorSpeed", revoluteJoint.MotorSpeed);
WorldXmlSerializer.WriteElement("ReferenceAngle", revoluteJoint.ReferenceAngle);
WorldXmlSerializer.WriteElement("UpperAngle", revoluteJoint.UpperLimit);
break;
}
case JointType.Prismatic:
{
PrismaticJoint prismaticJoint = (PrismaticJoint)joint;
WorldXmlSerializer.WriteElement("EnableLimit", prismaticJoint.LimitEnabled);
WorldXmlSerializer.WriteElement("EnableMotor", prismaticJoint.MotorEnabled);
WorldXmlSerializer.WriteElement("LocalAnchorA", prismaticJoint.LocalAnchorA);
WorldXmlSerializer.WriteElement("LocalAnchorB", prismaticJoint.LocalAnchorB);
WorldXmlSerializer.WriteElement("Axis", prismaticJoint.Axis);
WorldXmlSerializer.WriteElement("LowerTranslation", prismaticJoint.LowerLimit);
WorldXmlSerializer.WriteElement("UpperTranslation", prismaticJoint.UpperLimit);
WorldXmlSerializer.WriteElement("MaxMotorForce", prismaticJoint.MaxMotorForce);
WorldXmlSerializer.WriteElement("MotorSpeed", prismaticJoint.MotorSpeed);
break;
}
case JointType.Distance:
{
DistanceJoint distanceJoint = (DistanceJoint)joint;
WorldXmlSerializer.WriteElement("DampingRatio", distanceJoint.DampingRatio);
WorldXmlSerializer.WriteElement("FrequencyHz", distanceJoint.Frequency);
WorldXmlSerializer.WriteElement("Length", distanceJoint.Length);
WorldXmlSerializer.WriteElement("LocalAnchorA", distanceJoint.LocalAnchorA);
WorldXmlSerializer.WriteElement("LocalAnchorB", distanceJoint.LocalAnchorB);
break;
}
case JointType.Pulley:
{
PulleyJoint pulleyJoint = (PulleyJoint)joint;
WorldXmlSerializer.WriteElement("WorldAnchorA", pulleyJoint.WorldAnchorA);
WorldXmlSerializer.WriteElement("WorldAnchorB", pulleyJoint.WorldAnchorB);
WorldXmlSerializer.WriteElement("LengthA", pulleyJoint.LengthA);
WorldXmlSerializer.WriteElement("LengthB", pulleyJoint.LengthB);
WorldXmlSerializer.WriteElement("LocalAnchorA", pulleyJoint.LocalAnchorA);
WorldXmlSerializer.WriteElement("LocalAnchorB", pulleyJoint.LocalAnchorB);
WorldXmlSerializer.WriteElement("Ratio", pulleyJoint.Ratio);
WorldXmlSerializer.WriteElement("Constant", pulleyJoint.Constant);
break;
}
case JointType.Gear:
throw new Exception("Gear joint not supported by serialization");
case JointType.Wheel:
{
WheelJoint wheelJoint = (WheelJoint)joint;
WorldXmlSerializer.WriteElement("EnableMotor", wheelJoint.MotorEnabled);
WorldXmlSerializer.WriteElement("LocalAnchorA", wheelJoint.LocalAnchorA);
WorldXmlSerializer.WriteElement("LocalAnchorB", wheelJoint.LocalAnchorB);
WorldXmlSerializer.WriteElement("MotorSpeed", wheelJoint.MotorSpeed);
WorldXmlSerializer.WriteElement("DampingRatio", wheelJoint.DampingRatio);
WorldXmlSerializer.WriteElement("MaxMotorTorque", wheelJoint.MaxMotorTorque);
WorldXmlSerializer.WriteElement("FrequencyHz", wheelJoint.Frequency);
WorldXmlSerializer.WriteElement("Axis", wheelJoint.Axis);
break;
}
case JointType.Weld:
{
WeldJoint weldJoint = (WeldJoint)joint;
WorldXmlSerializer.WriteElement("LocalAnchorA", weldJoint.LocalAnchorA);
WorldXmlSerializer.WriteElement("LocalAnchorB", weldJoint.LocalAnchorB);
break;
}
case JointType.Friction:
{
FrictionJoint frictionJoint = (FrictionJoint)joint;
WorldXmlSerializer.WriteElement("LocalAnchorA", frictionJoint.LocalAnchorA);
WorldXmlSerializer.WriteElement("LocalAnchorB", frictionJoint.LocalAnchorB);
WorldXmlSerializer.WriteElement("MaxForce", frictionJoint.MaxForce);
WorldXmlSerializer.WriteElement("MaxTorque", frictionJoint.MaxTorque);
break;
}
case JointType.Rope:
{
RopeJoint ropeJoint = (RopeJoint)joint;
WorldXmlSerializer.WriteElement("LocalAnchorA", ropeJoint.LocalAnchorA);
WorldXmlSerializer.WriteElement("LocalAnchorB", ropeJoint.LocalAnchorB);
WorldXmlSerializer.WriteElement("MaxLength", ropeJoint.MaxLength);
break;
}
case JointType.Motor:
{
MotorJoint motorJoint = (MotorJoint)joint;
WorldXmlSerializer.WriteElement("AngularOffset", motorJoint.AngularOffset);
WorldXmlSerializer.WriteElement("LinearOffset", motorJoint.LinearOffset);
WorldXmlSerializer.WriteElement("MaxForce", motorJoint.MaxForce);
WorldXmlSerializer.WriteElement("MaxTorque", motorJoint.MaxTorque);
WorldXmlSerializer.WriteElement("CorrectionFactor", motorJoint.CorrectionFactor);
break;
}
case JointType.Angle:
{
AngleJoint angleJoint = (AngleJoint)joint;
WorldXmlSerializer.WriteElement("BiasFactor", angleJoint.BiasFactor);
WorldXmlSerializer.WriteElement("MaxImpulse", angleJoint.MaxImpulse);
WorldXmlSerializer.WriteElement("Softness", angleJoint.Softness);
WorldXmlSerializer.WriteElement("TargetAngle", angleJoint.TargetAngle);
break;
}
default:
throw new Exception("Joint not supported");
}
WorldXmlSerializer._writer.WriteEndElement();
}
/// <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>
// TS - public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, FP ratio = 1f)
public GearJoint(Body bodyA, Body bodyB, Joint2D jointA, Joint2D jointB, FP ratio)
{
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);
FP 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;
FP aA = _bodyA._sweep.A;
Transform xfC = _bodyC._xf;
FP aC = _bodyC._sweep.A;
if (_typeA == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointA;
_localAnchorC = revolute.LocalAnchorA;
_localAnchorA = revolute.LocalAnchorB;
_referenceAngleA = revolute.ReferenceAngle;
_localAxisC = TSVector2.zero;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointA;
_localAnchorC = prismatic.LocalAnchorA;
_localAnchorA = prismatic.LocalAnchorB;
_referenceAngleA = prismatic.ReferenceAngle;
_localAxisC = prismatic.LocalXAxis;
TSVector2 pC = _localAnchorC;
TSVector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
coordinateA = TSVector2.Dot(pA - pC, _localAxisC);
}
_bodyD = JointB.BodyA;
_bodyB = JointB.BodyB;
// Get geometry of joint2
Transform xfB = _bodyB._xf;
FP aB = _bodyB._sweep.A;
Transform xfD = _bodyD._xf;
FP aD = _bodyD._sweep.A;
if (_typeB == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointB;
_localAnchorD = revolute.LocalAnchorA;
_localAnchorB = revolute.LocalAnchorB;
_referenceAngleB = revolute.ReferenceAngle;
_localAxisD = TSVector2.zero;
coordinateB = aB - aD - _referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointB;
_localAnchorD = prismatic.LocalAnchorA;
_localAnchorB = prismatic.LocalAnchorB;
_referenceAngleB = prismatic.ReferenceAngle;
_localAxisD = prismatic.LocalXAxis;
TSVector2 pD = _localAnchorD;
TSVector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
coordinateB = TSVector2.Dot(pB - pD, _localAxisD);
}
_ratio = ratio;
_constant = coordinateA + _ratio * coordinateB;
_impulse = 0.0f;
}
