/// <summary> /// Scans the vertices on the polygon until either an advance or retard step can be made. /// Used when iterating over a section of the polygon not visible from z. /// </summary> /// <param name="v"></param> /// <param name="s"></param> /// <param name="iprev"></param> /// <param name="windowEnd"></param> /// <param name="ccw"></param> /// <returns> </returns> public static NextCall Scan(ref VsRep v, ref Stack <VertDispl> s, ref int i, ref VertDispl windowEnd, ref bool ccw) { while (i < v.n) { i++; if (ccw && // CounterClockWise MathUtil.GreaterEps(v.Get(i + 1).alpha, s.Peek().alpha) && MathUtil.GEQEps(s.Peek().alpha, v.Get(i).alpha)) { VertDispl intersec = IntersectWithWindow(v.Get(i), v.Get(i + 1), s.Peek(), windowEnd); if (intersec != null && !(windowEnd != null && MathUtil.EqualsEps(intersec.p.Cartesian, windowEnd.p.Cartesian))) { s.Push(intersec); return(NextCall.ADVANCE); } } else if (!ccw && // ClockWise MathUtil.LEQEps(v.Get(i + 1).alpha, s.Peek().alpha) && s.Peek().alpha < v.Get(i).alpha) { if (IntersectWithWindow(v.Get(i), v.Get(i + 1), s.Peek(), windowEnd) != null) { return(NextCall.RETARD); } } } return(NextCall.STOP); }
/// <summary> /// Locates the vertex s_j on the stack with either a(s_j) <= a(v_i+1) <= a(s_j+1) /// or a(v_i+1) <= a(s_j) and a(s_j) = a(s_j+1) and segments (v_i, v_i+1) and (s_j, s_j+1) intersect. /// Pops nodes from the stack until s_j found. /// </summary> /// <param name="vi"></param> /// <param name="vi1"></param> /// <param name="ss"></param> /// <param name="outSj"></param> /// <returns> The last node popped from the stack, s_j+1. </returns> public static VertDispl LocateSj(VertDispl vi, VertDispl vi1, Stack <VertDispl> ss) { var sj1 = ss.Pop(); while (ss.Count > 0) { VertDispl sj = ss.Peek(); if (MathUtil.LEQEps(sj.alpha, vi1.alpha) && MathUtil.LEQEps(vi1.alpha, sj1.alpha)) { // TODO check if order is correct return(sj1); } if (MathUtil.LEQEps(vi1.alpha, sj.alpha) && MathUtil.EqualsEps(sj.alpha, sj1.alpha)) { var y = (new LineSegment(vi.p.Cartesian, vi1.p.Cartesian)).Intersect(new LineSegment(sj.p.Cartesian, sj1.p.Cartesian)); if (y != null) { return(sj1); } } // remove top vertex and continue iterating sj1 = ss.Pop(); } throw new GeomException("LocateSj removed all vertices in stack"); }
/// <summary> /// Either intersects two line segments defined by the given four polar points /// or if endpoint is null, a line segment (a,b) and ray from point orig in the direction /// of its adjacent polygon segment. /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <param name="orig"></param> /// <param name="endpoint"></param> /// <returns> A vertex-displacement pair associated with the intersection. </returns> public static VertDispl IntersectWithWindow(VertDispl a, VertDispl b, VertDispl orig, VertDispl endpoint) { if (a == null || b == null || orig == null) { return(null); } var s1 = new LineSegment(a.p, b.p); // extra checks related to robustness issues if (s1.IsOnSegment(orig.p.Cartesian)) { return(orig); } Vector2?res; if (endpoint != null) { var s2 = new LineSegment(orig.p, endpoint.p); // check for parallel slopes if (s1.IsParallel(s2)) { res = s1.ClosestPoint(orig.p.Cartesian); if (res.HasValue && !s2.IsOnSegment(res.Value)) { res = null; } } else { res = s1.Intersect(s2); } } else { var ray = new Ray2D(orig.p.Cartesian, orig.Direction); res = s1.Intersect(ray); } if (!res.HasValue) { return(null); } return(DisplacementInBetween(new PolarPoint2D(res.Value), a, b)); }
/// <summary> /// Gives a displacement a(s) value between a(v1) and a(v2). /// Increments/decrements a(s) with 2 * PI until between a(v1) and a(v2). /// </summary> /// <param name="s"></param> /// <param name="v1"></param> /// <param name="v2"></param> /// <returns> The generated vertex-displacement pair. </returns> public static VertDispl DisplacementInBetween(PolarPoint2D s, VertDispl v1, VertDispl v2) { var bot = Math.Min(v1.alpha, v2.alpha); var top = Math.Max(v1.alpha, v2.alpha); if (MathUtil.EqualsEps(bot, top)) { return(new VertDispl(s, bot)); } var temp = s.Theta; while (MathUtil.GreaterEps(temp, top)) { temp -= MathUtil.PI2; } while (MathUtil.LessEps(temp, bot)) { temp += MathUtil.PI2; } return(new VertDispl(s, temp)); }
/// <summary> /// Pops all vertices from the stack that have become invisible after the addition /// of a new vertex. /// Calls appropriate method (advance, scan, retard) after being done based on next vertex. /// </summary> /// <param name="v"></param> /// <param name="sOld"></param> /// <param name="iprev"></param> /// <returns></returns> public static NextCall Retard(ref VsRep v, ref Stack <VertDispl> s, ref int i, ref VertDispl w, ref bool ccw) { // LocateSj will pop vertices from the stack // until appropriated s_j is found // see paper var sjNext = LocateSj(v.Get(i), v.Get(i + 1), s); if (s.Count == 0) { return(NextCall.STOP); } var sj = s.Peek(); if (sj.alpha < v.Get(i + 1).alpha) { i++; var vi = v.Get(i); var p = (new LineSegment(sj.p.Cartesian, sjNext.p.Cartesian)).Intersect(vi.p.Ray); // fallback method, return point closest to intersection with segment line if (p == null) { var line = new Line(vi.p.Ray.origin, vi.p.Ray.origin + vi.p.Ray.direction); p = (new LineSegment(sj.p.Cartesian, sjNext.p.Cartesian)).Line.Intersect(line); p = Vector2.Distance(p.Value, sj.p.Cartesian) < Vector2.Distance(p.Value, sjNext.p.Cartesian) ? sj.p.Cartesian : sjNext.p.Cartesian; } var st1 = DisplacementInBetween(new PolarPoint2D(p.Value), sj, sjNext); if (st1 != null) { s.Push(st1); } s.Push(vi); // paper does i == v.n if (i == v.n - 1) { // TODO order of returned list correct? (check stack to list conversion) return(NextCall.STOP); } else if (MathUtil.GEQEps(v.Get(i + 1).alpha, vi.alpha) && MathUtil.Orient2D(v.Get(i - 1).p.Cartesian, vi.p.Cartesian, v.Get(i + 1).p.Cartesian) <= 0) { // -1 is RighTurn return(NextCall.ADVANCE); } else if (MathUtil.GreaterEps(v.Get(i + 1).alpha, vi.alpha) && MathUtil.Orient2D(v.Get(i - 1).p.Cartesian, vi.p.Cartesian, v.Get(i + 1).p.Cartesian) > 0) { // 1 is LeftTurn s.Pop(); w = vi; ccw = false; return(NextCall.SCAN); } else { s.Pop(); return(NextCall.RETARD); } } else { if (MathUtil.EqualsEps(v.Get(i + 1).alpha, sj.alpha) && MathUtil.GreaterEps(v.Get(i + 2).alpha, v.Get(i + 1).alpha) && MathUtil.Orient2D(v.Get(i).p.Cartesian, v.Get(i + 1).p.Cartesian, v.Get(i + 2).p.Cartesian) <= 0) { // -1 is RightTurn s.Push(v.Get(i + 1)); return(NextCall.ADVANCE); } else { w = IntersectWithWindow(v.Get(i), v.Get(i + 1), sj, sjNext); ccw = true; if (w == null) { var seg = new LineSegment(v.Get(i).p, v.Get(i + 1).p); var res = seg.ClosestPoint(sj.p.Cartesian); w = DisplacementInBetween(new PolarPoint2D(res), v.Get(i), v.Get(i + 1)); } return(NextCall.SCAN); } } }
/// <summary> /// Computes the visibility polygon from the given point /// inside of a simple polygon (given as n vertices in CCW order) in O(n) time. /// Based on: https://cs.uwaterloo.ca/research/tr/1985/CS-85-38.pdf /// </summary> /// <param name="polygon"></param> /// <param name="z"></param> /// <returns></returns> public static Polygon2D Vision(Polygon2D polygon, Vector2 z) { // check for invalid polygon if (polygon.VertexCount < 3) { return(null); } if (!(polygon.ContainsInside(z) || polygon.OnBoundary(z))) { throw new ArgumentException("Visibility point must be inside polygon"); } // list v, satisfies assumptions made in paper (section 2, paragraph 1 // and 2). double initAngle; var vs = Preprocess(polygon, z, out initAngle); var s = new Stack <VertDispl>(); var i = 0; VertDispl w = null; var ccw = true; var v0 = vs.Get(0); s.Push(v0); Debug.Assert(vs.n > 1); NextCall m_nextCall; if (MathUtil.GEQEps(vs.Get(1).alpha, v0.alpha)) { m_nextCall = Advance(ref vs, ref s, ref i, ref w, ref ccw); } else { m_nextCall = Scan(ref vs, ref s, ref i, ref w, ref ccw); // CounterClockWise } while (m_nextCall != NextCall.STOP) { switch (m_nextCall) { case NextCall.ADVANCE: m_nextCall = Advance(ref vs, ref s, ref i, ref w, ref ccw); break; case NextCall.RETARD: m_nextCall = Retard(ref vs, ref s, ref i, ref w, ref ccw); break; case NextCall.SCAN: m_nextCall = Scan(ref vs, ref s, ref i, ref w, ref ccw); break; } } var sList = s.ToList(); // error occurred due to robustness if (sList.Count == 0) { return(new Polygon2D()); } Debug.Assert(MathUtil.EqualsEps(sList[s.Count - 1].p.Cartesian, v0.p.Cartesian)); var poly = Postprocess(sList, vs, z, initAngle); return(poly); }
/// <summary> /// Pushes a new vertex on the stack and calls the appropriated function /// (advance, retard, scan) depending on the next vertex on the polygon. /// </summary> /// <param name="v"></param> /// <param name="s"></param> /// <param name="i"></param> /// <returns></returns> public static NextCall Advance(ref VsRep v, ref Stack <VertDispl> s, ref int i, ref VertDispl w, ref bool ccw) { var n = v.n - 1; Debug.Assert(i + 1 <= n); if (MathUtil.LEQEps(v.Get(i + 1).alpha, MathUtil.PI2)) { i++; s.Push(v.Get(i)); // TODO check order of returned list if (i == n) { return(NextCall.STOP); } if (MathUtil.LessEps(v.Get(i + 1).alpha, v.Get(i).alpha) && MathUtil.Orient2D(v.Get(i - 1).p.Cartesian, v.Get(i).p.Cartesian, v.Get(i + 1).p.Cartesian) < 0) { // -1 is RightTurn w = null; ccw = true; return(NextCall.SCAN); } else if (MathUtil.LessEps(v.Get(i + 1).alpha, v.Get(i).alpha) && MathUtil.Orient2D(v.Get(i - 1).p.Cartesian, v.Get(i).p.Cartesian, v.Get(i + 1).p.Cartesian) > 0) { // 1 is LeftTurn return(NextCall.RETARD); } else { return(NextCall.ADVANCE); } } else { var v0 = v.Get(0); if (MathUtil.LEQEps(s.Peek().alpha, MathUtil.PI2)) { var isect = (new LineSegment(v.Get(i).p.Cartesian, v.Get(i + 1).p.Cartesian)).Intersect(v0.p.Ray); Debug.Assert(isect != null); var st = DisplacementInBetween(new PolarPoint2D(isect.Value), v.Get(i), v.Get(i + 1)); s.Push(st); } w = v0; ccw = false; return(NextCall.SCAN); } }