public static bool Contains(ref IntVector2 p1, ref IntVector2 p2, ref IntVector2 q)
{
var p1p2 = p1.To(p2);
var p1q = p1.To(q);
// not on line between p1 p2
if (GeometryOperations.Clockness(p1p2, p1q) != Clockness.Neither)
{
return(false);
}
var a = p1p2.Dot(p1q);
var b = p1p2.SquaredNorm2();
// outside segment p1-p2 on the side of p1
if (a < 0)
{
return(false);
}
// outside segment p1-p2 on the side of p2
if (a > b)
{
return(false);
}
return(true);
}
public static bool Intersects(cInt ax, cInt ay, cInt bx, cInt by, cInt cx, cInt cy, cInt dx, cInt dy)
{
// https://www.cdn.geeksforgeeks.org/check-if-two-given-line-segments-intersect/
// Note, didn't do SO variant because not robust to collinear segments
// http://stackoverflow.com/questions/3838329/how-can-i-check-if-two-segments-intersect
var o1 = GeometryOperations.Clockness(ax, ay, bx, by, cx, cy);
var o2 = GeometryOperations.Clockness(ax, ay, bx, by, dx, dy);
var o3 = GeometryOperations.Clockness(cx, cy, dx, dy, ax, ay);
var o4 = GeometryOperations.Clockness(cx, cy, dx, dy, bx, by);
if (o1 != o2 && o3 != o4)
{
return(true);
}
// handle endpoint containment.
if (o1 == 0 && Contains(ax, ay, bx, by, cx, cy))
{
return(true);
}
if (o2 == 0 && Contains(ax, ay, bx, by, dx, dy))
{
return(true);
}
if (o3 == 0 && Contains(cx, cy, dx, dy, ax, ay))
{
return(true);
}
if (o4 == 0 && Contains(cx, cy, dx, dy, bx, by))
{
return(true);
}
return(false);
}
public static bool Contains(cInt p1x, cInt p1y, cInt p2x, cInt p2y, cInt qx, cInt qy)
{
var p1p2x = p2x - p1x;
var p1p2y = p2y - p1y;
var p1qx = qx - p1x;
var p1qy = qy - p1y;
// not on line between p1 p2
if (GeometryOperations.Clockness(p1p2x, p1p2y, p1qx, p1qy) != Clockness.Neither)
{
return(false);
}
var a = (long)p1p2x * p1qx + (long)p1p2y * p1qy; //p1p2.Dot(p1q);
var b = (long)p1p2x * p1p2x + (long)p1p2y * p1p2y; //p1p2.SquaredNorm2();
// outside segment p1-p2 on the side of p1
if (a < 0)
{
return(false);
}
// outside segment p1-p2 on the side of p2
if (a > b)
{
return(false);
}
return(true);
}
public static int Compare(IntVector2 p, IntLineSegment2 a, IntLineSegment2 b)
{
#if DEBUG
if (GeometryOperations.Clockness(p.X, p.Y, a.X1, a.Y1, a.X2, a.Y2) != Clockness.Clockwise)
{
throw new InvalidStateException();
}
if (GeometryOperations.Clockness(p.X, p.Y, b.X1, b.Y1, b.X2, b.Y2) != Clockness.Clockwise)
{
throw new InvalidStateException();
}
#endif
var clk = GeometryOperations.Clockness(p.X, p.Y, a.X1, a.Y1, b.X1, b.Y1);
if (clk != Clockness.Clockwise)
{
// b before a; b \' a *origin
var res = (int)GeometryOperations.Clockness(b.First, b.Second, a.First);
if (res != 0)
{
return(res);
}
// just need something to resolve ambiguity. b1 b2 a1 is collinear.
// a1 must be within the angle b1 p b2 (see visibility polygon building algorithm)
// so a1 is BETWEEN b1 b2. a2 cannot be collinear with b1 b2 (disallow segments intersecting
// other than at endpoint), but still, a2 is either 'in front of' or 'behind' b.
res = (int)GeometryOperations.Clockness(b.First, b.Second, a.Second);
#if DEBUG
if (res == 0 && a != b)
{
throw new BadInputException();
}
#endif
return(res);
}
else
{
// a before b; a \' b *origin
var res = -(int)GeometryOperations.Clockness(a.First, a.Second, b.First);
if (res != 0)
{
return(res);
}
// just need something to resolve ambiguity. a1 a2 b1 is collinear.
res = -(int)GeometryOperations.Clockness(a.First, a.Second, b.Second);
#if DEBUG
if (res == 0 && a != b)
{
throw new BadInputException();
}
#endif
return(res);
}
}
/// <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);
}
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;
}