public NavPoint(INavigable node, INavigableCollection tree) { if (node.GetType() == typeof(SpatialNode)) { this.path = ((SpatialNode)node).path; this.tree = (NavTree)tree; this.position = node.Position(); } else { this.position = node.Position(); } }
public Route GetRoute(INavigableCollection tree, INavigable start, INavigable end, float minSize = 0) { Vector3 goalPosition = end.Position(); Vector3 startPosition = start.Position(); List <NavPoint> routePoints = new List <NavPoint>(); PriorityQueue <INavigable> fringe = new PriorityQueue <INavigable>(); fringe.Enqueue(start, 0); Dictionary <INavigable, INavigable> visited = new Dictionary <INavigable, INavigable>(); visited.Add(start, null); Dictionary <INavigable, float> visitedCosts = new Dictionary <INavigable, float>(); visitedCosts.Add(start, 0); INavigable current = null; while (!fringe.Empty()) { current = fringe.Dequeue(); // Debug.DrawLine(current.Position(), current.Position() + Vector3.up * 50f, Color.cyan, 10f); if (current.InBounds(goalPosition)) { break; } foreach (INavigable next in current.Neighbors()) { float cost = (next.Position() - startPosition).sqrMagnitude; if (next.Obstructed(minSize)) { continue; } if (!visited.ContainsKey(next)) { visited.Add(next, current); } else if (cost >= visitedCosts[next]) { continue; } visitedCosts[next] = cost; float priority = cost + (next.Position() - goalPosition).sqrMagnitude; fringe.Enqueue(next, priority); } } while (current != null) { routePoints.Add(new NavPoint(current, tree)); if (current.InBounds(startPosition)) { break; } current = visited[current]; } // routePoints.Reverse(); var route = new Route(); route.points = routePoints; return(route); }
public Route GetRoute(INavigableCollection tree, Vector3 startPosition, Vector3 goalPosition, float minSize = 0) { return(GetRoute(tree, tree.NodeAt(startPosition), tree.NodeAt(goalPosition), minSize)); }