Quadtree et path planning avec l'algorithme A*

On rencontre sa destinée souvent par les chemins qu'on prend pour l'éviter.

— Jean de La Fontaine, L'Horoscope

Ce texte décrit l'application de l'algorithme A* sur des quadtrees — arbres quaternaires. Dans la première partie du texte, une implantation en C++ de cette structure de données est présentée alors qu'en seconde partie une implantation de l'algorithme A* opérant sur cette dernière est décrite.

Quadtree

Essentiellement, un quadtree est une structure de données permettant de discrétiser un espace de façon optimale. En particulier, il est possible de modéliser à la fois des surfaces bidimensionnelles et tridimensionnelles à l'aide de quadtrees. Ceux-ci trouvent donc des applications autant dans le domaine de la cartographie numérique que dans celui de la compression d'image.

Les quadtrees permettent de discrétiser l'espace d'une manière efficace car, contrairement à un découpage en grille naïf, les espaces libres ne sont pas découpés en quartiers. En interprétant le résultat de ce découpage comme un graphe, il est clair que le nombre de nœuds nécessaires afin de représenter une surface à l'aide d'un quadtree est généralement avantageux, en particulier si la surface contient peu d'obstacles.

Pour les fins de l'exercice, les quadtrees seront ici utilisés afin de modéliser une surface bidimensionnelle sur laquelle des obstacles rectangulaires sont disposés. Par convention, les obstacles et autres éléments de l'espace sont toujours décrits par deux coordonnées associées aux coins inférieur gauche et supérieur droit.

class Obstacle {
protected:
    Point top_right;   /**< The obstacle top right corner's coordinates. */
    Point bottom_left; /**< The obstacle bottom left corner's coordinates. */

public:
    //! \brief Obstacle constructor.
    //! \brief origine Bottom left corner's coordinates.
    //! \brief width Obstacle's width.
    //! \brief height Obstacle's height.
    Obstacle(Point origine, int width, int height) :
        bottom_left(origine),
        top_right(Point(origine.x + width, origine.y + height)) {
    }

    //! \brief Obstacle constructor.
    //! \brief bottom_left Bottom left corner's coordinates.
    //! \brief top_right Top right corner's coordinates.
    Obstacle(Point bottom_left, Point top_right):
        bottom_left(bottom_left), top_right(top_right) {
    }

    //! \brief Given two points representing the bottom left and top right
    //! corners of an object, check if the obstacle intersect them.
    //! \brief bottom_left The object bottom left corner's coordinates.
    //! \brief top_right The object top right corner's coordinates.
    //! \return True if the obstacle intersect the object.
    bool intersect(const Point& bottom_left, const Point& top_right) const {

        // First, check if there is an overlap on the x-axis.
        bool x_overlap = this->bottom_left.x < top_right.x &&
                this->top_right.x > bottom_left.x;

        // Second, check if there is an overlap on the y-axis.
        bool y_overlap = this->bottom_left.y < top_right.y &&
                this->top_right.y > bottom_left.y;

        // If there is an overlap in both the y and x axis, we can safely
        // conclude that there is an overlap between the two objects.
        return x_overlap && y_overlap;
    }

    //! \brief Given two points representing the bottom left and top right
    //! corners of an object, check if the obstacle obstruct them.
    //! \brief bottom_left The object bottom left corner's coordinates.
    //! \brief top_right The object top right corner's coordinates.
    //! \return True if the obstacle obstruct the object.
    bool obstruct(const Point& bottom_left, const Point& top_right) const {

        // First, check if there is an obstruction on the x-axis.
        bool x_obstruct = this->bottom_left.x <= bottom_left.x &&
                this->top_right.x >= top_right.x;

        // Second, check if there is an obstruction on the y-axis.
        bool y_obstruct = this->bottom_left.y <= bottom_left.y &&
                this->top_right.y >= top_right.y;

        // If there is an obstruction in both the y and x axis, we can safely
        // conclude that there is an overlap between the two objects.
        return x_obstruct && y_obstruct;
    }
};

L'implantation des quadtrees est quelque peu particulière. Contrairement à un arbre binaire classique dans lequel des noeuds sont instanciés lorsque l'utilisateur insère des données, un quadtree doit générer tous ses nœuds dès sa construction. Sans cela, le quadtree se retrouve tout simplement dans un état incohérent et n'est d'aucune utilité.

Cette particularité implique qu'une certaine somme de travail doit être effectuée au sein des constructeurs. Si cette pratique n'est pas recommandable en temps normal car elle affecte la testabilité, il en va ici du bon sens. De plus, dans la mesure où il s'agit d'une structure de données, cette situation est très certainement acceptable.

En pratique, nous cherchons à représenter une grille semblable à celle présentée ci-dessus comme un arbre quaternaire semblable à celui-ci.

Les nœuds blancs indiquent que le quadrant correspondant est libre de tout obstacle alors que les nœuds noirs indiquent que le quadrant correspondant est obstrué par un obstacle. On réfère aux fils d'un nœud en fonction de leur position géographique: nord-est, nord-ouest, sud-ouest, sud-est. De même, on réfère aux voisins d'un nœud à l'aide de directions: nord, sud, est, ouest.

La figure ci-dessous montre à quoi ressemble le découpage induit par ce quadtree à vue d'oiseau.

Les deux figures ci-dessous illustrent ce à quoi peut ressembler le découpage d'une surface plus grande sur laquelle le nombre d'obstacles est plus important.

Les classes Quadnode et Quadtree forment le duo implantant les quadtrees.

La classe Quadtree est en quelque sorte le «driver» de la classe Quadnode. Le constructeur de la classe Quadtree n'effectue que quelques vérifications afin de s'assurer que le quadtree pourra être instancié convenablement et appelle le premier constructeur de la classe Quadnode où le véritable travail sera exécuté. La racine du quadtree est stockée dans un pointeur partagé. Les copies d'un quadtree sont donc toujours des copies de surface. Ce comportement n'est pas problématique car un quadtree n'est pas une structure de données que l'on modifie à la manière d'un arbre binaire. D'ailleurs, hormis les méthodes utilisées lors de sa construction, aucune n'altère la structure d'un quadtree une fois ce dernier instancié.

La méthode find_node permet de localiser le nœud d'un quadtree correspondant à une coordonnée particulière. Cette méthode est utilisée afin fournir à l'algorithme A* les nœuds de départ et d'arrivé.

class Quadtree {
protected:
    std::shared_ptr<Quadnode> root; /**< Quadtree's root node. */

public:
    Quadtree(const std::vector<Obstacle>& obstacles, int size = 2,
            int resolution = 1) throw (std::logic_error) {

        // To ensure a consistent behavior and eliminate corner cases, the
        // Quadtree's root node need to have children, i.e. it can't
        // be a leaf node. Thus, the first instantiated Quadnode need to
        // always be subdivided. These two conditions make sure that
        // even with this subdivision the resolution will be respected.
        if (resolution < 1) {
            throw std::logic_error("Resolution must be greater than 0.");
        } else if (size < resolution * 2) {
            throw std::logic_error("Grid size must be greater or equal "
                                   "to twice the resolution value.");
        }

        // Can't use make_shared since the Quadnode constructor is protected.
        root = std::shared_ptr<Quadnode>(new Quadnode(obstacles, Point(0, 0),
                Point(size, size), resolution));
    }

    //! \brief Given a point, find the leaf Quadnode associated containing it.
    //! \param point The point searched for.
    //! \return A pointer to the containing Quadnode.
    Quadnode* find_node(const Point& point) throw(std::logic_error) {

        // Before doing anything else, make sure
        // that the requested point is inbound.
        if (!root->inbound(point)) {
            throw std::logic_error("Input point is out of bounds.");
        }

        Quadnode* node = root.get();
        while (!node->is_leaf()) {
            int x_mean = node->bottom_left.x + node->width() / 2;
            int y_mean = node->bottom_left.y + node->height() / 2;

            if (point.x < x_mean) {
                if (point.y < y_mean) {
                    node = node->southwest;
                } else {
                    node = node->northwest;
                }
            } else {
                if (point.y < y_mean) {
                    node = node->southeast;
                } else {
                    node = node->northeast;
                }
            }
        }

        return node;
    }
};

La partie intéressante se trouve véritablement dans la classe Quadnode. Conceptuellement, les méthodes de cette classe peuvent être regroupées en deux familles:

L'algorithme de construction du quadtree peut être résumé de la manière suivante.

  • Diviser récursivement la surface S en quatre quartiers Q de taille identique.
  • Pour chaque quartier Q,
    • si un obstacle obstrue Q ou que la taille de Q est inférieure au seuil L, marquer Q comme étant obstrué;
    • si un obstacle intersecte Q, diviser récursivement Q;
    • sinon marquer Q comme étant libre.

Les deux constructeurs et la méthode subdivide implantent cet algorithme au sein de la classe Quadnode.

Les méthodes autorisant les déplacement de nœud en nœud au sein du quadtree sont toutes basées sur les algorithmes décrits par Hanan Samet dans l'article Neighbor Finding in Quadtrees. Parmi ces algorithmes, le plus important est certainement celui permettant de localiser le voisin immédiat d'un nœud dans une direction donnée.

class Quadnode {
protected:

    friend class Quadtree;

    enum Side { NORTH, EAST, SOUTH, WEST };
    enum Quadrant { NORTHWEST, NORTHEAST, SOUTHWEST, SOUTHEAST };
    enum State { FREE, MIXED, OBSTRUCTED };

    //         North
    //      .----.----.
    //      | NW | NE |
    // West '----'----' East
    //      | SW | SE |
    //      '----'----'
    //         South

    Quadnode* parent;       /**< Pointer to the parent node. */
    Quadnode* northwest;    /**< Pointer to the northwest child. */
    Quadnode* southwest;    /**< Pointer to the southwest child. */
    Quadnode* northeast;    /**< Pointer to the northeast child. */
    Quadnode* southeast;    /**< Pointer to the southeast child. */

    Point top_right;        /**< Quadnode top right corner coordinates. */
    Point bottom_left;      /**< Quadnode bottom left corner coordinates. */

    State status;           /**< Quadnode status. */

    //! \brief Quadtree constructor.
    //! \param obstacles List of obstacles.
    //! \param bottom_left Quadnode bottom left corner's coordinates.
    //! \param top_right Quadnode top right corner's coordinates.
    //! \param resolution The Quadtree resolution which is the minimal
    //! Quadnode size.
    //!
    //! This constructor is called by the Quadtree's constructor. It will
    //! instanciate the Quadtree's root node and launch the sub-dividing
    //! process.
    Quadnode(const std::vector<Obstacle>& obstacles, Point bottom_left,
            Point top_right, int resolution) :
        status(MIXED), northwest(NULL), southwest(NULL), northeast(NULL),
                southeast(NULL), parent(NULL), bottom_left(bottom_left),
                top_right(top_right) {

        subdivide(obstacles, resolution);
    }

    //! \brief Quadnode children constructor.
    //! \param obstacles List of obstacles.
    //! \param bottom_left Quadnode bottom left corner's coordinates.
    //! \param top_right Quadnode top right corner's coordinates.
    //! \param resolution The Quadtree resolution which is the minimal
    //! Quadnode size.
    //! \param parent A pointer to the parent Quadnode.
    //!
    //! This constructor is called when building inner Quadtree's nodes.
    //! It will only subdivide itself if it intersects with an obstacle and
    //! if the minimal resolution isn't reached yet.
    Quadnode(const std::vector<Obstacle>& obstacles, Point bottom_left,
            Point top_right, int resolution, Quadnode* parent) :
        status(MIXED), northwest(NULL), southwest(NULL), northeast(NULL),
                southeast(NULL), parent(parent), bottom_left(bottom_left),
                top_right(top_right) {

        // Loop over all obstacles checking if we intersect something.
        for (std::vector<Obstacle>::const_iterator it = obstacles.begin();
                it != obstacles.end(); ++it) {
            if (it->intersect(bottom_left, top_right)) {
                // We're intersecting something, i.e. the node is either
                // mixed or obstructed. If the node is obstructed by this
                // obstacle, no subdivision is needed, otherwise we subdivide.
                if (it->obstruct(bottom_left, top_right) ||
                        width() <= resolution || height() <= resolution) {
                    status = OBSTRUCTED;
                } else {
                    subdivide(obstacles, resolution);
                }
                return; // We're done.
            }
        }

        status = FREE;  // This node intersects no obstacle
                        // and is therefore marked as free.
    }

    // Both assignment operator and copy constructor declared
    // protected in order to disallow raw copy or assignment.
    void operator=(const Quadnode&) {}
    Quadnode(const Quadnode&) {}

    //! \brief Subdivide the current node into four children.
    //! \param obstacles The obstacle list.
    //! \param resolution The Quadtree resolution.
    //!
    //! This methode should be called once by the constructor if
    //! the current node intersect with an obstacle and its
    //! width and height are both greater than the resolution.
    void subdivide(const std::vector<Obstacle>& obstacles, int resolution) {

        status = MIXED; // This is the default.

        int x0 = bottom_left.x;                     //  y2 .----.-------.
        int x1 = bottom_left.x + width() / 2;       //     |    |       |
        int x2 = top_right.x;                       //     | NW |  NE   |
                                                    //     |    |       |
        int y0 = bottom_left.y;                     //  y1 '----'-------'
        int y1 = bottom_left.y + height() / 2;      //     | SW |  SE   |
        int y2 = top_right.y;                       //  y0 '----'-------'
                                                    //     x0   x1     x2

        northwest = new Quadnode(obstacles,
                Point(x0, y1), Point(x1, y2), resolution, this);
        southwest = new Quadnode(obstacles,
                Point(x0, y0), Point(x1, y1), resolution, this);
        northeast = new Quadnode(obstacles,
                Point(x1, y1), Point(x2, y2), resolution, this);
        southeast = new Quadnode(obstacles,
                Point(x1, y0), Point(x2, y1), resolution, this);
    }

    //! \brief Check if a quadrant is adjacent to a given side of this node.
    //! \param side The direction.
    //! \param quad The quadrant.
    //! \return True if the quadrant is adjacent to the given side of this node.
    bool adjacent(Side side, Quadrant quad) const {

        static const bool ADJACENT[5][5] = {
                        /* NW     NE     SW     SE  */
                /* N */ { true,  true,  false, false },
                /* E */ { false, true,  false, true  },
                /* S */ { false, false, true,  true  },
                /* W */ { true,  false, true,  false }
        };

        return ADJACENT[side][quad];
    }

    //! \brief Obtain the mirror image of a quadrant on a given side.
    //! \param side The direction.
    //! \param quad The quadrant.
    //! \return The mirror quadrant on the given side.
    Quadrant reflect(Side side, Quadrant quad) const {

        static const Quadrant REFLECT[5][5] = {
                        /*   NW         NE         SW         SE    */
                /* N */ { SOUTHWEST, SOUTHEAST, NORTHWEST, NORTHEAST },
                /* E */ { NORTHEAST, NORTHWEST, SOUTHEAST, SOUTHWEST },
                /* S */ { SOUTHWEST, SOUTHEAST, NORTHWEST, NORTHEAST },
                /* W */ { NORTHEAST, NORTHWEST, SOUTHEAST, SOUTHWEST }
        };

        return REFLECT[side][quad];
    }

    //! \brief Given a direction, obtain its opposite.
    //! \param side The direction.
    //! \return The opposite direction.
    Side opposite(Side side) const {
                                        /*  N     E      S     W  */
        static const Side OPPOSITE[4] = { SOUTH, WEST, NORTH, EAST };

        return OPPOSITE[side];
    }

    //! \brief Obtain this node's quadrant relative to its parent.
    //! \pre The current node isn't the tree root.
    //! \return This node's quadrant relative to its parent.
    Quadrant quadrant() const throw(std::logic_error) {
        if (!parent) {
            throw std::logic_error("The root node's quadrant isn't defined.");
        }

        if (parent->northwest == this) {
            return NORTHWEST;
        } else if (parent->southwest == this) {
            return SOUTHWEST;
        } else if (parent->northeast == this) {
            return NORTHEAST;
        } else {
            return SOUTHEAST;
        }
    }

    //! \brief Given a quadrant, obtain a pointer to the associated child node.
    //! \param quad The quadrant.
    //! \return A pointer to the child node associated to this quadrant.
    Quadnode* child(Quadrant quad) const {
        switch (quad) {
        case NORTHWEST:
            return northwest;
        case SOUTHWEST:
            return southwest;
        case NORTHEAST:
            return northeast;
        case SOUTHEAST:
            return southeast;
        }
    }

    //! \brief Append all the leaf children of this node, i.e. either free or
    //! obstructed node, that can be found in a given direction to a vector.
    //! \param direction The side where to look for leaf children.
    //! \param[out] children The vector to which the children will be appended.
    void children(Side direction,
            std::vector<Quadnode*>& children) const {

        if (is_leaf())
            return;

        Quadnode* s1;
        Quadnode* s2;

        switch (direction) {
        case NORTH:
            s1 = northeast;
            s2 = northwest;
            break;
        case EAST:
            s1 = northeast;
            s2 = southeast;
            break;
        case SOUTH:
            s1 = southeast;
            s2 = southwest;
            break;
        case WEST:
            s1 = northeast;
            s2 = southwest;
        }

        if (s1->is_leaf()) {
            children.push_back(s1);
        } else {
            s1->children(direction, children);
        }

        if (s2->is_leaf()) {
            children.push_back(s2);
        } else {
            s2->children(direction, children);
        }
    }

    //! \brief Locate an equal-sized neighbor of the current node in the
    //! vertical or horizontal direction; cf. Hanan Samet 1981 article Neighbor
    //! Finding in Quadtrees.
    //! \param direction The direction where to look for the neighbor.
    //! \return A pointer to the neighbor or NULL if it can't be found.
    Quadnode* neighbor(Side direction) const {
        Quadnode* neighbor = NULL;

        // Ascent the tree up to a common ancestor.
        if (parent and adjacent(direction, quadrant())) {
            neighbor = parent->neighbor(direction);
        } else {
            neighbor = parent;
        }

        // Backtrack mirroring the ascending moves.
        if (neighbor and !neighbor->is_leaf()) {
            return neighbor->child(reflect(direction, quadrant()));
        } else {
            return neighbor;
        }
    }

    //! \brief Locate a neighbor of the current quadnode in the horizontal or
    //! vertical direction which is adjacent to one of its corners. The
    //! neighboring node must be adjacent to this corner.
    //! \param direction The direction where to look for the neighbor.
    //! \param corner The corner to which the neighboring node must be adjacent.
    //! \return A pointer to the neighbor or NULL if it can't be found.
    Quadnode* neighbor(Side direction, Quadrant corner) const {

        // If no neighbor can be found in the given
        // direction, quadnode will be null.
        Quadnode* quadnode = neighbor(direction);

        if (!quadnode)
            return NULL;

        // Go down until we reach either a free or
        // an obstructed node, i.e. a leaf node.
        while (!quadnode->is_leaf()) {
            quadnode = quadnode->child(reflect(direction, corner));
        }

        return quadnode;
    }

    //! \brief Locate all leaf neighbours of the current node in the given
    //! direction, appending them to a given vector.
    //! \param direction The side where to look for neighbours.
    //! \param[out] neighbours The vector to which the neighbours
    //! will be appended.
    void neighbours(Side direction,
            std::vector<Quadnode*>& neighbours) const {

        // If no neighbor can be found in the given
        // direction, quadnode will be null.
        Quadnode* quadnode = neighbor(direction);

        if (quadnode != NULL) {
            if (quadnode->is_leaf()) {
                // Neighbor is already a leaf node, we're done.
                neighbours.push_back(quadnode);
            } else {
                // The neighbor isn't a leaf node so we need to
                // go further down matching its children, but in
                // the opposite direction from where we came.
                quadnode->children(opposite(direction), neighbours);
            }
        }
    }

public:

    //! \brief Retrieve a list of all leaf neighbours of the current node.
    //! \return A vector containing pointers to each node's neighbor.
    std::vector<Quadnode*> neighbours() const {
        std::vector<Quadnode*> _neighbours;
        neighbours(NORTH, _neighbours);
        neighbours(SOUTH, _neighbours);
        neighbours(EAST, _neighbours);
        neighbours(WEST, _neighbours);
        return _neighbours;
    }

    //! \brief Calculate the width of the region represented by this node.
    //! \return The width of the region represented by this node.
    inline int width() const {
        return top_right.x - bottom_left.x;
    }

    //! \brief Calculate the height of the region represented by this node.
    //! \return The height of the region represented by this node.
    inline int height() const {
        return top_right.y - bottom_left.y;
    }

    Point origine() const {
        return bottom_left;
    }

    //! \brief Check if this node is a leaf, i.e. is either obstructed or free.
    //! \return True if this node is a leaf.
    inline bool is_leaf() const {
        return status != MIXED;
    }

    //! \brief Check if this node is a free.
    //! \return True if this node is a free.
    inline bool is_free() const {
        return status == FREE;
    }

    //! \brief Obtain the straight-line distance between this node and another.
    //! \param other A pointer to the other node.
    //! \return The straight-line distance between the two nodes.
    float distance(const Quadnode* other) {

        // Pythagore is your friend !
        //
        //         a    (x1,y1)
        //    .-----------.
        //    |        .-'
        //  b |     .-'
        //    |  .-'
        //    '-'
        // (x2, y2)

        float x1 = bottom_left.x + width() / 2.0;
        float y1 = bottom_left.y + height() / 2.0;

        float x2 = other->bottom_left.x + other->width() / 2.0;
        float y2 = other->bottom_left.y + other->height() / 2.0;

        float a = abs(x1 - x2);
        float b = abs(y1 - y2);

        return sqrt(pow(a, 2) + pow(b, 2));
    }

    //! \brief Check if a given point is inside the region represented
    //! by this node.
    //! \param point The point to check for containment.
    //! \return True if the point is inside the region represented
    //! by this node.
    bool inbound(const Point& point) const {
        return (bottom_left.x <= point.x && point.x <= top_right.x)
                && (bottom_left.y <= point.y && point.y <= top_right.y);
    }

    //! \brief Destructor.
    ~Quadnode() {
        // Only mixed node have children.
        if (status == MIXED) {
            delete northwest;
            delete southwest;
            delete northeast;
            delete southeast;
        }
    }
};

Algorithme A*

L'algorithme A* n'a certainement pas besoin de présentation. Néanmoins, voici quelques notes sur cette implantation particulière.

Pour éviter de «polluer» les nœuds du quadtree avec des informations spécifiques à l'algorithme de path planning, celui-ci possède sa propre abstraction — AStarNode — pour les nœuds qu'il manipule.

struct AStarNode {
    //! \brief AStarNode constructor.
    //! \param ancestor A pointer to the AStarNode from where we came from.
    //! \param quadnode A pointer to the Quadnode that we're representing.
    //! \param path The actual cost from the stored Quadnode to the start one.
    //! \param estimate The estimated cost from the stored Quadnode to the
    //! destination one.
    AStarNode(AStarNode* ancestor, Quadnode* quadnode,
            float path = 0, float estimate = 0) :
        ancestor(ancestor), quadnode(quadnode), total_cost(path + estimate),
        path_cost(path), estimated_cost(estimate) {
    }

    //! \brief Check if this AStarNode contains (represents) a given Quadnode.
    //! \param quadnode The query quadnode.
    //! \return True if this AStarNode contains the quadnode.
    bool contains(const Quadnode* quadnode) const {
        return this->quadnode == quadnode;
    }

    //! \brief Wrapper around the stored Quadnode neighbours method.
    //! \return A vector containing the stored Quadnode neighbours.
    std::vector<Quadnode*> neighbours() const {
        return quadnode->neighbours();
    }

    //! \brief Calculate the distance between this AStarNode and another.
    //! \param other A pointer to another AStarNode.
    //! \return The distance between the two AStarNode.
    float distance(AStarNode* other) const {
        return quadnode->distance(other->quadnode);
    }

    //! \brief Calculate the distance between this AStarNode and a given Quadnode.
    //! \param other A pointer to a Quadnode.
    //! \return The distance between the two AStarNode.
    float distance(Quadnode* other) const {
        return quadnode->distance(other);
    }

    AStarNode* ancestor;    /**< The node ancestor, used to rebuild path. */
    Quadnode* quadnode;     /**< Corresponding quadnode. */
    float total_cost;       /**< (F) Total cost. */
    float path_cost;        /**< (G) Path cost from the starting point. */
    float estimated_cost;   /**< (H) Estimated path cost to the goal. */
};

Un des principaux déterminant de l'efficacité de l'algorithme A* est la manière dont la liste ouverte — l'open set — est implantée. Plusieurs approches sont envisageables, mais inspiré par les articles de Steve Rabin j'ai pris le parti d'implanter cette dernière avec un vecteur utilisé en tant que heap. Le prédicat binaire AStarHeapComparator est utilisé afin de trier ce heap en fonction du coup des nœuds.

//! \brief A* heap comparison functor.
struct AStarHeapComparator {

    //! \brief Given two AStarNode, check if the total cost induced by the
    //! first one is smaller than the total cost induced by the second one.
    //! \param node1 A pointer to an AStarNode
    //! \param node2 A pointer to an AStarNode
    //! \return True if the total cost of node1 is smaller than the total cost
    //! of node2.
    bool operator()(const AStarNode* node1, const AStarNode* node2) {
        // Sort in reverse order since pop_heap removes the largest element.
        return node1->total_cost > node2->total_cost;
    }
};

Le prédicat binaire AStarNodeFinder permet de localiser dans les listes ouvertes et fermées un AStarNode associé à — contenant — un Quadnode donné.

//! \brief A* open and closed set search functor.
//!
//! This binary functor is used to search both the A* algorithm open and closed
//! set. In order to use it in conjuction with the std::find_if algorithm, which
//! is expecting a unary predicate, bind the second argument with std::bind2nd.
struct AStarNodeFinder : public std::binary_function<AStarNode*, Quadnode*, bool> {
    //! \brief Check if a given AStarNode contains a given Quadnode.
    //! \param anode A pointer to an AStarNode
    //! \param qnode A pointer to a Quadnode
    //! \return True if the AStarNode contains the given Quadnode.
    bool operator()(const AStarNode* anode, const Quadnode* qnode) const {
       return anode->contains(qnode);
    }
};

Évidemment, ces deux structs pourraient aisément être remplacées par des lambdas en C++0x. Mais bon, il faut bien conserver un peu de matière à amélioration.

Pour le reste, tout coule de source. Il s'agit simplement d'éviter de confondre les nœuds utilisés à l'interne par l'algorithme A* — les AStarNode — des nœuds du quadtree — les Quadnode.

//! \brief Implementation of the A* algorithm.
//! \param start The starting node.
//! \param destination The destination (goal) node.
//! \return A vector containg pointers to node forming the path in ascendant
//! order, i.e. the start node is at position 0 and the goal node is at
//! position length - 1. If there is no existing path from the start node
//! to the destination node, the returned vector will be empty.
std::vector<Quadnode*> astar(Quadnode* start, Quadnode* destination) {

    std::vector<AStarNode*> open;   // Vector used as a heap, cf. Steve Rabin
    std::vector<AStarNode*> closed;

    // Prime the open set with the start node.
    open.push_back(new AStarNode(NULL, start, 0, start->distance(destination)));

    // Loop until either the destination Quadnode is reached or the
    // open set is exhausted, meaning that no solution where found.
    while (!open.empty() && !open.front()->contains(destination)) {

        // Retrieve the best node from the open set, i.e. the
        // one with the lowest total cost (path + estimate).
        AStarNode* current = open.front();
        std::pop_heap(open.begin(), open.end(), AStarHeapComparator());
        open.pop_back();

        // Retrieve the current (stored) quadnode neighbours.
        std::vector<Quadnode*> qneighbours = current->neighbours();

        // Go over each quadnode neighbor.
        for (std::vector<Quadnode*>::const_iterator iterator = qneighbours.begin();
                iterator != qneighbours.end(); ++iterator) {

            Quadnode* qneighbor = *iterator;

            // If this neighbor isn't free, then
            // don't waste time considering it.
            if (!qneighbor->is_free())
                continue;

            // Search the AStarNode of the closed list for this quadnode neighbor.
            // If do we find it, simply skip it since it has already been computed.
            if (std::find_if(closed.begin(), closed.end(),
                    std::bind2nd(AStarNodeFinder(), qneighbor)) != closed.end()) {
                continue;
            }

            // Search the AStarNode of the open list for this quadnode neighbor.
            std::vector<AStarNode*>::iterator open_set_iterator;
            open_set_iterator = std::find_if(open.begin(), open.end(),
                    std::bind2nd(AStarNodeFinder(), qneighbor));

            // If this neighbor is in the open list and our current g value
            // is lower; update the neighbor with the new, lower, g value
            // and change the neighbor's parent to our current node.
            if (open_set_iterator != open.end()) {

                AStarNode* astarnode = *open_set_iterator;

                // We'll need the total path cost from our current position
                // to this neighbor quadnode for the following operations.
                float path_cost = current->path_cost + current->distance(astarnode);

                if (path_cost < astarnode->path_cost) {

                    float estimated_cost = astarnode->estimated_cost;
                    astarnode->path_cost = path_cost;
                    astarnode->total_cost = estimated_cost + path_cost;
                    astarnode->ancestor = current;

                    std::make_heap(open.begin(), open.end(), AStarHeapComparator());
                }
            } else {
                // This quadnode neighbor is neither in the open nor in the closed
                // list; so add it to the open list and set its path value.
                float path_cost = current->path_cost + current->distance(qneighbor);
                float estimated_cost = qneighbor->distance(destination);

                open.push_back(new AStarNode(current, qneighbor,
                                             path_cost, estimated_cost));
                std::push_heap(open.begin(), open.end(), AStarHeapComparator());
            }
        }

        // We're done with this node.
        // Put it into the closed set.
        closed.push_back(current);
    }

    std::vector<Quadnode*> path;

    // If the open set isn't empty, it means that we've reached the
    // destination. From it, rebuild the path to the starting point.
    if (!open.empty()) {
        AStarNode* current = open.front();
        while (current) {
            path.push_back(current->quadnode);
            current = current->ancestor;
        }
        std::reverse(path.begin(), path.end());
    }

    // Before returning, clean up dynamically allocated assets.
    for (std::vector<AStarNode*>::iterator iter = open.begin();
            iter != open.end(); ++iter) {
        delete *iter; // Cleaning open set.
    }

    for (std::vector<AStarNode*>::iterator iter = closed.begin();
            iter != closed.end(); ++iter) {
        delete *iter; // Cleaning closed set.
    }

    return path;
}

Notons que malgré le fait que cet algorithme ait été codé en ayant les quadtrees en ligne de mire, il serait très simple de le rendre parfaitement générique. Dans cette optique, il suffit de constater que seulement deux méthodes sont véritablement requises sur les nœuds de la structure de données modélisant la surface:

Conclusion

En guise de conclusion, voici un exemple de recherche de chemin dans le quadtree de 8x8 présenté précédemment.

int main() {
    std::vector<Obstacle> obstacles;
    obstacles.push_back(Obstacle(Point(0, 3), Point(2, 4)));
    obstacles.push_back(Obstacle(Point(2, 1), Point(4, 2)));
    obstacles.push_back(Obstacle(Point(2, 2), Point(3, 4)));
    obstacles.push_back(Obstacle(Point(3, 3), Point(4, 7)));
    obstacles.push_back(Obstacle(Point(5, 2), Point(6, 4)));

    Quadtree quadtree(obstacles, 8);

    Quadnode* q00 = quadtree.find_node(Point(0, 0));
    Quadnode* q04 = quadtree.find_node(Point(0, 4));

    std::vector<Quadnode*> path = astar(q00, q04);

    for (std::vector<Quadnode*>::iterator it = path.begin();
            it != path.end(); ++it) {
        Point origine = (*it)->origine();
        std::cout << origine.x << ":" << origine.y << std::endl;
    }

    return EXIT_SUCCESS;
}

Lorsque l'algorithme A* est exécuté avec comme origine et destination les points (0,0) et (0,4), le chemin ci-dessous est obtenu.

Bien que cela ne soit pas nécessairement évident dans cet exemple, la complexité d'une recherche dans un quadtree est de beaucoup inférieure à celle d'une recherche dans une grille de points standard. Plus précisément, le quadtree d'une surface encombrée d'obstacles polygonaux comprendra approximativement 2/3 O(p) feuilles, où p représente la somme des périmètres des obstacles. L'algorithme A* n'aura donc qu'à considérer O(p) nœuds dans le cas d'un quadtree contre n2 points pour une grille standard. Lorsque le nombre d'obstacles est faible, cette caractéristique devient d'autant plus intéressante.

Tout comme la documentation doxygen, le code source peut être consulté en ligne: quadtree.html.