/// <summary>
/// Creates a Wheel Joint and adds it to the world
/// </summary>
/// <param name="world"></param>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="localanchorB"></param>
/// <param name="axis"></param>
/// <returns></returns>
public static FSWheelJoint CreateWheelJoint(FSWorld world, FSBody bodyA, FSBody bodyB, FVector2 localanchorB, FVector2 axis)
{
FSWheelJoint joint = CreateWheelJoint(bodyA, bodyB, localanchorB, axis);
world.AddJoint(joint);
return(joint);
}
/// <summary>
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
/// This requires two ground anchors,
/// two dynamic body anchor points, max lengths for each side,
/// and a pulley ratio.
/// </summary>
/// <param name="bA">The first body.</param>
/// <param name="bB">The second body.</param>
/// <param name="groundA">The ground anchor for the first body.</param>
/// <param name="groundB">The ground anchor for the second body.</param>
/// <param name="anchorA">The first body anchor.</param>
/// <param name="anchorB">The second body anchor.</param>
/// <param name="ratio">The ratio.</param>
public FSPulleyJoint(FSBody bA, FSBody bB, FVector2 groundA, FVector2 groundB, FVector2 anchorA, FVector2 anchorB, float ratio)
: base(bA, bB)
{
JointType = JointType.Pulley;
GroundAnchorA = groundA;
GroundAnchorB = groundB;
LocalAnchorA = anchorA;
LocalAnchorB = anchorB;
Debug.Assert(ratio != 0.0f);
Debug.Assert(ratio > FSSettings.Epsilon);
Ratio = ratio;
FVector2 dA = BodyA.GetWorldPoint(anchorA) - groundA;
LengthA = dA.Length();
FVector2 dB = BodyB.GetWorldPoint(anchorB) - groundB;
LengthB = dB.Length();
m_constant = LengthA + ratio * LengthB;
_impulse = 0.0f;
}
public virtual FSBody GetBodyAtMouse(bool includeStatic)
{
// Make a small box
mousePVec.X = MouseXWorldPhys;
mousePVec.Y = MouseYWorldPhys;
FVector2 lowerBound = new FVector2(MouseXWorldPhys - 0.001f, MouseYWorldPhys - 0.001f);
FVector2 upperBound = new FVector2(MouseXWorldPhys + 0.001f, MouseYWorldPhys + 0.001f);
AABB aabb = new AABB(lowerBound, upperBound);
FSBody body = null;
// Query the world for overlapping shapes
System.Func <FSFixture, bool> GetBodyCallback = delegate(FSFixture fixture0)
{
Shape shape = fixture0.Shape;
if (fixture0.Body.BodyType != BodyType.Static || includeStatic)
{
FarseerPhysics.Common.Transform transform0;
fixture0.Body.GetTransform(out transform0);
bool inside = shape.TestPoint(ref transform0, ref mousePVec);
if (inside)
{
body = fixture0.Body;
return(false);
}
}
return(true);
};
FSWorldComponent.PhysicsWorld.QueryAABB(GetBodyCallback, ref aabb);
return(body);
}
//private List<Contact> _lastContacts;
void Start()
{
_body = GetComponent <FSBodyComponent>().PhysicsBody;
//_lastContacts = new List<Contact>();
_body.OnCollision += OnCollisionEvent;
//var shape = GetComponent<FSShapeComponent>();
}
/// <summary>
/// Duplicates the given Body along the given path for approximatly the given copies.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="path">The path.</param>
/// <param name="shapes">The shapes.</param>
/// <param name="type">The type.</param>
/// <param name="copies">The copies.</param>
/// <param name="userData"></param>
/// <returns></returns>
public static List <FSBody> EvenlyDistributeShapesAlongPath(FSWorld world, Path path, IEnumerable <Shape> shapes,
BodyType type, int copies, object userData)
{
List <FVector3> centers = path.SubdivideEvenly(copies);
List <FSBody> bodyList = new List <FSBody>();
for (int i = 0; i < centers.Count; i++)
{
FSBody b = new FSBody(world);
// copy the type from original body
b.BodyType = type;
b.Position = new FVector2(centers[i].X, centers[i].Y);
b.Rotation = centers[i].Z;
foreach (Shape shape in shapes)
{
b.CreateFixture(shape, userData);
}
bodyList.Add(b);
}
return(bodyList);
}
/// <summary>
/// Creates a revolute joint.
/// </summary>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="localAnchorB">The anchor of bodyB in local coordinates</param>
/// <returns></returns>
public static FSRevoluteJoint CreateRevoluteJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorB)
{
FVector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(localAnchorB));
FSRevoluteJoint joint = new FSRevoluteJoint(bodyA, bodyB, localanchorA, localAnchorB);
return(joint);
}
public FSFrictionJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorA, FVector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Friction;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
}
///// <summary>
///// Creates the fixed revolute joint.
///// </summary>
///// <param name="world">The world.</param>
///// <param name="body">The body.</param>
///// <param name="bodyAnchor">The body anchor.</param>
///// <param name="worldAnchor">The world anchor.</param>
///// <returns></returns>
//public static FixedRevoluteJoint CreateFixedRevoluteJoint(World world, Body body, Vector2 bodyAnchor,
// Vector2 worldAnchor)
//{
// FixedRevoluteJoint fixedRevoluteJoint = new FixedRevoluteJoint(body, bodyAnchor, worldAnchor);
// world.AddJoint(fixedRevoluteJoint);
// return fixedRevoluteJoint;
//}
#endregion
#region Weld Joint
/// <summary>
/// Creates a weld joint
/// </summary>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="worldAnchor">World space coordinates of weld joint</param>
/// <returns></returns>
public static FSWeldJoint CreateWeldJoint(FSBody bodyA, FSBody bodyB, FVector2 worldAnchor)
{
FSWeldJoint joint = new FSWeldJoint(bodyA, bodyB, bodyA.GetLocalPoint(worldAnchor),
bodyB.GetLocalPoint(worldAnchor));
return(joint);
}
/// <summary>
/// Creates a revolute joint and adds it to the world
/// </summary>
/// <param name="world"></param>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="anchor"></param>
/// <returns></returns>
public static FSRevoluteJoint CreateRevoluteJoint(FSWorld world, FSBody bodyA, FSBody bodyB, FVector2 anchor)
{
FSRevoluteJoint joint = CreateRevoluteJoint(bodyA, bodyB, anchor);
world.AddJoint(joint);
return(joint);
}
/// <summary>
/// Creates a weld joint and adds it to the world
/// </summary>
/// <param name="world"></param>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="worldAnchor">World space coordinates of weld joint</param>
/// <returns></returns>
public static FSWeldJoint CreateWeldJoint(FSWorld world, FSBody bodyA, FSBody bodyB, FVector2 worldAnchor)
{
FSWeldJoint joint = CreateWeldJoint(bodyA, bodyB, worldAnchor);
world.AddJoint(joint);
return(joint);
}
public static FSFixedMouseJoint CreateFixedMouseJoint(FSWorld world, FSBody body, FVector2 target)
{
FSFixedMouseJoint joint = new FSFixedMouseJoint(body, target);
world.AddJoint(joint);
return(joint);
}
/// <summary>
/// Constructor for fixed joint
/// </summary>
protected FarseerJoint(FSBody body)
{
BodyA = body;
//Connected bodies should not collide by default
CollideConnected = false;
}
public static FSBody CreateCircle(FSWorld world, float radius, float density, FVector2 position, object userData)
{
FSBody body = CreateBody(world, position);
FixtureFactory.AttachCircle(radius, density, body, userData);
return(body);
}
public static FSBody CreateEdge(FSWorld world, FVector2 start, FVector2 end, object userData)
{
FSBody body = CreateBody(world);
FixtureFactory.AttachEdge(start, end, body, userData);
return(body);
}
public FSFrictionJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorA, FVector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Friction;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
}
/// <summary>
/// Creates a prsimatic joint
/// </summary>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="localanchorB"></param>
/// <param name="axis"></param>
/// <returns></returns>
public static FSPrismaticJoint CreatePrismaticJoint(FSBody bodyA, FSBody bodyB, FVector2 localanchorB, FVector2 axis)
{
FVector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(localanchorB));
FSPrismaticJoint joint = new FSPrismaticJoint(bodyA, bodyB, localanchorA, localanchorB, axis);
return(joint);
}
public static FSBody CreateBody(FSWorld world, FVector2 position, object userData)
{
FSBody body = CreateBody(world, userData);
body.Position = position;
return(body);
}
public virtual void MouseDrag()
{
// mouse press
if (Input.GetMouseButtonDown(0) && mouseJoint == null)
{
FSBody body = GetBodyAtMouse();
if (body != null)
{
FVector2 target = new FVector2(MouseXWorldPhys, MouseYWorldPhys);
mouseJoint = JointFactory.CreateFixedMouseJoint(FSWorldComponent.PhysicsWorld, body, target);
mouseJoint.CollideConnected = true;
mouseJoint.MaxForce = 300f * body.Mass;
body.Awake = true;
}
}
// mouse release
if (Input.GetMouseButtonUp(0))
{
if (mouseJoint != null)
{
FSWorldComponent.PhysicsWorld.RemoveJoint(mouseJoint);
mouseJoint = null;
}
}
// mouse move
if (mouseJoint != null)
{
FVector2 p2 = new FVector2(MouseXWorldPhys, MouseYWorldPhys);
mouseJoint.WorldAnchorB = p2;
}
}
public static FSBody CreateChainShape(FSWorld world, Vertices vertices, FVector2 position,
object userData)
{
FSBody body = CreateBody(world, position);
FixtureFactory.AttachChainShape(vertices, body, userData);
return(body);
}
public static FSBody CreateLineArc(FSWorld world, float radians, int sides, float radius, FVector2 position,
float angle, bool closed)
{
FSBody body = CreateBody(world);
FixtureFactory.AttachLineArc(radians, sides, radius, position, angle, closed, body);
return(body);
}
public static FSBody CreatePolygon(FSWorld world, Vertices vertices, float density, FVector2 position,
object userData)
{
FSBody body = CreateBody(world, position);
FixtureFactory.AttachPolygon(vertices, density, body, userData);
return(body);
}
public static FSBody CreateEllipse(FSWorld world, float xRadius, float yRadius, int edges, float density,
FVector2 position, object userData)
{
FSBody body = CreateBody(world, position);
FixtureFactory.AttachEllipse(xRadius, yRadius, edges, density, body, userData);
return(body);
}
public static FSSliderJoint CreateSliderJoint(FSWorld world, FSBody bodyA, FSBody bodyB, FVector2 anchorA,
FVector2 anchorB, float minLength, float maxLength)
{
FSSliderJoint sliderJoint = new FSSliderJoint(bodyA, bodyB, anchorA, anchorB, minLength, maxLength);
world.AddJoint(sliderJoint);
return(sliderJoint);
}
public static FSWeldJoint CreateWeldJoint(FSWorld world, FSBody bodyA, FSBody bodyB, FVector2 localAnchorA,
FVector2 localAnchorB)
{
FSWeldJoint weldJoint = new FSWeldJoint(bodyA, bodyB, localAnchorA, localAnchorB);
world.AddJoint(weldJoint);
return(weldJoint);
}
public static FSDistanceJoint CreateDistanceJoint(FSWorld world, FSBody bodyA, FSBody bodyB, FVector2 anchorA,
FVector2 anchorB)
{
FSDistanceJoint distanceJoint = new FSDistanceJoint(bodyA, bodyB, anchorA, anchorB);
world.AddJoint(distanceJoint);
return(distanceJoint);
}
//public static FixedDistanceJoint CreateFixedDistanceJoint(World world, Body body, Vector2 localAnchor,
// Vector2 worldAnchor)
//{
// FixedDistanceJoint distanceJoint = new FixedDistanceJoint(body, localAnchor, worldAnchor);
// world.AddJoint(distanceJoint);
// return distanceJoint;
//}
#endregion
#region Friction Joint
public static FSFrictionJoint CreateFrictionJoint(FSWorld world, FSBody bodyA, FSBody bodyB, FVector2 anchorA,
FVector2 anchorB)
{
FSFrictionJoint frictionJoint = new FSFrictionJoint(bodyA, bodyB, anchorA, anchorB);
world.AddJoint(frictionJoint);
return(frictionJoint);
}
/// <summary>
/// Creates an angle joint.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <returns></returns>
public static FSAngleJoint CreateAngleJoint(FSWorld world, FSBody bodyA, FSBody bodyB)
{
FSAngleJoint angleJoint = new FSAngleJoint(bodyA, bodyB);
world.AddJoint(angleJoint);
return(angleJoint);
}
/// <summary>
/// Creates a fixed angle joint.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="body">The body.</param>
/// <returns></returns>
public static FSFixedAngleJoint CreateFixedAngleJoint(FSWorld world, FSBody body)
{
FSFixedAngleJoint angleJoint = new FSFixedAngleJoint(body);
world.AddJoint(angleJoint);
return(angleJoint);
}
public static FSBody CreateSolidArc(FSWorld world, float density, float radians, int sides, float radius,
FVector2 position, float angle)
{
FSBody body = CreateBody(world);
FixtureFactory.AttachSolidArc(density, radians, sides, radius, position, angle, body);
return(body);
}
//TODO: Comment better
/// <summary>
/// Moves the body on the path.
/// </summary>
/// <param name="path">The path.</param>
/// <param name="body">The body.</param>
/// <param name="time">The time.</param>
/// <param name="strength">The strength.</param>
/// <param name="timeStep">The time step.</param>
public static void MoveBodyOnPath(Path path, FSBody body, float time, float strength, float timeStep)
{
FVector2 destination = path.GetPosition(time);
FVector2 positionDelta = body.Position - destination;
FVector2 velocity = (positionDelta / timeStep) * strength;
body.LinearVelocity = -velocity;
}
public FSWheelJoint(FSBody bA, FSBody bB, FVector2 anchor, FVector2 axis)
: base(bA, bB)
{
JointType = JointType.Wheel;
LocalAnchorA = bA.GetLocalPoint(anchor);
LocalAnchorB = bB.GetLocalPoint(anchor);
m_localXAxisA = bA.GetLocalVector(axis);
m_localYAxisA = MathUtils.Cross(1.0f, m_localXAxisA);
}
public FSWheelJoint(FSBody bA, FSBody bB, FVector2 anchor, FVector2 axis)
: base(bA, bB)
{
JointType = JointType.Wheel;
LocalAnchorA = bA.GetLocalPoint(anchor);
LocalAnchorB = bB.GetLocalPoint(anchor);
m_localXAxisA = bA.GetLocalVector(axis);
m_localYAxisA = MathUtils.Cross(1.0f, m_localXAxisA);
}
public FSFixedAngleJoint(FSBody bodyA)
: base(bodyA)
{
JointType = JointType.FixedAngle;
TargetAngle = 0;
BiasFactor = .2f;
Softness = 0f;
MaxImpulse = float.MaxValue;
}
/// <summary>
/// Initializes a new instance of the <see cref="FSSliderJoint"/> class.
/// Warning: Do not use a zero or short length.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second body anchor.</param>
/// <param name="minLength">The minimum length between anchorpoints</param>
/// <param name="maxlength">The maximum length between anchorpoints.</param>
public FSSliderJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorA, FVector2 localAnchorB, float minLength,
float maxlength)
: base(bodyA, bodyB)
{
JointType = JointType.Slider;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
MaxLength = maxlength;
MinLength = minLength;
}
public FSRopeJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorA, FVector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Rope;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
//FPE: Setting default MaxLength
FVector2 d = WorldAnchorB - WorldAnchorA;
MaxLength = d.Length();
}
/// <summary>
/// Initialize the bodies and local anchor.
/// This requires defining an
/// anchor point where the bodies are joined. The definition
/// uses local anchor points so that the initial configuration
/// can violate the constraint slightly. You also need to
/// specify the initial relative angle for joint limits. This
/// helps when saving and loading a game.
/// The local anchor points are measured from the body's origin
/// rather than the center of mass because:
/// 1. you might not know where the center of mass will be.
/// 2. if you add/remove shapes from a body and recompute the mass,
/// the joints will be broken.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second anchor.</param>
public FSRevoluteJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorA, FVector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Revolute;
// Changed to local coordinates.
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
_impulse = FVector3.Zero;
_limitState = LimitState.Inactive;
}
/// <summary>
/// Constructor for fixed joint
/// </summary>
protected FarseerJoint(FSBody body)
{
BodyA = body;
//Connected bodies should not collide by default
CollideConnected = false;
}
protected FarseerJoint(FSBody body, FSBody bodyB)
{
Debug.Assert(body != bodyB);
BodyA = body;
BodyB = bodyB;
//Connected bodies should not collide by default
CollideConnected = false;
}
/// <summary>
/// This requires a world target point,
/// tuning parameters, and the time step.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="worldAnchor">The target.</param>
public FSFixedMouseJoint(FSBody body, FVector2 worldAnchor)
: base(body)
{
JointType = JointType.FixedMouse;
Frequency = 5.0f;
DampingRatio = 0.7f;
MaxForce = 1000 * body.Mass;
Debug.Assert(worldAnchor.IsValid());
_targetA = worldAnchor;
LocalAnchorB = MathUtils.MulT(BodyA.Xf, worldAnchor);
}
/// <summary>
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
/// This requires two ground anchors,
/// two dynamic body anchor points, max lengths for each side,
/// and a pulley ratio.
/// </summary>
/// <param name="bA">The first body.</param>
/// <param name="bB">The second body.</param>
/// <param name="groundA">The ground anchor for the first body.</param>
/// <param name="groundB">The ground anchor for the second body.</param>
/// <param name="anchorA">The first body anchor.</param>
/// <param name="anchorB">The second body anchor.</param>
/// <param name="ratio">The ratio.</param>
public FSPulleyJoint(FSBody bA, FSBody bB, FVector2 groundA, FVector2 groundB, FVector2 anchorA, FVector2 anchorB, float ratio)
: base(bA, bB)
{
JointType = JointType.Pulley;
GroundAnchorA = groundA;
GroundAnchorB = groundB;
LocalAnchorA = anchorA;
LocalAnchorB = anchorB;
Debug.Assert(ratio != 0.0f);
Debug.Assert(ratio > FSSettings.Epsilon);
Ratio = ratio;
FVector2 dA = BodyA.GetWorldPoint(anchorA) - groundA;
LengthA = dA.Length();
FVector2 dB = BodyB.GetWorldPoint(anchorB) - groundB;
LengthB = dB.Length();
m_constant = LengthA + ratio * LengthB;
_impulse = 0.0f;
}
/// <summary>
/// You need to specify a local anchor point
/// where they are attached and the relative body angle. The position
/// of the anchor point is important for computing the reaction torque.
/// You can change the anchor points relative to bodyA or bodyB by changing LocalAnchorA
/// and/or LocalAnchorB.
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second body anchor.</param>
public FSWeldJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorA, FVector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Weld;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
}
/// <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>
public FSGearJoint(FarseerJoint jointA, FarseerJoint jointB, float ratio)
: base(jointA.BodyA, jointA.BodyB)
{
JointType = JointType.Gear;
JointA = jointA;
JointB = jointB;
Ratio = ratio;
m_typeA = jointA.JointType;
m_typeB = jointB.JointType;
// Make sure its the right kind of joint
Debug.Assert(m_typeA == JointType.Revolute || m_typeA == JointType.Prismatic || m_typeA == JointType.FixedRevolute || m_typeA == JointType.FixedPrismatic);
Debug.Assert(m_typeB == JointType.Revolute || m_typeB == JointType.Prismatic || m_typeB == JointType.FixedRevolute || m_typeB == JointType.FixedPrismatic);
float coordinateA = 0.0f, coordinateB = 0.0f;
m_bodyC = JointA.BodyA;
BodyA = JointA.BodyB;
// Get geometry of joint1
Transform xfA = BodyA.Xf;
float aA = BodyA.Sweep.A;
Transform xfC = m_bodyC.Xf;
float aC = m_bodyC.Sweep.A;
if (m_typeA == JointType.Revolute)
{
FSRevoluteJoint revolute = (FSRevoluteJoint)jointA;
m_localAnchorC = revolute.LocalAnchorA;
m_localAnchorA = revolute.LocalAnchorB;
m_referenceAngleA = revolute.ReferenceAngle;
m_localAxisC = FVector2.Zero;
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
FSPrismaticJoint prismatic = (FSPrismaticJoint)jointA;
m_localAnchorC = prismatic.LocalAnchorA;
m_localAnchorA = prismatic.LocalAnchorB;
m_referenceAngleA = prismatic.ReferenceAngle;
m_localAxisC = prismatic.LocalXAxisA;
FVector2 pC = m_localAnchorC;
FVector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = FVector2.Dot(pA - pC, m_localAxisC);
}
m_bodyD = JointB.BodyA;
BodyB = JointB.BodyB;
// Get geometry of joint2
Transform xfB = BodyB.Xf;
float aB = BodyB.Sweep.A;
Transform xfD = m_bodyD.Xf;
float aD = m_bodyD.Sweep.A;
if (m_typeB == JointType.Revolute)
{
FSRevoluteJoint revolute = (FSRevoluteJoint)jointB;
m_localAnchorD = revolute.LocalAnchorA;
m_localAnchorB = revolute.LocalAnchorB;
m_referenceAngleB = revolute.ReferenceAngle;
m_localAxisD = FVector2.Zero;
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
FSPrismaticJoint prismatic = (FSPrismaticJoint)jointB;
m_localAnchorD = prismatic.LocalAnchorA;
m_localAnchorB = prismatic.LocalAnchorB;
m_referenceAngleB = prismatic.ReferenceAngle;
m_localAxisD = prismatic.LocalXAxisA;
FVector2 pD = m_localAnchorD;
FVector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = FVector2.Dot(pB - pD, m_localAxisD);
}
_ratio = ratio;
m_constant = coordinateA + _ratio * coordinateB;
}
/// <summary>
/// This requires defining an
/// anchor point on both bodies and the non-zero length of the
/// distance joint. If you don't supply a length, the local anchor points
/// is used so that the initial configuration can violate the constraint
/// slightly. This helps when saving and loading a game.
/// @warning Do not use a zero or short length.
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
/// <param name="localAnchorA">The first body anchor</param>
/// <param name="localAnchorB">The second body anchor</param>
public FSDistanceJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorA, FVector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Distance;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
FVector2 d = WorldAnchorB - WorldAnchorA;
Length = d.Length();
}
