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