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;
}
void Awake()
{
// add our one-way platforms to our normal platform mask so that we can land on them from above
platformMask |= oneWayPlatformMask;
// cache some components
tfTransform = GetComponent <TFTransform>();
boxCollider = GetComponent <TFPolygonCollider>();
rigidBody2D = GetComponent <TFRigidbody>();
if (rigidBody2D)
{
rigidBody2D.OnTriggerEnter += TFOnTriggerEnter;
rigidBody2D.OnTriggerStay += TFOnTriggerStay;
rigidBody2D.OnTriggerExit += TFOnTriggerExit;
}
// here, we trigger our properties that have setters with bodies
skinWidth = _skinWidth;
// we want to set our CC2D to ignore all collision layers except what is in our triggerMask
for (var i = 0; i < 32; i++)
{
// see if our triggerMask contains this layer and if not ignore it
if ((triggerMask.mask & 1 << i) == 0)
{
Physics2D.IgnoreLayerCollision(gameObject.layer, i);
}
}
}
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;
}
}
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;
}