//
// Line-plane intersection
//
//2d
public static bool LinePlane(Plane2 plane, Edge2 line)
{
//To avoid floating point precision issues we can add a small value
float epsilon = MathUtility.EPSILON;
bool areIntersecting = false;
MyVector2 lineDir = MyVector2.Normalize(line.p1 - line.p2);
float denominator = MyVector2.Dot(-plane.normal, lineDir);
//Debug.Log(denominator);
//No intersection if the line and plane are perpendicular
if (denominator > epsilon || denominator < -epsilon)
{
MyVector2 vecBetween = plane.pos - line.p1;
float t = MyVector2.Dot(vecBetween, -plane.normal) / denominator;
MyVector2 intersectionPoint = line.p1 + lineDir * t;
//Gizmos.DrawWireSphere(intersectionPoint, 0.5f);
if (_Geometry.IsPointBetweenPoints(line.p1, line.p2, intersectionPoint))
{
areIntersecting = true;
}
}
return(areIntersecting);
}
//Whats the coordinate of the intersection point between two lines in 2d space if we know they are intersecting
//http://thirdpartyninjas.com/blog/2008/10/07/line-segment-intersection/
public static MyVector2 GetLineLineIntersectionPoint(Edge2 a, Edge2 b)
{
float denominator = (b.p2.y - b.p1.y) * (a.p2.x - a.p1.x) - (b.p2.x - b.p1.x) * (a.p2.y - a.p1.y);
float u_a = ((b.p2.x - b.p1.x) * (a.p1.y - b.p1.y) - (b.p2.y - b.p1.y) * (a.p1.x - b.p1.x)) / denominator;
MyVector2 intersectionPoint = a.p1 + u_a * (a.p2 - a.p1);
return(intersectionPoint);
}
//We know a line plane is intersecting and now we want the coordinate of intersection
//2d
public static MyVector2 GetLinePlaneIntersectionPoint(Plane2 plane, Edge2 line)
{
MyVector2 lineDir = MyVector2.Normalize(line.p1 - line.p2);
Ray2 ray = new Ray2(line.p1, lineDir);
MyVector2 intersectionPoint = GetIntersectionCoordinate(plane, ray);
return(intersectionPoint);
}
//Find the vertex which is not an edge
public MyVector2 GetVertexWhichIsNotPartOfEdge(Edge2 e)
{
if (!p1.Equals(e.p1) && !p1.Equals(e.p2))
{
return(p1);
}
if (!p2.Equals(e.p1) && !p2.Equals(e.p2))
{
return(p2);
}
return(p3);
}
//
// Line-point calculations
//
//From https://stackoverflow.com/questions/3120357/get-closest-point-to-a-line
//and https://www.youtube.com/watch?v=_ENEsV_kNx8
public static MyVector2 GetClosestPointOnLine(Edge2 e, MyVector2 p, bool withinSegment)
{
MyVector2 a = e.p1;
MyVector2 b = e.p2;
//Assume the line goes from a to b
MyVector2 ab = b - a;
//Vector from "start" of the line to the point outside of line
MyVector2 ap = p - a;
//Scalar projection https://en.wikipedia.org/wiki/Scalar_projection
//The scalar projection is a scalar, equal to the length of the orthogonal projection of ap on ab, with a negative sign if the projection has an opposite direction with respect to ab.
//scalarProjection = Dot(ap, ab) / Magnitude(ab) where the magnitude of ab is the distance between a and b
//If ab is normalized, we get scalarProjection = Dot(ap, ab)
//The distance from a to q (the closes point on the line):
//float aq_distance = MyVector2.Dot(ap, ab) / MyVector2.Magnitude(ab);
//To get the closest point on the line:
//MyVector2 q = a + MyVector2.Normalize(ab) * aq_distance;
//Can we do better?
//Magnitude is defined as: Mathf.Sqrt((ab * ab))
//Normalization is defined as (ab / magnitude(ab))
//We get: q = a + (ab / magnitude(ab)) * (1 / magnitude(ab)) * dot(ap, ab)
//Ignore the q and the dot and we get: (ab / Mathf.Sqrt((ab * ab))) * (1 / Mathf.Sqrt((ab * ab))) = ab / (ab * ab)
//So we can use the square magnitude of ab and then we don't need to normalize ab (to get q), so we save two square roots, which is good because square root is a slow operation
//The normalized "distance" from a to the closest point, so between 0 and 1 if we are within the line segment
float distance = MyVector2.Dot(ap, ab) / MyVector2.SqrMagnitude(ab);
//This point may not be on the line segment, if so return one of the end points
float epsilon = MathUtility.EPSILON;
if (withinSegment && distance < 0f - epsilon)
{
return(a);
}
else if (withinSegment && distance > 1f + epsilon)
{
return(b);
}
else
{
//This works because a_b is not normalized and distance is [0,1] if distance is within ab
return(a + ab * distance);
}
}
//Check if an edge is a part of this triangle
public bool IsEdgePartOfTriangle(Edge2 e)
{
if ((e.p1.Equals(p1) && e.p2.Equals(p2)) || (e.p1.Equals(p2) && e.p2.Equals(p1)))
{
return(true);
}
if ((e.p1.Equals(p2) && e.p2.Equals(p3)) || (e.p1.Equals(p3) && e.p2.Equals(p2)))
{
return(true);
}
if ((e.p1.Equals(p3) && e.p2.Equals(p1)) || (e.p1.Equals(p1) && e.p2.Equals(p3)))
{
return(true);
}
return(false);
}
//
// Are two lines intersecting?
//
//http://thirdpartyninjas.com/blog/2008/10/07/line-segment-intersection/
//Notice that there are more than one way to test if two line segments are intersecting
//but this is the fastest according to https://www.habrador.com/tutorials/math/5-line-line-intersection/
public static bool LineLine(Edge2 a, Edge2 b, bool includeEndPoints)
{
//To avoid floating point precision issues we can use a small value
float epsilon = MathUtility.EPSILON;
bool isIntersecting = false;
float denominator = (b.p2.y - b.p1.y) * (a.p2.x - a.p1.x) - (b.p2.x - b.p1.x) * (a.p2.y - a.p1.y);
//Make sure the denominator is != 0 (or the lines are parallel)
if (denominator > 0f + epsilon || denominator < 0f - epsilon)
{
float u_a = ((b.p2.x - b.p1.x) * (a.p1.y - b.p1.y) - (b.p2.y - b.p1.y) * (a.p1.x - b.p1.x)) / denominator;
float u_b = ((a.p2.x - a.p1.x) * (a.p1.y - b.p1.y) - (a.p2.y - a.p1.y) * (a.p1.x - b.p1.x)) / denominator;
//Are the line segments intersecting if the end points are the same
if (includeEndPoints)
{
//The only difference between endpoints not included is the =, which will never happen so we have to subtract 0 by epsilon
float zero = 0f - epsilon;
float one = 1f + epsilon;
//Are intersecting if u_a and u_b are between 0 and 1 or exactly 0 or 1
if (u_a >= zero && u_a <= one && u_b >= zero && u_b <= one)
{
isIntersecting = true;
}
}
else
{
float zero = 0f + epsilon;
float one = 1f - epsilon;
//Are intersecting if u_a and u_b are between 0 and 1
if (u_a > zero && u_a < one && u_b > zero && u_b < one)
{
isIntersecting = true;
}
}
}
return(isIntersecting);
}
//Find an edge in a triangulation and return the triangle the edge is attached to
private static void FindEdgeInTriangulation(Triangle2 tNew, HashSet <Triangle2> triangulation, out bool hasOppositeEdge, out Triangle2 tOpposite, out Edge2 edgeToSwap)
{
//Step 1. Find the triangle's biggest interior angle and its opposite edge
float angleP1 = CalculateInteriorAngle(tNew.p3, tNew.p1, tNew.p2);
float angleP2 = CalculateInteriorAngle(tNew.p1, tNew.p2, tNew.p3);
float angleP3 = Mathf.PI - (angleP1 + angleP2);
MyVector2 vertexWithBiggestInteriorAngle = tNew.p1;
if (angleP2 > angleP1)
{
vertexWithBiggestInteriorAngle = tNew.p2;
if (angleP3 > angleP2)
{
vertexWithBiggestInteriorAngle = tNew.p3;
}
}
else if (angleP3 > angleP1)
{
vertexWithBiggestInteriorAngle = tNew.p3;
}
edgeToSwap = tNew.FindOppositeEdgeToVertex(vertexWithBiggestInteriorAngle);
//Step 2. Check if this edge exists among the already generated triangles, which means we have a neighbor
hasOppositeEdge = false;
tOpposite = new Triangle2();
foreach (Triangle2 tTest in triangulation)
{
if (tTest.IsEdgePartOfTriangle(edgeToSwap))
{
hasOppositeEdge = true;
tOpposite = tTest;
break;
}
}
}
//Whats the coordinate of the intersection point between two lines in 2d space if we know they are intersecting
//http://thirdpartyninjas.com/blog/2008/10/07/line-segment-intersection/
public static MyVector2 GetLineLineIntersectionPoint(Edge2 l1, Edge2 l2)
{
return(GetLineLineIntersectionPoint(l1.p1, l1.p2, l2.p1, l2.p2));
}
//
// Are two lines intersecting?
//
//http://thirdpartyninjas.com/blog/2008/10/07/line-segment-intersection/
//Notice that there are more than one way to test if two line segments are intersecting
//but this is the fastest according to https://www.habrador.com/tutorials/math/5-line-line-intersection/
public static bool LineLine(Edge2 l1, Edge2 l2, bool includeEndPoints)
{
return(LineLine(l1.p1, l1.p2, l2.p1, l2.p2, includeEndPoints));
}
public static List <List <MyVector2> > ClipPolygons(List <MyVector2> polyVector2, List <MyVector2> clipPolyVector2, BooleanOperation booleanOperation)
{
List <List <MyVector2> > finalPoly = new List <List <MyVector2> >();
//Step 0. Create the data structure needed
List <ClipVertex> poly = InitDataStructure(polyVector2);
List <ClipVertex> clipPoly = InitDataStructure(clipPolyVector2);
//Step 1. Find intersection points
//Need to test if we have found an intersection point,
//if none is found, the polygons dont intersect, or one polygon is inside the other
bool hasFoundIntersection = false;
for (int i = 0; i < poly.Count; i++)
{
ClipVertex currentVertex = poly[i];
//Important to use iPlusOne because poly.next may change
int iPlusOne = MathUtility.ClampListIndex(i + 1, poly.Count);
MyVector2 a = poly[i].coordinate;
MyVector2 b = poly[iPlusOne].coordinate;
Edge2 a_b = new Edge2(a, b);
//Gizmos.DrawWireSphere(poly[i].coordinate, 0.02f);
//Gizmos.DrawWireSphere(poly[i].next.coordinate, 0.02f);
for (int j = 0; j < clipPoly.Count; j++)
{
int jPlusOne = MathUtility.ClampListIndex(j + 1, clipPoly.Count);
MyVector2 c = clipPoly[j].coordinate;
MyVector2 d = clipPoly[jPlusOne].coordinate;
Edge2 c_d = new Edge2(c, d);
//Are these lines intersecting?
if (_Intersections.LineLine(a_b, c_d, true))
{
hasFoundIntersection = true;
MyVector2 intersectionPoint2D = _Intersections.GetLineLineIntersectionPoint(a_b, c_d);
//Vector3 intersectionPoint = new Vector3(intersectionPoint2D.x, 0f, intersectionPoint2D.y);
//Gizmos.color = Color.red;
//Gizmos.DrawWireSphere(intersectionPoint, 0.04f);
//We need to insert this intersection vertex into both polygons
//Insert into the polygon
ClipVertex vertexOnPolygon = InsertIntersectionVertex(a, b, intersectionPoint2D, currentVertex);
//Insert into the clip polygon
ClipVertex vertexOnClipPolygon = InsertIntersectionVertex(c, d, intersectionPoint2D, clipPoly[j]);
//Also connect the intersection vertices with each other
vertexOnPolygon.neighbor = vertexOnClipPolygon;
vertexOnClipPolygon.neighbor = vertexOnPolygon;
}
}
}
//Debug in which order the vertices are in the linked list
//InWhichOrderAreVerticesAdded(poly);
//InWhichOrderAreVerticesAdded(clipPoly);
//If the polygons are intersecting
if (hasFoundIntersection)
{
//Step 2. Trace each polygon and mark entry and exit points to the other polygon's interior
MarkEntryExit(poly, clipPolyVector2);
MarkEntryExit(clipPoly, polyVector2);
//Debug entry exit points
DebugEntryExit(poly);
//DebugEntryExit(clipPoly);
//Step 3. Create the desired clipped polygon
if (booleanOperation == BooleanOperation.Intersection)
{
//Where the two polygons intersect
List <ClipVertex> intersectionVertices = GetClippedPolygon(poly, true);
//Debug.Log(intersectionVertices.Count);
AddPolygonToList(intersectionVertices, finalPoly, false);
//Debug.Log();
}
else if (booleanOperation == BooleanOperation.Difference)
{
//Whats outside of the polygon that doesnt intersect
List <ClipVertex> outsidePolyVertices = GetClippedPolygon(poly, false);
AddPolygonToList(outsidePolyVertices, finalPoly, true);
}
else if (booleanOperation == BooleanOperation.ExclusiveOr)
{
//Whats outside of the polygon that doesnt intersect
List <ClipVertex> outsidePolyVertices = GetClippedPolygon(poly, false);
AddPolygonToList(outsidePolyVertices, finalPoly, true);
//Whats outside of the polygon that doesnt intersect
List <ClipVertex> outsideClipPolyVertices = GetClippedPolygon(clipPoly, false);
AddPolygonToList(outsideClipPolyVertices, finalPoly, true);
}
else if (booleanOperation == BooleanOperation.Union)
{
//Where the two polygons intersect
List <ClipVertex> intersectionVertices = GetClippedPolygon(poly, true);
AddPolygonToList(intersectionVertices, finalPoly, false);
//Whats outside of the polygon that doesnt intersect
List <ClipVertex> outsidePolyVertices = GetClippedPolygon(poly, false);
AddPolygonToList(outsidePolyVertices, finalPoly, true);
//Whats outside of the polygon that doesnt intersect
List <ClipVertex> outsideClipPolyVertices = GetClippedPolygon(clipPoly, false);
AddPolygonToList(outsideClipPolyVertices, finalPoly, true);
}
//Where the two polygons intersect
//List<ClipVertex> intersectionVertices = GetClippedPolygon(poly, true);
//Whats outside of the polygon that doesnt intersect
//These will be in clockwise order so remember to change to counter clockwise
//List<ClipVertex> outsidePolyVertices = GetClippedPolygon(poly, false);
//List<ClipVertex> outsideClipPolyVertices = GetClippedPolygon(clipPoly, false);
}
//Check if one polygon is inside the other
else
{
//Is the polygon inside the clip polygon?
//Depending on the type of boolean operation, we might get a hole
if (IsPolygonInsidePolygon(polyVector2, clipPolyVector2))
{
Debug.Log("Poly is inside clip poly");
}
else if (IsPolygonInsidePolygon(clipPolyVector2, polyVector2))
{
Debug.Log("Clip poly is inside poly");
}
else
{
Debug.Log("Polygons are not intersecting");
}
}
return(finalPoly);
}
//
// Are two triangles intersecting?
//
//2d
public static bool TriangleTriangle(Triangle2 t1, Triangle2 t2, bool do_AABB_test)
{
bool isIntersecting = false;
//Step 0. AABB intersection which may speed up the algorithm if the triangles are far apart
if (do_AABB_test)
{
//Rectangle that covers t1
AABB2 r1 = new AABB2(t1.MinX(), t1.MaxX(), t1.MinY(), t1.MaxY());
//Rectangle that covers t2
AABB2 r2 = new AABB2(t2.MinX(), t2.MaxX(), t2.MinY(), t2.MaxY());
if (!AABB_AABB(r1, r2))
{
return(false);
}
}
//Step 1. Line-line instersection
Edge2 t1_e1 = new Edge2(t1.p1, t1.p2);
Edge2 t1_e2 = new Edge2(t1.p2, t1.p3);
Edge2 t1_e3 = new Edge2(t1.p3, t1.p1);
Edge2 t2_e1 = new Edge2(t2.p1, t2.p2);
Edge2 t2_e2 = new Edge2(t2.p2, t2.p3);
Edge2 t2_e3 = new Edge2(t2.p3, t2.p1);
//Line 1 of t1 against all lines of t2
if (
LineLine(t1_e1, t2_e1, true) ||
LineLine(t1_e1, t2_e2, true) ||
LineLine(t1_e1, t2_e3, true)
)
{
isIntersecting = true;
}
//Line 2 of t1 against all lines of t2
else if (
LineLine(t1_e2, t2_e1, true) ||
LineLine(t1_e2, t2_e2, true) ||
LineLine(t1_e2, t2_e3, true)
)
{
isIntersecting = true;
}
//Line 3 of t1 against all lines of t2
else if (
LineLine(t1_e3, t2_e1, true) ||
LineLine(t1_e3, t2_e2, true) ||
LineLine(t1_e3, t2_e3, true)
)
{
isIntersecting = true;
}
//Now we can return if we are intersecting so we dont need to spend time testing something else
if (isIntersecting)
{
return(isIntersecting);
}
//Step 2. Point-in-triangle intersection
//We only need to test one corner from each triangle
//If this point is not in the triangle, then the other points can't be in the triangle, because if this point is outside
//and another point is inside, then the line between them would have been covered by step 1: line-line intersections test
if (PointTriangle(t2, t1.p1, true) || PointTriangle(t1, t2.p1, true))
{
isIntersecting = true;
}
return(isIntersecting);
}
//Merge a single hole with the hull
//Basic idea is to find a vertex in the hole that can also see a vertex in the hull
//Connect these vertices with two edges, and the hole is now a part of the hull with an invisible seam
//between the hole and the hull
private static void MergeHoleWithHull(EarClippingPolygon hull, EarClippingPolygon hole)
{
//Step 1. Find the vertex in the hole which has the maximum x-value
//Has already been done when we created the data structure
//Step 2. Form a line going from this vertex towards (in x-direction) to a position outside of the hull
MyVector2 lineStart = hole.maxX_Vert;
//Just add some value so we know we are outside
MyVector2 lineEnd = new MyVector2(hull.maxX_Vert.x + 0.01f, hole.maxX_Vert.y);
//Important to add lineStart first because we will need it later
Edge2 line_hole_to_outside = new Edge2(lineStart, lineEnd);
//Step 3. Find a vertex on the hull which is visible to the point on the hole with max x pos
//The first and second point on the hull is defined as edge 0, and so on...
int closestEdge = -1;
MyVector2 visibleVertex;
FindVisibleVertexOnHUll(hull, hole, line_hole_to_outside, out closestEdge, out visibleVertex);
//This means we couldn't find a closest edge
if (closestEdge == -1)
{
Debug.Log("Couldn't find a closest edge to hole");
return;
}
//Step 4. Modify the hull vertices so we get an edge from the hull to the hole, around the hole, and back to the hull
//First reconfigure the hole list to start at the vertex with the largest x pos
//[a, b, c, d, e] and c is the one with the largest x pos, we get:
//[a, b, c, d, e, a, b]
//[c, d, e, a, b]
//We also need two extra vertices, one from the hole and one from the hull
//If p is the visible vertex, we get:
//[c, d, e, a, b, c, p]
//Maybe more efficient if we turn the list into a queue?
//Add to back of list
for (int i = 0; i < hole.maxX_ListPos; i++)
{
hole.Vertices.Add(hole.Vertices[i]);
}
//Remove those we added to the back of the list
hole.Vertices.RemoveRange(0, hole.maxX_ListPos);
//Add the two extra vertices we need
hole.Vertices.Add(hole.Vertices[0]);
hole.Vertices.Add(visibleVertex);
//Merge the hole with the hull
List <MyVector2> verticesHull = hull.Vertices;
//Find where we should insert the hole
int visibleVertex_ListPos = hull.GetLastListPos(visibleVertex);
if (visibleVertex_ListPos == -1)
{
Debug.Log("Cant find corresponding pos in list");
return;
}
//Insert the hole after the visible vertex
verticesHull.InsertRange(visibleVertex_ListPos + 1, hole.Vertices);
//Debug.Log($"Number of vertices on the hull after adding a hole: {verticesHull.Count}");
}
//Find a vertex on the hull that should be visible from the hole
private static void FindVisibleVertexOnHUll(EarClippingPolygon hull, EarClippingPolygon hole, Edge2 line_hole_to_outside, out int closestEdge, out MyVector2 visibleVertex)
{
//The first and second point on the hull is defined as edge 0, and so on...
closestEdge = -1;
//The vertex that should be visible to the hole (which is the max of the line that's intersecting with the line)
visibleVertex = new MyVector2(-1f, -1f);
//Do line-line intersection to find intersectionVertex which is the point of intersection that's the closest to the hole
MyVector2 intersectionVertex = new MyVector2(-1f, -1f);
float minDistanceSqr = Mathf.Infinity;
List <MyVector2> verticesHull = hull.Vertices;
for (int i = 0; i < verticesHull.Count; i++)
{
MyVector2 p1_hull = verticesHull[i];
MyVector2 p2_hull = verticesHull[MathUtility.ClampListIndex(i + 1, verticesHull.Count)];
//We dont need to check this line if it's to the left of the point on the hole
//If so they cant intersect
if (p1_hull.x < hole.maxX_Vert.x && p2_hull.x < hole.maxX_Vert.x)
{
continue;
}
Edge2 line_hull = new Edge2(p1_hull, p2_hull);
bool isIntersecting = _Intersections.LineLine(line_hole_to_outside, line_hull, includeEndPoints: true);
//Here we can maybe add a check if any of the vertices is on the line???
if (isIntersecting)
{
MyVector2 testIntersectionVertex = _Intersections.GetLineLineIntersectionPoint(line_hole_to_outside, line_hull);
//if (hole.id == 3) TestAlgorithmsHelpMethods.DebugDrawCircle(testIntersectionVertex.ToVector3(), 0.2f, Color.green);
float distanceSqr = MyVector2.SqrDistance(line_hole_to_outside.p1, testIntersectionVertex);
if (distanceSqr < minDistanceSqr)
{
closestEdge = i;
minDistanceSqr = distanceSqr;
intersectionVertex = testIntersectionVertex;
}
}
}
//This means we couldn't find a closest edge
if (closestEdge == -1)
{
Debug.Log("Couldn't find a closest edge to hole");
return;
}
//But we can't connect the hole with this intersection point, so we need to find a vertex which is visible from the hole
//The closest edge has two vertices. Pick the one with the highest x-value, which is the vertex
//that should be visible from the hole
MyVector2 p1 = hull.Vertices[closestEdge];
MyVector2 p2 = hull.Vertices[MathUtility.ClampListIndex(closestEdge + 1, hull.Vertices.Count)];
visibleVertex = p1;
//They are the same so pick the one that is the closest
if (Mathf.Abs(p1.x - p2.x) < MathUtility.EPSILON)
{
float hole_p1 = MyVector2.SqrDistance(hole.maxX_Vert, p1);
float hole_p2 = MyVector2.SqrDistance(hole.maxX_Vert, p2);
visibleVertex = hole_p1 < hole_p2 ? p1 : p2;
}
else if (p2.x > visibleVertex.x)
{
visibleVertex = p2;
}
if (hole.id == 3)
{
//TestAlgorithmsHelpMethods.DebugDrawCircle(intersectionVertex.ToVector3(), 0.4f, Color.black);
//TestAlgorithmsHelpMethods.DebugDrawCircle(visibleVertex.ToVector3(), 0.4f, Color.green);
}
//But the hull may still intersect with this edge between the point on the hole and the visible point on the hull,
//so the visible point on the hull might not be visible after all
//So we might have to find a new point which is visible
FindActualVisibleVertexOnHull(hull, hole, intersectionVertex, ref visibleVertex);
}