//Line-plane intersection
public static bool LinePlane(MyVector2 planePos, MyVector2 planeNormal, MyVector2 line_p1, MyVector2 line_p2)
{
//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(-planeNormal, lineDir);
//Debug.Log(denominator);
//No intersection if the line and plane are perpendicular
if (denominator > epsilon || denominator < -epsilon)
{
MyVector2 vecBetween = planePos - line_p1;
float t = MyVector2.Dot(vecBetween, -planeNormal) / denominator;
MyVector2 intersectionPoint = line_p1 + lineDir * t;
//Gizmos.DrawWireSphere(intersectionPoint, 0.5f);
if (_Geometry.IsPointBetweenPoints(line_p1, line_p2, intersectionPoint))
{
areIntersecting = true;
}
}
return(areIntersecting);
}
//If we know two planes are intersecting, what's the point of intersection?
public static MyVector2 GetPlanePlaneIntersectionPoint(MyVector2 planePos_1, MyVector2 planeNormal_1, MyVector2 planePos_2, MyVector2 planeNormal_2)
{
MyVector2 lineDir = MyVector2.Normalize(new MyVector2(planeNormal_2.y, -planeNormal_2.x));
MyVector2 intersectionPoint = GetIntersectionCoordinate(planePos_1, planeNormal_1, planePos_2, lineDir);
return(intersectionPoint);
}
//We know a line plane is intersecting and now we want the coordinate of intersection
public static MyVector2 GetLinePlaneIntersectionPoint(MyVector2 planePos, MyVector2 planeNormal, MyVector2 line_p1, MyVector2 line_p2)
{
MyVector2 lineDir = MyVector2.Normalize(line_p1 - line_p2);
MyVector2 intersectionPoint = GetIntersectionCoordinate(planePos, planeNormal, line_p1, lineDir);
return(intersectionPoint);
}
//Help method to calculate the normal from two points
private static MyVector2 GetNormal(MyVector2 a, MyVector2 b)
{
MyVector2 lineDir = b - a;
//Flip x with y and set one to negative to get the normal
MyVector2 normal = MyVector2.Normalize(new MyVector2(lineDir.y, -lineDir.x));
return(normal);
}
//If we know two planes are intersecting, what's the point of intersection?
//2d
public static MyVector2 GetPlanePlaneIntersectionPoint(Plane2 plane_1, Plane2 plane_2)
{
MyVector2 lineDir = MyVector2.Normalize(new MyVector2(plane_2.normal.y, -plane_2.normal.x));
Ray2 ray = new Ray2(plane_2.pos, lineDir);
MyVector2 intersectionPoint = GetIntersectionCoordinate(plane_1, ray);
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);
}
//Help method to calculate the average normal of two line segments
private static MyVector2 GetAverageNormal(MyVector2 a, MyVector2 b, MyVector2 c)
{
MyVector2 normal_1 = GetNormal(a, b);
MyVector2 normal_2 = GetNormal(b, c);
MyVector2 averageNormal = (normal_1 + normal_2) * 0.5f;
averageNormal = MyVector2.Normalize(averageNormal);
return(averageNormal);
}
//The angle between two vectors 0 <= angle <= 180
//Same as Vector2.Angle() but we are using MyVector2
public static float AngleBetween(MyVector2 from, MyVector2 to)
{
//dot(a_normalized, b_normalized) = cos(alpha) -> acos(dot(a_normalized, b_normalized)) = alpha
float dot = MyVector2.Dot(MyVector2.Normalize(from), MyVector2.Normalize(to));
//This shouldn't happen but may happen because of floating point precision issues
dot = Mathf.Clamp(dot, -1f, 1f);
float angleRad = Mathf.Acos(dot);
return(angleRad);
}
//
// Arrow
//
public static HashSet <Triangle2> Arrow(MyVector2 p1, MyVector2 p2, float lineWidth, float arrowSize)
{
HashSet <Triangle2> arrowTriangles = new HashSet <Triangle2>();
//An arrow consists of two parts: the pointy part and the rectangular part
//First we have to see if we can fit the parts
MyVector2 lineDir = p2 - p1;
float lineLength = MyVector2.Magnitude(lineDir);
if (lineLength < arrowSize)
{
Debug.Log("Cant make arrow because line is too short");
return(null);
}
//Make the arrow tip
MyVector2 lineDirNormalized = MyVector2.Normalize(lineDir);
MyVector2 arrowBottom = p2 - lineDirNormalized * arrowSize;
MyVector2 lineNormal = MyVector2.Normalize(new MyVector2(lineDirNormalized.y, -lineDirNormalized.x));
MyVector2 arrowBottom_R = arrowBottom + lineNormal * arrowSize * 0.5f;
MyVector2 arrowBottom_L = arrowBottom - lineNormal * arrowSize * 0.5f;
Triangle2 arrowTipTriangle = new Triangle2(p2, arrowBottom_R, arrowBottom_L);
arrowTriangles.Add(arrowTipTriangle);
//Make the arrow rectangle
float halfWidth = lineWidth * 0.5f;
MyVector2 p1_T = p1 + lineNormal * halfWidth;
MyVector2 p1_B = p1 - lineNormal * halfWidth;
MyVector2 p2_T = arrowBottom + lineNormal * halfWidth;
MyVector2 p2_B = arrowBottom - lineNormal * halfWidth;
HashSet <Triangle2> rectangle = LineSegment(p1_T, p1_B, p2_T, p2_B);
foreach (Triangle2 t in rectangle)
{
arrowTriangles.Add(t);
}
return(arrowTriangles);
}
//Help method to calculate the intersection point between two planes offset in normal direction by a width
private static MyVector2 GetIntersectionPoint(MyVector2 a, MyVector2 b, MyVector2 c, float halfWidth, bool isTopPoint)
{
//Direction of the lines going to and from point b
MyVector2 beforeDir = MyVector2.Normalize(b - a);
MyVector2 afterDir = MyVector2.Normalize(c - b);
MyVector2 beforeNormal = GetNormal(a, b);
MyVector2 afterNormal = GetNormal(b, c);
//Compare the normals!
//normalDirFactor is used to determine if we want to top point (same direction as normal)
float normalDirFactor = isTopPoint ? 1f : -1f;
//If they are the same it means we have a straight line and thus we cant do plane-plane intersection
//if (beforeNormal.Equals(afterNormal))
//When comparing the normals, we cant use the regular small value because then
//the line width goes to infinity when doing plane-plane intersection
float dot = MyVector2.Dot(beforeNormal, afterNormal);
//Dot is 1 if the point in the same dir and -1 if the point in the opposite dir
float one = 1f - 0.01f;
if (dot > one || dot < -one)
{
MyVector2 averageNormal = MyVector2.Normalize((afterNormal + beforeNormal) * 0.5f);
MyVector2 intersectionPoint = b + averageNormal * halfWidth * normalDirFactor;
return(intersectionPoint);
}
else
{
//Now we can calculate where the plane starts
MyVector2 beforePlanePos = b + beforeNormal * halfWidth * normalDirFactor;
MyVector2 afterPlanePos = b + afterNormal * halfWidth * normalDirFactor;
//Calculate the intersection point
//We know they are intersecting, so we don't need to test that
MyVector2 intersectionPoint = Intersections.GetPlanePlaneIntersectionPoint(beforePlanePos, beforeNormal, afterPlanePos, afterNormal);
return(intersectionPoint);
}
}
//
// Line segment
//
public static HashSet <Triangle2> LineSegment(MyVector2 p1, MyVector2 p2, float width)
{
MyVector2 lineDir = p2 - p1;
MyVector2 lineNormal = MyVector2.Normalize(new MyVector2(lineDir.y, -lineDir.x));
//Bake in the width in the normal
lineNormal *= width * 0.5f;
MyVector2 p1_T = p1 + lineNormal;
MyVector2 p2_T = p2 + lineNormal;
MyVector2 p1_B = p1 - lineNormal;
MyVector2 p2_B = p2 - lineNormal;
HashSet <Triangle2> lineTriangles = LineSegment(p1_T, p1_B, p2_T, p2_B);
return(lineTriangles);
}
//The angle between two vectors 0 <= angle <= 180
//Same as Vector2.Angle() but we are using MyVector2
public static float AngleBetween(MyVector2 from, MyVector2 to, bool shouldNormalize = true)
{
//from and to should be normalized
//But sometimes they are already normalized and then we dont need to do it again
if (shouldNormalize)
{
from = MyVector2.Normalize(from);
to = MyVector2.Normalize(to);
}
//dot(a_normalized, b_normalized) = cos(alpha) -> acos(dot(a_normalized, b_normalized)) = alpha
float dot = MyVector2.Dot(from, to);
//This shouldn't happen but may happen because of floating point precision issues
dot = Mathf.Clamp(dot, -1f, 1f);
float angleRad = Mathf.Acos(dot);
return(angleRad);
}
//In 2d space [radians]
//If you want to calculate the angle from vector a to b both originating from c, from is a-c and to is b-c
public static float AngleFromToCCW(MyVector2 from, MyVector2 to, bool shouldNormalize = false)
{
from = MyVector2.Normalize(from);
to = MyVector2.Normalize(to);
float angleRad = AngleBetween(from, to, shouldNormalize = false);
//The determinant is similar to the dot product
//The dot product is always 0 no matter in which direction the perpendicular vector is pointing
//But the determinant is -1 or 1 depending on which way the perpendicular vector is pointing (up or down)
//AngleBetween goes from 0 to 180 so we can now determine if we need to compensate to get 360 degrees
if (MathUtility.Det2(from, to) > 0f)
{
return(angleRad);
}
else
{
return((Mathf.PI * 2f) - angleRad);
}
}
//Assumes the polygons are oriented counter clockwise
//poly is the polygon we want to cut
//Assumes the polygon we want to remove from the other polygon is convex, so clipPolygon has to be convex
//We will end up with the intersection of the polygons
public static List <MyVector2> ClipPolygon(List <MyVector2> poly, List <Plane2> clippingPlanes)
{
//Clone the vertices because we will remove vertices from this list
List <MyVector2> vertices = new List <MyVector2>(poly);
//Save the new vertices temporarily in this list before transfering them to vertices
List <MyVector2> vertices_tmp = new List <MyVector2>();
//Clip the polygon
for (int i = 0; i < clippingPlanes.Count; i++)
{
Plane2 plane = clippingPlanes[i];
for (int j = 0; j < vertices.Count; j++)
{
int jPlusOne = MathUtility.ClampListIndex(j + 1, vertices.Count);
MyVector2 v1 = vertices[j];
MyVector2 v2 = vertices[jPlusOne];
//Calculate the distance to the plane from each vertex
//This is how we will know if they are inside or outside
//If they are inside, the distance is positive, which is why the planes normals have to be oriented to the inside
float dist_to_v1 = _Geometry.DistanceFromPointToPlane(plane.normal, plane.pos, v1);
float dist_to_v2 = _Geometry.DistanceFromPointToPlane(plane.normal, plane.pos, v2);
//TODO: What will happen if they are exactly 0? Should maybe use a tolerance of 0.001
//Case 1. Both are outside (= to the right), do nothing
//Case 2. Both are inside (= to the left), save v2
if (dist_to_v1 >= 0f && dist_to_v2 >= 0f)
{
vertices_tmp.Add(v2);
}
//Case 3. Outside -> Inside, save intersection point and v2
else if (dist_to_v1 < 0f && dist_to_v2 >= 0f)
{
MyVector2 rayDir = MyVector2.Normalize(v2 - v1);
MyVector2 intersectionPoint = _Intersections.GetRayPlaneIntersectionPoint(plane.pos, plane.normal, v1, rayDir);
vertices_tmp.Add(intersectionPoint);
vertices_tmp.Add(v2);
}
//Case 4. Inside -> Outside, save intersection point
else if (dist_to_v1 >= 0f && dist_to_v2 < 0f)
{
MyVector2 rayDir = MyVector2.Normalize(v2 - v1);
MyVector2 intersectionPoint = _Intersections.GetRayPlaneIntersectionPoint(plane.pos, plane.normal, v1, rayDir);
vertices_tmp.Add(intersectionPoint);
}
}
//Add the new vertices to the list of vertices
vertices.Clear();
vertices.AddRange(vertices_tmp);
vertices_tmp.Clear();
}
return(vertices);
}
//The original algorithm calculates the intersection between two polygons, this will instead get the outside
//Assumes the polygons are oriented counter clockwise
//poly is the polygon we want to cut
//Assumes the polygon we want to remove from the other polygon is convex, so clipPolygon has to be convex
//We will end up with the !intersection of the polygons
public static List <List <MyVector2> > ClipPolygonInverted(List <MyVector2> poly, List <Plane2> clippingPlanes)
{
//The result may be more than one polygons
List <List <MyVector2> > finalPolygons = new List <List <MyVector2> >();
List <MyVector2> vertices = new List <MyVector2>(poly);
//The remaining polygon after each cut
List <MyVector2> vertices_tmp = new List <MyVector2>();
//Clip the polygon
for (int i = 0; i < clippingPlanes.Count; i++)
{
Plane2 plane = clippingPlanes[i];
//A new polygon which is the part of the polygon which is outside of this plane
List <MyVector2> outsidePolygon = new List <MyVector2>();
for (int j = 0; j < vertices.Count; j++)
{
int jPlusOne = MathUtility.ClampListIndex(j + 1, vertices.Count);
MyVector2 v1 = vertices[j];
MyVector2 v2 = vertices[jPlusOne];
//Calculate the distance to the plane from each vertex
//This is how we will know if they are inside or outside
//If they are inside, the distance is positive, which is why the planes normals have to be oriented to the inside
float dist_to_v1 = _Geometry.GetSignedDistanceFromPointToPlane(v1, plane);
float dist_to_v2 = _Geometry.GetSignedDistanceFromPointToPlane(v2, plane);
//TODO: What will happen if they are exactly 0?
//Case 1. Both are inside (= to the left), save v2 to the other polygon
if (dist_to_v1 >= 0f && dist_to_v2 >= 0f)
{
vertices_tmp.Add(v2);
}
//Case 2. Both are outside (= to the right), save v1
else if (dist_to_v1 < 0f && dist_to_v2 < 0f)
{
outsidePolygon.Add(v2);
}
//Case 3. Outside -> Inside, save intersection point
else if (dist_to_v1 < 0f && dist_to_v2 >= 0f)
{
MyVector2 rayDir = MyVector2.Normalize(v2 - v1);
Ray2 ray = new Ray2(v1, rayDir);
MyVector2 intersectionPoint = _Intersections.GetRayPlaneIntersectionPoint(plane, ray);
outsidePolygon.Add(intersectionPoint);
vertices_tmp.Add(intersectionPoint);
vertices_tmp.Add(v2);
}
//Case 4. Inside -> Outside, save intersection point and v2
else if (dist_to_v1 >= 0f && dist_to_v2 < 0f)
{
MyVector2 rayDir = MyVector2.Normalize(v2 - v1);
Ray2 ray = new Ray2(v1, rayDir);
MyVector2 intersectionPoint = _Intersections.GetRayPlaneIntersectionPoint(plane, ray);
outsidePolygon.Add(intersectionPoint);
outsidePolygon.Add(v2);
vertices_tmp.Add(intersectionPoint);
}
}
//Add the polygon outside of this plane to the list of all polygons that are outside of all planes
if (outsidePolygon.Count > 0)
{
finalPolygons.Add(outsidePolygon);
}
//Add the polygon which was inside of this and previous planes to the polygon we want to test
vertices.Clear();
vertices.AddRange(vertices_tmp);
vertices_tmp.Clear();
}
return(finalPolygons);
}
//
// Connected line segments
//
//isConnected means if the end points are connected to form a loop
public static HashSet <Triangle2> ConnectedLineSegments(List <MyVector2> points, float width, bool isConnected)
{
if (points != null && points.Count < 2)
{
Debug.Log("Cant form a line with fewer than two points");
return(null);
}
//Generate the triangles
HashSet <Triangle2> lineTriangles = new HashSet <Triangle2>();
//If the lines are connected we need to do plane-plane intersection to find the
//coordinate where the lines meet at each point, or the line segments will
//not get the same size
//(There might be a better way to do it than with plane-plane intersection)
List <MyVector2> topCoordinate = new List <MyVector2>();
List <MyVector2> bottomCoordinate = new List <MyVector2>();
float halfWidth = width * 0.5f;
for (int i = 0; i < points.Count; i++)
{
MyVector2 p = points[i];
//First point = special case if the lines are not connected
if (i == 0 && !isConnected)
{
MyVector2 lineDir = points[1] - points[0];
MyVector2 lineNormal = MyVector2.Normalize(new MyVector2(lineDir.y, -lineDir.x));
topCoordinate.Add(p + lineNormal * halfWidth);
bottomCoordinate.Add(p - lineNormal * halfWidth);
}
//Last point = special case if the lines are not connected
else if (i == points.Count - 1 && !isConnected)
{
MyVector2 lineDir = p - points[points.Count - 2];
MyVector2 lineNormal = MyVector2.Normalize(new MyVector2(lineDir.y, -lineDir.x));
topCoordinate.Add(p + lineNormal * halfWidth);
bottomCoordinate.Add(p - lineNormal * halfWidth);
}
else
{
//Now we need to find the intersection points between the top line and the bottom line
MyVector2 p_before = points[MathUtility.ClampListIndex(i - 1, points.Count)];
MyVector2 p_after = points[MathUtility.ClampListIndex(i + 1, points.Count)];
MyVector2 pTop = GetIntersectionPoint(p_before, p, p_after, halfWidth, isTopPoint: true);
MyVector2 pBottom = GetIntersectionPoint(p_before, p, p_after, halfWidth, isTopPoint: false);
topCoordinate.Add(pTop);
bottomCoordinate.Add(pBottom);
}
}
//Debug.Log();
for (int i = 0; i < points.Count; i++)
{
//Skip the first point if it is not connected to the last point
if (i == 0 && !isConnected)
{
continue;
}
int i_minus_one = MathUtility.ClampListIndex(i - 1, points.Count);
MyVector2 p1_T = topCoordinate[i_minus_one];
MyVector2 p1_B = bottomCoordinate[i_minus_one];
MyVector2 p2_T = topCoordinate[i];
MyVector2 p2_B = bottomCoordinate[i];
HashSet <Triangle2> triangles = LineSegment(p1_T, p1_B, p2_T, p2_B);
foreach (Triangle2 t in triangles)
{
lineTriangles.Add(t);
}
}
//Debug.Log(lineTriangles.Count);
return(lineTriangles);
}
