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