public static FEdge[] GetEdgeNeigbhours(FEdge edge, ref Vector2[] vertices) { int lIndex = LeftIndex(edge.AI, vertices.Length); int rIndex = RightIndex(edge.BI, vertices.Length); return(new FEdge[2] { new FEdge(vertices[lIndex], edge.AV, lIndex, edge.AI), new FEdge(edge.BV, vertices[rIndex], edge.BI, rIndex) }); }
private static bool PolySAT(FCollPoly p1, FCollPoly p2, out CollisionContact manifold) { manifold = null; FSATAxis[] axis1 = GetAxis_sat(ref p1.Vertices); FSATAxis[] axis2 = GetAxis_sat(ref p2.Vertices); FSATAxis collisionAxis = axis1[0]; float penetration = 100000f; for (int i = 0; i < axis1.Length; i++) { Vector2 proj1 = ProjOntoAxis_sat(axis1[i], ref p1.Vertices); Vector2 proj2 = ProjOntoAxis_sat(axis1[i], ref p2.Vertices); float overlap = 0f; if (!GetOverlapFromProjection_sat(proj1, proj2, out axis1[i].BodyA, out overlap)) { return(false); } else { if (ContainsOtherProjection_sat(proj1, proj2) || ContainsOtherProjection_sat(proj2, proj1)) { float mins = Mathf.Abs(proj1.x - proj2.x); float maxs = Mathf.Abs(proj1.y - proj2.y); if (mins < maxs) { overlap += mins; } else { overlap += maxs; } } if (overlap < penetration) { collisionAxis = axis1[i]; penetration = overlap; } } } for (int i = 0; i < axis2.Length; i++) { Vector2 proj1 = ProjOntoAxis_sat(axis2[i], ref p1.Vertices); Vector2 proj2 = ProjOntoAxis_sat(axis2[i], ref p2.Vertices); float overlap = 0f; if (!GetOverlapFromProjection_sat(proj1, proj2, out axis2[i].BodyA, out overlap)) { return(false); } else { if (ContainsOtherProjection_sat(proj1, proj2) || ContainsOtherProjection_sat(proj2, proj1)) { float mins = Mathf.Abs(proj1.x - proj2.x); float maxs = Mathf.Abs(proj1.y - proj2.y); if (mins < maxs) { overlap += mins; } else { overlap += maxs; } } if (overlap < penetration) { collisionAxis = axis2[i]; penetration = overlap; } } } manifold = new CollisionContact(); FEdge eRef; FEdge eInc; if (collisionAxis.BodyA) { eRef = GetBestEdge_sat(ref p1.Vertices, collisionAxis.Axis); eInc = GetBestEdge_sat(ref p2.Vertices, -collisionAxis.Axis); } else { eInc = GetBestEdge_sat(ref p1.Vertices, -collisionAxis.Axis); eRef = GetBestEdge_sat(ref p2.Vertices, collisionAxis.Axis); } bool flipped = false; if (Mathf.Abs(Vector2.Dot(eInc.BV - eInc.AV, collisionAxis.Axis)) < Mathf.Abs(Vector2.Dot(eRef.BV - eRef.AV, collisionAxis.Axis))) { FEdge copy = eRef; eRef = eInc; eInc = copy; flipped = true; } Vector2 refDir = (eRef.BV - eRef.AV).normalized; float offset = Vector2.Dot(refDir, eRef.AV); List <Vector2> contactPoints = Clip_sat(eInc.AV, eInc.BV, refDir, offset); if (contactPoints.Count < 2) { return(true); } offset = Vector2.Dot(eRef.BV, refDir); contactPoints = Clip_sat(contactPoints[0], contactPoints[1], -refDir, -offset); if (contactPoints.Count < 2) { return(true); } eRef.Normal = MeshUtils.CalcNormal(eRef.AV, eRef.BV); eInc.Normal = MeshUtils.CalcNormal(eInc.AV, eInc.BV); //Debug.Log(eRef.Normal); if (Vector2.Dot(eRef.Normal, contactPoints[1] - eRef.AV) > 0f) { contactPoints.RemoveAt(1); } if (Vector2.Dot(eRef.Normal, contactPoints[0] - eRef.AV) > 0f) { contactPoints.RemoveAt(0); } manifold.ContactPoints = contactPoints.ToArray(); manifold.Penetration = penetration; if (flipped) { manifold.Normal = -collisionAxis.Axis; manifold.BodyAInc = collisionAxis.BodyA; manifold.EdgeNormalA = manifold.BodyAInc ? eInc.Normal : eRef.Normal; manifold.EdgeNormalB = manifold.BodyAInc ? eRef.Normal : eInc.Normal; } else { manifold.Normal = collisionAxis.Axis; manifold.BodyAInc = !collisionAxis.BodyA; manifold.EdgeNormalA = manifold.BodyAInc ? eInc.Normal : eRef.Normal; manifold.EdgeNormalB = manifold.BodyAInc ? eRef.Normal : eInc.Normal; } return(true); }