private Node TryGetValidNodeAroundGridPos(ZoneMap zoneMap, Dictionary <Position, Node> nodes, Vector2 gridPos) { Node node; var royTGraphPos = zoneMap.PoEGridPositionToRoyTPathFindingGraphPosition(gridPos); bool gotNode = nodes.TryGetValue(royTGraphPos, out node); if (gotNode) { return(node); } int distance = 10; int startX = (int)gridPos.X - distance; int endX = (int)gridPos.X + distance; int startY = (int)gridPos.Y - distance; int endY = (int)gridPos.Y + distance; for (int x = startX; x <= endX; x++) { for (int y = startY; y <= endY; y++) { var graphPos = zoneMap.PoEGridPositionToRoyTPathFindingGraphPosition(new Vector2(x, y)); gotNode = nodes.TryGetValue(graphPos, out node); if (gotNode) { return(node); } } } return(null); }
public bool TryToFindPathWithGraph(Vector2 start, Vector2 end, ZoneMap zoneMap) { try { PathFinder pathFinder = new PathFinder(); pathFindingIndex = 0; currentPathInPoEGridCoordinates.Clear(); const int radius = 7; /* If the player is too close to obstacles a path might not be found. * Shift the player "coordinate" away from obstacle */ //int maxAttemptsToFindAPath = 4 var royTGraphPosStart = zoneMap.PoEGridPositionToRoyTPathFindingGraphPosition(start); var royTGraphPosEnd = zoneMap.PoEGridPositionToRoyTPathFindingGraphPosition(end); var nodeDictionary = zoneMap.GetSubMap(start).NodeDictionary; Node startNode = TryGetValidNodeAroundGridPos(zoneMap, nodeDictionary, start); Node endNode = TryGetValidNodeAroundGridPos(zoneMap, nodeDictionary, end); //bool gotStartNode = zoneMap.GetSubMap(start).NodeDictionary.TryGetValue(royTGraphPosStart, out startNode); //bool gotEndNode = zoneMap.GetSubMap(start).NodeDictionary.TryGetValue(royTGraphPosEnd, out endNode); if (startNode == null || endNode == null) { Console.WriteLine("Unable to find correct navigation grid"); return(false); } if (startNode == endNode) { Console.WriteLine("Nodes are equal. abort"); return(false); } while (true) { var path = pathFinder.FindPath(startNode, endNode, Velocity.FromKilometersPerHour(100000)); if (path == null || path.Edges.Count == 0) // || path.Type == PathType.ClosestApproach) { Console.WriteLine("Unable to find a path with given start/end, randomizing start/end"); int xRandom = MathHepler.Randomizer.Next((int)start.X - radius, (int)start.X + radius); int yRandom = MathHepler.Randomizer.Next((int)start.Y - radius, (int)start.Y + radius); startNode = TryGetValidNodeAroundGridPos(zoneMap, nodeDictionary, new Vector2(xRandom, yRandom)); xRandom = MathHepler.Randomizer.Next((int)end.X - radius, (int)end.X + radius); yRandom = MathHepler.Randomizer.Next((int)end.Y - radius, (int)end.Y + radius); endNode = TryGetValidNodeAroundGridPos(zoneMap, nodeDictionary, new Vector2(xRandom, yRandom)); } else { RemainingPathDistanceSquaredToTarget = 0; int counter = 0; var previousEdgeVector = new Vector2(); foreach (var edge in path.Edges) { var compensatedEdgeVector = new Vector2(edge.Start.Position.X * PathfindingConsts.myPathfindingGridSize, edge.Start.Position.Y * PathfindingConsts.myPathfindingGridSize); if (counter != 0) { var distance = previousEdgeVector.Distance(compensatedEdgeVector); RemainingPathDistanceSquaredToTarget += distance; } currentPathInPoEGridCoordinates.Add(compensatedEdgeVector); previousEdgeVector = compensatedEdgeVector; counter += 1; } RemainingPathDistanceSquaredToTarget = RemainingPathDistanceSquaredToTarget * RemainingPathDistanceSquaredToTarget; return(true); } } } catch { return(false); } }