Beispiel #1
0
        private void Draw(IDrawDevice device, IReadOnlyList <ICollisionLayerNode> network, IDefinitionNodeNetwork definitionNodeNetwork)
        {
            var canvas = new Canvas(device, new CanvasBuffer());

            canvas.State.ZOffset = -8;
            for (var i = 0; i < network.Count; i++)
            {
                var node = network[i];
                canvas.State.ColorTint = ColorRgba.LightGrey;
                var nodeWorldPosition = definitionNodeNetwork.Transformer.ToWorld(node.DefinitionNode.Position);
                canvas.FillCircle(nodeWorldPosition.X, nodeWorldPosition.Y, NodeSize);
                canvas.State.ColorTint = ColorRgba.VeryLightGrey;
                if (node.DefinitionNode.Connections != null)
                {
                    canvas.State.ColorTint = new ColorRgba(199, 21, 133);
                    foreach (var connection in node.DefinitionNode.Connections)
                    {
                        var toNode = NodePointer.Dereference(connection.To, definitionNodeNetwork);
                        var vector = (definitionNodeNetwork.Transformer.ToWorld(toNode.Position) - nodeWorldPosition) * 0.5f;                         //Times 0.5f so we can see the connections in both directions.
                        canvas.DrawDashLine(nodeWorldPosition.X, nodeWorldPosition.Y, nodeWorldPosition.X + vector.X, nodeWorldPosition.Y + vector.Y);
                    }
                    if (!float.IsNaN(node.Clearance))
                    {
                        canvas.State.ColorTint = ColorRgba.Black;
                        canvas.DrawText(node.Clearance.ToString(), nodeWorldPosition.X, nodeWorldPosition.Y, -1f, Alignment.Center);
                    }
                }
            }
        }
Beispiel #2
0
        public NodePath FindPath(IPathfindNodeNetwork <AstarNode> nodeNetwork, IPathRequest pathRequest, out bool succes)
        {
            var pathfindingNetwork = nodeNetwork.GetCollisionLayerNetwork(pathRequest.CollisionCategory);
            var startNode          = NodePointer.Dereference(pathRequest.PathStart.Index, pathfindingNetwork);
            var endNode            = NodePointer.Dereference(pathRequest.PathEnd.Index, pathfindingNetwork);
            var path = FindPath(pathfindingNetwork, startNode, endNode, pathRequest.AgentSize, pathRequest.CollisionCategory);

            if (path == null)
            {
                succes = false;
                return(new NodePath(new[] { startNode.DefinitionNode }, nodeNetwork.DefinitionNodeNetwork.Transformer));
            }
            succes = true;
            switch (nodeNetwork.DefinitionNodeNetwork)
            {
            case IDefinitionNodeGrid definitionNodeGrid:
                var offset = GridClearanceHelper.GridNodeOffset(pathRequest.AgentSize, definitionNodeGrid.Transformer.Scale);
                return(new NodePath(path.ToArray(), definitionNodeGrid.Transformer));

            case IDefinitionNodeNetwork definitionNodeNetwork:
                return(new NodePath(path.ToArray(), definitionNodeNetwork.Transformer));

            default:
                throw new NotSupportedException($"{nodeNetwork.DefinitionNodeNetwork.GetType()} is not supported");
            }
        }
Beispiel #3
0
        private static List <DefinitionNode> RetracePath(AstarNode[] pathfindingNetwork, AstarNode startGridNode, AstarNode endGridNode)
        {
            var path        = new List <DefinitionNode>();
            var currentNode = endGridNode;

            while (true)
            {
                path.Add(currentNode.DefinitionNode);
                if (currentNode == startGridNode)
                {
                    break;
                }
                currentNode = NodePointer.Dereference(currentNode.Parent, pathfindingNetwork);
            }
            path.Reverse();
            return(path);
        }
Beispiel #4
0
        public bool FindPath(DijkstraNode[] pathfindingNetwork, DijkstraNode targetNode, DijkstraNode startNode, IPathRequest pathRequest)
        {
            if (targetNode.Clearance < pathRequest.AgentSize)
            {
                return(false);
            }
            ResetNetwork(pathfindingNetwork);
            var openSet   = new MaxHeap <DijkstraNode>(pathfindingNetwork.Length);
            var closedSet = new HashSet <DijkstraNode>();

            openSet.Add(targetNode);
            targetNode.GCost = 0f;
            while (openSet.Count > 0)
            {
                var currentNode = openSet.RemoveFirst();
                closedSet.Add(currentNode);

                foreach (var connection in currentNode.DefinitionNode.Connections)
                {
                    var toNode = NodePointer.Dereference(connection.To, pathfindingNetwork);
                    if ((connection.CollisionCategory & pathRequest.CollisionCategory) != 0 || closedSet.Contains(toNode))
                    {
                        continue;
                    }

                    if (toNode.Clearance < pathRequest.AgentSize)
                    {
                        toNode.GCost = float.NaN;
                    }
                    else
                    {
                        var newMovementCostToNeighbour = currentNode.GCost + GetDistance(currentNode.DefinitionNode, toNode.DefinitionNode) * currentNode.DefinitionNode.MovementCostModifier;
                        if (newMovementCostToNeighbour < toNode.GCost || !openSet.Contains(toNode))
                        {
                            toNode.GCost = newMovementCostToNeighbour;
                            if (!openSet.Contains(toNode))
                            {
                                openSet.Add(toNode);
                            }
                        }
                    }
                }
            }
            return(true);
        }
Beispiel #5
0
        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);
            }
        }
Beispiel #6
0
        private static List <DefinitionNode> FindPath(AstarNode[] pathfindingNetwork, AstarNode startNode, AstarNode targetNode, float neededClearance, PathfindaxCollisionCategory collisionCategory)
        {
            try
            {
                var sw = new Stopwatch();
                sw.Start();

                var pathSucces = false;
                if (startNode == targetNode)
                {
                    return(new List <DefinitionNode> {
                        targetNode.DefinitionNode
                    });
                }
                if (startNode.Clearance >= neededClearance && targetNode.Clearance >= neededClearance)
                {
                    var openSet          = new MaxHeap <AstarNode>(pathfindingNetwork.Length);
                    var closedSet        = new HashSet <AstarNode>();
                    var itterations      = 0;
                    var neighbourUpdates = 0;
                    openSet.Add(startNode);
                    while (openSet.Count > 0)
                    {
                        itterations++;
                        var currentNode = openSet.RemoveFirst();
                        closedSet.Add(currentNode);

                        if (currentNode == targetNode)
                        {
                            sw.Stop();
                            Debug.WriteLine($"NodePath found in {sw.ElapsedMilliseconds} ms. Itterations: {itterations} Neighbourupdates: {neighbourUpdates}");
                            pathSucces = true;
                            break;
                        }

                        foreach (var connection in currentNode.DefinitionNode.Connections)
                        {
                            var toNode = NodePointer.Dereference(connection.To, pathfindingNetwork);
                            if ((connection.CollisionCategory & collisionCategory) != 0 || closedSet.Contains(toNode))
                            {
                                continue;
                            }

                            if (toNode.Clearance >= neededClearance)
                            {
                                var newMovementCostToNeighbour = currentNode.GCost + GetDistance(currentNode.DefinitionNode, toNode.DefinitionNode) * currentNode.DefinitionNode.MovementCostModifier;
                                if (newMovementCostToNeighbour < toNode.GCost || !openSet.Contains(toNode))
                                {
                                    toNode.GCost  = newMovementCostToNeighbour;
                                    toNode.HCost  = GetDistance(toNode.DefinitionNode, targetNode.DefinitionNode);
                                    toNode.Parent = currentNode.DefinitionNode.Index;
                                    neighbourUpdates++;
                                    if (!openSet.Contains(toNode))
                                    {
                                        openSet.Add(toNode);
                                    }
                                }
                            }
                        }
                    }
                }
                if (pathSucces)
                {
                    return(RetracePath(pathfindingNetwork, startNode, targetNode));
                }
                Debug.WriteLine("Did not find a path :(");
                return(null);
            }
            catch (Exception ex)
            {
                Debug.WriteLine(ex);
                Debugger.Break();
                return(null);
            }
        }