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