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