private NearestRouteNodeTraceResult ShortestPath(RouteNode fromNode, Guid toNodeId, GraphHolder graphHolder) { // HH 10 if (fromNode.Id == Guid.Parse("d0e5ce6a-0d90-4355-a1ff-60db24e5b153")) { } var pathFinder = new PathFinder(); var graphFromNode = graphHolder.Nodes[fromNode.Id]; var graphToNode = graphHolder.Nodes[toNodeId]; var maxAgentSpeed = Velocity.FromKilometersPerHour(1); var path = pathFinder.FindPath(graphFromNode, graphToNode, maximumVelocity: maxAgentSpeed); List <Guid> segmentIds = new(); List <string> segmentGeometries = new(); double distance = 0; if (path != null) { foreach (var edge in path.Edges) { var segment = graphHolder.EdgeToSegment[edge]; segmentIds.Add(segment.Id); segmentGeometries.Add(segment.Coordinates); distance += graphHolder.EdgeLengths[segment]; } } return(new NearestRouteNodeTraceResult(fromNode.Id, fromNode?.NamingInfo?.Name, distance, segmentIds.ToArray(), segmentGeometries.ToArray())); }
public void GridWithGradientBench() { var maxSpeed = Velocity.FromKilometersPerHour((this.GridWithGradient.Rows * this.GridWithGradient.Columns) + 1); this.PathFinder.FindPath( this.GridWithGradient.GetNode(GridPosition.Zero), this.GridWithGradient.GetNode(new GridPosition(this.GridWithGradient.Columns - 1, this.GridWithGradient.Rows - 1)), maxSpeed); }
private static GraphHolder GetGraphForTracing(long version, IEnumerable <NodeCandidateHolder> nodeCandidates, IEnumerable <IGraphObject> traceResult) { GraphHolder result = new(); var nodeA = new Node(Position.Zero); var nodeB = new Node(new Position(10, 10)); foreach (var node in nodeCandidates) { result.Nodes.Add(node.Node.Id, new Node(new Position(node.X * 100, node.Y * 100))); } foreach (var grapObject in traceResult) { switch (grapObject) { case RouteSegment segment: var fromNode = result.Nodes[segment.InV(version).Id]; var toNode = result.Nodes[segment.OutV(version).Id]; var edgeForward = new Edge(fromNode, toNode, Velocity.FromKilometersPerHour(100)); var edgeBackward = new Edge(toNode, fromNode, Velocity.FromKilometersPerHour(100)); fromNode.Outgoing.Add(edgeForward); toNode.Incoming.Add(edgeForward); fromNode.Incoming.Add(edgeBackward); toNode.Outgoing.Add(edgeBackward); result.EdgeToSegment.Add(edgeForward, segment); result.EdgeToSegment.Add(edgeBackward, segment); System.Diagnostics.Debug.WriteLine($"{segment.InV(version).Id}->{segment.Id}->{segment.OutV(version).Id}"); var length = GetLength(segment.Coordinates); result.EdgeLengths.Add(segment, length); //fromNode.Connect(toNode, Velocity.FromKilometersPerHour(100)); break; } } return(result); }
/// <summary> /// Pseudo randomly assigns traversal velocities in [80..100km/h] to edges. /// </summary> public static void SetRandomTraversalVelocities(Grid grid) { var z = 0; for (var y = 0; y < grid.Rows; y++) { for (var x = 0; x < grid.Columns; x++) { var node = grid.GetNode(new GridPosition(x, y)); foreach (var edge in node.Incoming) { var speed = (RandomNumbers[z] / 100.0f * 20) + 80; edge.TraversalVelocity = Velocity.FromKilometersPerHour(speed); z = (z + 1) % RandomNumbers.Length; } } } }
/// <summary> /// Makes edges in the top-left of the graph faster to traverse, while /// making the edges in the bottom right of the graph slower to traverse. /// </summary> public static void SetGradientLimits(Grid grid) { var speedLimit = (grid.Rows * grid.Columns) + 1; for (var y = 0; y < grid.Rows; y++) { for (var x = 0; x < grid.Columns; x++) { var node = grid.GetNode(new GridPosition(x, y)); foreach (var edge in node.Incoming) { edge.TraversalVelocity = Velocity.FromKilometersPerHour(speedLimit); } speedLimit -= 1; } } }
public DumbMovementLogic() { this.Grid = Grid.CreateGridWithLateralAndDiagonalConnections(this.GridSize, this.CellSize, Velocity.FromKilometersPerHour(50)); this.Grid.DisconnectNode(new GridPosition(1, 0)); this.Grid.DisconnectNode(new GridPosition(1, 1)); this.Grid.DisconnectNode(new GridPosition(1, 2)); this.Grid.DisconnectNode(new GridPosition(0, 4)); this.Grid.DisconnectNode(new GridPosition(1, 4)); this.Grid.DisconnectNode(new GridPosition(2, 4)); this.Grid.DisconnectNode(new GridPosition(3, 4)); this.Grid.DisconnectNode(new GridPosition(4, 4)); this.Grid.DisconnectNode(new GridPosition(4, 3)); this.Grid.DisconnectNode(new GridPosition(4, 2)); this.PathFinder = new PathFinder(); this.Offset = new Vector2 ( -(this.GridSize.Columns * this.CellSize.Width.Meters * 0.5f), -(this.GridSize.Rows * this.CellSize.Height.Meters * 0.5f) ); }
public void GraphIsEqualAfterSerializeAndDeSerialize() { var grid = Grid.CreateGridWithLateralConnections(new GridSize(2, 4), new Size(Distance.FromMeters(2.0f), Distance.FromMeters(1.0f)), Velocity.FromKilometersPerHour(3)); var stringGrid = GridSerializer.SerializeGrid(grid); var deserializedGrid = GridSerializer.DeSerializeGrid(stringGrid); Assert.AreEqual(grid.Rows, deserializedGrid.Rows); Assert.AreEqual(grid.Columns, deserializedGrid.Columns); for (int i = 0; i < grid.Columns; i++) { for (int j = 0; j < grid.Rows; j++) { var gridPosition = new GridPosition(i, j); var originalNode = grid.GetNode(gridPosition); var deserializedNode = grid.GetNode(gridPosition); Assert.AreEqual(originalNode.Position, deserializedNode.Position); Assert.AreEqual(originalNode.Outgoing.Count, deserializedNode.Outgoing.Count); Assert.AreEqual(originalNode.Incoming.Count, deserializedNode.Incoming.Count); foreach (var edge in originalNode.Outgoing) { var matchingEdge = deserializedNode.Outgoing.Single(o => o.Start.Position.Equals(edge.Start.Position) && o.End.Position.Equals(edge.End.Position)); Assert.AreEqual(edge.Distance, matchingEdge.Distance); Assert.AreEqual(edge.TraversalDuration, matchingEdge.TraversalDuration); Assert.AreEqual(edge.TraversalVelocity, matchingEdge.TraversalVelocity); } foreach (var edge in originalNode.Incoming) { var matchingEdge = deserializedNode.Incoming.Single(o => o.Start.Position.Equals(edge.Start.Position) && o.End.Position.Equals(edge.End.Position)); Assert.AreEqual(edge.Distance, matchingEdge.Distance); Assert.AreEqual(edge.TraversalDuration, matchingEdge.TraversalDuration); Assert.AreEqual(edge.TraversalVelocity, matchingEdge.TraversalVelocity); } } } }
public SubMap(byte[,] processedFullSizeMapDataForPathFinding, byte[,] processedFullSizeMapDataForExploration, int[,] islandProcessedMapDataForPathFinding, int islandId) { var startTime = DateTime.Now; this.IslandId = islandId; this.mapDataForExploration = processedFullSizeMapDataForExploration.Clone() as byte[, ]; EraseNonIslandData(this.mapDataForExploration, islandProcessedMapDataForPathFinding, islandId); //var nodeA = new Node(new Position(1, 1)); //var nodeB = new Node(new Position(2, 2)); //nodeA.Connect(nodeB,2) int reducedWidth = processedFullSizeMapDataForPathFinding.GetLength(0) / PathfindingConsts.myPathfindingGridSize; int reducedHeight = processedFullSizeMapDataForPathFinding.GetLength(1) / PathfindingConsts.myPathfindingGridSize; var gridSize = new GridSize(columns: reducedWidth, rows: reducedHeight); var cellSize = new Size(Distance.FromMeters(PathfindingConsts.myPathfindingGridSize), Distance.FromMeters(PathfindingConsts.myPathfindingGridSize)); var traversalVelocity = Velocity.FromKilometersPerHour(100); var startTimeGridCreate = DateTime.Now; Grid = Grid.CreateGridWithLateralAndDiagonalConnections(gridSize, cellSize, traversalVelocity); Console.WriteLine($"Creating grid took: {DateTime.Now.Subtract(startTimeGridCreate).TotalMilliseconds} milliseconds"); //Parallel.For(0, reducedWidth, x => // { var startTimeGridPrep = DateTime.Now; for (int x = 0; x < reducedWidth; x++) { for (int y = 0; y < reducedHeight; y++) { int xAdjusted = x * PathfindingConsts.myPathfindingGridSize; int yAdjusted = y * PathfindingConsts.myPathfindingGridSize; if (processedFullSizeMapDataForPathFinding[xAdjusted, yAdjusted] == 0 || islandProcessedMapDataForPathFinding[xAdjusted, yAdjusted] != islandId) { // Grid.RemoveDiagonalConnectionsIntersectingWithNode(new GridPosition(x, y)); Grid.DisconnectNode(new GridPosition(x, y)); } //if (processedFullSizeMapDataForPathFinding[xAdjusted, yAdjusted] != 0 && islandProcessedMapDataForPathFinding[xAdjusted, yAdjusted] != islandId) //{ // Grid.RemoveDiagonalConnectionsIntersectingWithNode(new GridPosition(x, y)); // Grid.DisconnectNode(new GridPosition(x, y)); //} } } Console.WriteLine($"Prepping grid took: {DateTime.Now.Subtract(startTimeGridPrep).TotalMilliseconds} milliseconds"); //}); //for (int x = 0; x < reducedWidth; x++) //{ // for (int y = 0; y < reducedHeight; y++) // { // int xAdjusted = x * PathfindingConsts.myPathfindingGridSize; // int yAdjusted = y * PathfindingConsts.myPathfindingGridSize; // if (processedFullSizeMapDataForPathFinding[xAdjusted, yAdjusted] == 0) // { // // Grid.RemoveDiagonalConnectionsIntersectingWithNode(new GridPosition(x, y)); // Grid.DisconnectNode(new GridPosition(x, y)); // } // if (processedFullSizeMapDataForPathFinding[xAdjusted, yAdjusted] != 0 && islandProcessedMapDataForPathFinding[xAdjusted, yAdjusted] != islandId) // { // //Grid.RemoveDiagonalConnectionsIntersectingWithNode(new GridPosition(x, y)); // Grid.DisconnectNode(new GridPosition(x, y)); // } // } //} var startTimeExplorationPrep = DateTime.Now; Exploration = new Exploration(mapDataForExploration); Console.WriteLine($"Prepping exploration took: {DateTime.Now.Subtract(startTimeExplorationPrep).TotalMilliseconds} milliseconds"); Console.WriteLine($"Creating submap took: {DateTime.Now.Subtract(startTime).TotalMilliseconds} milliseconds"); }
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); } }