public PotentialField FindPath(DijkstraNodeGrid dijkstraNodeNetwork, IPathRequest pathRequest, out bool succes) { if (_potentialFieldCache == null || !_potentialFieldCache.TryGetValue(pathRequest, out var potentialField)) { var pathfindingNetwork = dijkstraNodeNetwork.GetCollisionLayerNetwork(pathRequest.CollisionCategory); _dijkstraAlgorithm.StartFindPath(pathfindingNetwork, dijkstraNodeNetwork.DefinitionNodeGrid.NodeArray, pathRequest.PathEnd); _dijkstraAlgorithm.FindPath(pathfindingNetwork, dijkstraNodeNetwork.DefinitionNodeGrid.NodeGrid.Array, pathRequest.PathStart, pathRequest.AgentSize, pathRequest.CollisionCategory); potentialField = FindPath(dijkstraNodeNetwork, pathfindingNetwork, pathRequest.PathEnd, pathRequest); _potentialFieldCache?.Add(pathRequest, potentialField); } ref var startDefinitionNode = ref dijkstraNodeNetwork.DefinitionNodeGrid.NodeArray[pathRequest.PathStart];
public PotentialField FindPath(DijkstraNodeGrid dijkstraNodeNetwork, IPathRequest pathRequest, out bool succes) { try { if (pathRequest.AgentSize % 2 == 0) { throw new InvalidAgentSizeException("Potential fields only support uneven agent sizes such as 1,3,5 etc."); } if (_potentialFieldCache == null || !_potentialFieldCache.TryGetValue(pathRequest, out var potentialField)) { var sw = Stopwatch.StartNew(); var pathfindingNetwork = dijkstraNodeNetwork.GetCollisionLayerNetwork(pathRequest.CollisionCategory); var startNode = NodePointer.Dereference(pathRequest.PathStart.Index, pathfindingNetwork); var targetNode = NodePointer.Dereference(pathRequest.PathEnd.Index, pathfindingNetwork); if (_dijkstraAlgorithm.FindPath(pathfindingNetwork, targetNode, startNode, pathRequest)) { potentialField = FindPath(dijkstraNodeNetwork, pathfindingNetwork, targetNode, pathRequest); } else { potentialField = new PotentialField(dijkstraNodeNetwork.DefinitionNodeGrid.Transformer, (Point2)targetNode.DefinitionNode.Position); } _potentialFieldCache?.Add(pathRequest, potentialField); Debug.WriteLine($"Potentialfield created in {sw.ElapsedMilliseconds} ms."); } var nodeWorldPosition = potentialField.GridTransformer.ToWorld(pathRequest.PathStart.Position); var offset = GridClearanceHelper.GridNodeOffset(pathRequest.AgentSize, dijkstraNodeNetwork.DefinitionNodeGrid.Transformer.Scale); succes = potentialField.GetHeading(nodeWorldPosition + offset).Length > 0; return(potentialField); } catch (Exception ex) { Debug.WriteLine(ex); Debugger.Break(); succes = false; return(null); } }