public static CollisionInfo CirclevsPolygon(Circle a, Polygon b) { CollisionInfo info = new CollisionInfo(); GJKoutput GJKoutput = GJK(a, b); if (!GJKoutput.result) { return new CollisionInfo() { ContactPoints = new List <CollisionInfo.ContactPoint>() } } ; EPAoutput EPAoutput = EPA(GJKoutput.simplex, a, b); Vector2 cp = GetFarthestPoint(a, EPAoutput.normal); info.ContactPoints.Add(new CollisionInfo.ContactPoint() { depth = EPAoutput.distance, normal = EPAoutput.normal, point = cp }); return(info); }
public static CollisionInfo PolygonvsPolygon(Polygon a, Polygon b) { int GetFarthestPointIndex(Vector2[] polygon, Vector2 direction) { int index = 0; if (direction.y == 0)//x coord check { for (int vert = 0; vert < polygon.Length; vert++) { if (sgn(polygon[vert].x - polygon[index].x) == sgn(direction.x))//if direction == right searching max(x) else min { index = vert; } } return(index); } float k = -direction.x / direction.y; //perpendicular direction angular coefficient //y = kx + b line equation. we find line equation for each point and b //is the intersection point of this line with Oy. Extremum of b means that there is the //ultimate point in the direction, perpendicular to this line i.e. "direction" vector for (int vert = 0; vert < polygon.Length; vert++) { if (sgn((polygon[vert].y - polygon[vert].x * k) - (polygon[index].y - polygon[index].x * k)) == sgn(direction.y))//if top direction searching max(b) else min(b) { index = vert; } } return(index); } float GetSqrDistanceFromPointToLine(Vector2 point, Line line) { //ax + by + c = 0 //distance = |a * x1 + b * y1 + c| / sqrt(a ^ 2 + b ^ 2) float a_k = line.start.y - line.end.y; float b_k = line.end.x - line.start.x; float c_k = Vector2.CrossProduct(line.start, line.end); return(sqr(a_k * point.x + b_k * point.y + c_k) / (sqr(a_k) + sqr(b_k))); } bool PolygonContainsPoint(Vector2[] poly, Vector2 point) { for (int i = 0; i < poly.Length; i++) { int j = (i + 1) % poly.Length; float a_k = poly[i].y - poly[j].y; float b_k = poly[j].x - poly[i].x; float c_k = Vector2.CrossProduct(poly[i], poly[j]); int k = (i + 2) % poly.Length; float vert_halfspace = sgn(a_k * poly[k].x + b_k * poly[k].y + c_k); float point_halfspace = sgn(a_k * point.x + b_k * point.y + c_k); if (point_halfspace == -vert_halfspace) { return(false); } } return(true); } CollisionInfo info = new CollisionInfo(); GJKoutput GJKoutput = GJK(a, b); if (!GJKoutput.result) { return new CollisionInfo() { ContactPoints = new List <CollisionInfo.ContactPoint>() } } ; EPAoutput EPAoutput = EPA(GJKoutput.simplex, a, b); #region shitcode Vector2 normal = EPAoutput.normal; float distance = EPAoutput.distance; info.ContactPoints = new List <CollisionInfo.ContactPoint>(); bool flip = false; int incident_edge_start = GetFarthestPointIndex(a.Verts, normal); int incident_edge_end = (incident_edge_start + 1) % a.Verts.Length; if (!(abs((a.Verts[incident_edge_start] - a.Verts[incident_edge_end]) * normal) < tolerance)) { incident_edge_end = (incident_edge_start - 1 + a.Verts.Length) % a.Verts.Length; } if (!(abs((a.Verts[incident_edge_start] - a.Verts[incident_edge_end]) * normal) < tolerance)) { flip = true; incident_edge_start = GetFarthestPointIndex(b.Verts, -normal); incident_edge_end = (incident_edge_start + 1) % b.Verts.Length; if (!(abs((b.Verts[incident_edge_start] - b.Verts[incident_edge_end]) * normal) < tolerance)) { incident_edge_end = (incident_edge_start - 1 + b.Verts.Length) % b.Verts.Length; } } Line incident_edge; if (!flip) { //a incident_edge = new Line(a.Verts[incident_edge_start], a.Verts[incident_edge_end]); } else { //b incident_edge = new Line(b.Verts[incident_edge_start], b.Verts[incident_edge_end]); } if (!flip) { //a int b_farthest = GetFarthestPointIndex(b.Verts, -normal); int b_farthest_left_neighb = (b_farthest + 1) % b.Verts.Length; int b_farthest_right_neighb = (b_farthest - 1 + b.Verts.Length) % b.Verts.Length; info.ContactPoints.Add(new CollisionInfo.ContactPoint() { depth = distance, normal = normal, point = b.Verts[b_farthest] + normal * distance }); bool a_contains_left = PolygonContainsPoint(a.Verts, b.Verts[b_farthest_left_neighb]); bool a_contains_right = PolygonContainsPoint(a.Verts, b.Verts[b_farthest_right_neighb]); if (a_contains_left && a_contains_right) { float left_sqr_dist = GetSqrDistanceFromPointToLine(b.Verts[b_farthest_left_neighb], incident_edge); float right_sqr_dist = GetSqrDistanceFromPointToLine(b.Verts[b_farthest_right_neighb], incident_edge); int b_almostfarthest = (right_sqr_dist > left_sqr_dist) ? b_farthest_right_neighb : b_farthest_left_neighb; float b_almost_farthest_distance = sqrt(max(right_sqr_dist, left_sqr_dist)); info.ContactPoints.Add(new CollisionInfo.ContactPoint() { depth = b_almost_farthest_distance, normal = normal, point = b.Verts[b_almostfarthest] + normal * b_almost_farthest_distance }); } else if (!a_contains_left && !a_contains_right) { return(info); } else if (a_contains_left) { float dist = sqrt(GetSqrDistanceFromPointToLine(b.Verts[b_farthest_left_neighb], incident_edge)); info.ContactPoints.Add(new CollisionInfo.ContactPoint() { depth = dist, normal = normal, point = b.Verts[b_farthest_left_neighb] + normal * dist }); } else { float dist = sqrt(GetSqrDistanceFromPointToLine(b.Verts[b_farthest_right_neighb], incident_edge)); info.ContactPoints.Add(new CollisionInfo.ContactPoint() { depth = dist, normal = normal, point = b.Verts[b_farthest_right_neighb] + normal * dist }); } return(info); } else { //b int a_farthest = GetFarthestPointIndex(a.Verts, normal); int a_farthest_left_neighb = (a_farthest + 1) % a.Verts.Length; int a_farthest_right_neighb = (a_farthest - 1 + a.Verts.Length) % a.Verts.Length; info.ContactPoints.Add(new CollisionInfo.ContactPoint() { depth = distance, normal = normal, point = a.Verts[a_farthest] + normal * distance }); bool b_contains_left = PolygonContainsPoint(b.Verts, a.Verts[a_farthest_left_neighb]); bool b_contains_right = PolygonContainsPoint(b.Verts, a.Verts[a_farthest_right_neighb]); if (b_contains_left && b_contains_right) { float left_sqr_dist = GetSqrDistanceFromPointToLine(a.Verts[a_farthest_left_neighb], incident_edge); float right_sqr_dist = GetSqrDistanceFromPointToLine(a.Verts[a_farthest_right_neighb], incident_edge); int a_almostfarthest = (right_sqr_dist > left_sqr_dist) ? a_farthest_right_neighb : a_farthest_left_neighb; float a_almost_farthest_distance = sqrt(max(right_sqr_dist, left_sqr_dist)); info.ContactPoints.Add(new CollisionInfo.ContactPoint() { depth = a_almost_farthest_distance, normal = normal, point = a.Verts[a_almostfarthest] + normal * a_almost_farthest_distance }); } else if (!b_contains_left && !b_contains_right) { return(info); } else if (b_contains_left) { float dist = sqrt(GetSqrDistanceFromPointToLine(a.Verts[a_farthest_left_neighb], incident_edge)); info.ContactPoints.Add(new CollisionInfo.ContactPoint() { depth = dist, normal = normal, point = a.Verts[a_farthest_left_neighb] + normal * dist }); } else { float dist = sqrt(GetSqrDistanceFromPointToLine(a.Verts[a_farthest_right_neighb], incident_edge)); info.ContactPoints.Add(new CollisionInfo.ContactPoint() { depth = dist, normal = normal, point = a.Verts[a_farthest_right_neighb] + normal * dist }); } return(info); } #endregion }