/// <summary>
/// Finds the axis of least penetration by finding the greatest support point in the direction opposite the face normal for each face of the polygon
/// </summary>
/// <param name="faceIndex">Saves the index of the face with the greatest support point, which is also the face that is being collided with</param>
/// <param name="A"></param>
/// <param name="B"></param>
/// <returns></returns>
public float FindAxisofLeastPenetration(ref int faceIndex, PerfectPolygon A, PerfectPolygon B)
{
float bestDistance = -float.MaxValue;
int bestIndex = 0;
for (int i = 0; i < A.points.Length; i++)
{
Vector2 n = A.faceNormals[i];
//Return vertex farthest in the -n direction
Vector2 s = B.GetSupport(-n);
Vector2 p = A.points[i];
//Calculates the distance along the face normal of the vector from p to s
float d = Vector2.Dot(n, s - p);
if (d > bestDistance)
{
bestDistance = d;
bestIndex = i;
}
}
faceIndex = bestIndex;
return(bestDistance);
}
/// <summary>
/// Calculates the objects mass based on it's area and the provided density
/// </summary>
/// <param name="shape"></param>
/// <param name="density"></param>
/// <param name="sides"></param>
public MassData(Shape shape, float density, int sides = 0)
{
float area = 0.0f;
PerfectPolygon poly = (shape as PerfectPolygon);
float sideLength = (float)Math.Sqrt(poly.faces[0].X * poly.faces[0].X + poly.faces[0].Y * poly.faces[0].Y);
area = (float)(0.25f * sides * ((sideLength * 0.1f) * (sideLength * 0.1f)) * (1 / Math.Tan(Math.PI / sides)));
mass = density * area;
invMass = mass > 0 ? 1 / mass : 0;
}
/// <summary>
/// Recalculates the bounding box for a polygon, which is neccissary because the min and max change during rotation
/// This process is not neccissary for circles as they are not affected by rotation
/// </summary>
/// <param name="pP"></param>
public void CalculateAABB(PerfectPolygon pP)
{
Vector2 minimum = new Vector2(float.MaxValue, float.MaxValue);
Vector2 maximum = new Vector2(float.MinValue, float.MinValue);
//Loops through all points and finds the min X and Y as well as the max for all the points
for (int i = 0; i < pP.points.Length; i++)
{
minimum = new Vector2(Math.Min(minimum.X, pP.points[i].X), Math.Min(minimum.Y, pP.points[i].Y));
maximum = new Vector2(Math.Max(maximum.X, pP.points[i].X), Math.Max(maximum.Y, pP.points[i].Y));
}
//set min and max equal to the minimum point and the maximum point
min = minimum;
max = maximum;
extent = max - min;
center = min + (extent / 2);
}
/// <summary>
/// Finds the incident face on the inciting polygon
/// </summary>
/// <param name="p">Saves the 2 points that make up the incident face</param>
/// <param name="reference"></param>
/// <param name="incident"></param>
/// <param name="referenceIndex">Index of the inciting face</param>
public void FindIncidentFace(ref Vector2[] p, PerfectPolygon reference, PerfectPolygon incident, int referenceIndex)
{
//Face of the reference polygon that is being collided with
Vector2 referenceNormal = reference.faceNormals[referenceIndex];
int incidentFace = 0;
float minD = float.MaxValue;
//Loops through all of the faces of the inciting polygon and determines the face most perpendicular
//to the face of the reference polygon that is being collided with
for (int i = 0; i < incident.points.Length; i++)
{
float d = Vector2.Dot(referenceNormal, incident.faceNormals[i]);
if (d < minD)
{
minD = d;
incidentFace = i;
}
}
//Saves the faces of the 2 points that make up the inciting face on the inciting polygon
p[0] = incident.points[incidentFace];
p[1] = incident.points[incidentFace + 1 >= (int)incident.points.Length ? 0 : incidentFace + 1];
}
/// <summary>
/// Tests to see if 2 polygons are colliding
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
public bool PolygonvsPolygon(Pair pair)
{
PerfectPolygon A = (pair.A.Shape as PerfectPolygon);
PerfectPolygon B = (pair.B.Shape as PerfectPolygon);
Pair.ContactCount = 0;
//find the index of the reference face and return the distance of the greatest support point
int faceA = 0;
float penetrationA = FindAxisofLeastPenetration(ref faceA, A, B);
//if the support point is positive it means the objects have not collided
if (penetrationA >= 0.0f)
{
return(false);
}
int faceB = 0;
float penetrationB = FindAxisofLeastPenetration(ref faceB, B, A);
if (penetrationB >= 0.0f)
{
return(false);
}
int referenceIndex = 0;
bool flip;
PerfectPolygon refPoly;
PerfectPolygon incPoly;
//check to see which of the penetrations is greater and assign the reference and incident polygons as such
if (BiasGreaterThan(penetrationA, penetrationB))
{
refPoly = A;
incPoly = B;
referenceIndex = faceA;
flip = false;
}
else
{
refPoly = B;
incPoly = A;
referenceIndex = faceB;
flip = true;
}
//Find the inciting face of the inciting polygon
Vector2[] incidentFacePoints = new Vector2[2];
FindIncidentFace(ref incidentFacePoints, refPoly, incPoly, referenceIndex);
//points that form the inciting face
Vector2 point1 = refPoly.points[referenceIndex];
Vector2 point2 = refPoly.points[referenceIndex + 1 == refPoly.points.Length ? 0 : referenceIndex + 1];
Vector2 refFaceNormal = refPoly.faceNormals[referenceIndex];
//Distance from the origion along the reference face normal
float refC = Vector2.Dot(refFaceNormal, point1);
//Calculations were made from the incient to the reference but the method is running to find A to B
//So if A is the inciting object then the normal is backwards and needs to be flipped
Pair.Normal = flip ? -refFaceNormal : refFaceNormal;
int cp = 0;
//Distance from origin of the first incident face point along reference face normal minus the one for reference face
float separation = Vector2.Dot(refFaceNormal, incidentFacePoints[0]) - refC;
if (separation <= 0.0f)
{
//add the currenct seperation to the penetration
Pair.Contacts[cp] = incidentFacePoints[0];
Pair.Penetration += -separation;
Pair.ContactCount++;
cp++;
}
else
{
Pair.Penetration = 0;
}
//same as previous but for the other incident face point
separation = Vector2.Dot(refFaceNormal, incidentFacePoints[1]) - refC;
if (separation <= 0.0f)
{
Pair.Contacts[cp] = incidentFacePoints[1];
Pair.Penetration += -separation;
Pair.ContactCount++;
cp++;
//average the penetration
Pair.Penetration /= (float)cp;
}
return(true);
}
