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