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