/// <summary>Solves the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
{
var vA = velocities[_tmp.IndexA].LinearVelocity;
var wA = velocities[_tmp.IndexA].AngularVelocity;
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
// cDot = dot(u, v + cross(w, r))
var vpA = vA + Vector2Util.Cross(wA, ref _tmp.RotA);
var vpB = vB + Vector2Util.Cross(wB, ref _tmp.RotB);
var cDot = Vector2Util.Dot(_tmp.U, vpB - vpA);
var impulse = -_tmp.Mass * (cDot + _tmp.Bias + _tmp.Gamma * _impulse);
_impulse += impulse;
var p = impulse * _tmp.U;
vA -= _tmp.InverseMassA * p;
wA -= _tmp.InverseInertiaA * Vector2Util.Cross(ref _tmp.RotA, ref p);
vB += _tmp.InverseMassB * p;
wB += _tmp.InverseInertiaB * Vector2Util.Cross(ref _tmp.RotB, ref p);
velocities[_tmp.IndexA].LinearVelocity = vA;
velocities[_tmp.IndexA].AngularVelocity = wA;
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
}
/// <summary>Solves the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
{
var vA = velocities[_tmp.IndexA].LinearVelocity;
var wA = velocities[_tmp.IndexA].AngularVelocity;
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
var vpA = vA + Vector2Util.Cross(wA, ref _tmp.RotA);
var vpB = vB + Vector2Util.Cross(wB, ref _tmp.RotB);
var cdot = -Vector2Util.Dot(ref _tmp.AxisA, ref vpA) - _ratio * Vector2Util.Dot(ref _tmp.AxisB, ref vpB);
var impulse = -_tmp.Mass * cdot;
_impulse += impulse;
var pA = -impulse * _tmp.AxisA;
var pB = -_ratio * impulse * _tmp.AxisB;
vA += _tmp.InverseMassA * pA;
wA += _tmp.InverseInertiaA * Vector2Util.Cross(ref _tmp.RotA, ref pA);
vB += _tmp.InverseMassB * pB;
wB += _tmp.InverseInertiaB * Vector2Util.Cross(ref _tmp.RotB, ref pB);
velocities[_tmp.IndexA].LinearVelocity = vA;
velocities[_tmp.IndexA].AngularVelocity = wA;
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
}
/// <summary>
/// Get the mass data for this fixture. The mass data is based on the density and the shape. The rotational
/// inertia is about the shape's origin. This operation may be expensive.
/// </summary>
/// <param name="mass">The mass of this fixture.</param>
/// <param name="center">The center of mass relative to the local origin.</param>
/// <param name="inertia">The inertia about the local origin.</param>
internal override void GetMassData(out float mass, out LocalPoint center, out float inertia)
{
mass = Density * MathHelper.Pi * Radius * Radius;
center = Center;
// Inertia about the local origin.
inertia = mass * (0.5f * Radius * Radius + Vector2Util.Dot(ref Center, ref Center));
}
/// <summary>This returns true if the position errors are within tolerance, allowing an early exit from the iteration loop.</summary>
/// <param name="positions">The positions of the related bodies.</param>
/// <returns>
/// <c>true</c> if the position errors are within tolerance.
/// </returns>
internal override bool SolvePositionConstraints(Position[] positions)
{
var cA = positions[_tmp.IndexA].Point;
var aA = positions[_tmp.IndexA].Angle;
var cB = positions[_tmp.IndexB].Point;
var aB = positions[_tmp.IndexB].Angle;
var qA = new Rotation(aA);
var qB = new Rotation(aB);
var rA = qA * (_localAnchorA - _tmp.LocalCenterA);
var rB = qB * (_localAnchorB - _tmp.LocalCenterB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var d = (Vector2)(cB - cA) + (rB - rA);
// ReSharper restore RedundantCast
var ay = qA * _localYAxisA;
var sAy = Vector2Util.Cross(d + rA, ay);
var sBy = Vector2Util.Cross(rB, ay);
var c = Vector2Util.Dot(ref d, ref ay);
var k = _tmp.InverseMassA + _tmp.InverseMassB + _tmp.InverseInertiaA * _tmp.SAy * _tmp.SAy +
_tmp.InverseInertiaB * _tmp.SBy * _tmp.SBy;
float impulse;
// ReSharper disable CompareOfFloatsByEqualityOperator Intentional.
if (k != 0.0f)
// ReSharper restore CompareOfFloatsByEqualityOperator
{
impulse = -c / k;
}
else
{
impulse = 0.0f;
}
var p = impulse * ay;
var lA = impulse * sAy;
var lB = impulse * sBy;
cA -= _tmp.InverseMassA * p;
aA -= _tmp.InverseInertiaA * lA;
cB += _tmp.InverseMassB * p;
aB += _tmp.InverseInertiaB * lB;
positions[_tmp.IndexA].Point = cA;
positions[_tmp.IndexA].Angle = aA;
positions[_tmp.IndexB].Point = cB;
positions[_tmp.IndexB].Angle = aB;
return(System.Math.Abs(c) <= Settings.LinearSlop);
}
/// <summary>Test the specified point for containment in this fixture.</summary>
/// <param name="localPoint">The point in local coordinates.</param>
/// <returns>Whether the point is contained in this fixture or not.</returns>
public override bool TestPoint(Vector2 localPoint)
{
for (var i = 0; i < Count; ++i)
{
if (Vector2Util.Dot(Normals[i], localPoint - Vertices[i]) > 0.0f)
{
return(false);
}
}
return(true);
}
/// Clipping for contact manifolds.
private static int ClipSegmentToLine(
out FixedArray2 <ClipVertex> vOut,
FixedArray2 <ClipVertex> vIn,
Vector2 normal,
float offset,
int vertexIndexA)
{
// Satisfy outs.
vOut = new FixedArray2 <ClipVertex>();
// Start with no output points
var numOut = 0;
// Calculate the distance of end points to the line
var distance0 = Vector2Util.Dot(ref normal, ref vIn.Item1.Vertex) - offset;
var distance1 = Vector2Util.Dot(ref normal, ref vIn.Item2.Vertex) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f)
{
vOut.Item1 = vIn.Item1;
++numOut;
if (distance1 <= 0.0f)
{
vOut.Item2 = vIn.Item2;
++numOut;
}
}
else if (distance1 <= 0.0f)
{
vOut.Item1 = vIn.Item2;
++numOut;
}
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
System.Diagnostics.Debug.Assert(numOut == 1);
// Find intersection point of edge and plane
var interp = distance0 / (distance0 - distance1);
vOut.Item2.Vertex = vIn.Item1.Vertex + interp * (vIn.Item2.Vertex - vIn.Item1.Vertex);
// VertexA is hitting edgeB.
vOut.Item2.Id.Feature.IndexA = (byte)vertexIndexA;
vOut.Item2.Id.Feature.IndexB = vIn.Item1.Id.Feature.IndexB;
vOut.Item2.Id.Feature.TypeA = (byte)ContactFeature.FeatureType.Vertex;
vOut.Item2.Id.Feature.TypeB = (byte)ContactFeature.FeatureType.Face;
++numOut;
}
return(numOut);
}
public float Evaluate(int indexA, int indexB, float t)
{
WorldTransform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case Type.Points:
{
var localPointA = _proxyA.Vertices[indexA];
var localPointB = _proxyB.Vertices[indexB];
var pointA = xfA.ToGlobal(localPointA);
var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(Vector2Util.Dot((Vector2)(pointB - pointA), _axis));
// ReSharper restore RedundantCast
}
case Type.FaceA:
{
var normal = xfA.Rotation * _axis;
var pointA = xfA.ToGlobal(_localPoint);
var localPointB = _proxyB.Vertices[indexB];
var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(Vector2Util.Dot((Vector2)(pointB - pointA), normal));
// ReSharper restore RedundantCast
}
case Type.FaceB:
{
var normal = xfB.Rotation * _axis;
var pointB = xfB.ToGlobal(_localPoint);
var localPointA = _proxyA.Vertices[indexA];
var pointA = xfA.ToGlobal(localPointA);
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(Vector2Util.Dot((Vector2)(pointA - pointB), normal));
// ReSharper restore RedundantCast
}
default:
throw new ArgumentOutOfRangeException();
}
}
/// Get the supporting vertex index in the given direction.
public int GetSupport(Vector2 d)
{
var bestIndex = 0;
var bestValue = Vector2Util.Dot(ref Vertices[0], ref d);
for (var i = 1; i < _count; ++i)
{
var value = Vector2Util.Dot(ref Vertices[i], ref d);
if (value > bestValue)
{
bestIndex = i;
bestValue = value;
}
}
return(bestIndex);
}
/// <summary>
/// Returns true if the point is contained by the triangle.
/// </summary>
/// <remarks>
/// <para>
/// The test is inclusive of the triangle edges.
/// </para>
/// </remarks>
/// <param name="p">The point to test.</param>
/// <param name="a">Vertex A of triangle ABC.</param>
/// <param name="b">Vertex B of triangle ABC</param>
/// <param name="c">Vertex C of triangle ABC</param>
/// <returns>True if the point is contained by the triangle ABC.</returns>
public static bool Contains(Vector2 p, Vector2 a, Vector2 b, Vector2 c)
{
Vector2 dirAB = b - a;
Vector2 dirAC = c - a;
Vector2 dirAP = p - a;
float dotABAB = Vector2Util.Dot(dirAB, dirAB);
float dotACAB = Vector2Util.Dot(dirAC, dirAB);
float dotACAC = Vector2Util.Dot(dirAC, dirAC);
float dotACAP = Vector2Util.Dot(dirAC, dirAP);
float dotABAP = Vector2Util.Dot(dirAB, dirAP);
float invDenom = 1 / (dotACAC * dotABAB - dotACAB * dotACAB);
float u = (dotABAB * dotACAP - dotACAB * dotABAP) * invDenom;
float v = (dotACAC * dotABAP - dotACAB * dotACAP) * invDenom;
// Altered this slightly from the reference so that points on the
// vertices and edges are considered to be inside the triangle.
return((u >= 0) && (v >= 0) && (u + v <= 1));
}
// Find the separation between poly1 and poly2 for a give edge normal on poly1.
private static float EdgeSeparation(
int edge1,
PolygonFixture poly1,
WorldTransform xf1,
PolygonFixture poly2,
WorldTransform xf2)
{
var vertices1 = poly1.Vertices;
var normals1 = poly1.Normals;
var count2 = poly2.Count;
var vertices2 = poly2.Vertices;
System.Diagnostics.Debug.Assert(0 <= edge1 && edge1 < poly1.Count);
// Convert normal from poly1's frame into poly2's frame.
var normal1World = xf1.Rotation * normals1[edge1];
var normal1 = -xf2.Rotation * normal1World;
// Find support vertex on poly2 for -normal.
var index = 0;
var minDot = float.MaxValue;
for (var i = 0; i < count2; ++i)
{
var dot = Vector2Util.Dot(ref vertices2[i], ref normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
var v1 = xf1.ToGlobal(vertices1[edge1]);
var v2 = xf2.ToGlobal(vertices2[index]);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var separation = Vector2Util.Dot((Vector2)(v2 - v1), normal1World);
// ReSharper restore RedundantCast
return(separation);
}
public static bool CollideCircles(
Fixture fixtureA,
WorldTransform xfA,
Fixture fixtureB,
WorldTransform xfB,
out Manifold manifold)
{
manifold = new Manifold();
var circleA = fixtureA as CircleFixture;
var circleB = fixtureB as CircleFixture;
System.Diagnostics.Debug.Assert(circleA != null);
System.Diagnostics.Debug.Assert(circleB != null);
var pA = xfA.ToGlobal(circleA.Center);
var pB = xfB.ToGlobal(circleB.Center);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var d = (Vector2)(pB - pA);
// ReSharper restore RedundantCast
var distanceSquared = Vector2Util.Dot(ref d, ref d);
var radius = circleA.Radius + circleB.Radius;
if (distanceSquared > radius * radius)
{
return(false);
}
manifold.Type = Manifold.ManifoldType.Circles;
manifold.LocalPoint = circleA.Center;
manifold.LocalNormal = Vector2.Zero;
manifold.PointCount = 1;
manifold.Points.Item1.LocalPoint = circleB.Center;
manifold.Points.Item1.Id.Key = 0;
return(true);
}
/// <summary>Solves the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
{
var vA = velocities[_tmp.IndexA].LinearVelocity;
var wA = velocities[_tmp.IndexA].AngularVelocity;
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
var vC = velocities[_tmp.IndexC].LinearVelocity;
var wC = velocities[_tmp.IndexC].AngularVelocity;
var vD = velocities[_tmp.IndexD].LinearVelocity;
var wD = velocities[_tmp.IndexD].AngularVelocity;
var cDot = Vector2Util.Dot(_tmp.JvAC, vA - vC) + Vector2Util.Dot(_tmp.JvBD, vB - vD);
cDot += (_tmp.JwA * wA - _tmp.JwC * wC) + (_tmp.JwB * wB - _tmp.JwD * wD);
var impulse = -_tmp.Mass * cDot;
_impulse += impulse;
vA += (_tmp.InverseMassA * impulse) * _tmp.JvAC;
wA += _tmp.InverseInertiaA * impulse * _tmp.JwA;
vB += (_tmp.InverseMassB * impulse) * _tmp.JvBD;
wB += _tmp.InverseInertiaB * impulse * _tmp.JwB;
vC -= (_tmp.InverseMassC * impulse) * _tmp.JvAC;
wC -= _tmp.InverseInertiaC * impulse * _tmp.JwC;
vD -= (_tmp.InverseMassD * impulse) * _tmp.JvBD;
wD -= _tmp.InverseInertiaD * impulse * _tmp.JwD;
velocities[_tmp.IndexA].LinearVelocity = vA;
velocities[_tmp.IndexA].AngularVelocity = wA;
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
velocities[_tmp.IndexC].LinearVelocity = vC;
velocities[_tmp.IndexC].AngularVelocity = wC;
velocities[_tmp.IndexD].LinearVelocity = vD;
velocities[_tmp.IndexD].AngularVelocity = wD;
}
/// <summary>Solves the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
{
var vA = velocities[_tmp.IndexA].LinearVelocity;
var wA = velocities[_tmp.IndexA].AngularVelocity;
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
// Cdot = dot(u, v + cross(w, r))
var vpA = vA + Vector2Util.Cross(wA, ref _tmp.RotA);
var vpB = vB + Vector2Util.Cross(wB, ref _tmp.RotB);
var c = _length - _maxLength;
var cdot = Vector2Util.Dot(_tmp.Axis, vpB - vpA);
// Predictive constraint.
if (c < 0.0f)
{
cdot += step.InverseDeltaT * c;
}
var impulse = -_tmp.Mass * cdot;
var oldImpulse = _impulse;
_impulse = System.Math.Min(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
var p = impulse * _tmp.Axis;
vA -= _tmp.InverseMassA * p;
wA -= _tmp.InverseInertiaA * Vector2Util.Cross(ref _tmp.RotA, ref p);
vB += _tmp.InverseMassB * p;
wB += _tmp.InverseInertiaB * Vector2Util.Cross(ref _tmp.RotB, ref p);
velocities[_tmp.IndexA].LinearVelocity = vA;
velocities[_tmp.IndexA].AngularVelocity = wA;
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
}
// Solve a line segment using barycentric coordinates.
//
// p = a1 * w1 + a2 * w2
// a1 + a2 = 1
//
// The vector from the origin to the closest point on the line is
// perpendicular to the line.
// e12 = w2 - w1
// dot(p, e) = 0
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
//
// 2-by-2 linear system
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
//
// Define
// d12_1 = dot(w2, e12)
// d12_2 = -dot(w1, e12)
// d12 = d12_1 + d12_2
//
// Solution
// a1 = d12_1 / d12
// a2 = d12_2 / d12
public void Solve2()
{
var w1 = Vertices.Item1.VertexDelta;
var w2 = Vertices.Item2.VertexDelta;
var e12 = w2 - w1;
// w1 region
var d12_2 = -Vector2Util.Dot(ref w1, ref e12);
if (d12_2 <= 0.0f)
{
// a2 <= 0, so we clamp it to 0
Vertices.Item1.Alpha = 1.0f;
Count = 1;
return;
}
// w2 region
var d12_1 = Vector2Util.Dot(ref w2, ref e12);
if (d12_1 <= 0.0f)
{
// a1 <= 0, so we clamp it to 0
Vertices.Item2.Alpha = 1.0f;
Count = 1;
Vertices.Item1 = Vertices.Item2;
return;
}
// Must be in e12 region.
var inv_d12 = 1.0f / (d12_1 + d12_2);
Vertices.Item1.Alpha = d12_1 * inv_d12;
Vertices.Item2.Alpha = d12_2 * inv_d12;
Count = 2;
}
/// <summary>This returns true if the position errors are within tolerance, allowing an early exit from the iteration loop.</summary>
/// <param name="positions">The positions of the related bodies.</param>
/// <returns>
/// <c>true</c> if the position errors are within tolerance.
/// </returns>
internal override bool SolvePositionConstraints(Position[] positions)
{
var cA = positions[_tmp.IndexA].Point;
var aA = positions[_tmp.IndexA].Angle;
var cB = positions[_tmp.IndexB].Point;
var aB = positions[_tmp.IndexB].Angle;
var cC = positions[_tmp.IndexC].Point;
var aC = positions[_tmp.IndexC].Angle;
var cD = positions[_tmp.IndexD].Point;
var aD = positions[_tmp.IndexD].Angle;
var qA = new Rotation(aA);
var qB = new Rotation(aB);
var qC = new Rotation(aC);
var qD = new Rotation(aD);
const float linearError = 0.0f;
float coordinateA, coordinateB;
Vector2 JvAC, JvBD;
float JwA, JwB, JwC, JwD;
var mass = 0.0f;
if (_typeA == JointType.Revolute)
{
JvAC = Vector2.Zero;
JwA = 1.0f;
JwC = 1.0f;
mass += _tmp.InverseInertiaA + _tmp.InverseInertiaC;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
var u = qC * _localAxisC;
var rC = qC * (_localAnchorC - _tmp.LcC);
var rA = qA * (_localAnchorA - _tmp.LcA);
JvAC = u;
JwC = Vector2Util.Cross(ref rC, ref u);
JwA = Vector2Util.Cross(ref rA, ref u);
mass += _tmp.InverseMassC + _tmp.InverseMassA + _tmp.InverseInertiaC * JwC * JwC +
_tmp.InverseInertiaA * JwA * JwA;
var pC = _localAnchorC - _tmp.LcC;
// ReSharper disable RedundantCast Necessary for FarPhysics.
var pA = -qC * (rA + (Vector2)(cA - cC));
// ReSharper restore RedundantCast
coordinateA = Vector2Util.Dot(pA - pC, _localAxisC);
}
if (_typeB == JointType.Revolute)
{
JvBD = Vector2.Zero;
JwB = _ratio;
JwD = _ratio;
mass += _ratio * _ratio * (_tmp.InverseInertiaB + _tmp.InverseInertiaD);
coordinateB = aB - aD - _referenceAngleB;
}
else
{
var u = qD * _localAxisD;
var rD = qD * (_localAnchorD - _tmp.LcD);
var rB = qB * (_localAnchorB - _tmp.LcB);
JvBD = _ratio * u;
JwD = _ratio * Vector2Util.Cross(ref rD, ref u);
JwB = _ratio * Vector2Util.Cross(ref rB, ref u);
mass += _ratio * _ratio * (_tmp.InverseMassD + _tmp.InverseMassB) + _tmp.InverseInertiaD * JwD * JwD +
_tmp.InverseInertiaB * JwB * JwB;
var pD = _localAnchorD - _tmp.LcD;
// ReSharper disable RedundantCast Necessary for FarPhysics.
var pB = -qD * (rB + (Vector2)(cB - cD));
// ReSharper restore RedundantCast
coordinateB = Vector2Util.Dot(pB - pD, _localAxisD);
}
var c = (coordinateA + _ratio * coordinateB) - _constant;
var impulse = 0.0f;
if (mass > 0.0f)
{
impulse = -c / mass;
}
cA += _tmp.InverseMassA * impulse * JvAC;
aA += _tmp.InverseInertiaA * impulse * JwA;
cB += _tmp.InverseMassB * impulse * JvBD;
aB += _tmp.InverseInertiaB * impulse * JwB;
cC -= _tmp.InverseMassC * impulse * JvAC;
aC -= _tmp.InverseInertiaC * impulse * JwC;
cD -= _tmp.InverseMassD * impulse * JvBD;
aD -= _tmp.InverseInertiaD * impulse * JwD;
positions[_tmp.IndexA].Point = cA;
positions[_tmp.IndexA].Angle = aA;
positions[_tmp.IndexB].Point = cB;
positions[_tmp.IndexB].Angle = aB;
positions[_tmp.IndexC].Point = cC;
positions[_tmp.IndexC].Angle = aC;
positions[_tmp.IndexD].Point = cD;
positions[_tmp.IndexD].Angle = aD;
// TODO_ERIN not implemented
return(linearError < Settings.LinearSlop);
}
/// <summary>Initializes this joint with the specified parameters.</summary>
internal void Initialize(IManager manager, Joint jointA, Joint jointB, Body bodyA, Body bodyB, float ratio)
{
Manager = manager;
_typeA = jointA.Type;
_typeB = jointB.Type;
System.Diagnostics.Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic);
System.Diagnostics.Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
_bodyIdA = bodyA.Id;
_bodyIdB = bodyB.Id;
_bodyIdC = jointA.BodyA == bodyA ? jointA.BodyB.Id : jointA.BodyA.Id;
_bodyIdD = jointB.BodyA == bodyB ? jointB.BodyB.Id : jointB.BodyA.Id;
// Get geometry of joint1
var xfA = BodyA.Transform;
var aA = BodyA.Sweep.Angle;
var xfC = BodyC.Transform;
var aC = BodyC.Sweep.Angle;
if (_typeA == JointType.Revolute)
{
var revolute = (RevoluteJoint)jointA;
if (bodyA == jointA.BodyA)
{
_localAnchorA = revolute.LocalAnchorA;
_localAnchorC = revolute.LocalAnchorB;
}
else
{
_localAnchorA = revolute.LocalAnchorB;
_localAnchorC = revolute.LocalAnchorA;
}
_referenceAngleA = revolute.ReferenceAngle;
_localAxisC = Vector2.Zero;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
var prismatic = (PrismaticJoint)jointA;
if (bodyA == jointA.BodyA)
{
_localAnchorA = prismatic.LocalAnchorA;
_localAnchorC = prismatic.LocalAnchorB;
}
else
{
_localAnchorA = prismatic.LocalAnchorB;
_localAnchorC = prismatic.LocalAnchorA;
}
_referenceAngleA = prismatic.ReferenceAngle;
_localAxisC = prismatic.LocalAxisA;
var pC = _localAnchorC;
// ReSharper disable RedundantCast Necessary for FarPhysics.
var pA = -xfC.Rotation *
((xfA.Rotation * _localAnchorA) + (Vector2)(xfA.Translation - xfC.Translation));
// ReSharper restore RedundantCast
coordinateA = Vector2Util.Dot(pA - pC, _localAxisC);
}
// Get geometry of joint2
var xfB = BodyB.Transform;
var aB = BodyB.Sweep.Angle;
var xfD = BodyD.Transform;
var aD = BodyD.Sweep.Angle;
if (_typeB == JointType.Revolute)
{
var revolute = (RevoluteJoint)jointB;
if (bodyB == revolute.BodyA)
{
_localAnchorB = revolute.LocalAnchorA;
_localAnchorD = revolute.LocalAnchorB;
}
else
{
_localAnchorB = revolute.LocalAnchorB;
_localAnchorD = revolute.LocalAnchorA;
}
_referenceAngleB = revolute.ReferenceAngle;
_localAxisD = Vector2.Zero;
coordinateB = aB - aD - _referenceAngleB;
}
else
{
var prismatic = (PrismaticJoint)jointB;
if (bodyB == prismatic.BodyA)
{
_localAnchorB = prismatic.LocalAnchorA;
_localAnchorD = prismatic.LocalAnchorB;
}
else
{
_localAnchorB = prismatic.LocalAnchorB;
_localAnchorD = prismatic.LocalAnchorA;
}
_referenceAngleB = prismatic.ReferenceAngle;
_localAxisD = prismatic.LocalAxisA;
var pD = _localAnchorD;
// ReSharper disable RedundantCast Necessary for FarPhysics.
var pB = -xfD.Rotation *
((xfB.Rotation * _localAnchorB) + (Vector2)(xfB.Translation - xfD.Translation));
// ReSharper restore RedundantCast
coordinateB = Vector2Util.Dot(pB - pD, _localAxisD);
}
_ratio = ratio;
_constant = coordinateA + _ratio * coordinateB;
_impulse = 0.0f;
}
public static bool CollidePolygonAndCircle(
Fixture fixtureA,
WorldTransform xfA,
Fixture fixtureB,
WorldTransform xfB,
out Manifold manifold)
{
manifold = new Manifold();
var polygonA = fixtureA as PolygonFixture;
var circleB = fixtureB as CircleFixture;
System.Diagnostics.Debug.Assert(polygonA != null);
System.Diagnostics.Debug.Assert(circleB != null);
// Compute circle position in the frame of the polygon.
var centerInA = xfA.ToLocal(xfB.ToGlobal(circleB.Center));
var totalRadius = polygonA.Radius + circleB.Radius;
var vertexCountA = polygonA.Count;
var verticesA = polygonA.Vertices;
var normalsA = polygonA.Normals;
// Find the min separating edge.
var normalIndexA = 0;
var separation = float.MinValue;
for (var i = 0; i < vertexCountA; ++i)
{
var s = Vector2Util.Dot(normalsA[i], centerInA - verticesA[i]);
if (s > totalRadius)
{
// Early out.
manifold = new Manifold();
return(false);
}
if (s > separation)
{
separation = s;
normalIndexA = i;
}
}
// Vertices that subtend the incident face.
var vertexIndexA1 = normalIndexA;
var vertexIndexA2 = vertexIndexA1 + 1 < vertexCountA ? vertexIndexA1 + 1 : 0;
var vertexA1 = verticesA[vertexIndexA1];
var vertexA2 = verticesA[vertexIndexA2];
// If the center is inside the polygon ...
if (separation < Settings.Epsilon)
{
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.PointCount = 1;
manifold.Points.Item1.LocalPoint = circleB.Center;
manifold.Points.Item1.Id.Key = 0;
manifold.LocalNormal = normalsA[normalIndexA];
manifold.LocalPoint = 0.5f * (vertexA1 + vertexA2);
return(true);
}
// Compute barycentric coordinates
var u1 = Vector2Util.Dot(centerInA - vertexA1, vertexA2 - vertexA1);
var u2 = Vector2Util.Dot(centerInA - vertexA2, vertexA1 - vertexA2);
if (u1 <= 0.0f)
{
if (LocalPoint.DistanceSquared(centerInA, vertexA1) <= totalRadius * totalRadius)
{
var normalInA = centerInA - vertexA1;
normalInA.Normalize();
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.PointCount = 1;
manifold.Points.Item1.LocalPoint = circleB.Center;
manifold.Points.Item1.Id.Key = 0;
manifold.LocalNormal = normalInA;
manifold.LocalPoint = vertexA1;
return(true);
}
}
else if (u2 <= 0.0f)
{
if (LocalPoint.DistanceSquared(centerInA, vertexA2) <= totalRadius * totalRadius)
{
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.PointCount = 1;
manifold.Points.Item1.LocalPoint = circleB.Center;
manifold.Points.Item1.Id.Key = 0;
manifold.LocalNormal = centerInA - vertexA2;
manifold.LocalNormal.Normalize();
manifold.LocalPoint = vertexA2;
return(true);
}
}
else
{
var faceCenter = 0.5f * (vertexA1 + vertexA2);
if (Vector2Util.Dot(centerInA - faceCenter, normalsA[vertexIndexA1]) <= totalRadius)
{
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.PointCount = 1;
manifold.Points.Item1.LocalPoint = circleB.Center;
manifold.Points.Item1.Id.Key = 0;
manifold.LocalNormal = normalsA[vertexIndexA1];
manifold.LocalPoint = faceCenter;
return(true);
}
}
return(false);
}
/// <summary>
/// Get the mass data for this fixture. The mass data is based on the density and the shape. The rotational
/// inertia is about the shape's origin. This operation may be expensive.
/// </summary>
/// <param name="mass">The mass of this fixture.</param>
/// <param name="center">The center of mass relative to the local origin.</param>
/// <param name="inertia">The inertia about the local origin.</param>
internal override void GetMassData(out float mass, out LocalPoint center, out float inertia)
{
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.X = (1/mass) * rho * int(x * dA)
// centroid.Y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
System.Diagnostics.Debug.Assert(Count >= 3);
var centroid = LocalPoint.Zero;
var area = 0f;
var I = 0f;
// s is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
var s = LocalPoint.Zero;
// This code would put the reference point inside the polygon.
for (var i = 0; i < Count; ++i)
{
s += Vertices[i];
}
s /= Count;
const float inv3 = 1.0f / 3.0f;
for (var i = 0; i < Count; ++i)
{
// Triangle vertices.
var e1 = Vertices[i] - s;
var e2 = i + 1 < Count
? Vertices[i + 1] - s
: Vertices[0] - s;
var d = Vector2Util.Cross(ref e1, ref e2);
var triangleArea = 0.5f * d;
area += triangleArea;
// Area weighted centroid
centroid += triangleArea * inv3 * (e1 + e2);
var intx2 = e1.X * e1.X + e2.X * e1.X + e2.X * e2.X;
var inty2 = e1.Y * e1.Y + e2.Y * e1.Y + e2.Y * e2.Y;
I += (0.25f * inv3 * d) * (intx2 + inty2);
}
// Total mass
mass = Density * area;
// Center of mass
System.Diagnostics.Debug.Assert(area > Settings.Epsilon);
centroid *= 1.0f / area;
center = centroid + s;
// Inertia tensor relative to the local origin (point s).
inertia = Density * I;
// Shift to center of mass then to original body origin.
inertia += mass * (Vector2Util.Dot(ref center, ref center) - Vector2Util.Dot(ref centroid, ref centroid));
}
public static void Initialize(
out SeparationFunction f,
SimplexCache cache,
DistanceProxy proxyA,
DistanceProxy proxyB,
Sweep sweepA,
Sweep sweepB,
float t1)
{
System.Diagnostics.Debug.Assert(0 < cache.Count && cache.Count < 3);
f._proxyA = proxyA;
f._proxyB = proxyB;
f._sweepA = sweepA;
f._sweepB = sweepB;
WorldTransform xfA, xfB;
f._sweepA.GetTransform(out xfA, t1);
f._sweepB.GetTransform(out xfB, t1);
if (cache.Count == 1)
{
f._type = Type.Points;
var localPointA = f._proxyA.Vertices[cache.IndexA.Item1];
var localPointB = f._proxyB.Vertices[cache.IndexB.Item1];
var pointA = xfA.ToGlobal(localPointA);
var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
f._axis = (Vector2)(pointB - pointA);
// ReSharper restore RedundantCast
f._axis.Normalize();
f._localPoint = LocalPoint.Zero;
}
else if (cache.IndexA.Item1 == cache.IndexA.Item2)
{
// Two points on B and one on A.
f._type = Type.FaceB;
var localPointB1 = proxyB.Vertices[cache.IndexB.Item1];
var localPointB2 = proxyB.Vertices[cache.IndexB.Item2];
f._axis = Vector2Util.Cross(localPointB2 - localPointB1, 1.0f);
f._axis.Normalize();
var normal = xfB.Rotation * f._axis;
f._localPoint = 0.5f * (localPointB1 + localPointB2);
var pointB = xfB.ToGlobal(f._localPoint);
var localPointA = proxyA.Vertices[cache.IndexA[0]];
var pointA = xfA.ToGlobal(localPointA);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var s = Vector2Util.Dot((Vector2)(pointA - pointB), normal);
// ReSharper restore RedundantCast
if (s < 0.0f)
{
f._axis = -f._axis;
}
}
else
{
// Two points on A and one or two points on B.
f._type = Type.FaceA;
var localPointA1 = f._proxyA.Vertices[cache.IndexA.Item1];
var localPointA2 = f._proxyA.Vertices[cache.IndexA.Item2];
f._axis = Vector2Util.Cross(localPointA2 - localPointA1, 1.0f);
f._axis.Normalize();
var normal = xfA.Rotation * f._axis;
f._localPoint = 0.5f * (localPointA1 + localPointA2);
var pointA = xfA.ToGlobal(f._localPoint);
var localPointB = f._proxyB.Vertices[cache.IndexB[0]];
var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var s = Vector2Util.Dot((Vector2)(pointB - pointA), normal);
// ReSharper restore RedundantCast
if (s < 0.0f)
{
f._axis = -f._axis;
}
}
}
// ReSharper disable RedundantCast Necessary for FarPhysics.
private static void InitializePositionSolverManifold(
ContactPositionConstraint pc,
WorldTransform xfA,
WorldTransform xfB,
int index,
out Vector2 normal,
out WorldPoint point,
out float separation)
{
System.Diagnostics.Debug.Assert(pc.PointCount > 0);
switch (pc.Type)
{
case Manifold.ManifoldType.Circles:
{
var pointA = xfA.ToGlobal(pc.LocalPoint);
var pointB = xfB.ToGlobal(pc.LocalPoints[0]);
normal.X = (float)(pointB.X - pointA.X);
normal.Y = (float)(pointB.Y - pointA.Y);
normal.Normalize();
#if FARMATH
// Avoid multiplication of far values.
point.X = pointA.X + 0.5f * (float)(pointB.X - pointA.X);
point.Y = pointA.Y + 0.5f * (float)(pointB.Y - pointA.Y);
#else
point.X = 0.5f * (pointA.X + pointB.X);
point.Y = 0.5f * (pointA.Y + pointB.Y);
#endif
separation = Vector2Util.Dot((Vector2)(pointB - pointA), normal) - pc.RadiusA - pc.RadiusB;
break;
}
case Manifold.ManifoldType.FaceA:
{
normal.X = xfA.Rotation.Cos * pc.LocalNormal.X - xfA.Rotation.Sin * pc.LocalNormal.Y;
normal.Y = xfA.Rotation.Sin * pc.LocalNormal.X + xfA.Rotation.Cos * pc.LocalNormal.Y;
var planePoint = xfA.ToGlobal(pc.LocalPoint);
point = xfB.ToGlobal(pc.LocalPoints[index]);
separation = Vector2Util.Dot((Vector2)(point - planePoint), normal) - pc.RadiusA - pc.RadiusB;
break;
}
case Manifold.ManifoldType.FaceB:
{
normal.X = xfB.Rotation.Cos * pc.LocalNormal.X - xfB.Rotation.Sin * pc.LocalNormal.Y;
normal.Y = xfB.Rotation.Sin * pc.LocalNormal.X + xfB.Rotation.Cos * pc.LocalNormal.Y;
var planePoint = xfB.ToGlobal(pc.LocalPoint);
point = xfA.ToGlobal(pc.LocalPoints[index]);
separation = Vector2Util.Dot((Vector2)(point - planePoint), normal) - pc.RadiusA - pc.RadiusB;
// Ensure normal points from A to B
normal = -normal;
break;
}
default:
throw new InvalidOperationException();
}
}
/// <summary>Solves the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
{
var mA = _tmp.InverseMassA;
var mB = _tmp.InverseMassB;
var iA = _tmp.InverseInertiaA;
var iB = _tmp.InverseInertiaB;
var vA = velocities[_tmp.IndexA].LinearVelocity;
var wA = velocities[_tmp.IndexA].AngularVelocity;
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
// Solve spring constraint
{
var cdot = Vector2Util.Dot(_tmp.Ax, vB - vA) + _tmp.SBx * wB - _tmp.SAx * wA;
var impulse = -_tmp.SpringMass * (cdot + _tmp.Bias + _tmp.Gamma * _springImpulse);
_springImpulse += impulse;
var p = impulse * _tmp.Ax;
var lA = impulse * _tmp.SAx;
var lB = impulse * _tmp.SBx;
vA -= mA * p;
wA -= iA * lA;
vB += mB * p;
wB += iB * lB;
}
// Solve rotational motor constraint
{
var cdot = wB - wA - _motorSpeed;
var impulse = -_tmp.MotorMass * cdot;
var oldImpulse = _motorImpulse;
var maxImpulse = step.DeltaT * _maxMotorTorque;
_motorImpulse = MathHelper.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve point to line constraint
{
var cdot = Vector2Util.Dot(_tmp.Ay, vB - vA) + _tmp.SBy * wB - _tmp.SAy * wA;
var impulse = -_tmp.Mass * cdot;
_impulse += impulse;
var p = impulse * _tmp.Ay;
var lA = impulse * _tmp.SAy;
var lB = impulse * _tmp.SBy;
vA -= mA * p;
wA -= iA * lA;
vB += mB * p;
wB += iB * lB;
}
velocities[_tmp.IndexA].LinearVelocity = vA;
velocities[_tmp.IndexA].AngularVelocity = wA;
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
}
/// <summary>Solves the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
{
var vA = velocities[_tmp.IndexA].LinearVelocity;
var wA = velocities[_tmp.IndexA].AngularVelocity;
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
var mA = _tmp.InverseMassA;
var mB = _tmp.InverseMassB;
var iA = _tmp.InverseInertiaA;
var iB = _tmp.InverseInertiaB;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
var cdot = Vector2Util.Dot(_tmp.Axis, vB - vA) + _tmp.A2 * wB - _tmp.A1 * wA;
var impulse = _tmp.MotorMass * (_motorSpeed - cdot);
var oldImpulse = _motorImpulse;
var maxImpulse = step.DeltaT * _maxMotorForce;
_motorImpulse = MathHelper.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
var p = impulse * _tmp.Axis;
var lA = impulse * _tmp.A1;
var lB = impulse * _tmp.A2;
vA -= mA * p;
wA -= iA * lA;
vB += mB * p;
wB += iB * lB;
}
Vector2 cdot1;
cdot1.X = Vector2Util.Dot(_tmp.Perp, vB - vA) + _tmp.S2 * wB - _tmp.S1 * wA;
cdot1.Y = wB - wA;
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
var cdot2 = Vector2Util.Dot(_tmp.Axis, vB - vA) + _tmp.A2 * wB - _tmp.A1 * wA;
var cdot = new Vector3(cdot1.X, cdot1.Y, cdot2);
var f1 = _impulse;
var df = _tmp.K.Solve33(-cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Z = System.Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Z = System.Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
var b = -cdot1 - (_impulse.Z - f1.Z) * new Vector2(_tmp.K.Column3.X, _tmp.K.Column3.Y);
var fToR = _tmp.K.Solve22(b) + new Vector2(f1.X, f1.Y);
_impulse.X = fToR.X;
_impulse.Y = fToR.Y;
df = _impulse - f1;
var p = df.X * _tmp.Perp + df.Z * _tmp.Axis;
var lA = df.X * _tmp.S1 + df.Y + df.Z * _tmp.A1;
var lB = df.X * _tmp.S2 + df.Y + df.Z * _tmp.A2;
vA -= mA * p;
wA -= iA * lA;
vB += mB * p;
wB += iB * lB;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
var df = _tmp.K.Solve22(-cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
var p = df.X * _tmp.Perp;
var lA = df.X * _tmp.S1 + df.Y;
var lB = df.X * _tmp.S2 + df.Y;
vA -= mA * p;
wA -= iA * lA;
vB += mB * p;
wB += iB * lB;
}
velocities[_tmp.IndexA].LinearVelocity = vA;
velocities[_tmp.IndexA].AngularVelocity = wA;
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
}
// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
public static bool CollideEdgeAndCircle(
Fixture fixtureA,
WorldTransform xfA,
Fixture fixtureB,
WorldTransform xfB,
out Manifold manifold)
{
manifold = new Manifold();
var edgeA = fixtureA as EdgeFixture;
var circleB = fixtureB as CircleFixture;
System.Diagnostics.Debug.Assert(edgeA != null);
System.Diagnostics.Debug.Assert(circleB != null);
// Compute circle in frame of edge
var q = xfA.ToLocal(xfB.ToGlobal(circleB.Center));
var a = edgeA.Vertex1;
var b = edgeA.Vertex2;
var e = b - a;
// Barycentric coordinates
var u = Vector2Util.Dot(e, b - q);
var v = Vector2Util.Dot(e, q - a);
var radius = edgeA.Radius + circleB.Radius;
ContactFeature cf;
cf.IndexB = 0;
cf.TypeB = (byte)ContactFeature.FeatureType.Vertex;
// Region A
if (v <= 0.0f)
{
var p = a;
var d = q - p;
var dd = Vector2Util.Dot(ref d, ref d);
if (dd > radius * radius)
{
return(false);
}
// Is there an edge connected to A?
if (edgeA.HasVertex0)
{
var a1 = edgeA.Vertex0;
var b1 = a;
var e1 = b1 - a1;
var u1 = Vector2Util.Dot(e1, b1 - q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return(false);
}
}
cf.IndexA = 0;
cf.TypeA = (byte)ContactFeature.FeatureType.Vertex;
manifold.Type = Manifold.ManifoldType.Circles;
manifold.LocalPoint = p;
manifold.LocalNormal = Vector2.Zero;
manifold.PointCount = 1;
manifold.Points.Item1.Id.Key = 0;
manifold.Points.Item1.Id.Feature = cf;
manifold.Points.Item1.LocalPoint = circleB.Center;
return(true);
}
// Region B
if (u <= 0.0f)
{
var p = b;
var d = q - p;
var dd = Vector2Util.Dot(ref d, ref d);
if (dd > radius * radius)
{
return(false);
}
// Is there an edge connected to B?
if (edgeA.HasVertex3)
{
var a2 = b;
var b2 = edgeA.Vertex3;
var e2 = b2 - a2;
var v2 = Vector2Util.Dot(e2, q - a2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return(false);
}
}
cf.IndexA = 1;
cf.TypeA = (byte)ContactFeature.FeatureType.Vertex;
manifold.Type = Manifold.ManifoldType.Circles;
manifold.LocalPoint = p;
manifold.LocalNormal = Vector2.Zero;
manifold.PointCount = 1;
manifold.Points.Item1.Id.Key = 0;
manifold.Points.Item1.Id.Feature = cf;
manifold.Points.Item1.LocalPoint = circleB.Center;
return(true);
}
// Region AB
var den = Vector2Util.Dot(ref e, ref e);
System.Diagnostics.Debug.Assert(den > 0.0f);
{
var p = (1.0f / den) * (u * a + v * b);
var d = q - p;
var dd = Vector2Util.Dot(ref d, ref d);
if (dd > radius * radius)
{
return(false);
}
}
Vector2 n;
n.X = -e.Y;
n.Y = e.X;
if (Vector2Util.Dot(n, q - a) < 0.0f)
{
n = -n;
}
n.Normalize();
cf.IndexA = 0;
cf.TypeA = (byte)ContactFeature.FeatureType.Face;
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.LocalPoint = a;
manifold.LocalNormal = n;
manifold.PointCount = 1;
manifold.Points.Item1.Id.Key = 0;
manifold.Points.Item1.Id.Feature = cf;
manifold.Points.Item1.LocalPoint = circleB.Center;
return(true);
}
/// <summary>
/// This resets the mass properties to the sum of the mass properties of the fixtures. This normally does not need
/// to be called unless you called SetMassData to override the mass and you later want to reset the mass.
/// </summary>
public void ResetMassData()
{
// Compute mass data from shapes. Each shape has its own density.
MassInternal = 0.0f;
InverseMass = 0.0f;
_inertia = 0.0f;
InverseInertia = 0.0f;
Sweep.LocalCenter = Vector2.Zero;
// Static and kinematic bodies have zero mass.
if (TypeInternal == BodyType.Static || TypeInternal == BodyType.Kinematic)
{
Sweep.CenterOfMass0 = Transform.Translation;
Sweep.CenterOfMass = Transform.Translation;
Sweep.Angle0 = Sweep.Angle;
return;
}
System.Diagnostics.Debug.Assert(TypeInternal == BodyType.Dynamic);
// Accumulate mass over all fixtures.
var localCenter = Vector2.Zero;
foreach (Fixture fixture in Fixtures)
{
// ReSharper disable CompareOfFloatsByEqualityOperator Intentional.
if (fixture.Density == 0)
// ReSharper restore CompareOfFloatsByEqualityOperator
{
continue;
}
float mass, inertia;
Vector2 center;
fixture.GetMassData(out mass, out center, out inertia);
MassInternal += mass;
localCenter += mass * center;
_inertia += inertia;
}
// Compute center of mass.
if (MassInternal <= 0)
{
// Force all dynamic bodies to have a positive mass.
MassInternal = 1;
InverseMass = 1;
}
else
{
InverseMass = 1 / MassInternal;
localCenter *= InverseMass;
}
if (_isRotationFixed || _inertia <= 0)
{
_inertia = 0;
InverseInertia = 0;
}
else
{
// Center the inertia about the center of mass.
_inertia -= MassInternal * Vector2Util.Dot(ref localCenter, ref localCenter);
InverseInertia = 1 / _inertia;
}
// Move center of mass.
var oldCenter = Sweep.CenterOfMass;
Sweep.LocalCenter = localCenter;
Sweep.CenterOfMass0 = Sweep.CenterOfMass = Transform.ToGlobal(Sweep.LocalCenter);
// Update center of mass velocity.
// ReSharper disable RedundantCast Necessary for FarPhysics.
LinearVelocityInternal += Vector2Util.Cross(
AngularVelocityInternal, (Vector2)(Sweep.CenterOfMass - oldCenter));
// ReSharper restore RedundantCast
}
/// <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();
}
}
// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
public void Solve3()
{
var w1 = Vertices.Item1.VertexDelta;
var w2 = Vertices.Item2.VertexDelta;
var w3 = Vertices.Item3.VertexDelta;
// Edge12
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
// a3 = 0
var e12 = w2 - w1;
var w1e12 = Vector2Util.Dot(ref w1, ref e12);
var w2e12 = Vector2Util.Dot(ref w2, ref e12);
var d12_1 = w2e12;
var d12_2 = -w1e12;
// Edge13
// [1 1 ][a1] = [1]
// [w1.e13 w3.e13][a3] = [0]
// a2 = 0
var e13 = w3 - w1;
var w1e13 = Vector2Util.Dot(ref w1, ref e13);
var w3e13 = Vector2Util.Dot(ref w3, ref e13);
var d13_1 = w3e13;
var d13_2 = -w1e13;
// Edge23
// [1 1 ][a2] = [1]
// [w2.e23 w3.e23][a3] = [0]
// a1 = 0
var e23 = w3 - w2;
var w2e23 = Vector2Util.Dot(ref w2, ref e23);
var w3e23 = Vector2Util.Dot(ref w3, ref e23);
var d23_1 = w3e23;
var d23_2 = -w2e23;
// Triangle123
var n123 = Vector2Util.Cross(ref e12, ref e13);
var d123_1 = n123 * Vector2Util.Cross(ref w2, ref w3);
var d123_2 = n123 * Vector2Util.Cross(ref w3, ref w1);
var d123_3 = n123 * Vector2Util.Cross(ref w1, ref w2);
// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
{
Vertices.Item1.Alpha = 1.0f;
Count = 1;
return;
}
// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
{
var inv_d12 = 1.0f / (d12_1 + d12_2);
Vertices.Item1.Alpha = d12_1 * inv_d12;
Vertices.Item2.Alpha = d12_2 * inv_d12;
Count = 2;
return;
}
// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
{
var inv_d13 = 1.0f / (d13_1 + d13_2);
Vertices.Item1.Alpha = d13_1 * inv_d13;
Vertices.Item3.Alpha = d13_2 * inv_d13;
Count = 2;
Vertices.Item2 = Vertices.Item3;
return;
}
// w2 region
if (d12_1 <= 0.0f && d23_2 <= 0.0f)
{
Vertices.Item2.Alpha = 1.0f;
Count = 1;
Vertices.Item1 = Vertices.Item2;
return;
}
// w3 region
if (d13_1 <= 0.0f && d23_1 <= 0.0f)
{
Vertices.Item3.Alpha = 1.0f;
Count = 1;
Vertices.Item1 = Vertices.Item3;
return;
}
// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
{
var inv_d23 = 1.0f / (d23_1 + d23_2);
Vertices.Item2.Alpha = d23_1 * inv_d23;
Vertices.Item3.Alpha = d23_2 * inv_d23;
Count = 2;
Vertices.Item1 = Vertices.Item3;
return;
}
// Must be in triangle123
var inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
Vertices.Item1.Alpha = d123_1 * inv_d123;
Vertices.Item2.Alpha = d123_2 * inv_d123;
Vertices.Item3.Alpha = d123_3 * inv_d123;
Count = 3;
}
// Linear constraint (point-to-line)
// d = p2 - p1 = x2 + r2 - x1 - r1
// C = dot(perp, d)
// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
//
// Angular constraint
// C = a2 - a1 + a_initial
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
//
// K = J * invM * JT
//
// J = [-a -s1 a s2]
// [0 -1 0 1]
// a = perp
// s1 = cross(d + r1, a) = cross(p2 - x1, a)
// s2 = cross(r2, a) = cross(p2 - x2, a)
// Motor/Limit linear constraint
// C = dot(ax1, d)
// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
// Block Solver
// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
// when the mass has poor distribution (leading to large torques about the joint anchor points).
//
// The Jacobian has 3 rows:
// J = [-uT -s1 uT s2] // linear
// [0 -1 0 1] // angular
// [-vT -a1 vT a2] // limit
//
// u = perp
// v = axis
// s1 = cross(d + r1, u), s2 = cross(r2, u)
// a1 = cross(d + r1, v), a2 = cross(r2, v)
// M * (v2 - v1) = JT * df
// J * v2 = bias
//
// v2 = v1 + invM * JT * df
// J * (v1 + invM * JT * df) = bias
// K * df = bias - J * v1 = -Cdot
// K = J * invM * JT
// Cdot = J * v1 - bias
//
// Now solve for f2.
// df = f2 - f1
// K * (f2 - f1) = -Cdot
// f2 = invK * (-Cdot) + f1
//
// Clamp accumulated limit impulse.
// lower: f2(3) = max(f2(3), 0)
// upper: f2(3) = min(f2(3), 0)
//
// Solve for correct f2(1:2)
// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
//
// Now compute impulse to be applied:
// df = f2 - f1
/// <summary>Initializes the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="positions">The positions of the related bodies.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void InitializeVelocityConstraints(TimeStep step, Position[] positions, Velocity[] velocities)
{
_tmp.IndexA = BodyA.IslandIndex;
_tmp.IndexB = BodyB.IslandIndex;
_tmp.LocalCenterA = BodyA.Sweep.LocalCenter;
_tmp.LocalCenterB = BodyB.Sweep.LocalCenter;
_tmp.InverseMassA = BodyA.InverseMass;
_tmp.InverseMassB = BodyB.InverseMass;
_tmp.InverseInertiaA = BodyA.InverseInertia;
_tmp.InverseInertiaB = BodyB.InverseInertia;
var cA = positions[_tmp.IndexA].Point;
var aA = positions[_tmp.IndexA].Angle;
var vA = velocities[_tmp.IndexA].LinearVelocity;
var wA = velocities[_tmp.IndexA].AngularVelocity;
var cB = positions[_tmp.IndexB].Point;
var aB = positions[_tmp.IndexB].Angle;
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
var qA = new Rotation(aA);
var qB = new Rotation(aB);
// Compute the effective masses.
var rA = qA * (_localAnchorA - _tmp.LocalCenterA);
var rB = qB * (_localAnchorB - _tmp.LocalCenterB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var d = (Vector2)(cB - cA) + (rB - rA);
// ReSharper restore RedundantCast
var mA = _tmp.InverseMassA;
var mB = _tmp.InverseMassB;
var iA = _tmp.InverseInertiaA;
var iB = _tmp.InverseInertiaB;
// Compute motor Jacobian and effective mass.
{
_tmp.Axis = qA * _localXAxisA;
_tmp.A1 = Vector2Util.Cross(d + rA, _tmp.Axis);
_tmp.A2 = Vector2Util.Cross(rB, _tmp.Axis);
_tmp.MotorMass = mA + mB + iA * _tmp.A1 * _tmp.A1 + iB * _tmp.A2 * _tmp.A2;
if (_tmp.MotorMass > 0.0f)
{
_tmp.MotorMass = 1.0f / _tmp.MotorMass;
}
}
// Prismatic constraint.
{
_tmp.Perp = qA * _localYAxisA;
_tmp.S1 = Vector2Util.Cross(d + rA, _tmp.Perp);
_tmp.S2 = Vector2Util.Cross(rB, _tmp.Perp);
var k11 = mA + mB + iA * _tmp.S1 * _tmp.S1 + iB * _tmp.S2 * _tmp.S2;
var k12 = iA * _tmp.S1 + iB * _tmp.S2;
var k13 = iA * _tmp.S1 * _tmp.A1 + iB * _tmp.S2 * _tmp.A2;
var k22 = iA + iB;
// ReSharper disable CompareOfFloatsByEqualityOperator Intentional.
if (k22 == 0.0f)
// ReSharper restore CompareOfFloatsByEqualityOperator
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
var k23 = iA * _tmp.A1 + iB * _tmp.A2;
var k33 = mA + mB + iA * _tmp.A1 * _tmp.A1 + iB * _tmp.A2 * _tmp.A2;
Vector3Util.Set(out _tmp.K.Column1, k11, k12, k13);
Vector3Util.Set(out _tmp.K.Column2, k12, k22, k23);
Vector3Util.Set(out _tmp.K.Column3, k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
var jointTranslation = Vector2Util.Dot(ref _tmp.Axis, ref d);
if (System.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
var p = _impulse.X * _tmp.Perp + (_motorImpulse + _impulse.Z) * _tmp.Axis;
var lA = _impulse.X * _tmp.S1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _tmp.A1;
var lB = _impulse.X * _tmp.S2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _tmp.A2;
vA -= mA * p;
wA -= iA * lA;
vB += mB * p;
wB += iB * lB;
velocities[_tmp.IndexA].LinearVelocity = vA;
velocities[_tmp.IndexA].AngularVelocity = wA;
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
}
// Linear constraint (point-to-line)
// d = pB - pA = xB + rB - xA - rA
// C = dot(ay, d)
// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
// Spring linear constraint
// C = dot(ax, d)
// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
// Motor rotational constraint
// Cdot = wB - wA
// J = [0 0 -1 0 0 1]
/// <summary>Initializes the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="positions">The positions of the related bodies.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void InitializeVelocityConstraints(TimeStep step, Position[] positions, Velocity[] velocities)
{
_tmp.IndexA = BodyA.IslandIndex;
_tmp.IndexB = BodyB.IslandIndex;
_tmp.LocalCenterA = BodyA.Sweep.LocalCenter;
_tmp.LocalCenterB = BodyB.Sweep.LocalCenter;
_tmp.InverseMassA = BodyA.InverseMass;
_tmp.InverseMassB = BodyB.InverseMass;
_tmp.InverseInertiaA = BodyA.InverseInertia;
_tmp.InverseInertiaB = BodyB.InverseInertia;
var mA = _tmp.InverseMassA;
var mB = _tmp.InverseMassB;
var iA = _tmp.InverseInertiaA;
var iB = _tmp.InverseInertiaB;
var cA = positions[_tmp.IndexA].Point;
var aA = positions[_tmp.IndexA].Angle;
var vA = velocities[_tmp.IndexA].LinearVelocity;
var wA = velocities[_tmp.IndexA].AngularVelocity;
var cB = positions[_tmp.IndexB].Point;
var aB = positions[_tmp.IndexB].Angle;
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
var qA = new Rotation(aA);
var qB = new Rotation(aB);
// Compute the effective masses.
var rA = qA * (_localAnchorA - _tmp.LocalCenterA);
var rB = qB * (_localAnchorB - _tmp.LocalCenterB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var d = (Vector2)(cB - cA) + (rB - rA);
// ReSharper restore RedundantCast
// Point to line constraint
{
_tmp.Ay = qA * _localYAxisA;
_tmp.SAy = Vector2Util.Cross(d + rA, _tmp.Ay);
_tmp.SBy = Vector2Util.Cross(rB, _tmp.Ay);
_tmp.Mass = mA + mB + iA * _tmp.SAy * _tmp.SAy + iB * _tmp.SBy * _tmp.SBy;
if (_tmp.Mass > 0.0f)
{
_tmp.Mass = 1.0f / _tmp.Mass;
}
}
// Spring constraint
_tmp.SpringMass = 0.0f;
_tmp.Bias = 0.0f;
_tmp.Gamma = 0.0f;
if (_frequency > 0.0f)
{
_tmp.Ax = qA * _localXAxisA;
_tmp.SAx = Vector2Util.Cross(d + rA, _tmp.Ax);
_tmp.SBx = Vector2Util.Cross(rB, _tmp.Ax);
var invMass = mA + mB + iA * _tmp.SAx * _tmp.SAx + iB * _tmp.SBx * _tmp.SBx;
if (invMass > 0.0f)
{
_tmp.SpringMass = 1.0f / invMass;
var c = Vector2Util.Dot(ref d, ref _tmp.Ax);
// Frequency
var omega = 2.0f * MathHelper.Pi * _frequency;
// Damping coefficient
var dc = 2.0f * _tmp.SpringMass * _dampingRatio * omega;
// Spring stiffness
var k = _tmp.SpringMass * omega * omega;
// magic formulas
var h = step.DeltaT;
_tmp.Gamma = h * (dc + h * k);
if (_tmp.Gamma > 0.0f)
{
_tmp.Gamma = 1.0f / _tmp.Gamma;
}
_tmp.Bias = c * h * k * _tmp.Gamma;
_tmp.SpringMass = invMass + _tmp.Gamma;
if (_tmp.SpringMass > 0.0f)
{
_tmp.SpringMass = 1.0f / _tmp.SpringMass;
}
}
}
else
{
_springImpulse = 0.0f;
}
// Rotational motor
if (_enableMotor)
{
_tmp.MotorMass = iA + iB;
if (_tmp.MotorMass > 0.0f)
{
_tmp.MotorMass = 1.0f / _tmp.MotorMass;
}
}
else
{
_tmp.MotorMass = 0.0f;
_motorImpulse = 0.0f;
}
var p = _impulse * _tmp.Ay + _springImpulse * _tmp.Ax;
var lA = _impulse * _tmp.SAy + _springImpulse * _tmp.SAx + _motorImpulse;
var lB = _impulse * _tmp.SBy + _springImpulse * _tmp.SBx + _motorImpulse;
vA -= _tmp.InverseMassA * p;
wA -= _tmp.InverseInertiaA * lA;
vB += _tmp.InverseMassB * p;
wB += _tmp.InverseInertiaB * lB;
velocities[_tmp.IndexA].LinearVelocity = vA;
velocities[_tmp.IndexA].AngularVelocity = wA;
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
}
/// <summary>This returns true if the position errors are within tolerance, allowing an early exit from the iteration loop.</summary>
/// <param name="positions">The positions of the related bodies.</param>
/// <returns>
/// <c>true</c> if the position errors are within tolerance.
/// </returns>
internal override bool SolvePositionConstraints(Position[] positions)
{
var cA = positions[_tmp.IndexA].Point;
var aA = positions[_tmp.IndexA].Angle;
var cB = positions[_tmp.IndexB].Point;
var aB = positions[_tmp.IndexB].Angle;
var qA = new Rotation(aA);
var qB = new Rotation(aB);
var mA = _tmp.InverseMassA;
var mB = _tmp.InverseMassB;
var iA = _tmp.InverseInertiaA;
var iB = _tmp.InverseInertiaB;
// Compute fresh Jacobians
var rA = qA * (_localAnchorA - _tmp.LocalCenterA);
var rB = qB * (_localAnchorB - _tmp.LocalCenterB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var d = (Vector2)(cB - cA) + (rB - rA);
// ReSharper restore RedundantCast
var axis = qA * _localXAxisA;
var a1 = Vector2Util.Cross(d + rA, axis);
var a2 = Vector2Util.Cross(rB, axis);
var perp = qA * _localYAxisA;
var s1 = Vector2Util.Cross(d + rA, perp);
var s2 = Vector2Util.Cross(rB, perp);
Vector3 impulse;
Vector2 c1;
c1.X = Vector2Util.Dot(ref perp, ref d);
c1.Y = aB - aA - _referenceAngle;
var linearError = System.Math.Abs(c1.X);
var angularError = System.Math.Abs(c1.Y);
var active = false;
var c2 = 0.0f;
if (_enableLimit)
{
var translation = Vector2Util.Dot(ref axis, ref d);
if (System.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
c2 = MathHelper.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = System.Math.Max(linearError, System.Math.Abs(translation));
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
c2 = MathHelper.Clamp(
translation - _lowerTranslation + Settings.LinearSlop,
-Settings.MaxLinearCorrection,
0.0f);
linearError = System.Math.Max(linearError, _lowerTranslation - translation);
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
c2 = MathHelper.Clamp(
translation - _upperTranslation - Settings.LinearSlop,
0.0f,
Settings.MaxLinearCorrection);
linearError = System.Math.Max(linearError, translation - _upperTranslation);
active = true;
}
}
if (active)
{
var k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
var k12 = iA * s1 + iB * s2;
var k13 = iA * s1 * a1 + iB * s2 * a2;
var k22 = iA + iB;
// ReSharper disable CompareOfFloatsByEqualityOperator Intentional.
if (k22 == 0.0f)
// ReSharper restore CompareOfFloatsByEqualityOperator
{
// For fixed rotation
k22 = 1.0f;
}
var k23 = iA * a1 + iB * a2;
var k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Matrix33 k;
Vector3Util.Set(out k.Column1, k11, k12, k13);
Vector3Util.Set(out k.Column2, k12, k22, k23);
Vector3Util.Set(out k.Column3, k13, k23, k33);
Vector3 c;
c.X = c1.X;
c.Y = c1.Y;
c.Z = c2;
impulse = k.Solve33(-c);
}
else
{
var k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
var k12 = iA * s1 + iB * s2;
var k22 = iA + iB;
// ReSharper disable CompareOfFloatsByEqualityOperator Intentional.
if (k22 == 0.0f)
// ReSharper restore CompareOfFloatsByEqualityOperator
{
k22 = 1.0f;
}
Matrix22 k;
Vector2Util.Set(out k.Column1, k11, k12);
Vector2Util.Set(out k.Column2, k12, k22);
var impulse1 = k.Solve(-c1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
var p = impulse.X * perp + impulse.Z * axis;
var lA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
var lB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
cA -= mA * p;
aA -= iA * lA;
cB += mB * p;
aB += iB * lB;
positions[_tmp.IndexA].Point = cA;
positions[_tmp.IndexA].Angle = aA;
positions[_tmp.IndexB].Point = cB;
positions[_tmp.IndexB].Angle = aB;
return(linearError <= (Settings.LinearSlop + Settings.Epsilon) && angularError <= (Settings.AngularSlop + Settings.Epsilon));
}
// This function collides and edge and a polygon.
// This takes into account edge adjacency.
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
public static bool CollideEdgeAndPolygon(
Fixture fixtureA,
WorldTransform xfA,
Fixture fixtureB,
WorldTransform xfB,
out Manifold manifold)
{
manifold = new Manifold();
var edgeA = fixtureA as EdgeFixture;
var polygonB = fixtureB as PolygonFixture;
System.Diagnostics.Debug.Assert(edgeA != null);
System.Diagnostics.Debug.Assert(polygonB != null);
// This holds polygon B expressed in frame A.
var tpv = new Vector2[Settings.MaxPolygonVertices];
var tpn = new Vector2[Settings.MaxPolygonVertices];
Vector2 normal0 = Vector2.Zero, normal1, normal2 = Vector2.Zero;
var xf = xfA.MulT(xfB);
var centroidB = xf.ToOther(polygonB.Centroid);
var v0 = edgeA.Vertex0;
var v1 = edgeA.Vertex1;
var v2 = edgeA.Vertex2;
var v3 = edgeA.Vertex3;
var hasVertex0 = edgeA.HasVertex0;
var hasVertex3 = edgeA.HasVertex3;
var edge1 = v2 - v1;
edge1.Normalize();
normal1.X = edge1.Y;
normal1.Y = -edge1.X;
var offset1 = Vector2Util.Dot(normal1, centroidB - v1);
var offset0 = 0.0f;
var offset2 = 0.0f;
var convex1 = false;
var convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
var edge0 = v1 - v0;
edge0.Normalize();
normal0.X = edge0.Y;
normal0.Y = -edge0.X;
convex1 = Vector2Util.Cross(ref edge0, ref edge1) >= 0.0f;
offset0 = Vector2Util.Dot(normal0, centroidB - v0);
}
// Is there a following edge?
if (hasVertex3)
{
var edge2 = v3 - v2;
edge2.Normalize();
normal2.X = edge2.Y;
normal2.Y = -edge2.X;
convex2 = Vector2Util.Cross(ref edge1, ref edge2) > 0.0f;
offset2 = Vector2Util.Dot(normal2, centroidB - v2);
}
// Determine front or back collision. Determine collision normal limits.
bool front;
Vector2 normal, lowerLimit, upperLimit;
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = -normal1;
}
}
else if (convex1)
{
front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = -normal1;
}
}
else if (convex2)
{
front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = -normal0;
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = -normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
front = offset0 >= 0.0f || offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = -normal1;
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = -normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
front = offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = normal1;
}
}
else
{
front = offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = normal1;
}
}
}
else
{
front = offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = normal1;
}
}
// Get polygonB in frameA.
var tpc = polygonB.Count;
for (var i = 0; i < tpc; ++i)
{
tpv[i] = xf.ToOther(polygonB.Vertices[i]);
tpn[i] = xf.Rotation * polygonB.Normals[i];
}
const float radius = 2.0f * Settings.PolygonRadius;
Axis edgeAxis;
edgeAxis.Type = Axis.AxisType.EdgeA;
edgeAxis.Index = front ? 0 : 1;
edgeAxis.Separation = float.MaxValue;
for (var i = 0; i < tpc; ++i)
{
var s = Vector2Util.Dot(normal, tpv[i] - v1);
if (s < edgeAxis.Separation)
{
edgeAxis.Separation = s;
}
}
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.Type == Axis.AxisType.None)
{
return(false);
}
if (edgeAxis.Separation > radius)
{
return(false);
}
Axis polygonAxis;
polygonAxis.Type = Axis.AxisType.None;
polygonAxis.Index = -1;
polygonAxis.Separation = float.MinValue;
Vector2 perp;
perp.X = -normal.Y;
perp.Y = normal.X;
for (var i = 0; i < tpc; ++i)
{
var n = -tpn[i];
var s1 = Vector2Util.Dot(n, tpv[i] - v1);
var s2 = Vector2Util.Dot(n, tpv[i] - v2);
var s = System.Math.Min(s1, s2);
if (s > radius)
{
// No collision
polygonAxis.Type = Axis.AxisType.EdgeB;
polygonAxis.Index = i;
polygonAxis.Separation = s;
break;
}
// Adjacency
if (Vector2Util.Dot(ref n, ref perp) >= 0.0f)
{
if (Vector2Util.Dot(n - upperLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
else
{
if (Vector2Util.Dot(n - lowerLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
if (s > polygonAxis.Separation)
{
polygonAxis.Type = Axis.AxisType.EdgeB;
polygonAxis.Index = i;
polygonAxis.Separation = s;
}
}
if (polygonAxis.Type != Axis.AxisType.None && polygonAxis.Separation > radius)
{
return(false);
}
// Use hysteresis for jitter reduction.
const float relativeTol = 0.98f;
const float absoluteTol = 0.001f;
Axis primaryAxis;
if (polygonAxis.Type == Axis.AxisType.None)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.Separation > relativeTol * edgeAxis.Separation + absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
FixedArray2 <ClipVertex> incidentEdge;
// Reference face used for clipping
int rfi1, rfi2;
Vector2 rfv1, rfv2;
Vector2 rfnormal;
Vector2 rfsideNormal1;
if (primaryAxis.Type == Axis.AxisType.EdgeA)
{
manifold.Type = Manifold.ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
var bestIndex = 0;
var bestValue = Vector2Util.Dot(ref normal, ref tpn[0]);
for (var i = 1; i < tpc; ++i)
{
var value = Vector2Util.Dot(ref normal, ref tpn[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
var i1 = bestIndex;
var i2 = i1 + 1 < tpc ? i1 + 1 : 0;
incidentEdge = new FixedArray2 <ClipVertex>
{
Item1 = new ClipVertex
{
Vertex = tpv[i1],
Id =
{
Feature =
{
IndexA = 0,
IndexB = (byte)i1,
TypeA = (byte)ContactFeature.FeatureType.Face,
TypeB = (byte)ContactFeature.FeatureType.Vertex
}
}
},
Item2 = new ClipVertex
{
Vertex = tpv[i2],
Id =
{
Feature =
{
IndexA = 0,
IndexB = (byte)i2,
TypeA = (byte)ContactFeature.FeatureType.Face,
TypeB = (byte)ContactFeature.FeatureType.Vertex
}
}
}
};
if (front)
{
rfi1 = 0;
rfi2 = 1;
rfv1 = v1;
rfv2 = v2;
rfnormal = normal1;
}
else
{
rfi1 = 1;
rfi2 = 0;
rfv1 = v2;
rfv2 = v1;
rfnormal = -normal1;
}
}
else
{
manifold.Type = Manifold.ManifoldType.FaceB;
incidentEdge = new FixedArray2 <ClipVertex>
{
Item1 = new ClipVertex
{
Vertex = v1,
Id =
{
Feature =
{
IndexA = 0,
IndexB = (byte)primaryAxis.Index,
TypeA = (byte)ContactFeature.FeatureType.Vertex,
TypeB = (byte)ContactFeature.FeatureType.Face
}
}
},
Item2 = new ClipVertex
{
Vertex = v2,
Id =
{
Feature =
{
IndexA = 0,
IndexB = (byte)primaryAxis.Index,
TypeA = (byte)ContactFeature.FeatureType.Vertex,
TypeB = (byte)ContactFeature.FeatureType.Face
}
}
}
};
rfi1 = primaryAxis.Index;
rfi2 = rfi1 + 1 < tpc ? rfi1 + 1 : 0;
rfv1 = tpv[rfi1];
rfv2 = tpv[rfi2];
rfnormal = tpn[rfi1];
}
rfsideNormal1.X = rfnormal.Y;
rfsideNormal1.Y = -rfnormal.X;
var rfsideNormal2 = -rfsideNormal1;
var rfsideOffset1 = Vector2Util.Dot(ref rfsideNormal1, ref rfv1);
var rfsideOffset2 = Vector2Util.Dot(ref rfsideNormal2, ref rfv2);
// Clip incident edge against extruded edge1 side edges.
FixedArray2 <ClipVertex> clipPoints1, clipPoints2;
// Clip to box side 1
var np = ClipSegmentToLine(
out clipPoints1,
incidentEdge,
rfsideNormal1,
rfsideOffset1,
rfi1);
if (np < 2)
{
return(false);
}
// Clip to negative box side 1
np = ClipSegmentToLine(
out clipPoints2,
clipPoints1,
rfsideNormal2,
rfsideOffset2,
rfi2);
if (np < 2)
{
return(false);
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == Axis.AxisType.EdgeA)
{
manifold.LocalPoint = rfv1;
manifold.LocalNormal = rfnormal;
}
else
{
manifold.LocalPoint = polygonB.Vertices[rfi1];
manifold.LocalNormal = polygonB.Normals[rfi1];
}
var pointCount = 0;
for (var i = 0; i < 2; ++i)
{
if (Vector2Util.Dot(rfnormal, clipPoints2[i].Vertex - rfv1) <= radius)
{
var cp = manifold.Points[pointCount];
if (primaryAxis.Type == Axis.AxisType.EdgeA)
{
cp.LocalPoint = xf.FromOther(clipPoints2[i].Vertex);
cp.Id = clipPoints2[i].Id;
}
else
{
cp.LocalPoint = clipPoints2[i].Vertex;
cp.Id.Feature.TypeA = clipPoints2[i].Id.Feature.TypeB;
cp.Id.Feature.TypeB = clipPoints2[i].Id.Feature.TypeA;
cp.Id.Feature.IndexA = clipPoints2[i].Id.Feature.IndexB;
cp.Id.Feature.IndexB = clipPoints2[i].Id.Feature.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
return(pointCount > 0);
}