private void Initialize()
{
_graph = new UndirectedGraph <PointVertex, Edge <PointVertex> >();
_nodes.Clear();
foreach (Obstacle obstacle in _obstacles)
{
foreach (RotationTreeNode node in obstacle.Nodes)
{
_nodes.Add(node);
_graph.AddVertex(new PointVertex(node.Point));
}
_graph.AddEdgeRange(obstacle.Segments.Select(s => new Edge <PointVertex>(new PointVertex(s.Point1.Point), new PointVertex(s.Point2.Point))));
}
foreach (Point point in _singlePoints)
{
Obstacle obstacle = _obstacles.FirstOrDefault(o => o.Contains(point));
var newPoint = new RotationTreeNode(obstacle, point, true);
_graph.AddVertex(new PointVertex(point));
_nodes.Add(newPoint);
}
double maxX = _nodes.Max(p => p.Point.X);
_plusInf = new RotationTreeNode(new Point(maxX + 100, double.PositiveInfinity));
_minusInf = new RotationTreeNode(new Point(maxX + 100, double.NegativeInfinity));
_plusInf.AddChild(_minusInf);
foreach (RotationTreeNode node in _nodes.OrderByDescending(n => n))
{
_minusInf.AddChild(node);
}
}
private static bool LeftTurn(RotationTreeNode p, RotationTreeNode q, RotationTreeNode r)
{
if (r == null)
{
return(false);
}
if (double.IsNegativeInfinity(p.Y) || double.IsNegativeInfinity(q.Y) || double.IsNegativeInfinity(r.Y) ||
double.IsPositiveInfinity(p.Y) || double.IsPositiveInfinity(q.Y))
{
return(false);
}
if (double.IsPositiveInfinity(r.Y))
{
return(p.X <= q.X || (Math.Abs(p.X - q.X) < double.Epsilon && p.Y > q.Y));
}
double m = (q.Y - p.Y) / (q.X - p.X);
double b = p.Y - (m * p.X);
double rValue = (m * r.X) + b;
if (r.Y > rValue)
{
return(p.X <= q.X);
}
if (r.Y < rValue)
{
return(p.X > q.X);
}
return(false);
}
private static ObstacleSegment GetNearerSegment(RotationTreeNode node, RotationTreeNode p)
{
ObstacleSegment segment1 = node.Segments[0];
ObstacleSegment segment2 = node.Segments[1];
return(CalcDistance(p.Point, segment1.Point1.Point, segment1.Point2.Point) < CalcDistance(p.Point, segment2.Point1.Point, segment2.Point2.Point) ? segment1 : segment2);
}
private void AddEdge(RotationTreeNode p, RotationTreeNode q)
{
var pVertex = new PointVertex(p.Point);
var qVertex = new PointVertex(q.Point);
if (_graph.ContainsVertex(pVertex) && _graph.ContainsVertex(qVertex))
{
_graph.AddEdge(new Edge <PointVertex>(pVertex, qVertex));
}
}
private void HandleWithPoints(RotationTreeNode p, RotationTreeNode q)
{
if (p == null || q == null)
{
return;
}
if (p.X > q.X)
{
return;
}
if (p.IsSinglePoint && !q.IsSinglePoint && p.Obstacle == q.Obstacle)
{
p.VisibleSegment = q.VisibleSegment;
//AddEdge(p, q);
}
else if (q.IsSinglePoint && !p.IsSinglePoint && q.Obstacle == p.Obstacle)
{
//AddEdge(p, q);
}
else if (q.IsSinglePoint && p.VisibleSegment != null && q.Obstacle == p.VisibleSegment.Obstacle)
{
AddEdge(p, q);
}
else if (p.IsAdjacent(q))
{
ObstacleSegment higher = GetHigherSegment(q, p);
p.VisibleSegment = higher ?? q.VisibleSegment;
if (!SameObstacle(p, q))
{
AddEdge(p, q);
}
}
else if (p.VisibleSegment != null && p.VisibleSegment.Contains(q))
{
ObstacleSegment left = GetLeftSegment(q, p);
p.VisibleSegment = left ?? q.VisibleSegment;
if (!SameObstacle(p, q))
{
AddEdge(p, q);
}
}
else if (PointLiesNearerThanSegment(p, q))
{
p.VisibleSegment = q.IsSinglePoint ? q.VisibleSegment : GetNearerSegment(q, p);
if (!SameObstacle(p, q))
{
AddEdge(p, q);
}
}
}
private static ObstacleSegment GetHigherSegment(RotationTreeNode node, RotationTreeNode p)
{
foreach (ObstacleSegment seg in node.Segments)
{
RotationTreeNode other = seg.GetOtherPoint(node);
if (other != p && other.Y > node.Y)
{
return(seg);
}
}
return(null);
}
private int GetMinIndex()
{
RotationTreeNode minNode = null;
int minIndex = -1;
for (int i = 0; i < _nodes.Count; i++)
{
if (minNode == null || _nodes[i].CompareTo(minNode) < 0)
{
minNode = _nodes[i];
minIndex = i;
}
}
return(minIndex);
}
private void ComputeRotationTree()
{
var stack = new Stack <RotationTreeNode>();
stack.Push(_minusInf.FirstChild);
while (stack.Count > 0)
{
RotationTreeNode p = stack.Pop();
RotationTreeNode pr = p.Next;
RotationTreeNode q = p.Parent;
if (q != _minusInf)
{
HandleWithPoints(p, q);
}
p.Remove();
RotationTreeNode z = q.Prev;
if (z == null || !LeftTurn(p, z, z.Parent))
{
q.AddBefore(p);
}
else
{
while (!z.IsLeaf && LeftTurn(p, z.LastChild, z))
{
z = z.LastChild;
}
z.AddChild(p);
if (stack.Count > 0 && z == stack.Peek())
{
stack.Pop();
}
}
if (p.Prev == null && p.Parent != _plusInf)
{
stack.Push(p);
}
if (pr != null)
{
stack.Push(pr);
}
}
}
public Obstacle(IEnumerable <Point> points)
{
_points = points.ToList();
_nodes = new List <RotationTreeNode>();
_segments = new List <ObstacleSegment>();
RotationTreeNode lastPoint = null;
foreach (Point point in _points)
{
var newPoint = new RotationTreeNode(this, point, false);
_nodes.Add(newPoint);
if (lastPoint != null)
{
_segments.Add(new ObstacleSegment(this, lastPoint, newPoint));
}
lastPoint = newPoint;
}
if (_nodes.Count > 2)
{
_segments.Add(new ObstacleSegment(this, _nodes[_nodes.Count - 1], _nodes[0]));
}
}
private static bool PointLiesNearerThanSegment(RotationTreeNode p, RotationTreeNode q)
{
ObstacleSegment visSegment = p.VisibleSegment;
if (visSegment == null) // if there is no segment, it can not lie nearer (to p) than q
{
return(true);
}
// using a linear equation: y = mx + b
// for the line pq
double m1 = (q.Y - p.Y) / (q.X - p.X);
double b1 = p.Y - (m1 * p.X);
// getting the points of the segment
RotationTreeNode point1 = visSegment.Point1;
RotationTreeNode point2 = visSegment.Point2;
// check if both segment points lie on the
// same side of that line.
// if they do, there is no crossing.
if (!double.IsInfinity(m1))
{
double valuePoint1 = (m1 * point1.X) + b1;
double valuePoint2 = (m1 * point2.X) + b1;
if ((point1.Y > valuePoint1 && point2.Y > valuePoint2) ||
(point1.Y < valuePoint1 && point2.Y < valuePoint2))
{
return(true);
}
}
else if (point1.X < p.X && point2.X < p.X || point1.X > p.X && point2.X > p.X)
{
return(true);
}
// get the line for the segment
double m2 = (point2.Y - point1.Y) / (point2.X - point1.X);
double b2 = point1.Y - (m2 * point1.X);
if (Math.Abs(m1 - m2) < double.Epsilon) // avoiding divide-by-zero; lines are parallel
{
return(true); // if pq and the segment are parallel, the segment can be discarded and the point "lies nearer"
}
// calculate the crossing point
double x;
if (!double.IsInfinity(m2) && !double.IsInfinity(m1))
{
x = (b2 - b1) / (m1 - m2);
}
else if (!double.IsInfinity(m2) && double.IsInfinity(m1))
{
x = p.X;
}
else
{
x = point1.X;
}
double y = m1 * x + b1;
var crossingPoint = new Point(x, y);
// check if distancePQ is the smallest
return(CalcDistance(p.Point, q.Point) <= CalcDistance(p.Point, crossingPoint));
}
private static ObstacleSegment GetLeftSegment(RotationTreeNode node, RotationTreeNode p)
{
ObstacleSegment seg1 = node.Segments[0];
ObstacleSegment seg2 = node.Segments[1];
RotationTreeNode pointSeg1 = seg1.GetOtherPoint(node);
RotationTreeNode pointSeg2 = seg2.GetOtherPoint(node);
// using a linear equation: y = mx + b
double m = (node.Y - p.Y) / (node.X - p.X);
double b = p.Y - (m * p.X);
// there now are several cases:
// 1) one of the two segments lies to the left
// 2) none lies to the left
// 3) both lie to the left
// 4) one segment lies directly behind the line p to q
// in case
// 1) we return that segment
// 2) we return null
// 3) we return the segment nearer to p
// 4) we return that segment (if the other does not lie to the left)
// check the cases if the line p-to-this
// is vertical
if (double.IsInfinity(m))
{
if (p.Y > node.Y)
{ // consider the direction of the vertical line
// case 1
if (pointSeg1.X < node.X && node.X <= pointSeg2.X)
{
return(seg1); // Segment 1 lies to the left of pq
}
if (pointSeg2.X < node.X && node.X <= pointSeg1.X)
{
return(seg2); // Segment 2 lies to the left of pq
}
// case 3
if (pointSeg1.X < node.X && pointSeg2.X < node.X)
{
//return node.GetNearestSegment(p);
return(GetNearerSegment(node, p));
}
// case 4
if (Math.Abs(pointSeg1.X - node.X) < double.Epsilon)
{
return(seg1);
}
if (Math.Abs(pointSeg2.X - node.X) < double.Epsilon)
{
return(seg2);
}
// case 2
if (pointSeg1.X > node.X && pointSeg2.X > node.X)
{
return(null);
}
}
else
{
// case 1
if (pointSeg1.X > node.X && node.X >= pointSeg2.X)
{
return(seg1); // Segment 1 lies to the left of pq
}
if (pointSeg2.X > node.X && node.X >= pointSeg1.X)
{
return(seg2); // Segment 2 lies to the left of pq
}
// case 3
if (pointSeg1.X > node.X && pointSeg2.X > node.X)
{
//return node.GetNearestSegment(p);
return(GetNearerSegment(node, p));
}
// case 4
if (Math.Abs(pointSeg1.X - node.X) < double.Epsilon)
{
return(seg1);
}
if (Math.Abs(pointSeg2.X - node.X) < double.Epsilon)
{
return(seg2);
}
// case 2
if (pointSeg1.X < node.X && pointSeg2.X < node.X)
{
return(null);
}
}
}
else
{ // check for non-vertical lines
// function value for end point of segment 1
double value1 = (m * pointSeg1.X) + b;
// function value for end point of segment 2
double value2 = (m * pointSeg2.X) + b;
// case 1
if (pointSeg1.Y > value1 && value2 >= pointSeg2.Y)
{
return(seg1); // Segment 1 lies to the left of pq
}
if (pointSeg2.Y > value2 && value1 >= pointSeg1.Y)
{
return(seg2); // Segment 2 lies to the left of pq
}
// case 3
if (pointSeg1.Y > value1 && pointSeg2.Y > value2)
{
//return node.GetNearestSegment(p);
return(GetNearerSegment(node, p));
}
// case 4
if (Math.Abs(pointSeg1.Y - value1) < double.Epsilon)
{
return(seg1);
}
if (Math.Abs(pointSeg2.Y - value2) < double.Epsilon)
{
return(seg2);
}
// case 2
if (pointSeg1.Y < value1 && pointSeg2.Y < value2)
{
return(null);
}
}
// return null if this.equals(p)
return(null);
}
private bool SameObstacle(RotationTreeNode p, RotationTreeNode q)
{
return(!p.IsSinglePoint && !q.IsSinglePoint && p.Obstacle.Nodes.Contains(q));
}
