コード例 #1
0
ファイル: Program.cs プロジェクト: Neojate/PathfindingA-
        //método que arranca el algoritmo de pathfinding y establece los nodos inicial y final
        private void findPath()
        {
            Node start = null;
            Node end   = null;

            PathFinding path = new PathFinding(map);

            start = createNode();
            end   = createNode();

            path.init(start.X, start.Y, end.X, end.Y);
        }
コード例 #2
0
        public static List <Offset> FindClosedPosByAStar(Offset startNode, int startDirection, Offset endNode, out List <int> rotationPathWay)
        {
            Console.Error.WriteLine("Path finding from {0} to {1} at direction of {2}", startNode, endNode, startDirection);

            rotationPathWay = new List <int>();
            List <Offset> pathWay = new List <Offset>();

            if (startNode.Equals(endNode))
            {
                return(pathWay);
            }

            List <HexSearchNode> frontier = new List <HexSearchNode>();
            List <HexSearchNode> explored = new List <HexSearchNode>();

            HexSearchNode start = new HexSearchNode(startNode, startDirection, 0, GetHexHeuristic(startNode, endNode));

            frontier.Add(start);

            bool found = false;

            while (frontier.Count > 0)
            {
                HexSearchNode current = frontier[0];
                frontier.RemoveAt(0);
                explored.Add(current);

                if (current.Pos.Equals(endNode))
                {
                    HexSearchNode parent = current;

                    while (parent != null && parent != start)
                    {
                        //Console.Error.WriteLine( "cost to {0} is {1}", parent.Pos, parent.CostSoFar );
                        int rotation = Offset.GetRotation(parent.Parent.Pos, parent.Pos);
                        rotationPathWay.Add(rotation);
                        pathWay.Add(parent.Pos);
                        parent = parent.Parent;
                    }

                    found = true;
                    break;
                }

                // constraints for early exit such as line of sight
                if (Offset.GetDistance(current.Pos, start.Pos) < 5)
                {
                    List <HexSearchNode> neighbors = GetHexNeighbors(current, endNode);
                    foreach (HexSearchNode node in neighbors)
                    {
                        if (explored.Contains(node))
                        {
                            continue;
                        }

                        int costSoFar = current.CostSoFar;
                        int costToEnd = PathFinding.GetHexHeuristic(node.Pos, endNode);

                        int index = frontier.IndexOf(node);
                        if (index > 0)
                        {
                            if (costSoFar < frontier[index].CostSoFar)
                            {
                                // already exist in the container and found better way
                                frontier[index].Parent    = current;
                                frontier[index].Rotation  = current.Rotation;
                                frontier[index].CostSoFar = costSoFar;
                                frontier[index].CostToEnd = costToEnd;
                            }
                        }
                        else
                        {
                            // Not found
                            node.Parent    = current;
                            node.CostSoFar = costSoFar;
                            node.CostToEnd = costToEnd;
                            frontier.Add(node);
                        }
                    }
                }
                frontier.Sort((item1, item2) => (item1.CostSoFar + item1.CostToEnd) - (item2.CostSoFar + item2.CostToEnd));
                //Console.Error.WriteLine( "Frontier is {0}", frontier.ToDebugString() );
            }

            if (pathWay.Count == 0 && explored.Count > 0)
            {
                explored.Sort((item1, item2) => (item1.CostSoFar + item1.CostToEnd) - (item2.CostSoFar + item2.CostToEnd));
                HexSearchNode parent = explored[0];
                while (parent != null && parent != start)
                {
                    rotationPathWay.Add(Offset.GetRotation(parent.Parent.Pos, parent.Pos));
                    pathWay.Add(parent.Pos);
                    parent = parent.Parent;
                }
            }

            pathWay.Reverse();
            rotationPathWay.Reverse();

            if (found)
            {
                Console.Error.WriteLine("Found : Pathway from {0} to target({1}) is {2}, rotation = {3} at distance of {4}", startNode, endNode, pathWay.ToDebugString(), rotationPathWay.ToDebugString(), pathWay.Count);
            }
            else
            {
                Console.Error.WriteLine("Not Found : Closest Pathway from {0} to target({1}) is {2}, rotation = {3} at distance of {4}", startNode, endNode, pathWay.ToDebugString(), rotationPathWay.ToDebugString(), pathWay.Count);
            }

            return(pathWay);
        }
コード例 #3
0
ファイル: AStar.cs プロジェクト: hard-coded-dev/PathFinding
        public static List <Direction8Way> Search8WayNode(Node start, Node end, IMovable grid, out int distance)
        {
            Console.Error.WriteLine("Finding from {0} to {1} by A*", start, end);
            distance = -1;

            List <Direction8Way> pathWay = new List <Direction8Way>();

            if (start.Equals(end))
            {
                distance = 0;
                return(pathWay);
            }

            List <SearchNode> frontier = new List <SearchNode>();
            List <SearchNode> explored = new List <SearchNode>();

            SearchNode startNode = new SearchNode(start, null, 0, PathFinding.GetManhattanHeuristic(start, end));

            frontier.Add(startNode);

            bool found = false;

            while (frontier.Count > 0)
            {
                SearchNode current = frontier[0];
                frontier.RemoveAt(0);
                explored.Add(current);

                if (current.Pos.Equals(end))
                {
                    distance = current.CostSoFar + current.CostToEnd;
                    SearchNode parent = current;
                    while (parent != null && parent.Pos.Equals(start) == false)
                    {
                        Direction8Way dir = PathFinding.Get8WayDirection(parent.Parent.Pos, parent.Pos);
                        pathWay.Add(dir);
                        parent = parent.Parent;
                    }
                    found = true;
                    break;
                }

                List <SearchNode> neighbors = PathFinding.Get8WayNeighbors(current, grid);
                foreach (SearchNode node in neighbors)
                {
                    if (explored.Contains(node))
                    {
                        continue;
                    }

                    node.CostSoFar = current.CostSoFar + 1;
                    node.CostToEnd = PathFinding.GetManhattanHeuristic(node.Pos, end);

                    int index = frontier.IndexOf(node);
                    if (index > 0)
                    {
                        if (node.CostSoFar < frontier[index].CostSoFar)
                        {
                            // if found better way
                            frontier[index].Parent    = current;
                            frontier[index].CostSoFar = node.CostSoFar;
                            frontier[index].CostToEnd = node.CostToEnd;
                        }
                    }
                    else
                    {
                        frontier.Add(node);
                    }
                }
                frontier.Sort((item1, item2) => (item1.CostSoFar + item1.CostToEnd) - (item2.CostSoFar + item2.CostToEnd));
                //Console.Error.WriteLine( "frontier = {0}", frontier.ToDebugString() );
            }

            pathWay.Reverse();

            if (found)
            {
                Console.Error.WriteLine("Found : {0} to {1} : {2} of distance {3}", start, end, pathWay.ToDebugString(), distance);
            }
            else
            {
                Console.Error.WriteLine("No Way! : {0} to {1}", start, end);
            }
            return(pathWay);
        }
コード例 #4
0
        public static List <Direction4Way> Search4WayNode(Node start, IReachable searchGrid, IMovable moveGrid, out int distance)
        {
            Console.Error.WriteLine("Finding from {0} by BFS", start);
            distance = -1;

            List <Direction4Way> pathWay = new List <Direction4Way>();

            if (searchGrid.IsReachedDestination(start))
            {
                distance = 0;
                return(pathWay);
            }

            List <SearchNode> frontier = new List <SearchNode>();
            List <SearchNode> explored = new List <SearchNode>();

            SearchNode startNode = new SearchNode(start, null, 0, 0);

            frontier.Add(startNode);

            bool found = false;

            while (frontier.Count > 0)
            {
                SearchNode current = ( SearchNode )frontier[0];
                frontier.RemoveAt(0);
                explored.Add(current);

                if (searchGrid.IsReachedDestination(current.Pos))
                {
                    Console.Error.WriteLine("End node found : {0}", current.Pos);
                    distance = current.CostSoFar + current.CostToEnd;
                    SearchNode parent = current;
                    while (parent != null && parent.Pos.Equals(start) == false)
                    {
                        Direction4Way dir = PathFinding.Get4WayDirection(parent.Parent.Pos, parent.Pos);
                        //Console.Error.WriteLine( "iterate back to parent {0}", parent.Pos );
                        pathWay.Add(dir);
                        parent = parent.Parent;
                    }
                    found = true;
                    break;
                }

                List <SearchNode> neighbors = PathFinding.Get4WayNeighbors(current, moveGrid);
                foreach (SearchNode node in neighbors)
                {
                    if (explored.Contains(node))
                    {
                        continue;
                    }

                    node.CostSoFar = current.CostSoFar + 1;

                    int index = frontier.IndexOf(node);
                    if (index == -1)
                    {
                        frontier.Add(node);
                    }
                }
                //Console.Error.WriteLine( "frontier = {0}", frontier.ToDebugString() );
            }

            pathWay.Reverse();
            if (found)
            {
                Console.Error.WriteLine("Found : from {0} through {1} of distance {2}", start, pathWay.ToDebugString(), distance);
            }
            else
            {
                Console.Error.WriteLine("No Way! Found from {0}", start);
            }
            return(pathWay);
        }
コード例 #5
0
        public List <Direction4Way> SearchGraphByDijkstra(Node start, PathGrid grid, out int cost)
        {
            cost = -1;

            List <Direction4Way> pathWay = new List <Direction4Way>();

            if (grid.IsReachedDestination(start))
            {
                cost = 0;
                return(pathWay);
            }

            List <SearchNode> frontier = new List <SearchNode>();
            List <SearchNode> explored = new List <SearchNode>();

            SearchNode startNode = new SearchNode(start, null, 0, 0);

            frontier.Add(startNode);

            bool found = false;

            while (frontier.Count > 0)
            {
                SearchNode current = ( SearchNode )frontier[0];
                frontier.RemoveAt(0);
                explored.Add(current);

                if (grid.IsReachedDestination(current.Pos))
                {
                    cost = current.CostSoFar + current.CostToEnd;
                    SearchNode parent = current;
                    while (parent != null && parent.Pos.Equals(start) == false)
                    {
                        Direction4Way dir = PathFinding.Get4WayDirection(parent.Parent.Pos as Node, parent.Pos as Node);
                        pathWay.Add(dir);
                        parent = parent.Parent;
                    }
                    found = true;
                    break;
                }

                List <SearchNode> neighbors = PathFinding.Get4WayNeighbors(current, grid);
                foreach (SearchNode node in neighbors)
                {
                    if (explored.Contains(node))
                    {
                        continue;
                    }

                    node.CostSoFar = current.CostSoFar + 1;
                    node.CostToEnd = 0;

                    int index = frontier.IndexOf(node);
                    if (index > 0)
                    {
                        if (node.CostSoFar < frontier[index].CostSoFar)
                        {
                            // if found better way
                            frontier[index].Parent    = current;
                            frontier[index].CostSoFar = node.CostSoFar;
                            frontier[index].CostToEnd = node.CostToEnd;
                        }
                    }
                    else
                    {
                        frontier.Add(node);
                    }
                }
                frontier.Sort((item1, item2) => (item1.CostSoFar + item1.CostToEnd) - (item2.CostSoFar + item2.CostToEnd));
                //Console.Error.WriteLine( "frontier = {0}", frontier.ToDebugString() );
            }

            pathWay.Reverse();

            if (found)
            {
                Console.Error.WriteLine("Found from {0} : {1} of distance {2}", start, pathWay.ToDebugString(), cost);
            }
            else
            {
                Console.Error.WriteLine("No Way! from {0}", start);
            }
            return(pathWay);
        }
コード例 #6
0
        static List <HexSearchNode> GetHexNeighbors(HexSearchNode center, Offset endNode)
        {
            //Console.Error.WriteLine( "Finding neighbors of {0}", center );
            List <HexSearchNode> neighbors = new List <HexSearchNode>();

            var parity = center.Pos.row & 1;    // 0 for even line, 1 for odd line
            var dir    = Hexagonal.OffsetDirections[parity];

            const int WIDTH  = 23;
            const int HEIGHT = 21;

            // 0 : Right
            // 1 : Upper Right
            // 2 : Upper Left
            // 3 : Left
            // 4 : Lower left
            // 5 : Lower Right
            for (int i = 0; i < 6; ++i)
            {
                // not allowed 2,3,4 since they are only able to go ahead.
                if (i >= 2 && i <= 4)
                {
                    continue;
                }
                // to sustain the direction of its previous direction
                int    rotation  = (6 + i + center.Rotation) % 6;
                Offset newPos    = new Offset(center.Pos.row + dir[rotation, 1], center.Pos.col + dir[rotation, 0]);
                int    direction = Offset.GetRotation(center.Pos, newPos);

                if (newPos.row < 0 || newPos.row >= HEIGHT)
                {
                    continue;
                }
                if (newPos.col < 0 || newPos.col >= WIDTH)
                {
                    continue;
                }

                // for specific constraints
                //if( IsMovable( newPos, direction ) == false )
                //	continue;

                int           gridCost = 1; // + ( int )Caribbean.GetGridProperty( newPos );
                HexSearchNode newNode  = new HexSearchNode(newPos, rotation, center.CostSoFar + gridCost, PathFinding.GetHexHeuristic(newPos, endNode));

                neighbors.Add(newNode);
            }
            return(neighbors);
        }