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