/// <summary>
/// Adds a distance joint. A distance joint constrains two points on two bodies to remain at a fixed distance from
/// each other. You can view this as a massless, rigid rod.
/// </summary>
/// <param name="manager">The manager.</param>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="anchorA">The anchor on the first body, in world coordinates.</param>
/// <param name="anchorB">The anchor on the second body, in world coordinates.</param>
/// <param name="frequency">The mass-spring-damper frequency in Hertz. A value of 0 disables softness.</param>
/// <param name="dampingRatio">The damping ratio. 0 = no damping, 1 = critical damping.</param>
/// <param name="collideConnected">Whether the two bodies still collide.</param>
/// <returns>The created joint.</returns>
/// <remarks>Do not use a zero or short length.</remarks>
public static DistanceJoint AddDistanceJoint(
this IManager manager,
Body bodyA,
Body bodyB,
WorldPoint anchorA,
WorldPoint anchorB,
float frequency = 0,
float dampingRatio = 0,
bool collideConnected = false)
{
if (bodyA == null)
{
throw new ArgumentNullException("bodyA");
}
if (bodyB == null)
{
throw new ArgumentNullException("bodyB");
}
if (bodyA == bodyB)
{
throw new ArgumentException("Joints must attach to two different bodies.", "bodyA");
}
if (WorldPoint.DistanceSquared(anchorA, anchorB) < Settings.LinearSlop * Settings.LinearSlop)
{
throw new ArgumentException("Points are too close together.");
}
var joint = (DistanceJoint)manager.GetSimulation()
.CreateJoint(Joint.JointType.Distance, bodyA, bodyB, collideConnected);
joint.Initialize(anchorA, anchorB, frequency, dampingRatio);
return(joint);
}
/// <summary>
/// Computes the world manifold data from this manifold with the specified properties for the two involved
/// objects.
/// </summary>
/// <param name="xfA">The transform of object A.</param>
/// <param name="radiusA">The radius of object A.</param>
/// <param name="xfB">The transform of object B.</param>
/// <param name="radiusB">The radius of object B.</param>
/// <param name="normal">The normal.</param>
/// <param name="points">The world contact points.</param>
public void ComputeWorldManifold(
WorldTransform xfA,
float radiusA,
WorldTransform xfB,
float radiusB,
out Vector2 normal,
out FixedArray2 <WorldPoint> points)
{
points = new FixedArray2 <WorldPoint>(); // satisfy out
switch (Type)
{
case ManifoldType.Circles:
{
normal = Vector2.UnitX;
var pointA = xfA.ToGlobal(LocalPoint);
var pointB = xfB.ToGlobal(Points[0].LocalPoint);
if (WorldPoint.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
{
// ReSharper disable RedundantCast Necessary for FarPhysics.
normal = (Vector2)(pointB - pointA);
// ReSharper restore RedundantCast
normal.Normalize();
}
var cA = pointA + radiusA * normal;
var cB = pointB - radiusB * normal;
points.Item1 = 0.5f * (cA + cB);
break;
}
case ManifoldType.FaceA:
{
normal = xfA.Rotation * LocalNormal;
var planePoint = xfA.ToGlobal(LocalPoint);
for (var i = 0; i < PointCount; ++i)
{
var clipPoint = xfB.ToGlobal(Points[i].LocalPoint);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var cA = clipPoint + (radiusA - Vector2Util.Dot((Vector2)(clipPoint - planePoint), normal)) * normal;
// ReSharper restore RedundantCast
var cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
break;
}
case ManifoldType.FaceB:
{
normal = xfB.Rotation * LocalNormal;
var planePoint = xfB.ToGlobal(LocalPoint);
for (var i = 0; i < PointCount; ++i)
{
var clipPoint = xfA.ToGlobal(Points[i].LocalPoint);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var cB = clipPoint + (radiusB - Vector2Util.Dot((Vector2)(clipPoint - planePoint), normal)) * normal;
// ReSharper restore RedundantCast
var cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
break;
}
default:
throw new ArgumentOutOfRangeException();
}
}