/// <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); }