public static void ComputeCircleAABB(ref Vector2 pos, float radius, ref Transform transform, out AABB aabb)
{
var p = transform.p + MathUtils.Mul(transform.q, pos);
aabb.LowerBound = new Vector2(p.x - radius, p.y - radius);
aabb.UpperBound = new Vector2(p.x + radius, p.y + radius);
}
public static Vector2 Mul(ref Transform T, ref Vector2 v)
{
var x = T.q.c * v.x - T.q.s * v.y + T.p.x;
var y = T.q.s * v.x + T.q.c * v.y + T.p.y;
return(new Vector2(x, y));
}
/// <summary>
/// Compute the collision manifold between two circles.
/// </summary>
public static void CollideCircles(ref Manifold manifold, CircleShape circleA, ref Transform xfA,
CircleShape circleB, ref Transform xfB)
{
manifold.PointCount = 0;
var pA = MathUtils.Mul(ref xfA, circleA.Position);
var pB = MathUtils.Mul(ref xfB, circleB.Position);
var d = pB - pA;
var distSqr = Vector2.Dot(d, d);
float rA = circleA.Radius, rB = circleB.Radius;
var radius = rA + rB;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = ManifoldType.Circles;
manifold.LocalPoint = circleA.Position;
manifold.LocalNormal = Vector2.zero;
manifold.PointCount = 1;
var p0 = manifold.Points[0];
p0.LocalPoint = circleB.Position;
p0.Id.Key = 0;
manifold.Points[0] = p0;
}
// v2 = A.q' * (B.q * v1 + B.p - A.p)
// = A.q' * B.q * v1 + A.q' * (B.p - A.p)
public static Transform MulT(Transform A, Transform B)
{
var C = new Transform();
C.q = MulT(A.q, B.q);
C.p = MulT(A.q, B.p - A.p);
return(C);
}
public static Vector2 MulT(Transform T, Vector2 v)
{
var px = v.x - T.p.x;
var py = v.y - T.p.y;
var x = T.q.c * px + T.q.s * py;
var y = -T.q.s * px + T.q.c * py;
return(new Vector2(x, y));
}
public static void ComputeEdgeAABB(ref Vector2 start, ref Vector2 end, ref Transform transform, out AABB aabb)
{
var v1 = MathUtils.Mul(ref transform, ref start);
var v2 = MathUtils.Mul(ref transform, ref end);
aabb.LowerBound = Vector2.Min(v1, v2);
aabb.UpperBound = Vector2.Max(v1, v2);
var r = new Vector2(Settings.PolygonRadius, Settings.PolygonRadius);
aabb.LowerBound = aabb.LowerBound - r;
aabb.UpperBound = aabb.UpperBound + r;
}
internal void ReadCache(ref SimplexCache cache, ref DistanceProxy proxyA, ref Transform transformA,
ref DistanceProxy proxyB, ref Transform transformB)
{
Debug.Assert(cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
for (var i = 0; i < Count; ++i)
{
var v = V[i];
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
var wALocal = proxyA.Vertices[v.IndexA];
var wBLocal = proxyB.Vertices[v.IndexB];
v.WA = MathUtils.Mul(ref transformA, wALocal);
v.WB = MathUtils.Mul(ref transformB, wBLocal);
v.W = v.WB - v.WA;
v.A = 0.0f;
V[i] = v;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (Count > 1)
{
var metric1 = cache.Metric;
var metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.Epsilon)
{
// Reset the simplex.
Count = 0;
}
}
// If the cache is empty or invalid ...
if (Count == 0)
{
var v = V[0];
v.IndexA = 0;
v.IndexB = 0;
var wALocal = proxyA.Vertices[0];
var wBLocal = proxyB.Vertices[0];
v.WA = MathUtils.Mul(ref transformA, wALocal);
v.WB = MathUtils.Mul(ref transformB, wBLocal);
v.W = v.WB - v.WA;
v.A = 1.0f;
V[0] = v;
Count = 1;
}
}
public static bool TestPointPolygon(Vertices vertices, Vertices normals, ref Vector2 point,
ref Transform transform)
{
var pLocal = MathUtils.MulT(transform.q, point - transform.p);
for (var i = 0; i < vertices.Count; ++i)
{
var dot = Vector2.Dot(normals[i], pLocal - vertices[i]);
if (dot > 0.0f)
{
return(false);
}
}
return(true);
}
/// <summary>
/// Build vertices to represent an oriented box.
/// </summary>
/// <param name="hx">the half-width.</param>
/// <param name="hy">the half-height.</param>
/// <param name="center">the center of the box in local coordinates.</param>
/// <param name="angle">the rotation of the box in local coordinates.</param>
public static Vertices CreateRectangle(float hx, float hy, Vector2 center, float angle)
{
var vertices = CreateRectangle(hx, hy);
var xf = new Transform();
xf.p = center;
xf.q.Set(angle);
// Transform vertices
for (var i = 0; i < 4; ++i)
{
vertices[i] = MathUtils.Mul(ref xf, vertices[i]);
}
return(vertices);
}
public static void ComputePolygonAABB(Vertices vertices, ref Transform transform, out AABB aabb)
{
var lower = MathUtils.Mul(ref transform, vertices[0]);
var upper = lower;
for (var i = 1; i < vertices.Count; ++i)
{
var v = MathUtils.Mul(ref transform, vertices[i]);
lower = Vector2.Min(lower, v);
upper = Vector2.Max(upper, v);
}
var r = new Vector2(Settings.PolygonRadius, Settings.PolygonRadius);
aabb.LowerBound = lower - r;
aabb.UpperBound = upper + r;
}
private static void FindIncidentEdge(out FixedArray2 <ClipVertex> c, PolygonShape poly1, ref Transform xf1,
int edge1, PolygonShape poly2, ref Transform xf2)
{
var normals1 = poly1.Normals;
var count2 = poly2.Vertices.Count;
var vertices2 = poly2.Vertices;
var normals2 = poly2.Normals;
Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);
// Get the normal of the reference edge in poly2's frame.
var normal1 = MathUtils.MulT(ref xf2.q, MathUtils.Mul(ref xf1.q, normals1[edge1]));
// Find the incident edge on poly2.
var index = 0;
var minDot = Settings.MaxFloat;
for (var i = 0; i < count2; ++i)
{
var dot = Vector2.Dot(normal1, normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
var i1 = index;
var i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c = new FixedArray2 <ClipVertex>();
c.Value0.V = MathUtils.Mul(ref xf2, vertices2[i1]);
c.Value0.ID.ContactFeature.IndexA = (byte)edge1;
c.Value0.ID.ContactFeature.IndexB = (byte)i1;
c.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Face;
c.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Vertex;
c.Value1.V = MathUtils.Mul(ref xf2, vertices2[i2]);
c.Value1.ID.ContactFeature.IndexA = (byte)edge1;
c.Value1.ID.ContactFeature.IndexB = (byte)i2;
c.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Face;
c.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Vertex;
}
/// <summary>
/// Find the max separation between poly1 and poly2 using edge normals from poly1.
/// </summary>
private static float FindMaxSeparation(out int edgeIndex, PolygonShape poly1, ref Transform xf1,
PolygonShape poly2, ref Transform xf2)
{
var count1 = poly1.Vertices.Count;
var count2 = poly2.Vertices.Count;
var n1s = poly1.Normals;
var v1s = poly1.Vertices;
var v2s = poly2.Vertices;
var xf = MathUtils.MulT(xf2, xf1);
var bestIndex = 0;
var maxSeparation = -Settings.MaxFloat;
for (var i = 0; i < count1; ++i)
{
// Get poly1 normal in frame2.
var n = MathUtils.Mul(ref xf.q, n1s[i]);
var v1 = MathUtils.Mul(ref xf, v1s[i]);
// Find deepest point for normal i.
var si = Settings.MaxFloat;
for (var j = 0; j < count2; ++j)
{
var sij = Vector2.Dot(n, v2s[j] - v1);
if (sij < si)
{
si = sij;
}
}
if (si > maxSeparation)
{
maxSeparation = si;
bestIndex = i;
}
}
edgeIndex = bestIndex;
return(maxSeparation);
}
public static bool RayCastEdge(ref Vector2 start, ref Vector2 end, ref RayCastInput input,
ref Transform transform, out RayCastOutput output)
{
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
output = new RayCastOutput();
// Put the ray into the edge's frame of reference.
var p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
var p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
var d = p2 - p1;
var v1 = start;
var v2 = end;
var e = v2 - v1;
var normal = new Vector2(e.y, -e.x); //TODO: Could possibly cache the normal.
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
var numerator = Vector2.Dot(normal, v1 - p1);
var denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
var t = numerator / denominator;
if (t < 0.0f || input.MaxFraction < t)
{
return(false);
}
var q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
var r = v2 - v1;
var rr = Vector2.Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
var s = Vector2.Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.Fraction = t;
if (numerator > 0.0f)
{
output.Normal = -MathUtils.MulT(transform.q, normal);
}
else
{
output.Normal = MathUtils.MulT(transform.q, normal);
}
return(true);
}
public static bool RayCastCircle(ref Vector2 pos, float radius, ref RayCastInput input, ref Transform transform,
out RayCastOutput output)
{
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
output = new RayCastOutput();
var position = transform.p + MathUtils.Mul(transform.q, pos);
var s = input.Point1 - position;
var b = Vector2.Dot(s, s) - radius * radius;
// Solve quadratic equation.
var r = input.Point2 - input.Point1;
var c = Vector2.Dot(s, r);
var rr = Vector2.Dot(r, r);
var sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.Epsilon)
{
return(false);
}
// Find the point of intersection of the line with the circle.
var a = -(c + Mathf.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.MaxFraction * rr)
{
a /= rr;
output.Fraction = a;
output.Normal = s + a * r;
output.Normal.Normalize();
return(true);
}
return(false);
}
public static bool RayCastPolygon(Vertices vertices, Vertices normals, ref RayCastInput input,
ref Transform transform, out RayCastOutput output)
{
output = new RayCastOutput();
// Put the ray into the polygon's frame of reference.
var p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
var p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
var d = p2 - p1;
float lower = 0.0f, upper = input.MaxFraction;
var index = -1;
for (var i = 0; i < vertices.Count; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
var numerator = Vector2.Dot(normals[i], vertices[i] - p1);
var denominator = Vector2.Dot(normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return(false);
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
// The use of epsilon here causes the assert on lower to trip
// in some cases. Apparently the use of epsilon was to make edge
// shapes work, but now those are handled separately.
//if (upper < lower - b2_epsilon)
if (upper < lower)
{
return(false);
}
}
Debug.Assert(0.0f <= lower && lower <= input.MaxFraction);
if (index >= 0)
{
output.Fraction = lower;
output.Normal = MathUtils.Mul(transform.q, normals[index]);
return(true);
}
return(false);
}
/// <summary> /// Draw a transform. Choose your own length scale. /// </summary> /// <param name="transform">The transform.</param> public abstract void DrawTransform(ref Transform transform);
public static Vector2 MulT(ref Transform T, Vector2 v)
{
return(MulT(ref T, ref v));
}
/// <summary>
/// Initialize position dependent portions of the velocity constraints.
/// </summary>
public void InitializeVelocityConstraints()
{
for (var i = 0; i < _count; ++i)
{
var vc = VelocityConstraints[i];
var pc = _positionConstraints[i];
var radiusA = pc.RadiusA;
var radiusB = pc.RadiusB;
var manifold = _contacts[vc.ContactIndex].Manifold;
var indexA = vc.IndexA;
var indexB = vc.IndexB;
var mA = vc.InvMassA;
var mB = vc.InvMassB;
var iA = vc.InvIA;
var iB = vc.InvIB;
var localCenterA = pc.LocalCenterA;
var localCenterB = pc.LocalCenterB;
var cA = _positions[indexA].C;
var aA = _positions[indexA].A;
var vA = _velocities[indexA].V;
var wA = _velocities[indexA].W;
var cB = _positions[indexB].C;
var aB = _positions[indexB].A;
var vB = _velocities[indexB].V;
var wB = _velocities[indexB].W;
Debug.Assert(manifold.PointCount > 0);
var xfA = new Transform();
var xfB = new Transform();
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out var normal,
out var points, out _);
vc.Normal = normal;
var pointCount = vc.PointCount;
for (var j = 0; j < pointCount; ++j)
{
var vcp = vc.Points[j];
vcp.rA = points[j] - cA;
vcp.rB = points[j] - cB;
var rnA = MathUtils.Cross(vcp.rA, vc.Normal);
var rnB = MathUtils.Cross(vcp.rB, vc.Normal);
var kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
var tangent = MathUtils.Cross(vc.Normal, 1.0f);
var rtA = MathUtils.Cross(vcp.rA, tangent);
var rtB = MathUtils.Cross(vcp.rB, tangent);
var kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
vcp.TangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
// Setup a velocity bias for restitution.
vcp.VelocityBias = 0.0f;
var vRel = Vector2.Dot(vc.Normal,
vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA));
if (vRel < -Settings.VelocityThreshold)
{
vcp.VelocityBias = -vc.Restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc.PointCount == 2 && Settings.BlockSolve)
{
var vcp1 = vc.Points[0];
var vcp2 = vc.Points[1];
var rn1A = MathUtils.Cross(vcp1.rA, vc.Normal);
var rn1B = MathUtils.Cross(vcp1.rB, vc.Normal);
var rn2A = MathUtils.Cross(vcp2.rA, vc.Normal);
var rn2B = MathUtils.Cross(vcp2.rB, vc.Normal);
var k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
var k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
var k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 1000.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
vc.K.ex = new Vector2(k11, k12);
vc.K.ey = new Vector2(k12, k22);
vc.NormalMass = vc.K.Inverse;
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
vc.PointCount = 1;
}
}
}
}
// Sequential position solver for position constraints.
public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
{
var minSeparation = 0.0f;
for (var i = 0; i < _count; ++i)
{
var pc = _positionConstraints[i];
var indexA = pc.IndexA;
var indexB = pc.IndexB;
var localCenterA = pc.LocalCenterA;
var localCenterB = pc.LocalCenterB;
var pointCount = pc.PointCount;
var mA = 0.0f;
var iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc.InvMassA;
iA = pc.InvIA;
}
var mB = 0.0f;
var iB = 0.0f;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc.InvMassB;
iB = pc.InvIB;
}
var cA = _positions[indexA].C;
var aA = _positions[indexA].A;
var cB = _positions[indexB].C;
var aB = _positions[indexB].A;
// Solve normal constraints
for (var j = 0; j < pointCount; ++j)
{
var xfA = new Transform();
var xfB = new Transform();
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
PositionSolverManifold.Initialize(pc, xfA, xfB, j, out var normal, out var point,
out var separation);
var rA = point - cA;
var rB = point - cB;
// Track max constraint error.
minSeparation = Mathf.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
var C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop),
-Settings.MaxLinearCorrection, 0.0f);
// Compute the effective mass.
var rnA = MathUtils.Cross(rA, normal);
var rnB = MathUtils.Cross(rB, normal);
var K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
var impulse = K > 0.0f ? -C / K : 0.0f;
var P = impulse * normal;
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
_positions[indexA].C = cA;
_positions[indexA].A = aA;
_positions[indexB].C = cB;
_positions[indexB].A = aB;
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return(minSeparation >= -1.5f * Settings.LinearSlop);
}
/// <summary>
/// Compute the collision manifold between a polygon and a circle.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="polygonA">The polygon A.</param>
/// <param name="xfA">The transform of A.</param>
/// <param name="circleB">The circle B.</param>
/// <param name="xfB">The transform of B.</param>
public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygonA, ref Transform xfA,
CircleShape circleB, ref Transform xfB)
{
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
var c = MathUtils.Mul(ref xfB, circleB.Position);
var cLocal = MathUtils.MulT(ref xfA, c);
// Find the min separating edge.
var normalIndex = 0;
var separation = -Settings.MaxFloat;
var radius = polygonA.Radius + circleB.Radius;
var vertexCount = polygonA.Vertices.Count;
var vertices = polygonA.Vertices;
var normals = polygonA.Normals;
for (var i = 0; i < vertexCount; ++i)
{
var s = Vector2.Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
var vertIndex1 = normalIndex;
var vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
var v1 = vertices[vertIndex1];
var v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Settings.Epsilon)
{
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = normals[normalIndex];
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points.Value0.LocalPoint = circleB.Position;
manifold.Points.Value0.Id.Key = 0;
return;
}
// Compute barycentric coordinates
var u1 = Vector2.Dot(cLocal - v1, v2 - v1);
var u2 = Vector2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (Mathf.Sqrt(Vector2.Distance(cLocal, v1)) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = cLocal - v1;
manifold.LocalNormal.Normalize();
manifold.LocalPoint = v1;
manifold.Points.Value0.LocalPoint = circleB.Position;
manifold.Points.Value0.Id.Key = 0;
}
else if (u2 <= 0.0f)
{
if (Mathf.Sqrt(Vector2.Distance(cLocal, v2)) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = cLocal - v2;
manifold.LocalNormal.Normalize();
manifold.LocalPoint = v2;
manifold.Points.Value0.LocalPoint = circleB.Position;
manifold.Points.Value0.Id.Key = 0;
}
else
{
var faceCenter = 0.5f * (v1 + v2);
var s = Vector2.Dot(cLocal - faceCenter, normals[vertIndex1]);
if (s > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = normals[vertIndex1];
manifold.LocalPoint = faceCenter;
manifold.Points.Value0.LocalPoint = circleB.Position;
manifold.Points.Value0.Id.Key = 0;
}
}
/// <summary>
/// Compute the collision manifold between two polygons.
/// </summary>
public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform xfA,
PolygonShape polyB, ref Transform xfB)
{
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
manifold.PointCount = 0;
var totalRadius = polyA.Radius + polyB.Radius;
int edgeA;
var separationA = FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB;
var separationB = FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
{
return;
}
PolygonShape poly1; // reference polygon
PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
bool flip;
const float k_tol = 0.1f * Settings.LinearSlop;
if (separationB > separationA + k_tol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.Type = ManifoldType.FaceB;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.Type = ManifoldType.FaceA;
flip = false;
}
FixedArray2 <ClipVertex> incidentEdge;
FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
var count1 = poly1.Vertices.Count;
var vertices1 = poly1.Vertices;
var iv1 = edge1;
var iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
var v11 = vertices1[iv1];
var v12 = vertices1[iv2];
var localTangent = v12 - v11;
localTangent.Normalize();
var localNormal = MathUtils.Cross(localTangent, 1.0f);
var planePoint = 0.5f * (v11 + v12);
var tangent = MathUtils.Mul(ref xf1.q, localTangent);
var normal = MathUtils.Cross(tangent, 1.0f);
v11 = MathUtils.Mul(ref xf1, v11);
v12 = MathUtils.Mul(ref xf1, v12);
// Face offset.
var frontOffset = Vector2.Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
var sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius;
var sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
FixedArray2 <ClipVertex> clipPoints1;
FixedArray2 <ClipVertex> clipPoints2;
// Clip to box side 1
var np = Collision.ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = Collision.ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.LocalNormal = localNormal;
manifold.LocalPoint = planePoint;
var pointCount = 0;
for (var i = 0; i < Settings.MaxManifoldPoints; ++i)
{
var separation = Vector2.Dot(normal, clipPoints2[i].V) - frontOffset;
if (separation <= totalRadius)
{
var cp = manifold.Points[pointCount];
cp.LocalPoint = MathUtils.MulT(ref xf2, clipPoints2[i].V);
cp.Id = clipPoints2[i].ID;
if (flip)
{
// Swap features
var cf = cp.Id.ContactFeature;
cp.Id.ContactFeature.IndexA = cf.IndexB;
cp.Id.ContactFeature.IndexB = cf.IndexA;
cp.Id.ContactFeature.TypeA = cf.TypeB;
cp.Id.ContactFeature.TypeB = cf.TypeA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
/// <summary>
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the Shapes
/// that generated the manifold.
/// </summary>
public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB,
float radiusB, out Vector2 normal, out FixedArray2 <Vector2> points, out FixedArray2 <float> separations)
{
normal = Vector2.zero;
points = new FixedArray2 <Vector2>();
separations = new FixedArray2 <float>();
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
normal = new Vector2(1.0f, 0.0f);
var pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint);
var pointB = MathUtils.Mul(ref xfB, manifold.Points.Value0.LocalPoint);
if (Mathf.Sqrt(Vector2.Distance(pointA, pointB)) > Settings.Epsilon * Settings.Epsilon)
{
normal = pointB - pointA;
normal.Normalize();
}
var cA = pointA + radiusA * normal;
var cB = pointB - radiusB * normal;
points.Value0 = 0.5f * (cA + cB);
separations.Value0 = Vector2.Dot(cB - cA, normal);
}
break;
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, manifold.LocalNormal);
var planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint);
for (var i = 0; i < manifold.PointCount; ++i)
{
var clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
var cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
var cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
separations[i] = Vector2.Dot(cB - cA, normal);
}
}
break;
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, manifold.LocalNormal);
var planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint);
for (var i = 0; i < manifold.PointCount; ++i)
{
var clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint);
var cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
var cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
separations[i] = Vector2.Dot(cA - cB, normal);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}
public static bool TestPointCircle(ref Vector2 pos, float radius, ref Vector2 point, ref Transform transform)
{
var center = transform.p + MathUtils.Mul(transform.q, pos);
var d = point - center;
return(Vector2.Dot(d, d) <= radius * radius);
}
// v2 = A.q' * (B.q * v1 + B.p - A.p)
// = A.q' * B.q * v1 + A.q' * (B.p - A.p)
public static void MulT(ref Transform A, ref Transform B, out Transform C)
{
C = new Transform();
C.q = MulT(A.q, B.q);
C.p = MulT(A.q, B.p - A.p);
}
