public bool Intersects(ref IntLineSegment2 other)
{
cInt ax = X1, ay = Y1, bx = X2, by = Y2;
cInt cx = other.X1, cy = other.Y1, dx = other.X2, dy = other.Y2;
return(Intersects(ax, ay, bx, by, cx, cy, dx, dy));
}
public static DoubleVector2 FindNearestPoint(IntLineSegment2 segment, DoubleVector2 query)
{
var p1 = segment.First.ToDoubleVector2();
var p2 = segment.Second.ToDoubleVector2();
return(FindNearestPoint(p1, p2, query));
}
// todo: this needs love
public static bool TryFindLineLineIntersection(IntLineSegment2 a, IntLineSegment2 b, out DoubleVector2 result)
{
var p1 = a.First;
var p2 = a.Second;
var p3 = b.First;
var p4 = b.Second;
var v21 = p1 - p2; // (x1 - x2, y1 - y2)
var v43 = p3 - p4; // (x3 - x4, y3 - y4)
var denominator = Cross(v21, v43);
if (denominator == 0)
{
result = DoubleVector2.Zero;
return(false);
}
var p1xp2 = Cross(p1, p2); // x1y2 - y1x2
var p3xp4 = Cross(p3, p4); // x3y4 - y3x4
var numeratorX = p1xp2 * v43.X - v21.X * p3xp4;
var numeratorY = p1xp2 * v43.Y - v21.Y * p3xp4;
result = new DoubleVector2(numeratorX / (double)denominator, numeratorY / (double)denominator);
return(true);
}
public static bool IsCollinearWith(this IntLineSegment2 a, IntLineSegment2 b)
{
var a1a2 = a.First.To(a.Second);
var b1b2 = b.First.To(b.Second);
var a1b1 = a.First.To(b.First);
var isParallel = Cross(a1a2, b1b2) == 0;
var isA1A2CollinearB1 = Cross(a1a2, a1b1) == 0;
return(isParallel && isA1A2CollinearB1);
}
/// <summary>
/// https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm#Pseudo_code
/// Fails if result would be empty
/// </summary>
public static bool TryConvexClip(Polygon2 subject, Polygon2 clip, out Polygon2 result)
{
bool Inside(IntVector2 p, IntLineSegment2 edge) => GeometryOperations.Clockness(edge.First, edge.Second, p) != Clockness.CounterClockwise;
List <IntVector2> outputList = subject.Points;
for (var i = 0; i < clip.Points.Count - 1; i++)
{
var clipEdge = new IntLineSegment2(clip.Points[i], clip.Points[i + 1]);
List <IntVector2> inputList = outputList;
outputList = new List <IntVector2>();
var S = inputList[inputList.Count - 2];
for (var j = 0; j < inputList.Count - 1; j++)
{
var E = inputList[j];
if (Inside(E, clipEdge))
{
if (!Inside(S, clipEdge))
{
var SE = new IntLineSegment2(S, E);
if (!GeometryOperations.TryFindLineLineIntersection(SE, clipEdge, out var intersection))
{
throw new NotImplementedException();
}
outputList.Add(intersection.LossyToIntVector2());
}
outputList.Add(E);
}
else if (Inside(S, clipEdge))
{
var SE = new IntLineSegment2(S, E);
if (!GeometryOperations.TryFindLineLineIntersection(SE, clipEdge, out var intersection))
{
throw new NotImplementedException();
}
outputList.Add(intersection.LossyToIntVector2());
}
S = E;
}
if (outputList.Count == 0)
{
result = null;
return(false);
}
outputList.Add(outputList[0]);
}
result = new Polygon2(outputList);
return(true);
}
public static IntLineSegment2 Dilate(this IntLineSegment2 segment, int dilationRadius)
{
var a = segment.First;
var b = segment.Second;
var aToB = a.To(b);
var aToBMagSquared = aToB.SquaredNorm2();
var aToBMag = Math.Sqrt(aToBMagSquared);
var shrink = aToB.LossyScale(dilationRadius / aToBMag);
return(new IntLineSegment2(a - shrink, b + shrink));
}
// NOTE: Assumes segments are valid (two distinct endpoints) NOT line-OVERLAPPING
// that is, segments should not have more than 1 point of intersection.
// if segments DO have more than 1 point of intersection, this returns no intersection found.
public static bool TryFindSegmentSegmentIntersection(ref IntLineSegment2 a, ref IntLineSegment2 b, out DoubleVector2 result)
{
if (TryFindNonoverlappingSegmentSegmentIntersectionT(ref a, ref b, out double t))
{
var p = a.First;
var r = a.First.To(a.Second);
result = new DoubleVector2(p.X + t * r.X, p.Y + t * r.Y);
return(true);
}
result = DoubleVector2.Zero;
return(false);
}
// assumes p is ccw ordered, edge is counted as interior (neither case)
public static bool SegmentIntersectsNonDegenerateConvexPolygonInterior(IntLineSegment2 s, IntVector2[] p)
{
#if DEBUG
if (Clockness(p[0], p[1], p[2]) == Clk.Clockwise)
{
throw new BadInputException("p not ccw");
}
if (p.Length < 3)
{
throw new BadInputException("len(p) < 3");
}
#endif
var(x, y) = s;
bool xInterior = true, yInterior = true;
IntVector2 a = p[p.Length - 1], b;
int i = 0;
for (; i < p.Length && (xInterior || yInterior); i++, a = b)
{
b = p[i];
var abx = Clockness(a, b, x);
var aby = Clockness(a, b, y);
if (abx == Clk.Clockwise && aby == Clk.Clockwise)
{
return(false);
}
xInterior &= abx != Clk.Clockwise;
yInterior &= aby != Clk.Clockwise;
if (abx == (Clk)(-(int)aby) || abx == Clk.Neither || aby == Clk.Neither)
{
// The below is equivalent to:
// // (a, b) places x, y onto opposite half-planes.
// // Intersect if (x, y) places a, b onto opposite half-planes.
// var xya = Clockness(x, y, a);
// var xyb = Clockness(x, y, b);
// if (xya != xyb || xya == Clk.Neither || xyb == Clk.Neither) return true;
if (IntLineSegment2.Intersects(a.X, a.Y, b.X, b.Y, x.X, x.Y, y.X, y.Y))
{
return(true);
}
}
}
for (; i < p.Length; i++, a = b)
{
b = p[i];
if (IntLineSegment2.Intersects(a.X, a.Y, b.X, b.Y, x.X, x.Y, y.X, y.Y))
{
return(true);
}
}
return(xInterior && yInterior);
}
public static bool SegmentIntersectsConvexPolygonInterior(IntLineSegment2 s, IntVector2[] p)
{
if (p.Length == 1)
{
return(false);
}
else if (p.Length == 2)
{
return(s.Intersects(new IntLineSegment2(p[0], p[1])));
}
else
{
return(SegmentIntersectsNonDegenerateConvexPolygonInterior(s, p));
}
}
public void ClearBefore(IntLineSegment2 s, bool supportOverlappingLines = false)
{
var theta1 = FindXYRadiansRelativeToOrigin(s.First.X, s.First.Y);
var theta2 = FindXYRadiansRelativeToOrigin(s.Second.X, s.Second.Y);
var thetaLower = theta1 < theta2 ? theta1 : theta2;
var thetaUpper = theta1 < theta2 ? theta2 : theta1;
if (Math.Abs(theta1 - theta2) > Math.PI)
{
// covered angle range wraps around through theta=0.
InsertInternalInternal(s, RANGE_ID_INFINITELY_FAR, 0.0, thetaLower, supportOverlappingLines, true);
InsertInternalInternal(s, RANGE_ID_INFINITELY_FAR, thetaUpper, TwoPi, supportOverlappingLines, true);
}
else
{
InsertInternalInternal(s, RANGE_ID_INFINITELY_FAR, thetaLower, thetaUpper, supportOverlappingLines, true);
}
}
public static bool TryErode(this IntLineSegment2 segment, int erosionRadius, out IntLineSegment2 result)
{
var a = segment.First;
var b = segment.Second;
var aToB = a.To(b);
var aToBMagSquared = aToB.SquaredNorm2();
var erosionDiameter = 2 * erosionRadius;
if (aToBMagSquared <= erosionDiameter * erosionDiameter)
{
result = default(IntLineSegment2);
return(false);
}
var aToBMag = Math.Sqrt(aToBMagSquared);
var shrink = aToB.LossyScale(erosionRadius / aToBMag);
result = new IntLineSegment2(a + shrink, b - shrink);
return(true);
}
private void InsertInternal(ref IntLineSegment2 s, double insertionThetaLower, double insertionThetaUpper, bool supportOverlappingLines)
{
if (insertionThetaLower == insertionThetaUpper)
{
return;
}
// Console.WriteLine($"InsertInternal: {s}, {thetaLower} {thetaUpper}");
// cull if wall faces away from origin
var sperp = new DoubleVector2(s.Y2 - s.Y1, -(s.X2 - s.X1));
var os1 = _origin.To(s.First.ToDoubleVector2());
if (sperp.Dot(os1) < 0)
{
//return;
}
var rangeId = rangeIdCounter++;
InsertInternalInternal(s, rangeId, insertionThetaLower, insertionThetaUpper, supportOverlappingLines, false);
}
public static PolygonContainmentResult SegmentInPolygon(this PolyNode node, IntLineSegment2 query)
{
var bvh = node.visibilityGraphNodeData.ContourBvh;
if (bvh != null)
{
if (bvh.Intersects(ref query))
{
return(PolygonContainmentResult.IntersectsPolygon);
}
}
else
{
var last = node.Contour[node.Contour.Count - 1];
var q1 = query.First;
var q2 = query.Second;
for (var i = 0; i < node.Contour.Count; i++)
{
var current = node.Contour[i];
if ((current == q1 && last == q2) || (current == q2 && last == q1))
{
return(PolygonContainmentResult.OnPolygon);
}
if (IntLineSegment2.Intersects(current.X, current.Y, last.X, last.Y, q1.X, q1.Y, q2.X, q2.Y))
{
return(PolygonContainmentResult.IntersectsPolygon);
}
last = current;
}
}
// Query segment isn't on-contour or intersecting the contour, so it's either fully in or out.
var endpointContainment = Clipper.PointInPolygon(query.First, node.Contour);
if (endpointContainment == PolygonContainmentResult.OnPolygon)
{
return(PolygonContainmentResult.IntersectsPolygon);
}
return(endpointContainment);
}
// NOTE: Assumes segments are valid (two distinct endpoints) NOT line-OVERLAPPING
// that is, segments should not have more than 1 point of intersection.
// if segments DO have more than 1 point of intersection, this returns no intersection found.
public static bool TryFindNonoverlappingSegmentSegmentIntersectionT(ref IntLineSegment2 a, ref IntLineSegment2 b, out double tForA)
{
// via http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
var p = a.First;
var r = a.First.To(a.Second);
var q = b.First;
var s = b.First.To(b.Second);
var rxs = Cross(r, s);
if (rxs == 0)
{
goto fail;
}
var qmp = q - p;
var t = Cross(qmp, s) / (double)rxs;
if (t < 0.0 || t > 1.0)
{
goto fail;
}
var u = Cross(qmp, r) / (double)rxs;
if (u < 0.0 || u > 1.0)
{
goto fail;
}
tForA = t;
return(true);
fail:
tForA = Double.NaN;
return(false);
}
// Equality by endpoints, not line geometry
public bool Equals(IntLineSegment2 other)
{
return(First == other.First && Second == other.Second);
}
public static bool Intersects(this IntLineSegment2 a, IntLineSegment2 b)
{
return(TryFindSegmentSegmentIntersection(ref a, ref b, out DoubleVector2 _));
}
// NOTE: Assumes lines are valid (two distinct endpoints) NOT line-OVERLAPPING
// that is, lines should not have more than 1 point of intersection.
// if lines DO have more than 1 point of intersection, this returns no intersection found.
public static bool TryFindNonoverlappingLineLineIntersectionT(ref IntLineSegment2 a, ref IntLineSegment2 b, out double tForA)
{
return(TryFindNonoverlappingLineLineIntersectionT(a.First, a.Second, b.First, b.Second, out tForA));
}
private void InsertInternalInternal(IntLineSegment2 s, int sRangeId, double insertionThetaLower, double insertionThetaUpper, bool supportOverlappingLines, bool furthestSegmentWins)
{
// ReSharper disable once CompareOfFloatsByEqualityOperator
if (insertionThetaLower == insertionThetaUpper)
{
return;
}
// See distrsxy for why this makes sense.
// var sDist = _origin.To(sMidpoint).SquaredNorm2D();
var srange = new IntervalRange {
Id = sRangeId,
ThetaStart = insertionThetaLower,
ThetaEnd = insertionThetaUpper,
Segment = s
};
var splittableBeginIndexInclusive = FindOverlappingRangeIndex(insertionThetaLower, 0, true);
var splittableEndIndexInclusive = FindOverlappingRangeIndex(insertionThetaUpper, splittableBeginIndexInclusive, false);
// a given segment can be split into 3 at max - technically this overallocates because it's impossible
// for two 3-splits to happen in a row. Actually, assuming no overlaps one can only really produce
// # splittables + 2 total new segments (new segments on left/right side).
var n = new IntervalRange[(splittableEndIndexInclusive - splittableBeginIndexInclusive + 1) * 3];
//new IntervalRange[(splittableEndIndexInclusive - splittableBeginIndexInclusive + 1) + 2];
var nSize = 0;
void EmitRange(int rangeId, ref IntLineSegment2 segment, double thetaStart, double thetaEnd)
{
if (thetaStart == thetaEnd)
{
return;
}
if (nSize > 0 && n[nSize - 1].Id == rangeId)
{
n[nSize - 1].ThetaEnd = thetaEnd;
}
else
{
n[nSize] = new IntervalRange {
Id = rangeId, Segment = segment, ThetaStart = thetaStart, ThetaEnd = thetaEnd
};
nSize++;
}
}
// near and far unioned must cover thetaUpper
void HandleNearFarSplit(IntervalRange nearRange, IntervalRange farRange, double thetaLower, double thetaUpper)
{
// case: near covers range
if (nearRange.ThetaStart <= thetaLower && thetaUpper <= nearRange.ThetaEnd)
{
EmitRange(nearRange.Id, ref nearRange.Segment, thetaLower, thetaUpper);
return;
// return new[] { new IntervalRange { Id = nearRange.Id, ThetaStart = thetaLower, ThetaEnd = thetaUpper, Segment = nearRange.Segment } };
}
// case: near exclusively within range
if (thetaLower < nearRange.ThetaStart && nearRange.ThetaEnd < thetaUpper)
{
EmitRange(farRange.Id, ref farRange.Segment, thetaLower, nearRange.ThetaStart);
EmitRange(nearRange.Id, ref nearRange.Segment, nearRange.ThetaStart, nearRange.ThetaEnd);
EmitRange(farRange.Id, ref farRange.Segment, nearRange.ThetaEnd, thetaUpper);
return;
// return new[] {
// new IntervalRange { Id = farRange.Id, ThetaStart = thetaLower, ThetaEnd = nearRange.ThetaStart, Segment = farRange.Segment},
// new IntervalRange { Id = nearRange.Id, ThetaStart = nearRange.ThetaStart, ThetaEnd = nearRange.ThetaEnd, Segment = nearRange.Segment },
// new IntervalRange { Id = farRange.Id, ThetaStart = nearRange.ThetaEnd, ThetaEnd = thetaUpper, Segment = farRange.Segment}
// };
}
// case: near covers left of range (as in, covers the lower thetas of range)
if (nearRange.ThetaStart <= thetaLower && nearRange.ThetaEnd < thetaUpper)
{
EmitRange(nearRange.Id, ref nearRange.Segment, thetaLower, nearRange.ThetaEnd);
EmitRange(farRange.Id, ref farRange.Segment, nearRange.ThetaEnd, thetaUpper);
return;
// return new[] {
// new IntervalRange { Id = nearRange.Id, ThetaStart = thetaLower, ThetaEnd = nearRange.ThetaEnd, Segment = nearRange.Segment },
// new IntervalRange { Id = farRange.Id, ThetaStart = nearRange.ThetaEnd, ThetaEnd = thetaUpper, Segment = farRange.Segment }
// };
}
// case: near covers right of range
if (nearRange.ThetaStart > thetaLower && thetaUpper <= nearRange.ThetaEnd)
{
EmitRange(farRange.Id, ref farRange.Segment, thetaLower, nearRange.ThetaStart);
EmitRange(nearRange.Id, ref nearRange.Segment, nearRange.ThetaStart, thetaUpper);
return;
// return new[] {
// new IntervalRange { Id = farRange.Id, ThetaStart = thetaLower, ThetaEnd = nearRange.ThetaStart, Segment = farRange.Segment },
// new IntervalRange { Id = nearRange.Id, ThetaStart = nearRange.ThetaStart, ThetaEnd = thetaUpper, Segment = nearRange.Segment }
// };
}
// impossible to reach here
throw new Exception($"Impossible state at null split of {nameof(HandleNearFarSplit)}.");
}
void HandleSplit(IntervalRange range)
{
Debug.Assert(IsRangeOverlap(insertionThetaLower, insertionThetaUpper, range.ThetaStart, range.ThetaEnd));
if (range.Id == RANGE_ID_INFINITELY_FAR)
{
HandleNearFarSplit(srange, range, range.ThetaStart, range.ThetaEnd);
return;
}
var rsxy = range.Segment;
// // is this code necessary? Seems like not... though not sure why. We do have intersecting segments
// // but the intersect is quite minor (just at corners)...
DoubleVector2 intersection;
// HACK: No segment-segment intersect point implemented
// sxy.Intersects(rsxy) && GeometryOperations.TryFindLineLineIntersection(sxy, rsxy, out intersection)
if (GeometryOperations.TryFindSegmentSegmentIntersection(ref s, ref rsxy, out intersection))
{
// conceptually a ray from _origin to intersection hits s and rs at the same time.
// If shifted perpendicular to angle of intersection, then the near segment emerges.
var thetaIntersect = FindXYRadiansRelativeToOrigin(intersection.X, intersection.Y);
if (range.ThetaStart <= thetaIntersect && thetaIntersect <= range.ThetaEnd)
{
var directionToLower = DoubleVector2.FromRadiusAngle(1.0, thetaIntersect - PiDiv2);
var vsxy = s.First.To(s.Second).ToDoubleVector2().ToUnit();
var vrsxy = rsxy.First.To(rsxy.Second).ToDoubleVector2().ToUnit();
var lvsxy = vsxy.ProjectOntoComponentD(directionToLower) > 0 ? vsxy : -1.0 * vsxy;
var lvrsxy = vrsxy.ProjectOntoComponentD(directionToLower) > 0 ? vrsxy : -1.0 * vrsxy;
var originToIntersect = _origin.To(intersection);
var clvsxy = lvsxy.ProjectOntoComponentD(originToIntersect);
var clvrsxy = lvrsxy.ProjectOntoComponentD(originToIntersect);
var isInserteeNearerAtLower = clvsxy < clvrsxy;
// Console.WriteLine("IINAL: " + isInserteeNearerAtLower);
if (isInserteeNearerAtLower)
{
HandleNearFarSplit(range, srange, range.ThetaStart, thetaIntersect);
HandleNearFarSplit(srange, range, thetaIntersect, range.ThetaEnd);
}
else
{
HandleNearFarSplit(srange, range, range.ThetaStart, thetaIntersect);
HandleNearFarSplit(range, srange, thetaIntersect, range.ThetaEnd);
}
return;
}
}
// At here, one segment completely overlaps the other for the theta range
// Either that, or inserted segment in front of (but not totally covering) range
// Either way, it will always be the case that any point on the "near" segment is closer
// to _origin than any point on the "far" segment assuming within correct theta.
// I take center of segments as their endpoints are ambiguous between neighboring segments
// of a polygon.
// var distrsxy = range.MidpointDistanceToOriginSquared;
bool ComputeIsInserteeNearer()
{
//range.Id != RANGE_ID_INFINITELY_FAR && (sRangeId == RANGE_ID_INFINITELY_FAR || _segmentComparer.Compare(s, rsxy) < 0);
if (range.Id == RANGE_ID_INFINITELY_FAR)
{
return(true);
}
if (range.Id == RANGE_ID_INFINITESIMALLY_NEAR)
{
return(false);
}
if (srange.Id == RANGE_ID_INFINITELY_FAR)
{
return(false);
}
if (srange.Id == RANGE_ID_INFINITESIMALLY_NEAR)
{
return(true);
}
return(_segmentComparer.Compare(s, rsxy) < 0);
}
bool inserteeNearer = ComputeIsInserteeNearer();
var nearRange = inserteeNearer ? srange : range;
var farRange = inserteeNearer ? range : srange;
if (furthestSegmentWins)
{
(nearRange, farRange) = (farRange, nearRange);
}
HandleNearFarSplit(nearRange, farRange, range.ThetaStart, range.ThetaEnd);
}
// n.AddRange(_intervalRanges.Take(ibegin));
for (int it = splittableBeginIndexInclusive; it <= splittableEndIndexInclusive; it++)
{
HandleSplit(_intervalRanges[it]);
}
// n.AddRange(_intervalRanges.Skip(iend + 1));
bool segmentInserted = false;
for (int i = 0; i < nSize && !segmentInserted; i++)
{
if (n[i].Id == srange.Id)
{
segmentInserted = true;
}
}
if (!segmentInserted)
{
return;
}
var nhead = splittableBeginIndexInclusive;
var ntail = _intervalRanges.Length - splittableEndIndexInclusive - 1;
var result = new IntervalRange[nhead + nSize + ntail];
Array.Copy(_intervalRanges, 0, result, 0, nhead);
Array.Copy(n, 0, result, nhead, nSize);
Array.Copy(_intervalRanges, _intervalRanges.Length - ntail, result, nhead + nSize, ntail);
_intervalRanges = result;
}
