public IEnumerable <Vector3> CalculatePath(Vector3 start, Vector3 end) { var startNode = GetClosestNode(start); var endNode = GetClosestNode(end); if (startNode == null || endNode == null || !startNode.IsPassable || !endNode.IsPassable) { Logger.Info("Aborting pathfinding because start and/or end are null or impassable."); yield break; } var route = _graph.Search(startNode, endNode); if (route == null) { Logger.Warn($"Graph search failed to find a path between {start} and {end}."); yield break; } PathOptimizer.RemoveRedundantNodes(route); foreach (var node in route) { var pos = GetNodePosition(node); yield return(new Vector3(pos.X, pos.Y, _heightMap.GetHeight(pos.X, pos.Y))); } }
public IList <Vector3> CalculatePath(Vector3 start, Vector3 end, out bool endIsPassable) { var result = new List <Vector3>(); endIsPassable = true; var startNode = GetClosestNode(start); var endNode = GetClosestNode(end); if (!startNode.IsPassable) { startNode = GetNextPassablePosition(start, end); } if (!endNode.IsPassable) { endNode = GetNextPassablePosition(end, start); endIsPassable = false; } if (startNode == null || endNode == null) { Logger.Info("Aborting pathfinding because start and/or end are null"); return(result); } var route = _graph.Search(startNode, endNode); if (route == null) { Logger.Warn($"Graph search failed to find a path between {start} and {end}."); return(result); } PathOptimizer.RemoveRedundantNodes(route); PathOptimizer.SmoothPath(route, _graph); foreach (var node in route) { var pos = GetNodePosition(node); result.Add(new Vector3(pos.X, pos.Y, _heightMap.GetHeight(pos.X, pos.Y))); } return(result); }