Pathfinder Code

static public function calculatePath(nodes : Array<INode>, start: INode, end :INode): Array<INode>
    {
        var ret : Array<INode> = new Array<INode>();
        if (start == end) return ret;

        // reset all values
        for (i in 0...nodes.length)
        {
            var n : INode = nodes[i];
            n.Clear();
        }

        var toCheck : Array<INode> = new Array<INode>();
        toCheck.push(end);
        end.NodeValue = 0;


        var toCheckNext : Array<INode> = new Array<INode>();
        var estimateMap : Map<INode, Float> = new Map<INode, Float>();
        var hasfound : Bool = false;
        while (!hasfound)
        {
            if (toCheck.length <= 0) break;
            for (i in 0... toCheck.length)
            {
                var n : INode = toCheck[i];
                n.Visit();

                if (n == start)
                {
                    hasfound = true;
                    break;
                }

                for (k in n.neighbours.keys())
                {
                    if (!k.Visited)
                    {
                        if (k.NodeValue == -1 || k.NodeValue > n.NodeValue + n.neighbours.get(k))
                        {
                            k.NodeValue = n.NodeValue + n.neighbours.get(k);
                        }
                        //toCheckNext.push(k);
                        if (!k.isInEstimateMap)
                        {
                            var v : Float = k.estimate(start);
                            k.isInEstimateMap = true;
                            estimateMap.set(k, v);
                        }
                    }
                }
                // get closest estimate from estimate map
                var min : Float = 5000000;
                var newTile : INode = null;
                for (e in estimateMap.keys())
                {
                    if (estimateMap.get(e) < min)
                    {
                        newTile = e;
                        min = estimateMap.get(e);
                    }
                }
                if (newTile != null)
                {
                    estimateMap.remove(newTile);
                    toCheckNext.push(newTile);
                }

            }
            while (toCheck.length != 0) toCheck.pop();
            while (toCheckNext.length != 0)
            {
                var n : INode = toCheckNext.pop();
                if (!n.Visited) toCheck.push(n);
            }

        }

        // map has been created, now walk it downwards

        var current : INode = start;
        ret.push(current);
        var next : INode = null;
        var atTheEnd : Bool = false;
        while (!atTheEnd)
        {
            if (current == end)
            {
                atTheEnd = true;
                break;
            }

            // find biggest slope
            var Min : Float = current.NodeValue;
            var minTile : INode = null;
            var M : Map<INode, Float> = current.neighbours;
            for (k in M.keys())
            {
                trace(123);
                if (k.NodeValue < Min && k.NodeValue != -1)
                {
                    Min = k.NodeValue;
                    minTile = k;
                    if (k.NodeID == end.NodeID)
                    {
                        atTheEnd = true;
                        break;
                    }
                }
            }
            trace(minTile);
            ret.push(minTile);
            current = minTile;
        }



        return ret;

    }