A* PathFinding Poor Performance

После нескольких часов отладки алгоритм работает. Прямо сейчас, чтобы проверить, работает ли это, я проверяю позицию конечного узла на позицию currentNode, когда цикл while завершается. Пока значения выглядят правильно. Проблема в том, что чем дальше я удаляюсь от NPC, который в данный момент неподвижен, тем хуже становится производительность. Доходит до того, что игра становится неиграбельной со скоростью менее 10 кадров в секунду. Мой текущий PathGraph составляет 2500 узлов, что, как мне кажется, довольно мало, верно? Любые идеи о том, как улучшить производительность?

struct Node
{
    bool walkable;      //Whether this node is blocked or open
    vect2 position;     //The tile's position on the map in pixels
    int xIndex, yIndex; //The index values of the tile in the array
    Node*[4] connections; //An array of pointers to nodes this current node connects to
    Node* parent;
    int gScore;
    int hScore;
    int fScore;
}

class AStar
{
    private:
    SList!Node openList;    //List of nodes who have been visited, with F scores but not processed
    SList!Node closedList;  //List of nodes who have had their connections processed

    //Node*[4] connections;     //The connections of the current node;

    Node currentNode;           //The current node being processed

    Node[] Path;        //The path found;

    const int connectionCost = 10;

    Node start, end;

//////////////////////////////////////////////////////////

    void AddToList(ref SList!Node list, ref Node node )
    {
        list.insert( node );
    }

    void RemoveFrom(ref SList!Node list, ref Node node )
    {
        foreach( elem; list )
        {
            if( node.xIndex == elem.xIndex && node.yIndex == elem.yIndex )
            {
                auto a = find( list[] , elem );
                list.linearRemove( take(a, 1 ) );
            }
        }
    }


    bool IsInList( SList!Node list, ref Node node )
    {
        foreach( elem; list )
        {
            if( node.xIndex == elem.xIndex && node.yIndex == elem.yIndex )
                return true;
        }

        return false;
    }

    void ClearList( SList!Node list )
    {
        list.clear;
    }

    void SetParentNode( ref Node parent, ref Node child )
    {
        child.parent = &parent;
    }

    void SetStartAndEndNode( vect2 vStart, vect2 vEnd, Node[] PathGraph )
    {
        int startXIndex, startYIndex;
        int endXIndex, endYIndex;

        startXIndex = cast(int)( vStart.x / 32 );
        startYIndex = cast(int)( vStart.y / 32 );

        endXIndex = cast(int)( vEnd.x / 32 );
        endYIndex = cast(int)( vEnd.y / 32 );

        foreach( node; PathGraph )
        {
            if( node.xIndex == startXIndex && node.yIndex == startYIndex )
            {
                start = node;
            }
            if( node.xIndex == endXIndex && node.yIndex == endYIndex )
            {
                end = node;
            }
        }
    }

    void SetStartScores( ref Node start )
    {
        start.gScore = 0;

        start.hScore = CalculateHScore( start, end );

        start.fScore = CalculateFScore( start );

    }

    Node GetLowestFScore()
    {
        Node lowest;

        lowest.fScore = 10000;

        foreach( elem; openList )
        {
            if( elem.fScore < lowest.fScore )
                lowest = elem;
        }

        return lowest;
    }

    //This function current sets the program into an infinite loop
    //I still need to debug to figure out why the parent nodes aren't correct
    void GeneratePath()
    {
        while( currentNode.position != start.position )
        {
            Path ~= currentNode;
            currentNode = *currentNode.parent;
        }
    }

    void ReversePath()
    {
        Node[] temp;
        for(int i = Path.length - 1; i >= 0; i-- )
        {
            temp ~= Path[i];
        }
        Path = temp.dup;
    }

    public:
    //@FIXME It seems to find the path, but now performance is terrible
    void FindPath( vect2 vStart, vect2 vEnd, Node[] PathGraph )
    {
        openList.clear;
        closedList.clear;

        SetStartAndEndNode( vStart, vEnd, PathGraph );
        SetStartScores( start );
        AddToList( openList, start );

        while( currentNode.position != end.position )
        {
            currentNode = GetLowestFScore();

            if( currentNode.position == end.position )
                break;
            else
            {
                RemoveFrom( openList, currentNode );
                AddToList( closedList, currentNode );

                for( int i = 0; i < currentNode.connections.length; i++ )
                {
                    if( currentNode.connections[i] is null )
                        continue;
                    else
                    {
                        if( IsInList( closedList, *currentNode.connections[i] ) 
                           && currentNode.gScore < currentNode.connections[i].gScore )
                        {
                            currentNode.connections[i].gScore = currentNode.gScore + connectionCost;
                                 currentNode.connections[i].hScore = abs( currentNode.connections[i].xIndex - end.xIndex ) 
                            + abs(     currentNode.connections[i].yIndex - end.yIndex );
                            currentNode.connections[i].fScore =     currentNode.connections[i].gScore +   currentNode.connections[i].hScore;
                            currentNode.connections[i].parent = &currentNode;
                        }
                        else if( IsInList( openList, *currentNode.connections[i] ) 
                                && currentNode.gScore < currentNode.connections[i].gScore )
                        {
                            currentNode.connections[i].gScore = currentNode.gScore + connectionCost;
                            currentNode.connections[i].hScore = abs( currentNode.connections[i].xIndex - end.xIndex ) 
                            + abs(     currentNode.connections[i].yIndex - end.yIndex );
                            currentNode.connections[i].fScore = currentNode.connections[i].gScore + currentNode.connections[i].hScore;
                            currentNode.connections[i].parent = &currentNode;
                        }
                        else
                        {

                            currentNode.connections[i].gScore = currentNode.gScore + connectionCost;
                            currentNode.connections[i].hScore = abs( currentNode.connections[i].xIndex - end.xIndex ) 
                            + abs( currentNode.connections[i].yIndex - end.yIndex );
                             currentNode.connections[i].fScore = currentNode.connections[i].gScore +     currentNode.connections[i].hScore;
                            currentNode.connections[i].parent = &currentNode;
                            AddToList( openList, *currentNode.connections[i] );
                        }
                    }   
                }
            }
        }

        writeln( "Current Node Position: ", currentNode.position );
        writeln( "End Node Position: ", end.position );

        if( currentNode.position == end.position )
        {
            writeln( "Current Node Parent: ", currentNode.parent );
           //GeneratePath();
           //ReversePath();
        }
    }

    Node[] GetPath()
    {
        return Path;
    }
}