public static DoubleVector2 FindNearestPoint(IntLineSegment2 segment, DoubleVector2 query) { var p1 = segment.First.ToDoubleVector2(); var p2 = segment.Second.ToDoubleVector2(); return(FindNearestPoint(p1, p2, query)); }
public static bool TryFindNonoverlappingLineLineIntersectionT(DoubleVector2 a1, DoubleVector2 a2, DoubleVector2 b1, DoubleVector2 b2, out double tForA) { // via http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect var p = a1; var r = a1.To(a2); var q = b1; var s = b1.To(b2); var rxs = Cross(r, s); if (rxs == 0) // iffy? { goto fail; } var qmp = q - p; var t = Cross(qmp, s) / (double)rxs; tForA = t; return(true); fail: tForA = Double.NaN; return(false); }
// todo: this needs love public static bool TryFindLineLineIntersection(DoubleVector2 a1, DoubleVector2 a2, DoubleVector2 b1, DoubleVector2 b2, out DoubleVector2 result) { var p1 = a1; var p2 = a2; var p3 = b1; var p4 = b2; var v21 = p1 - p2; // (x1 - x2, y1 - y2) var v43 = p3 - p4; // (x3 - x4, y3 - y4) var denominator = Cross(v21, v43); if (denominator == 0.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); }
/// <summary> /// Projects this vector onto other vector. /// </summary> /// <param name="other">The vector being projected onto</param> /// <returns></returns> public DoubleVector2 ProjectOnto(DoubleVector2 other) { var numerator = other.Dot(this); var denominator = other.SquaredNorm2D(); return(new DoubleVector2( (other.X * numerator) / denominator, (other.Y * numerator) / denominator)); }
public static int Compare(DoubleVector2 p, DoubleLineSegment2 a, DoubleLineSegment2 b) { // TODO: Why did I comment this //#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); } }
public VisibilityPolygon(DoubleVector2 origin) : this( origin, new[] { new IntervalRange { Id = RANGE_ID_INFINITELY_FAR, ThetaStart = 0.0, ThetaEnd = TwoPi } }) { }
// 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(DoubleLineSegment2 s, DoubleVector2[] 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; DoubleVector2 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 (DoubleLineSegment2.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 (DoubleLineSegment2.Intersects(a.X, a.Y, b.X, b.Y, x.X, x.Y, y.X, y.Y)) { return(true); } } return(xInterior && yInterior); }
public static (DoubleVector2, DoubleVector2) FindCollinearBounds(DoubleVector2 a, DoubleVector2 b, DoubleVector2 c) { var ab = a.To(b).SquaredNorm2D(); var ac = a.To(c).SquaredNorm2D(); var bc = b.To(c).SquaredNorm2D(); if (ab > ac) { return(ab > bc ? (a, b) : (b, c)); } else { return(ac > bc ? (a, c) : (b, c)); } }
public static DoubleVector2[] ConvexHull3(DoubleVector2 a, DoubleVector2 b, DoubleVector2 c) { var abc = Clockness(a, b, c); if (abc == Clk.Neither) { var(s, t) = FindCollinearBounds(a, b, c); return(s == t ? new[] { s } : new[] { s, t }); } if (abc == Clk.Clockwise) { return(new[] { c, b, a }); } return(new[] { a, b, c }); }
public static DoubleVector2 FindNearestPoint(DoubleVector2 p1, DoubleVector2 p2, DoubleVector2 query) { var p1p2 = p2 - p1; var p1Query = query - p1; var p1QueryProjP1P2Component = p1Query.ProjectOntoComponentD(p1p2); if (p1QueryProjP1P2Component <= 0) { return(p1); } else if (p1QueryProjP1P2Component >= 1) { return(p2); } else { return(p1 + p1QueryProjP1P2Component * p1p2); } }
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); }
// assumes p is ccw ordered public static bool ConvexPolygonContainsPoint(DoubleVector2 x, DoubleVector2[] 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 for (var i = 0; i < p.Length; i++) { var a = p[i == 0 ? p.Length - 1 : i - 1]; var b = p[i]; if (Clockness(a, b, x) == Clk.Clockwise) { return(false); } } return(true); }
public static DoubleVector2 FindNearestPoint(DoubleLineSegment2 segment, DoubleVector2 query) { return(FindNearestPoint(segment.First, segment.Second, query)); }
public bool Equals(DoubleVector2 other) => X == other.X && Y == other.Y;
public static ContourNearestPointResult2 FindNearestPointOnContour(List <IntVector2> contour, DoubleVector2 query) { var result = new ContourNearestPointResult2 { Distance = double.PositiveInfinity, Query = query }; var pointCount = contour.First().Equals(contour.Last()) ? contour.Count - 1 : contour.Count; for (int i = 0; i < pointCount; i++) { var p1 = contour[i].ToDoubleVector2(); var p2 = contour[(i + 1) % pointCount].ToDoubleVector2(); var nearestPoint = FindNearestPoint(p1, p2, query); var distance = (query - nearestPoint).Norm2D(); if (distance < result.Distance) { result.Distance = distance; result.SegmentFirstPointContourIndex = i; result.NearestPoint = nearestPoint; } } return(result); }
public static bool IsReal(DoubleVector2 v) => IsReal(v.X) && IsReal(v.Y);
public static bool TryIntersectRayWithContainedOriginForVertexIndexOpposingEdge(DoubleVector2 origin, DoubleVector2 direction, ref Triangle3 triangle, out int indexOpposingEdge) { // See my explanation on http://math.stackexchange.com/questions/2139740/fast-3d-algorithm-to-find-a-ray-triangle-edge-intersection/2197942#2197942 // Note: Triangle points (A = p1, B = p2, C = p3) are CCW, origin is p, direction is v. // Results are undefined if ray origin is not in triangle (though you can probably math out what it means). // If a point is on the edge of the triangle, there will be neither-neither for clockness on the correct edge. for (int i = 0; i < 3; i++) { var va = triangle.Points[i] - origin; var vb = triangle.Points[(i + 1) % 3] - origin; var cvad = Clockness(va, direction); var cdvb = Clockness(direction, vb); // In-triangle case if (cvad == Geometry.Clockness.CounterClockwise && cdvb == Geometry.Clockness.CounterClockwise) { indexOpposingEdge = (i + 2) % 3; return(true); } // On-edge case if (cvad == Geometry.Clockness.Neither && cdvb == Geometry.Clockness.Neither) { indexOpposingEdge = (i + 2) % 3; return(true); } } indexOpposingEdge = -1; return(false); // throw new ArgumentException("Presumably origin wasn't in triangle (is this case reachable even with malformed input?)"); }
/// <summary> /// result * other ~= Proj(this onto other) /// </summary> /// <param name="other"></param> /// <returns></returns> public double ProjectOntoComponentD(DoubleVector2 other) { return(other.Dot(this) / other.SquaredNorm2D()); }
public static int Compare(DoubleVector2 p, IntLineSegment2 a, IntLineSegment2 b) { return(Compare(ref p, ref a, ref b)); }
[MethodImpl(MethodImplOptions.AggressiveInlining)] public static Clockness Clockness(DoubleVector2 ba, DoubleVector2 bc) => Clockness(ba.X, ba.Y, bc.X, bc.Y);
[MethodImpl(MethodImplOptions.AggressiveInlining)] public static Clockness Clockness(DoubleVector2 a, DoubleVector2 b, DoubleVector2 c) => Clockness(b - a, b - c);
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; }
// See https://stackoverflow.com/questions/2122305/convex-hull-of-4-points public static DoubleVector2[] ConvexHull4(DoubleVector2 a, DoubleVector2 b, DoubleVector2 c, DoubleVector2 d) { var abc = Clockness(a, b, c); if (abc == Clk.Neither) { var(s, t) = FindCollinearBounds(a, b, c); return(ConvexHull3(s, t, d)); } // make abc ccw if (abc == Clk.Clockwise) { (a, c) = (c, a); } var abd = Clockness(a, b, d); var bcd = Clockness(b, c, d); var cad = Clockness(c, a, d); if (abd == Clk.Neither) { var(s, t) = FindCollinearBounds(a, b, d); return(ConvexHull3(s, t, c)); } if (bcd == Clk.Neither) { var(s, t) = FindCollinearBounds(b, c, d); return(ConvexHull3(s, t, a)); } if (cad == Clk.Neither) { var(s, t) = FindCollinearBounds(c, a, d); return(ConvexHull3(s, t, b)); } if (abd == Clk.CounterClockwise) { if (bcd == Clk.CounterClockwise && cad == Clk.CounterClockwise) { return new[] { a, b, c } } ; if (bcd == Clk.CounterClockwise && cad == Clk.Clockwise) { return new[] { a, b, c, d } } ; if (bcd == Clk.Clockwise && cad == Clk.CounterClockwise) { return new[] { a, b, d, c } } ; if (bcd == Clk.Clockwise && cad == Clk.Clockwise) { return new[] { a, b, d } } ; throw new InvalidStateException(); } else { if (bcd == Clk.CounterClockwise && cad == Clk.CounterClockwise) { return new[] { a, d, b, c } } ; if (bcd == Clk.CounterClockwise && cad == Clk.Clockwise) { return new[] { d, b, c } } ; if (bcd == Clk.Clockwise && cad == Clk.CounterClockwise) { return new[] { a, d, c } } ; // 4th state impossible throw new InvalidStateException(); } } } }
public static Vector2 ToDotNetVector(this DoubleVector2 v) => new Vector2((float)v.X, (float)v.Y);
public VisibilityPolygon(DoubleVector2 origin, IntervalRange[] intervalRanges, IComparer <IntLineSegment2> segmentComparer = null) { _origin = origin; _intervalRanges = intervalRanges; _segmentComparer = segmentComparer ?? new OverlappingIntSegmentOriginDistanceComparator(_origin); }
public OverlappingIntSegmentOriginDistanceComparator(DoubleVector2 origin) { _origin = origin; }
[MethodImpl(MethodImplOptions.AggressiveInlining)] public static double Cross(this DoubleVector2 a, DoubleVector2 b) => Cross(a.X, a.Y, b.X, b.Y);
[DebuggerStepThrough] public DoubleVector3(DoubleVector2 v, double z = 0) : this(v.X, v.Y, z) { }
[Pure] public DoubleVector2 To(DoubleVector2 other) => other - this;