public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b)
{
TFEdgeCollider A = (TFEdgeCollider)a.coll;
TFCircleCollider B = (TFCircleCollider)b.coll;
// No line segments, return out.
if (A.vertices.Count < 2)
{
return;
}
// Transform circle center to Edge model space
FixVec2 circleCenter = A.u.Transposed() * (b.Position - a.Position);
// Iterate through all the line segments to find contact point.
for (int i = 0; i < A.vertices.Count - 1; i++)
{
FixVec2 rayDir = (A.vertices[i + 1] - A.vertices[i]);
FixVec2 centerRay = (A.vertices[i] - circleCenter);
Fix k = rayDir.Dot(rayDir);
Fix l = 2 * centerRay.Dot(rayDir);
Fix n = centerRay.Dot(centerRay) - (B.radius * B.radius);
Fix discriminant = l * l - 4 * k * n;
// No intersection.
if (discriminant <= Fix.zero)
{
continue;
}
discriminant = FixMath.Sqrt(discriminant);
Fix t1 = (-l - discriminant) / (2 * k);
Fix t2 = (-l + discriminant) / (2 * k);
Fix s = FixVec2.Dot(A.normals[i], circleCenter - A.vertices[i]);
if (t1 >= Fix.zero && t1 <= Fix.one)
{
//t1 is the intersection, and it's closer than t2.
m.contactCount = 1;
m.contacts[0] = (A.u * A.vertices[i] + a.Position) + (t1 * rayDir);
m.normal = A.normals[i];
m.penetration = B.radius - s;
return;
}
if (t2 >= Fix.zero && t2 <= Fix.one)
{
// t1 didn't insection, so we either started inside the circle
// or completely past it.
m.contactCount = 1;
m.contacts[0] = (A.u * A.vertices[i] + a.Position) + (t2 * rayDir);
m.normal = A.normals[i];
m.penetration = B.radius - s;
return;
}
}
}
private void FindIncidentFace(FixVec2[] v, TFPolygonCollider refPoly, TFPolygonCollider incPoly, int referenceIndex)
{
FixVec2 referenceNormal = refPoly.normals[referenceIndex];
// Calculate normal in incident's frame of reference
referenceNormal = refPoly.u * referenceNormal; // To world space
referenceNormal = incPoly.u.Transposed() * referenceNormal; // To incident's model space
// Find most anti-normal face on incident polygon
int incidentFace = 0;
Fix minDot = Fix.MaxValue;
for (int i = 0; i < incPoly.VertexCount; ++i)
{
Fix dot = FixVec2.Dot(referenceNormal, incPoly.normals[i]);
if (dot < minDot)
{
minDot = dot;
incidentFace = i;
}
}
// Assign face vertices for incidentFace
v[0] = incPoly.u * incPoly.GetVertex(incidentFace) + incPoly.body.info.position;
incidentFace = incidentFace + 1 >= (int)incPoly.VertexCount ? 0 : incidentFace + 1;
v[1] = incPoly.u * incPoly.GetVertex(incidentFace) + incPoly.body.info.position;
}
private int Clip(FixVec2 n, Fix c, FixVec2[] face)
{
int sp = 0;
FixVec2[] o = { face[0], face[1] };
// Retrieve distances from each endpoint to the line
Fix d1 = FixVec2.Dot(n, face[0]) - c;
Fix d2 = FixVec2.Dot(n, face[1]) - c;
// If negative (behind plane) clip
if (d1 <= Fix.zero)
{
o[sp++] = face[0];
}
if (d2 <= Fix.zero)
{
o[sp++] = face[1];
}
// If the points are on different sides of the plane
if (d1 * d2 < Fix.zero) // less than to ignore -0.0f
{
// Push interesection point
Fix alpha = d1 / (d1 - d2);
o[sp] = face[0] + alpha * (face[1] - face[0]);
++sp;
}
// Assign our new converted values
face[0] = o[0];
face[1] = o[1];
return(sp);
}
public override bool Raycast(out TFRaycastHit2D hit, FixVec2 pointA, FixVec2 pointB, Fix maxFraction)
{
hit = default;
FixVec2 center = (FixVec2)tdTransform.Position;
FixVec2 s = pointA - center;
Fix b = FixVec2.Dot(s, s) - radius * radius;
// Solve quadratic equation.
FixVec2 r = pointB - pointA;
Fix c = FixVec2.Dot(s, r);
Fix rr = FixVec2.Dot(r, r);
Fix sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < Fix.zero || rr < Fix.Epsilon)
{
return(false);
}
// Find the point of intersection on the line with the circle.
Fix a = -(c + FixMath.Sqrt(sigma));
// Is the intersection point on the segment?
if (Fix.zero <= a && a <= maxFraction * rr)
{
a /= rr;
hit.fraction = a;
hit.normal = s + a * r;
hit.normal.Normalize();
return(true);
}
return(false);
}
public FixVec2 getSupport(FixVec2 dir)
{
Fix bestProjection = -Fix.MaxValue;
FixVec2 bestVertex = new FixVec2(0, 0);
for (int i = 0; i < vertices.Count; ++i)
{
FixVec2 v = vertices[i];
Fix projection = FixVec2.Dot(v, dir);
if (projection > bestProjection)
{
bestVertex = v;
bestProjection = projection;
}
}
return(bestVertex);
}
// Calculate the projection of a polygon on an axis and returns it as a [min, max] interval
private static void ProjectPolygon(FixVec2 axis, FixPolygon polygon, out Fix min, out Fix max)
{
// To project a point on an axis use the dot product
var d = axis.Dot(polygon.Points[0]);
min = d;
max = d;
for (var i = 0; i < polygon.Points.Length; i++)
{
d = polygon.Points[i].Dot(axis);
if (d < min)
{
min = d;
}
else if (d > max)
{
max = d;
}
}
}
public Fix FindAxisLeastPenetration(int[] faceIndex, TFPolygonCollider A, TFPolygonCollider B)
{
Fix bestDistance = -Fix.MaxValue;
int bestIndex = 0;
Mat22 buT;
for (int i = 0; i < A.VertexCount; ++i)
{
// Retrieve a face normal from A
FixVec2 nw = A.u * A.normals[i];
// Transform face normal into B's model space
buT = B.u;
buT.Transpose();
FixVec2 n = buT * nw;
// Retrieve support point from B along -n
// Vec2 s = B->GetSupport( -n );
FixVec2 s = B.getSupport(-n);
// Retrieve vertex on face from A, transform into
FixVec2 v = A.GetVertex(i);
v = A.u * v + A.body.Position;
v -= B.body.info.position;
v = buT * v;
// Compute penetration distance (in B's model space)
Fix d = FixVec2.Dot(n, s - v);
// Store greatest distance
if (d > bestDistance)
{
bestDistance = d;
bestIndex = i;
}
}
faceIndex[0] = bestIndex;
return(bestDistance);
}
public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b)
{
TFCircleCollider A = (TFCircleCollider)a.coll;
TFPolygonCollider B = (TFPolygonCollider)b.coll;
m.contactCount = 0;
// Transform circle center to Polygon model space
FixVec2 center = B.u.Transposed() * (a.Position - b.Position);
// Find edge with minimum penetration
// Exact concept as using support points in Polygon vs Polygon
Fix separation = -Fix.MaxValue;
int faceNormal = 0;
for (int i = 0; i < B.VertexCount; ++i)
{
Fix s = FixVec2.Dot(B.normals[i], center - B.GetVertex(i));
if (s > A.radius)
{
return;
}
if (s > separation)
{
separation = s;
faceNormal = i;
}
}
// Grab face's vertices
FixVec2 v1 = B.GetVertex(faceNormal);
int i2 = (faceNormal + 1) < B.VertexCount ? faceNormal + 1 : 0;
FixVec2 v2 = B.GetVertex(i2);
// Check to see if center is within polygon
if (separation < Fix.Epsilon)
{
m.contactCount = 1;
m.normal = -(B.u * B.normals[faceNormal]);
m.contacts[0] = m.normal * A.radius + a.Position;
m.penetration = A.radius;
return;
}
// Determine which voronoi region of the edge center of circle lies within
Fix dot1 = FixVec2.Dot(center - v1, v2 - v1);
Fix dot2 = FixVec2.Dot(center - v2, v1 - v2);
m.penetration = A.radius - separation;
//Closest to v1
if (dot1 <= Fix.zero)
{
if ((center - v1).GetMagnitudeSquared() > A.radius * A.radius)
{
return;
}
m.contactCount = 1;
FixVec2 n = v1 - center;
n = B.u * n;
n = n.Normalized();
m.normal = n;
v1 = B.u * v1 + b.Position;
m.contacts[0] = v1;
}
else if (dot2 <= Fix.zero)
{
//Closest to v2
if ((center - v2).GetMagnitudeSquared() > A.radius * A.radius)
{
return;
}
m.contactCount = 1;
FixVec2 n = v2 - center;
v2 = B.u * v2 + b.Position;
m.contacts[0] = v2;
n = B.u * n;
n = n.Normalized();
m.normal = n;
}
else
{
//Closest to face
FixVec2 n = B.normals[faceNormal];
if (FixVec2.Dot(center - v1, n) > A.radius)
{
return;
}
n = B.u * n;
m.normal = -n;
m.contacts[0] = m.normal * A.radius + a.Position;
m.contactCount = 1;
}
}
internal TFRaycastHit2D Raycast(ITreeRaycastCallback callback, FixVec2 pointA, FixVec2 pointB, TFLayerMask mask)
{
TFRaycastHit2D hit = new TFRaycastHit2D();
FixVec2 r = pointB - pointA;
if (r.GetMagnitudeSquared() <= Fix.zero)
{
return(hit);
}
r.Normalize();
// v is perpendicular to the segment.
FixVec2 v = FixVec2.Cross(Fix.one, r);
FixVec2 abs_v = FixVec2.Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Fix maxFraction = Fix.one;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
FixVec2 t = pointA + maxFraction * (pointB - pointA);
segmentAABB.min = FixVec2.Min(pointA, t);
segmentAABB.max = FixVec2.Max(pointA, t);
Stack <int> stack = new Stack <int>();
stack.Push(rootIndex);
List <TFRaycastOutput> hitNodes = new List <TFRaycastOutput>(2);
while (stack.Count > 0)
{
var nodeId = stack.Pop();
if (nodeId == nullNode)
{
continue;
}
var node = nodes[nodeId];
if (!node.aabb.Overlaps(segmentAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
var c = node.aabb.GetCenter();
var h = node.aabb.GetExtents();
var separation = FixMath.Abs(FixVec2.Dot(v, pointA - c)) - FixVec2.Dot(abs_v, h);
if (separation > Fix.zero)
{
continue;
}
if (node.IsLeaf())
{
// If value is >= 0, then we hit the node.
TFRaycastHit2D rHit;
Fix value = callback.RayCastCallback(pointA, pointB, maxFraction, nodeId, out rHit, mask);
if (value == Fix.zero)
{
// The client has terminated the ray cast.
if (rHit)
{
// We actually hit the node, add it to the list.
hitNodes.Add(new TFRaycastOutput(nodeId, rHit));
}
break;
}
if (value == maxFraction)
{
if (rHit)
{
// We actually hit the node, add it to the list.
hitNodes.Add(new TFRaycastOutput(nodeId, rHit));
}
}
else if (value > Fix.zero)
{
if (rHit)
{
// We actually hit the node, add it to the list.
hitNodes.Add(new TFRaycastOutput(nodeId, rHit));
}
// Update segment bounding box.
maxFraction = value;
FixVec2 g = pointA + maxFraction * (pointB - pointA);
segmentAABB.min = FixVec2.Min(pointA, g);
segmentAABB.max = FixVec2.Max(pointA, g);
}
}
else
{
stack.Push(node.leftChildIndex);
stack.Push(node.rightChildIndex);
}
}
// Decide which node was the closest to the starting point.
Fix closestNode = maxFraction;
for (int i = 0; i < hitNodes.Count; i++)
{
if (hitNodes[i].hit.fraction < closestNode)
{
closestNode = hitNodes[i].hit.fraction;
hit = hitNodes[i].hit;
}
}
return(hit);
}
public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b)
{
TFPolygonCollider A = (TFPolygonCollider)a.coll;
TFPolygonCollider B = (TFPolygonCollider)b.coll;
m.contactCount = 0;
// Check for a separating axis with A's face planes
int[] faceA = { 0 };
Fix penetrationA = FindAxisLeastPenetration(faceA, A, B);
if (penetrationA >= Fix.zero)
{
return;
}
int[] faceB = { 0 };
Fix penetrationB = FindAxisLeastPenetration(faceB, B, A);
if (penetrationB >= Fix.zero)
{
return;
}
int referenceIndex;
bool flip; //Always point from a to b
TFPolygonCollider refPoly; //Reference
TFPolygonCollider incPoly; //Incident
//Determine which shape contains reference face
if (TFPhysics.instance.BiasGreaterThan(penetrationA, penetrationB))
{
refPoly = A;
incPoly = B;
referenceIndex = faceA[0];
flip = false;
}
else
{
refPoly = B;
incPoly = A;
referenceIndex = faceB[0];
flip = true;
}
// World space incident face
FixVec2[] incidentFace = new FixVec2[2];
FindIncidentFace(incidentFace, refPoly, incPoly, referenceIndex);
// Setup reference face certices
FixVec2 v1 = refPoly.GetVertex(referenceIndex);
referenceIndex = referenceIndex + 1 == refPoly.VertexCount ? 0 : referenceIndex + 1;
FixVec2 v2 = refPoly.GetVertex(referenceIndex);
// Transform vertices to world space
v1 = refPoly.u * v1 + refPoly.body.info.position;
v2 = refPoly.u * v2 + refPoly.body.info.position;
//Calculate reference face side normal in world space
FixVec2 sidePlaneNormal = v2 - v1;
sidePlaneNormal = sidePlaneNormal.Normalized();
// Orthogonalize
FixVec2 refFaceNormal = new FixVec2(sidePlaneNormal.Y, -sidePlaneNormal.X);
// ax + by = c
// c is distance from origin
Fix refC = FixVec2.Dot(refFaceNormal, v1);
Fix negSide = -FixVec2.Dot(sidePlaneNormal, v1);
Fix posSide = FixVec2.Dot(sidePlaneNormal, v2);
// Clip incident face to reference face side planes
if (Clip(-sidePlaneNormal, negSide, incidentFace) < 2)
{
return; // Due to floating point error, possible to not have required points
}
if (Clip(sidePlaneNormal, posSide, incidentFace) < 2)
{
return;
}
// Flip
m.normal = flip ? -refFaceNormal : refFaceNormal;
// Keep points behind reference face
int cp = 0; // clipped points behind reference face
Fix separation = FixVec2.Dot(refFaceNormal, incidentFace[0]) - refC;
if (separation <= Fix.zero)
{
m.contacts[cp] = incidentFace[0];
m.penetration = -separation;
++cp;
}
else
{
m.penetration = 0;
}
separation = FixVec2.Dot(refFaceNormal, incidentFace[1]) - refC;
if (separation <= Fix.zero)
{
m.contacts[cp] = incidentFace[1];
m.penetration += -separation;
++cp;
// Average penetration
m.penetration /= cp;
}
m.contactCount = cp;
}
internal void Raycast(ITreeRaycastCallback callback, FixVec2 pointA, FixVec2 pointB)
{
FixVec2 r = pointB - pointA;
if (r.GetMagnitudeSquared() <= Fix.zero)
{
return;
}
r.Normalize();
// v is perpendicular to the segment.
FixVec2 v = FixVec2.Cross(Fix.one, r);
FixVec2 abs_v = FixVec2.Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Fix maxFraction = Fix.one;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
FixVec2 t = pointA + maxFraction * (pointB - pointA);
segmentAABB.min = FixVec2.Min(pointA, t);
segmentAABB.max = FixVec2.Max(pointA, t);
Stack <int> stack = new Stack <int>();
stack.Push(rootIndex);
while (stack.Count > 0)
{
var nodeId = stack.Pop();
if (nodeId == nullNode)
{
continue;
}
var node = nodes[nodeId];
if (!node.aabb.Overlaps(segmentAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
var c = node.aabb.GetCenter();
var h = node.aabb.GetExtents();
var separation = FixMath.Abs(FixVec2.Dot(v, pointA - c)) - FixVec2.Dot(abs_v, h);
if (separation > Fix.zero)
{
continue;
}
if (node.IsLeaf())
{
Fix value = callback.RayCastCallback(pointA, pointB, maxFraction, nodeId);
if (value == Fix.zero)
{
// The client has terminated the ray cast.
return;
}
if (value > Fix.zero)
{
// Update segment bounding box.
maxFraction = value;
FixVec2 g = pointA + maxFraction * (pointB - pointA);
segmentAABB.min = FixVec2.Min(pointA, g);
segmentAABB.max = FixVec2.Max(pointA, g);
}
}
else
{
stack.Push(node.leftChildIndex);
stack.Push(node.rightChildIndex);
}
}
}
public void ApplyImpulse()
{
// Early out and positional correct if both objects have infinite mass
if (A.invMass + B.invMass == 0)
{
InfiniteMassCorrection();
return;
}
for (int i = 0; i < contactCount; ++i)
{
// Calculate radii from COM to contact
FixVec2 ra = contacts[i] - A.Position;
FixVec2 rb = contacts[i] - B.Position;
//Relative velocity
FixVec2 rv = B.info.velocity + FixVec2.Cross(B.info.angularVelocity, rb) - A.info.velocity - FixVec2.Cross(A.info.angularVelocity, ra);
//Relative velocity along the normal
Fix contactVel = rv.Dot(normal);
if (contactVel > 0)
{
return;
}
Fix raCrossN = FixVec2.Cross(ra, normal);
Fix rbCrossN = FixVec2.Cross(rb, normal);
Fix invMassSum = A.invMass + B.invMass
+ (raCrossN * raCrossN) * A.invInertia
+ (rbCrossN * rbCrossN) * B.invInertia;
// Calculate impulse scalar
Fix j = -(Fix.one + e) * contactVel;
j /= invMassSum;
j /= contactCount;
// Apply impulse
FixVec2 impulse = normal * j;
A.ApplyImpulse(-impulse, ra);
B.ApplyImpulse(impulse, rb);
// Friction Impulse
rv = B.info.velocity + FixVec2.Cross(B.info.angularVelocity, rb)
- A.info.velocity - FixVec2.Cross(A.info.angularVelocity, ra);
FixVec2 t = rv - (normal * FixVec2.Dot(rv, normal));
t = t.Normalized();
// j tangent magnitude
Fix jt = -FixVec2.Dot(rv, t);
jt /= invMassSum;
jt /= contactCount;
//Don't apply tiny friction impulses
if (FixMath.Abs(jt) <= Fix.zero)
{
return;
}
// Coulumb's law
FixVec2 tangentImpulse;
if (FixMath.Abs(jt) < j * sf)
{
tangentImpulse = t * jt;
}
else
{
tangentImpulse = t * -j * df;
}
// Apply friction impulse
A.ApplyImpulse(-tangentImpulse, ra);
B.ApplyImpulse(tangentImpulse, rb);
}
}
public override bool Raycast(out TFRaycastHit2D hit, FixVec2 pointA, FixVec2 pointB, Fix maxFraction)
{
hit = new TFRaycastHit2D();
// Put the ray into the polygon's frame of reference.
var p1 = u.Transposed() * (pointA - (FixVec2)tdTransform.Position);
var p2 = u.Transposed() * (pointB - (FixVec2)tdTransform.Position);
var d = p2 - p1;
Fix lower = Fix.zero, upper = 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 = FixVec2.Dot(normals[i], GetVertex(i) - p1);
var denominator = FixVec2.Dot(normals[i], d);
if (denominator == Fix.zero)
{
if (numerator < Fix.zero)
{
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 < Fix.zero && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > Fix.zero && 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);
}
}
if (index >= 0)
{
hit.fraction = lower;
hit.normal = tdTransform.Rotation * normals[index];
hit.collider = this;
return(true);
}
return(false);
}
// Check if polygon A is going to collide with polygon B for the given velocity
public static PolygonCollisionResult CheckCollision(FixPolygon polygonA, FixPolygon polygonB, FixVec2 velocity)
{
var result = new PolygonCollisionResult {
Intersect = true, WillIntersect = true
};
var edgeCountA = polygonA.Edges.Length;
var edgeCountB = polygonB.Edges.Length;
var minIntervalDistance = Fix.MaxValue;
var translationAxis = new FixVec2();
// Loop through all the edges of both polygons
for (var edgeIndex = 0; edgeIndex < edgeCountA + edgeCountB; edgeIndex++)
{
var edge = edgeIndex < edgeCountA ? polygonA.Edges[edgeIndex] : polygonB.Edges[edgeIndex - edgeCountA];
// ===== 1. Find if the polygons are currently intersecting =====
// Find the axis perpendicular to the current edge
var axis = new FixVec2(-edge.Y, edge.X).Normalize();
// Find the projection of the polygon on the current axis
Fix minA;
Fix maxA;
Fix minB;
Fix maxB;
ProjectPolygon(axis, polygonA, out minA, out maxA);
ProjectPolygon(axis, polygonB, out minB, out maxB);
// Check if the polygon projections are currently intersecting
if (IntervalDistance(minA, maxA, minB, maxB) > 0)
{
result.Intersect = false;
}
// ===== 2. Now find if the polygons *will* intersect =====
// Project the velocity on the current axis
var velocityProjection = axis.Dot(velocity);
// Get the projection of polygon A during the movement
if (velocityProjection < 0)
{
minA += velocityProjection;
}
else
{
maxA += velocityProjection;
}
// Do the same test as above for the new projection
var intervalDistance = IntervalDistance(minA, maxA, minB, maxB);
if (intervalDistance > 0)
{
result.WillIntersect = false;
}
// If the polygons are not intersecting and won't intersect, exit the loop
if (!result.Intersect && !result.WillIntersect)
{
break;
}
// Check if the current interval distance is the minimum one. If so store
// the interval distance and the current distance.
// This will be used to calculate the minimum translation FixVec2
intervalDistance = FixMath.Abs(intervalDistance);
if (intervalDistance < minIntervalDistance)
{
minIntervalDistance = intervalDistance;
translationAxis = axis;
var d = polygonA.Center - polygonB.Center;
if (d.Dot(translationAxis) < 0)
{
translationAxis = -translationAxis;
}
}
}
// The minimum translation FixVec2 can be used to push the polygons apart.
// First moves the polygons by their velocity
// then move polygonA by MinimumTranslationVector.
if (result.WillIntersect)
{
result.MinimumTranslationVector = translationAxis * minIntervalDistance;
}
return(result);
}
