Exemple #1
0
    // Give a path from p1 to an other point near p2 that is optimized in order to go to p3.
    // If going from this intermediate point to p3 is impossible, the len returned will always be infinity even if the len from p1 to the intermediate point is finite.
    float OptimizedRASofLine(Vector3 p1, Vector3 p2, Vector3 p3, int max_depth, int opti_max_depth, out Vector3[] output, float max_len)
    {
        Vector3 tmp_conf;
        float   len = 0;

        if (opti_max_depth <= 0)
        {
            // No point optimization
            len = RASofLine(p1, p2, max_depth, opti_max_depth, out output, max_len);
        }
        else
        {
            // Point optimization
            CostFunc cost = (Vector3 v, float max_cost) =>
            {
                Vector3[] devnull;
                float     tmp_len = RASofLine(p1, v, max_depth, Mathf.Min(rasApproxDepth, opti_max_depth - 1), out devnull, max_cost);
                if (tmp_len >= Mathf.Infinity)
                {
                    return(Mathf.Infinity);
                }
                tmp_len += RASofLine(v, p3, max_depth, Mathf.Min(rasApproxDepth, opti_max_depth - 1), out devnull, max_cost - tmp_len);
                return(tmp_len);
            };
            len    = optimizePoint(p2, cost, out tmp_conf);
            output = null;
            if (len < Mathf.Infinity) // The len that interest us is only the len from p1 to p2
            {
                len = RASofLine(p1, tmp_conf, max_depth, opti_max_depth - 1, out output, max_len);
            }
        }
        return(len);
    }
Exemple #2
0
        // runs all search algorithms for start and goal state
        private static void RunSingleSearch(PuzzleState initState, PuzzleState goalState)
        {
            var searcher = new Searcher(initState, goalState);

            // for 15-puzzle, cost from one state to next is always 1
            CostFunc cost = (fromState, toState) => 1;

            // do each search on a separate thread to speed things up
            var searchThreads = new List <Thread>
            {
                new Thread(() => Console.WriteLine(searcher.DepthFirstSearch(6).ToString())),
                new Thread(() => Console.WriteLine(searcher.DepthFirstSearch(12).ToString())),
                new Thread(() => Console.WriteLine(searcher.DepthFirstSearch(18).ToString())),
                new Thread(() => Console.WriteLine(searcher.IterativeDeepeningDfs().ToString())),
                new Thread(() => Console.WriteLine(searcher.BreadthFirstSearch().ToString())),
                new Thread(() => Console.WriteLine(searcher.UniformCostSearch(cost).ToString())),
                new Thread(() => Console.WriteLine(searcher.AStarSearch(cost, new MyHeuristic()).ToString())),
                new Thread(() => Console.WriteLine(searcher.GreedySearch(cost, new MyHeuristic()).ToString())),
                new Thread(() => Console.WriteLine(searcher.GreedySearch(cost, new ManhattanDistanceHeuristic()).ToString())),
                new Thread(() => Console.WriteLine(searcher.AStarSearch(cost, new ManhattanDistanceHeuristic()).ToString())),
            };

            // start all searches
            searchThreads.ForEach(x => x.Start());

            // wait for all searches to complete
            searchThreads.ForEach(x => x.Join());
        }
Exemple #3
0
    float optimizePoint(Vector3 conf, CostFunc cost, out Vector3 conf_min, bool smallStep = use_directly_small_step)
    {
        float delta       = CarAI.delta;
        float angle_delta = CarAI.angle_delta;
        bool  test_all_angles_in_one_iteration = CarAI.test_all_angles_in_one_iteration;

        if (smallStep)
        {
            delta       = CarAI.small_delta;
            angle_delta = CarAI.small_angle_delta;
            test_all_angles_in_one_iteration = CarAI.small_test_all_angles_in_one_iteration;
        }

        // Compute all possible adjacent conf
        Vector3[] r2_pos = new Vector3[] { conf + new Vector3(delta, 0, 0), conf + new Vector3(-delta, 0, 0),
                                           conf + new Vector3(0, delta, 0), conf + new Vector3(0, -delta, 0) };
        List <Vector3> all_pos = new List <Vector3>(r2_pos);

        if (test_all_angles_in_one_iteration)
        {
            int nb = Mathf.CeilToInt(360 / angle_delta);
            for (int i = 1; i < nb; i++)
            {
                float angle = CarController.normalizeAngle(conf.z + i * angle_delta);
                all_pos.Add(new Vector3(conf.x, conf.y, angle));
            }
        }
        else
        {
            float angle = CarController.normalizeAngle(conf.z + angle_delta);
            all_pos.Add(new Vector3(conf.x, conf.y, angle));
            angle = CarController.normalizeAngle(conf.z - angle_delta);
            all_pos.Add(new Vector3(conf.x, conf.y, angle));
        }
        // Remove those in collision
        all_pos.RemoveAll((c => phy.configurationInCollision(c)));
        // Compute best position
        float   min      = cost(conf, Mathf.Infinity);
        Vector3?min_conf = null;

        foreach (Vector3 v in all_pos)
        {
            float c = cost(v, min);
            if (c < min)
            {
                min      = c;
                min_conf = v;
            }
        }
        if (min_conf.HasValue)
        {
            return(optimizePoint(min_conf.Value, cost, out conf_min, smallStep));
        }
        if (!smallStep)
        {
            return(optimizePoint(conf, cost, out conf_min, true));
        }
        conf_min = conf;
        return(min);
    }
        public SearchResult UCS()
        {
            NodeComparator comparator = (node1, node2) => node1.GHat.CompareTo(node2.GHat);
            CostFunc       cost       = (state1, state2) => 1;

            return(GenericSearch(_initialState, _goalState, "UCS", comparator, cost, null));
        }
Exemple #5
0
        /// <summary>
        /// Performs SGD on a set of training data using a mini-batch size provided.
        /// </summary>
        /// <param name="trainingSet"></param>
        /// <param name="batchSize"></param>
        /// <returns>The average cost function evaluation for each batch</returns>
        internal override void Learn(HashSet <TrainingData> trainingSet, int batchSize)
        {
            batchSize = Math.Min(batchSize, trainingSet.Count);
            Vector <double> output;
            int             count       = 0;
            double          cost        = 0;
            int             batchNumber = 0;

            foreach (TrainingData td in trainingSet)
            {
                output = Process(td.Data);

                cost += CostFunc.Of(td.Response, output) / batchSize;
                PropogateError(CostFunc.Derivative(td.Response, output), batchSize);

                count++;
                if (Abort)
                {
                    return;
                }
                if (count > 0 && count % batchSize == 0)
                {
                    batchNumber++;
                    LastCost = cost;
                    ApplyError();
                    if (!Abort)                          // Trying to make this kind of threadsafe
                    {
                        Hook?.Invoke(batchNumber, this); // Trigger the batch level external control
                    }
                    count = 0;
                    cost  = 0;
                }
            }
        }
        private void PropogateError(TrainingData ts, int batchSize)
        {
            // Initialize the error from the future
            List <Vector <double> > futureErrorCache = new List <Vector <double> >();

            for (var i = 0; i < Layers.Count; i++)
            {
                futureErrorCache.Add(new DenseVector(Layers[i].OutputDimension)); // Only store the error relevant to that layer
            }
            // Step backwards through the memory
            for (var t = OutputCache.Count - 1; t > 0; t--)
            {
                // Error on the output layer from the training data
                Vector <double> error = CostFunc.Derivative(ts[t - 1].Response, OutputCache[t].Last());
                // Step backwards through the net.
                for (var i = Layers.Count - 1; i >= 0; i--)
                {
                    error += futureErrorCache[i];
                    Vector <double> lastInput       = Concatenate(Layers[i].InputDimension, OutputCache[t - 1][i + 1], OutputCache[t][i]);// [t-1][i+1] is the output of the current layer at a previous time
                    Vector <double> jointInputError = Layers[i].PropogateError(error, LearningRate / batchSize, lastInput);
                    Vector <double> pastStateError;

                    // If this is the first layer, error would be the error on the training input, so we can just ignore it.
                    Split(jointInputError, Layers[i].OutputDimension, out pastStateError, out error, OutputCache[t][i]?.Count ?? 1);
                    futureErrorCache[i] = pastStateError; // Store the most recent error from the future.
                }
            }
        }
Exemple #7
0
        private Node GetNode(Tile t, Node parent, IDictionary <Tile, Node> nodes, CostFunc getCost, HeuristicFunc getH)
        {
            Node newNode = new Node(parent, t, getCost(t, parent?.Value), getH(t));
            Node existingNode;

            if (nodes.TryGetValue(t, out existingNode) && existingNode.F <= newNode.F)
            {
                return(existingNode);
            }
            nodes[t] = newNode;
            return(newNode);
        }
        internal override void Learn(HashSet <TrainingData> trainSet, int batchSize = 1)
        {
            batchSize = Math.Min(batchSize, trainSet.Count);
            Vector <double> output;
            double          cost   = 0;
            int             nBatch = 0;

            foreach (TrainingData trainSeq in trainSet)
            {
                // Start over for each different training sequence provided.
                WipeMemory();
                PreviousResponse = null;

                for (var i = 0; i < trainSeq.Count; i++)
                {
                    // Process the pair
                    TrainingData.TrainingPair pair = trainSeq[i];
                    output = Process(pair.Data);
                    cost  += CostFunc.Of(trainSeq[i].Response, output) / (batchSize * MaxMemory);
                    // If we have completely overwriten our short term memory, then
                    // update the weights based on how we performed this time.
                    if (i > 0 && i % MaxMemory == 0)
                    {
                        PropogateError(trainSeq.SubSequence(Math.Min(i - MaxMemory, 0), i), batchSize);
                    }
                    // Count batches by number of error propogations
                    if (i % (batchSize * MaxMemory) == 0)
                    {
                        nBatch++;
                        LastCost = cost;
                        Hook?.Invoke(nBatch, this);
                        ApplyError();
                        cost = 0;
                    }

                    // Keep the last... uhh. this is a PIPI (Parallel Implementation Prone to Inconsistency)
                    // See this.Process
                    if (ForceOutput)
                    {
                        PreviousResponse = pair.Response;
                    }
                    else
                    {
                        PreviousResponse = output;
                    }
                    if (Abort)
                    {
                        Abort = false;
                        return;
                    }
                }
            }
        }
Exemple #9
0
 public SearchNode(CostFunc cost, StateBase state, string op, SearchNode parent, Func <StateBase, int> hHatGetter)
 {
     State    = state;
     Operator = op;
     Parent   = parent;
     Hhat     = hHatGetter(state);
     if (parent != null)
     {
         Ghat  = parent.Ghat + cost(parent.State, state);
         Depth = parent.Depth + 1;
     }
 }
Exemple #10
0
        public Node(StateBase state, CostFunc cost, string action, Node parent, Func <StateBase, int> hHatGetter)
        {
            State  = state;
            Action = action;
            Parent = parent;

            HHat = hHatGetter(state);
            if (parent != null)
            {
                GHat  = parent.GHat + cost(parent.State, state);
                Depth = Parent.Depth + 1;
            }
        }
Exemple #11
0
        private static void RunSingleSearch(PuzzleState initState, PuzzleState goalState)
        {
            Searcher searcher = new Searcher(initState, goalState);
            CostFunc cost     = (state1, state2) => 1;

            List <Thread> searchThreads = new List <Thread>()
            {
                new Thread(() => Console.WriteLine(searcher.BFS().ToString())),
                new Thread(() => Console.WriteLine(searcher.DFS(6).ToString())),
                new Thread(() => Console.WriteLine(searcher.DFS(12).ToString())),
                new Thread(() => Console.WriteLine(searcher.DFS(18).ToString())),
                new Thread(() => Console.WriteLine(searcher.UCS().ToString())),
                new Thread(() => Console.WriteLine(searcher.GreedyBestFirstSearch(new ManhattanDistanceHeuristic()).ToString())),
                new Thread(() => Console.WriteLine(searcher.AStarSearch(new ManhattanDistanceHeuristic()).ToString())),
                new Thread(() =>
                           Console.WriteLine(searcher.GreedyBestFirstSearch(new ManhanttanDistanceWithLinearConflictHeuristic()).ToString())),
                new Thread(() =>
                           Console.WriteLine(searcher.AStarSearch(new ManhanttanDistanceWithLinearConflictHeuristic()).ToString()))
            };

            searchThreads.ForEach(x => x.Start());

            searchThreads.ForEach(x => x.Join());
        }
Exemple #12
0
 /// <summary>
 /// Copmares G-hat values for open list
 /// </summary>
 public SearchResult UniformCostSearch(CostFunc cost)
 {
     OpenListComparator compareNodes = (node1, node2) => node1.Ghat.CompareTo(node2.Ghat);
     return GenericSearch(_initialState, _goalState, "UCS", compareNodes, cost, null);
 }
Exemple #13
0
 /// <summary>
 /// Compares H-hat values for open list
 /// </summary>
 public SearchResult GreedySearch(CostFunc cost, IHeuristic heuristic)
 {
     OpenListComparator compareNodes = (node1, node2) => node1.Hhat.CompareTo(node2.Hhat);
     return GenericSearch(_initialState, _goalState, "Greedy", compareNodes, cost, heuristic);
 }
Exemple #14
0
        /// <summary>
        /// Generic search algorithm
        /// </summary>
        private static SearchResult GenericSearch(StateBase initialState, StateBase goalState, string algName, OpenListComparator comparator, CostFunc cost, IHeuristic heuristic, int? depthLimit = null)
        {
            var openList = new OpenList(comparator);
            var closedList = new ClosedList();
            var cache = new HeuristicCache(goalState, heuristic);
            var nodesGenerated = 0;
            var nodesPrevGenerated = 0;

            // add initial node to open list
            openList.Push(new SearchNode(cost, initialState, null, null, cache.Evaluate));

            while (true)
            {
                // if nothing on open list, fail
                if (openList.Count == 0)
                {
                    return new SearchResult(null, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic);
                }

                // get next node to expand
                var node = openList.Pop();
                closedList.Push(node);

                // if at goal state, success
                if (node.State.Equals(goalState))
                {
                    return new SearchResult(node, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic);
                }

                // if at depth limit, don't generate successors
                if (depthLimit != null && node.Depth == depthLimit)
                {
                    continue;
                }

                var daughters = node.Successors(cost, cache.Evaluate);

                foreach (var daughter in daughters)
                {
                    nodesGenerated++;

                    // if this state is already in open list, replace old node with new node if g-hat is better
                    var foundInOpen = openList.Find(daughter.State);
                    if (foundInOpen != null)
                    {
                        nodesPrevGenerated++;
                        if (daughter.Ghat < foundInOpen.Ghat)
                        {
                            openList.Replace(foundInOpen, daughter);
                        }
                    }
                    else
                    {
                        // else if this state is already in closed list, move from closed to open if g-hat is better
                        var foundInClosed = closedList.Find(daughter.State);
                        if (foundInClosed != null)
                        {
                            nodesPrevGenerated++;
                            if (daughter.Ghat < foundInClosed.Ghat)
                            {
                                openList.Push(daughter);
                                closedList.Remove(foundInClosed);
                            }
                        }
                        else
                        {
                            // else didn't find in open or closed, add to open
                            openList.Push(daughter);
                        }
                    }
                }
            }
        }
        public SearchResult GenericSearch(StateBase initialState,
                                          StateBase goalState,
                                          string algName,
                                          NodeComparator comparator,
                                          CostFunc cost,
                                          IHeuristic heuristic,
                                          int?depthLimit = null)
        {
            Frontier       frontier           = new Frontier(comparator);
            Explored       explored           = new Explored();
            HeuristicCache cache              = new HeuristicCache(goalState, heuristic);
            int            nodesGenerated     = 0;
            int            nodesPrevGenerated = 0;

            Node initNode = new Node(initialState, cost, null, null, cache.Evaluate);

            frontier.Push(initNode);

            while (true)
            {
                if (frontier.Count == 0)
                {
                    return(new SearchResult(null, nodesGenerated, nodesPrevGenerated, 0, explored.Count, algName, heuristic));
                }

                Node node = frontier.Pop();
                explored.Push(node);

                // goal test
                if (node.State.Equals(goalState))
                {
                    return(new SearchResult(node, nodesGenerated, nodesPrevGenerated, frontier.Count, explored.Count, algName, heuristic));
                }

                if (depthLimit != null && node.Depth == depthLimit)
                {
                    continue;
                }

                IEnumerable <Node> children = node.Successors(cost, cache.Evaluate);
                foreach (Node child in children)
                {
                    nodesGenerated++;

                    // If this state is found in the Frontier, replace the old state with the new state if its GHat is smaller
                    Node foundInFrontier = frontier.Find(child.State);
                    if (foundInFrontier != null)
                    {
                        nodesPrevGenerated++;
                        if (foundInFrontier.GHat > child.GHat)
                        {
                            frontier.Replace(foundInFrontier, child);
                        }
                    }
                    else
                    {
                        Node foundInExplored = explored.Find(child.State);

                        // If this state is found in the Explored, replace the old state with the new state if its GHat is smaller
                        if (foundInExplored != null)
                        {
                            nodesPrevGenerated++;
                            if (foundInExplored.GHat > child.GHat)
                            {
                                explored.Remove(foundInExplored);
                                frontier.Push(child);
                            }
                        }
                        else
                        {
                            // doesn't exist in frontier or explored, adding to frontier.
                            frontier.Push(child);
                        }
                    }
                }
            }
        }
Exemple #16
0
 /// <summary>Dijkstra pathfinder</summary>
 /// <param name="start">Starting tile</param>
 /// <param name="getG">Cost function for the tile, also tells pathfinder when to stop and where walls are</param>
 /// <returns>The path, or <seealso cref="Enumerable.Empty{Tile}"/> if no path found</returns>
 private IEnumerable <Tile> FindPathCustom(Tile start, CostFunc getG) => this.FindPathCustom(start, getG, this.NoHeuristic);
Exemple #17
0
        /// <summary>Dijkstra pathfinder with heuristic</summary>
        /// <param name="start">Starting tile</param>
        /// <param name="getCost">Cost function for the tile, also tells pathfinder when to stop and where walls are</param>
        /// <param name="getH">Heuristic function for the tile</param>
        /// <returns>The path, or <seealso cref="Enumerable.Empty{Tile}"/> if no path found</returns>
        private IEnumerable <Tile> FindPathCustom(Tile start, CostFunc getCost, HeuristicFunc getH)
        {
            Dictionary <Tile, Node> nodes    = new Dictionary <Tile, Node>();
            List <Node>             openList = new List <Node> {
                this.GetNode(start, null, nodes, getCost, getH)
            };
            HashSet <string> closedList = new HashSet <string>();

            while (openList.Any())
            {
                // Get current tile and close it
                Node curNode = openList.First();
                Tile curTile = curNode.Value;
                openList.Remove(curNode);
                closedList.Add(curTile.Id);

                // Skip it if it's a wall and not the first tile
                if (curNode.Cost < 0 && curTile != start)
                {
                    continue;
                }

                // Check if done
                // ReSharper disable once CompareOfFloatsByEqualityOperator
                if (curNode.Cost == 0)
                {
                    Stack <Tile> path = new Stack <Tile>();
                    while (curNode != null)
                    {
                        path.Push(curNode.Value);
                        curNode = curNode.Parent;
                    }
                    return(path);
                }

                // Add neighbors
                Tile neighbor = curTile.TileNorth;
                if (neighbor != null && !closedList.Contains(neighbor.Id))
                {
                    openList.RemoveAll(n => n.Value.Id == neighbor.Id);
                    openList.Add(this.GetNode(neighbor, curNode, nodes, getCost, getH));
                }
                neighbor = curTile.TileEast;
                if (neighbor != null && !closedList.Contains(neighbor.Id))
                {
                    openList.RemoveAll(n => n.Value.Id == neighbor.Id);
                    openList.Add(this.GetNode(neighbor, curNode, nodes, getCost, getH));
                }
                neighbor = curTile.TileSouth;
                if (neighbor != null && !closedList.Contains(neighbor.Id))
                {
                    openList.RemoveAll(n => n.Value.Id == neighbor.Id);
                    openList.Add(this.GetNode(neighbor, curNode, nodes, getCost, getH));
                }
                neighbor = curTile.TileWest;
                if (neighbor != null && !closedList.Contains(neighbor.Id))
                {
                    openList.RemoveAll(n => n.Value.Id == neighbor.Id);
                    openList.Add(this.GetNode(neighbor, curNode, nodes, getCost, getH));
                }

                // Sort openList
                openList.Sort((a, b) => (int)(a.F - b.F));
            }

            return(Enumerable.Empty <Tile>());
        }
Exemple #18
0
        /// <summary>
        /// Compares H-hat values for open list
        /// </summary>
        public SearchResult GreedySearch(CostFunc cost, IHeuristic heuristic)
        {
            OpenListComparator compareNodes = (node1, node2) => node1.Hhat.CompareTo(node2.Hhat);

            return(GenericSearch(_initialState, _goalState, "Greedy", compareNodes, cost, heuristic));
        }
Exemple #19
0
        /// <summary>
        /// Copmares G-hat values for open list
        /// </summary>
        public SearchResult UniformCostSearch(CostFunc cost)
        {
            OpenListComparator compareNodes = (node1, node2) => node1.Ghat.CompareTo(node2.Ghat);

            return(GenericSearch(_initialState, _goalState, "UCS", compareNodes, cost, null));
        }
Exemple #20
0
        /// <summary>
        /// Generic search algorithm
        /// </summary>
        private static SearchResult GenericSearch(StateBase initialState, StateBase goalState, string algName, OpenListComparator comparator, CostFunc cost, IHeuristic heuristic, int?depthLimit = null)
        {
            var openList           = new OpenList(comparator);
            var closedList         = new ClosedList();
            var cache              = new HeuristicCache(goalState, heuristic);
            var nodesGenerated     = 0;
            var nodesPrevGenerated = 0;

            // add initial node to open list
            openList.Push(new SearchNode(cost, initialState, null, null, cache.Evaluate));

            while (true)
            {
                // if nothing on open list, fail
                if (openList.Count == 0)
                {
                    return(new SearchResult(null, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic));
                }

                // get next node to expand
                var node = openList.Pop();
                closedList.Push(node);

                // if at goal state, success
                if (node.State.Equals(goalState))
                {
                    return(new SearchResult(node, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic));
                }

                // if at depth limit, don't generate successors
                if (depthLimit != null && node.Depth == depthLimit)
                {
                    continue;
                }

                var daughters = node.Successors(cost, cache.Evaluate);

                foreach (var daughter in daughters)
                {
                    nodesGenerated++;

                    // if this state is already in open list, replace old node with new node if g-hat is better
                    var foundInOpen = openList.Find(daughter.State);
                    if (foundInOpen != null)
                    {
                        nodesPrevGenerated++;
                        if (daughter.Ghat < foundInOpen.Ghat)
                        {
                            openList.Replace(foundInOpen, daughter);
                        }
                    }
                    else
                    {
                        // else if this state is already in closed list, move from closed to open if g-hat is better
                        var foundInClosed = closedList.Find(daughter.State);
                        if (foundInClosed != null)
                        {
                            nodesPrevGenerated++;
                            if (daughter.Ghat < foundInClosed.Ghat)
                            {
                                openList.Push(daughter);
                                closedList.Remove(foundInClosed);
                            }
                        }
                        else
                        {
                            // else didn't find in open or closed, add to open
                            openList.Push(daughter);
                        }
                    }
                }
            }
        }
Exemple #21
0
 public IEnumerable <SearchNode> Successors(CostFunc costFunc, Func <StateBase, int> hHatGetter)
 {
     return(State.Successors().Select(x => new SearchNode(costFunc, x.Value, x.Key, this, hHatGetter)));
 }
Exemple #22
0
        public IEnumerator FindPath <T>(ArrayGrid <T> map, Vector2Int start, Vector2Int end, CostFunc <T> costFunc = null, bool searchDiagonal = false, bool debug = false)
        {
            result = null;

            var pathCostFunc = costFunc ?? GetCost;

            //we'll use var here for now so we can change the type easier later
            // The set of nodes already evaluated
            var closed = new Dictionary <int, List <int> >();

            // The set of currently discovered nodes that are not evaluated yet.
            // Initially, only the start node is known.
            //var open = new List<Vector2Int>() { start };
            var open          = new SortedDictionary <HeapKey, Vector2Int>();
            var openPositions = new Dictionary <int, List <int> >();

            // For each node, which node it can most efficiently be reached from.
            // If a node can be reached from many nodes, cameFrom will eventually contain the
            // most efficient previous step.
            ArrayGrid <Vector2Int> cameFrom = new ArrayGrid <Vector2Int>(map.Size);

            // For each node, the cost of getting from the start node to that node.
            ArrayGrid <float> gScore = new ArrayGrid <float>(map.Size, float.MaxValue);

            // The cost of going from start to start is zero.
            gScore[start] = 0f;

            // For each node, the total cost of getting from the start node to the goal
            // by passing by that node. That value is partly known, partly heuristic.
            ArrayGrid <float> fScore = new ArrayGrid <float>(map.Size, float.MaxValue);

            // For the first node, that value is completely heuristic.
            fScore[start] = GetCostEstimate(map, start, end);

            AddToOpen(open, openPositions, fScore[start], start);

            int throttleCounter = 0;

            while (!IsEmpty(open))
            {
                throttleCounter++;
                if (throttleCounter % throttle == 0)
                {
                    yield return(new WaitForEndOfFrame());
                }

                KeyValuePair <HeapKey, Vector2Int> current = open.First();

                if (IsGoal(current.Value, end))
                {
                    result = ReconstructPath(cameFrom, start, current.Value);
                    yield break;//done
                }

                open.Remove(current.Key);
                RemoveFromSet(openPositions, (int)current.Value.x, (int)current.Value.y);

                if (debug)
                {
                    if (debugPath == null)
                    {
                        debugPath = new List <Vector2Int>();
                    }

                    debugPath.Add(current.Value);
                }

                AddToSet(closed, (int)current.Value.x, (int)current.Value.y);

                List <Vector2Int> neighbors = map.GetAdjacentPositions(current.Value, searchDiagonal);

                for (int i = 0; i < neighbors.Count; ++i)
                {
                    if (IsInSet(closed, (int)neighbors[i].x, (int)neighbors[i].y))
                    {
                        continue;
                    }

                    bool addToOpen = false;

                    if (!IsInSet(openPositions, (int)neighbors[i].x, (int)neighbors[i].y))
                    {
                        addToOpen = true;
                    }

                    // The distance from start to a neighbor

                    float gScoreTemp = gScore[current.Value] + pathCostFunc(map, current.Value, neighbors[i]);
                    if (gScoreTemp >= gScore[neighbors[i]])
                    {
                        if (addToOpen)
                        {
                            AddToOpen(open, openPositions, float.MaxValue, neighbors[i]);
                        }
                        continue;
                    }

                    // This path is the best until now. Record it!
                    cameFrom[neighbors[i]] = current.Value;
                    gScore[neighbors[i]]   = gScoreTemp;
                    fScore[neighbors[i]]   = gScore[neighbors[i]] + GetCostEstimate(map, neighbors[i], end);

                    if (addToOpen)
                    {
                        AddToOpen(open, openPositions, fScore[neighbors[i]], neighbors[i]);
                    }
                }
            }

            //TODO: notify failure?

            yield break;
        }