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