Example #1
0
        public static float euclidianHeuristic(PathNode node, PathNode destinationNode, float cost = 1.0f)
        {
            float dx = node.x - destinationNode.x;
            float dy = node.y - destinationNode.y;

            return (float)Math.Sqrt(dx * dx + dy * dy) * cost;
        }
Example #2
0
        public static List<PathNode> buildPath(PathNode destinationNode, PathNode startNode)
        {
            List<PathNode> path = new List<PathNode>();

            PathNode node = destinationNode;
            path.Add(node);

            while (node != startNode)
            {
                node = node.parentNode;
                path.Remove(node);
            }

            return path;
        }
Example #3
0
        public static List<PathNode> findPath(PathNode firstNode, PathNode destinationNode)
        {
            List<PathNode> openNodes = new List<PathNode>();
            List<PathNode> closedNodes = new List<PathNode>();

            PathNode currentNode = firstNode;
            PathNode testNode;

            int l = 0;
            int i = 0;
            float g;
            float h;
            float f;

            List<PathNode> connectedNodes;
            float travelCost = 1.0f;

            currentNode.g = 0;
            currentNode.h = euclidianHeuristic(currentNode, destinationNode, travelCost);
            currentNode.f = currentNode.g + currentNode.h;

            while (currentNode != destinationNode)
            {
                Console.WriteLine("Working");
                connectedNodes = currentNode.getConnections();

                l = connectedNodes.Count;

                for (int x = 0; x < l; x++)
                {
                    testNode = connectedNodes[x];

                    if (testNode == currentNode || testNode.traversable == false) continue;

                    g = currentNode.g + travelCost;
                    h = euclidianHeuristic(testNode, destinationNode, travelCost);
                    f = g + h;

                    if (isOpen(testNode, openNodes) || isClosed(testNode, closedNodes))
                    {
                        if (testNode.f > f)
                        {
                            testNode.f = f;
                            testNode.g = g;
                            testNode.h = h;
                            testNode.parentNode = currentNode;
                        }
                    }
                    else
                    {
                        testNode.f = f;
                        testNode.g = g;
                        testNode.h = h;
                        testNode.parentNode = currentNode;
                        openNodes.Add(testNode);
                    }
                }

                closedNodes.Add(currentNode);

                if (openNodes.Count == 0)
                {
                    return null;
                }

                openNodes.OrderBy(on => on.f);
                currentNode = openNodes[0];
            }

            return buildPath(destinationNode, firstNode);
        }
Example #4
0
        public static bool isOpen(PathNode node, List<PathNode> openNodes)
        {
            int l = openNodes.Count;;
            for (int i = 0; i < l; ++i)
            {
                if ( openNodes[i] == node ) return true;
            }

            return false;
        }
Example #5
0
        public static bool isClosed(PathNode node, List<PathNode> closedNodes)
        {
            int l = closedNodes.Count;
            for (int i = 0; i < l; ++i)
            {
                if (closedNodes[i] == node ) return true;
            }

            return false;
        }