internal static bool Intersects(V2d[] poly, V2d p0, V2d p1, ref List <V2d> known, out bool enter, out int index0, out V2d p)
{
int index = 0;
Line2d l;
int i0 = 0;
int i1 = 1;
for (int i = 1; i <= poly.Length; i++)
{
if (i < poly.Length)
{
i0 = i - 1;
i1 = i;
}
else
{
i0 = poly.Length - 1;
i1 = 0;
}
l = new Line2d(poly[i0], poly[i1]);
if (l.Intersects(new Line2d(p0, p1), 4.0 * double.Epsilon, out p))
{
if (!Fun.IsTiny(known.Closest(p)))
{
enter = (l.LeftValueOfPos(p1) >= -4.0 * double.Epsilon);
index0 = index;
return(true);
}
}
index++;
}
p = V2d.NaN;
index0 = -1;
enter = false;
return(false);
}
public static bool ClosestIntersection(this Polygon2d poly, V2d start, V2d end, out bool enter, out int otherIndex, out V2d p)
{
double min = double.MaxValue;
bool intersects = false;
V2d point = V2d.NaN;
bool tempenter = false;
int tempindex = -1;
V2d temppoint = V2d.NaN;
double templength = double.MaxValue;
Line2d line = new Line2d(start, end);
int i = 0;
foreach (var l in poly.EdgeLines)
{
if (line.Intersects(l, 4.0 * double.Epsilon, out temppoint))
{
templength = (temppoint - line.P0).Length;
if (templength < min)
{
tempenter = l.LeftValueOfDir(line.Direction) >= 0.0;
tempindex = i;
min = templength;
point = temppoint;
}
intersects = true;
}
i++;
}
enter = tempenter;
otherIndex = tempindex;
p = point;
return(intersects);
}
private static int[] Simplify(V2d[] polyline, double eps, int indexFirst, int indexLast)
{
if (indexFirst == indexLast - 1)
{
return new[] { indexFirst, indexLast }
}
;
var pFirst = polyline[indexFirst]; var pLast = polyline[indexLast];
var line = new Line2d(pFirst, pLast);
var distMax = 0.0; var indexMax = 0;
for (var i = indexFirst + 1; i < indexLast; i++)
{
var d = line.GetDistanceToLine(polyline[i]);
if (d > distMax)
{
distMax = d; indexMax = i;
}
}
if (distMax < eps)
{
return new[] { indexFirst, indexLast }
}
;
var left = Simplify(polyline, eps, indexFirst, indexMax);
var right = Simplify(polyline, eps, indexMax, indexLast);
var result = new int[left.Length + right.Length - 1];
left.CopyTo(result, 0);
right.CopyTo(1, right.Length - 1, result, left.Length);
return(result);
}
public static bool IsParallelTo(this Line2d l0, Line2d l1, double epsilon = 1e-6) => l0.Direction.IsParallelTo(l1.Direction, epsilon);
public static bool IsParallelTo(this Line2d l0, Line2d l1) => l0.Direction.IsParallelTo(l1.Direction);
private static int[] ComputeNonConcaveSubPolygon(
this Polygon2d poly, ref int[] indices, double absoluteEpsilon)
{
int count = indices.Length;
if (count <= 3)
{
var result = indices;
indices = new int[0];
return(result);
}
Func <int, int> addFun = i => (i + 1) % count;
Func <int, int> subFun = i => (i + count - 1) % count;
for (int vi = 0; vi < count; vi++) // we possibly have to try all vertices
{
int nowMinIndex = vi, nowMaxIndex = vi;
var nowMinPoint = poly[indices[nowMinIndex]];
var nowMaxPoint = poly[indices[nowMaxIndex]];
var oldMinPoint = poly[indices[addFun(nowMinIndex)]];
var oldMaxPoint = poly[indices[subFun(nowMinIndex)]];
var subCount = 1;
bool add = true;
for (int pass = 0, goodPass = 0; pass - goodPass < 3; pass++, add = !add)
{
// alternate between adding a point at the two ends
var nextFun = add ? addFun : subFun;
int lastIndex = add ? nowMinIndex : nowMaxIndex;
int newIndex = add ? addFun(nowMaxIndex) : subFun(nowMinIndex);
if (newIndex == lastIndex)
{
var finalIndices = indices;
indices = new int[0];
return(finalIndices);
}
var newPoint = poly[indices[newIndex]];
var pMax = new Line2d(oldMaxPoint, nowMaxPoint).Plane2d;
var pMin = new Line2d(oldMinPoint, nowMinPoint).Plane2d;
var npMax = pMax.Normalized;
var npMin = pMin.Normalized;
var hMax = new Line2d(oldMaxPoint, nowMaxPoint).Plane2d.Normalized.Height(newPoint);
var hMin = new Line2d(oldMinPoint, nowMinPoint).Plane2d.Normalized.Height(newPoint);
if (hMax >= -absoluteEpsilon && hMin <= absoluteEpsilon)
{
bool good = true;
if (nowMinIndex != nowMaxIndex)
{
var plane0 = new Line2d(nowMinPoint, nowMaxPoint).Plane2d.Normalized;
var plane1 = new Line2d(nowMaxPoint, newPoint).Plane2d.Normalized;
var plane2 = new Line2d(newPoint, nowMinPoint).Plane2d.Normalized;
// check if new triangle does not contain any other points of the polygon
for (int i = nextFun(newIndex); i != lastIndex; i = nextFun(i))
{
var point = poly[indices[i]];
if (plane0.Height(point) > -absoluteEpsilon &&
plane1.Height(point) >= absoluteEpsilon &&
plane2.Height(point) >= absoluteEpsilon)
{
good = false; break;
}
}
}
if (good)
{
if (add)
{
nowMaxIndex = newIndex; oldMaxPoint = nowMaxPoint; nowMaxPoint = newPoint;
}
else
{
nowMinIndex = newIndex; oldMinPoint = nowMinPoint; nowMinPoint = newPoint;
}
goodPass = pass;
++subCount;
}
}
}
if (subCount < 3)
{
continue;
}
// build new arrays here
var subIndices = new int[subCount];
for (int i = 0, ni = nowMinIndex; i < subCount; i++, ni = addFun(ni))
{
subIndices[i] = indices[ni];
}
int newCount = 2 + count - subCount;
var newIndices = new int[newCount];
for (int i = 0, ni = nowMaxIndex; i < newCount; i++, ni = addFun(ni))
{
newIndices[i] = indices[ni];
}
indices = newIndices;
return(subIndices);
}
return(null);
}
/// <summary>
/// Sets the index-th edge of this polygon.
/// </summary>
public static IImmutablePolygon <V2d> SetEdge(this IImmutablePolygon <V2d> self, int index, Line2d edge)
{
index = self.RepairIndex(index);
var i0 = index++;
var i1 = index < self.Count ? index : 0;
return(self.SetPoint(i0, edge.P0).SetPoint(i1, edge.P1));
}
/// <summary>
/// Returns the Line-Segments of line inside the Polygon (CCW ordered).
/// Works only with Convex-Polygons
/// </summary>
public static Line2d ClipWithConvex(this Line2d line, Polygon2d poly)
{
V2d p = V2d.NaN;
bool i0, i1;
i0 = poly.Contains(line.P0);
i1 = poly.Contains(line.P1);
if (i0 && i1)
{
return(line);
}
else if ((!i0 && i1) || (i0 && !i1))
{
foreach (var l in poly.EdgeLines)
{
if (line.Intersects(l, out p))
{
break;
}
}
if (i0)
{
return(new Line2d(line.P0, p));
}
else
{
return(new Line2d(p, line.P1));
}
}
else
{
V2d p0 = V2d.NaN;
V2d p1 = V2d.NaN;
int c = 0;
foreach (var l in poly.EdgeLines)
{
if (line.Intersects(l, out p))
{
if (c == 0)
{
p0 = p;
}
else
{
p1 = p;
}
c++;
}
}
if (c == 2)
{
V2d u = p1 - p0;
if (u.Dot(line.Direction) > 0)
{
return(new Line2d(p0, p1));
}
else
{
return(new Line2d(p1, p0));
}
}
else
{
return(new Line2d(V2d.NaN, V2d.NaN));
}
}
}
/// <summary>
/// Returns the Line-Segments of line inside the Polygon (CCW ordered).
/// Works with all (convex and non-convex) Polygons
/// </summary>
public static IEnumerable <Line2d> ClipWith(this Line2d line, Polygon2d poly)
{
bool i0, i1;
i0 = poly.Contains(line.P0);
i1 = poly.Contains(line.P1);
List <V2d> resulting = new List <V2d>();
List <bool> enter = new List <bool>();
if (i0)
{
resulting.Add(line.P0);
enter.Add(true);
}
if (i1)
{
resulting.Add(line.P1);
enter.Add(false);
}
V2d p = V2d.NaN;
V2d direction = line.Direction;
foreach (var l in poly.EdgeLines)
{
if (line.Intersects(l, out p))
{
V2d d = l.Direction;
V2d n = new V2d(-d.Y, d.X);
if (!p.IsNaN)
{
bool addflag = true;
bool flag = direction.Dot(n) > 0;
for (int i = 0; i < resulting.Count; i++)
{
if (Fun.IsTiny((resulting[i] - p).Length))
{
if (flag != enter[i])
{
resulting.RemoveAt(i);
enter.RemoveAt(i);
}
addflag = false;
break;
}
}
if (addflag)
{
resulting.Add(p);
enter.Add(flag);
}
}
}
}
V2d dir = line.P1 - line.P0;
resulting = (from r in resulting select r).OrderBy(x => x.Dot(dir)).ToList();
int counter = resulting.Count;
List <Line2d> lines = new List <Line2d>();
for (int i = 0; i < counter - 1; i += 2)
{
lines.Add(new Line2d(resulting[i], resulting[i + 1]));
}
return(lines);
}
public static bool IsParallelTo(this Line2d l0, Line2d l1)
{
return(l0.Direction.IsParallelTo(l1.Direction));
}
