public static bool Intersect(P2D a1, P2D b1, P2D a2, P2D b2)
{
int o1 = Orientation(a1, b1, a2);
int o2 = Orientation(a1, b1, b2);
int o3 = Orientation(a2, b2, a1);
int o4 = Orientation(a2, b2, b1);
if (o1 != o2 && o3 != o4)
{
return(true);
}
if (o1 == 0 && a2.X <= Math.Max(a1.X, b1.X) && a2.X >= Math.Min(a1.X, b1.X) && a2.Y <= Math.Max(a1.Y, b1.Y) && a2.Y >= Math.Min(a1.Y, b1.Y))
{
return(true);
}
if (o2 == 0 && b2.X <= Math.Max(a1.X, b1.X) && b2.X >= Math.Min(a1.X, b1.X) && b2.Y <= Math.Max(a1.Y, b1.Y) && b2.Y >= Math.Min(a1.Y, b1.Y))
{
return(true);
}
if (o3 == 0 && a1.X <= Math.Max(a2.X, b2.X) && a1.X >= Math.Min(a2.X, b2.X) && a1.Y <= Math.Max(a2.Y, b2.Y) && a1.Y >= Math.Min(a2.Y, b2.Y))
{
return(true);
}
if (o4 == 0 && b1.X <= Math.Max(a2.X, b2.X) && b1.X >= Math.Min(a2.X, b2.X) && b1.Y <= Math.Max(a2.Y, b2.Y) && b1.Y >= Math.Min(a2.Y, b2.Y))
{
return(true);
}
return(false);
}
// ----- Convex Hull ---------------------------------------------------
//
// -- find a convex hull (envelop) of a set of points [points]
//
// IEnumerable<P2D> GetConvexHull(IEnumerable<P2D> points)
// ---------------------------------------------------------------------
public static IEnumerable <P2D> GetConvexHull(IEnumerable <P2D> points)
{
List <P2D> plist = new List <P2D>(points);
plist.Sort((a, b) => a.X == b.X ? a.Y.CompareTo(b.Y) : (a.X > b.X ? 1 : -1));
List <P2D> hull = new List <P2D>();
int L = 0, U = 0;
for (int i = plist.Count - 1; i >= 0; i--)
{
P2D p = plist[i], p1;
while (L >= 2 && ((p1 = hull[hull.Count - 1]) - hull[hull.Count - 2]) % (p - p1) >= 0)
{
hull.RemoveAt(hull.Count - 1);
L--;
}
hull.Add(p);
L++;
while (U >= 2 && ((p1 = hull[0]) - hull[1]) % (p - p1) <= 0)
{
hull.RemoveAt(0);
U--;
}
if (U != 0)
{
hull.Insert(0, p);
}
U++;
}
hull.RemoveAt(hull.Count - 1);
return(hull);
}
public override bool Equals(object o)
{
if (o is P2D)
{
P2D c = (P2D)o; return(this == c);
}
return(false);
}
public bool Inside(P2D p)
{
P2D ap = p - A;
double apab = ap * AB;
double apad = ap * AD;
return(le(0, apab) && le(apab, AB * AB) && le(0, apad) && le(apad, AD * AD));
}
public P2D[] ToPolygon(int n)
{
P2D[] pgon = new P2D[n];
for (int i = 0; i < n; i++)
{
pgon[i] = new P2D(C.X + Math.Cos(Math.PI * 2 * i / n), C.Y + Math.Sin(Math.PI * 2 * i / n));
}
return(pgon);
}
private static bool IsInside(Edge edge, P2D test)
{
bool?isLeft = IsLeftOf(edge, test);
if (isLeft == null)
{
return(true);
}
return(!isLeft.Value);
}
// ----- Line Segment Intersection -------------------------------------
//
// -- a relative orientation of points [p1], [p2], [p3]
//
// -- 0: colinear
// -- 1: clock wise
// -- 2: counterclock wise
//
// int Orientation(P2D p1, P2D p2, P2D p3)
//
// -- do line segments [a1, b1] and [a2, b2] intersect
//
// bool Intersect(P2D a1, P2D b1, P2D a2, P2D b2)
// ---------------------------------------------------------------------
static int Orientation(P2D p1, P2D p2, P2D p3)
{
double val = (p2.Y - p1.Y) * (p3.X - p2.X) - (p2.X - p1.X) * (p3.Y - p2.Y);
if (val == 0)
{
return(0);
}
return((val > 0) ? 1 : 2);
}
// Returns the intersection of the two lines (line segments are passed in, but they are treated like infinite lines)
private static P2D GetIntersect(P2D line1From, P2D line1To, P2D line2From, P2D line2To)
{
P2D direction1 = line1To - line1From;
P2D direction2 = line2To - line2From;
double dotPerp = (direction1.X * direction2.Y) - (direction1.Y * direction2.X);
// If it's 0, it means the lines are parallel so have infinite intersection points
if (IsNearZero(dotPerp))
{
return(new P2D(double.MaxValue, double.MaxValue));
}
P2D c = line2From - line1From;
double t = (c.X * direction2.Y - c.Y * direction2.X) / dotPerp;
// Return the intersection point
return(line1From + (direction1 * t));
}
// Tells if the test point lies on the left side of the edge line
private static bool?IsLeftOf(Edge edge, P2D test)
{
P2D tmp1 = edge.To - edge.From;
P2D tmp2 = test - edge.To;
//dot product of perpendicular?
double x = (tmp1.X * tmp2.Y) - (tmp1.Y * tmp2.X);
if (x < 0)
{
return(false);
}
else
if (x > 0)
{
return(true);
}
else
{
//Colinear points;
return(null);
}
}
public P2D Rot(P2D O, double theta)
{
return(new P2D(Math.Cos(theta) * (X - O.X) - Math.Sin(theta) * (Y - O.Y) + O.X, Math.Sin(theta) * (X - O.X) + Math.Cos(theta) * (Y - O.Y) + O.Y));
}
// This clips the subject polygon against the clip polygon (gets the intersection of the two polygons)
public static P2D[] GetIntersectedPolygon(P2D[] subjectPoly, P2D[] clipPoly)
{
List <P2D> outputList = subjectPoly.ToList();
// Make sure it's clockwise
bool?cw = IsClockwise(subjectPoly);
if (cw == false)
{
outputList.Reverse();
}
// Walk around the clip polygon clockwise
foreach (Edge clipEdge in IterateEdgesClockwise(clipPoly))
{
// clone it
List <P2D> inputList = outputList.ToList();
outputList.Clear();
if (inputList.Count == 0)
{
// Sometimes when the polygons don't intersect, this list goes to zero. Jump out to avoid an index out of range exception
break;
}
P2D S = inputList[inputList.Count - 1];
foreach (P2D E in inputList)
{
if (IsInside(clipEdge, E))
{
if (!IsInside(clipEdge, S))
{
P2D point = GetIntersect(S, E, clipEdge.From, clipEdge.To);
if (point.X == double.MaxValue)
{
// may be colinear, or may be a bug
throw new ApplicationException("Line segments don't intersect");
}
else
{
outputList.Add(point);
}
}
outputList.Add(E);
}
else if (IsInside(clipEdge, S))
{
P2D point = GetIntersect(S, E, clipEdge.From, clipEdge.To);
if (point.X == double.MaxValue)
{
// may be colinear, or may be a bug
throw new ApplicationException("Line segments don't intersect");
}
else
{
outputList.Add(point);
}
}
S = E;
}
}
int ix = 0;
while (ix < outputList.Count - 1)
{
if (outputList[ix].X == outputList[ix + 1].X && outputList[ix].Y == outputList[ix + 1].Y)
{
outputList.RemoveAt(ix + 1);
}
else
{
ix++;
}
}
return(outputList.ToArray());
}
public Edge(P2D from, P2D to)
{
From = from;
To = to;
}
public bool Inside(P2D p)
{
return(le((p - C).Abs(), R));
}
public C2D(P2D c, double r)
{
C = c; R = r;
}
public C2D(double x, double y, double r)
{
C = new P2D(x, y); R = r;
}
public R2D(double xa, double ya, double xb, double yb, double xd, double yd)
{
A = new P2D(xa, ya); AB = new P2D(xb, yb) - A; AD = new P2D(xd, yd) - A;
}
public R2D(P2D a, P2D b, P2D d)
{
A = a; AB = b - a; AD = d - a;
}
