0

我正在研究移动机器人的路径规划算法。

我正在尝试实现四叉树算法来对空间信息进行建模。

每个节点都包含一个布尔变量，如果四边形与路径相交，则该变量为真。

对于如图所示的路径

如图所示，我得到了一个糟糕的空间分区

如果我移动路径的顶点，我将得到一个四叉树，其中包含来自旧四叉树的一些四叉树，如此处所示，移动顶点后的四叉树

你能帮忙找出这个问题的根源吗？

解决方案 该问题是由返回 false true 的 plan->intersects 函数引起的。为了解决这个问题，我实现了一个检查多边形和单元格之间是否存在交集的函数。这是我得到的结果。

二维空间的四叉树划分

这就是我构建四叉树的方式

    QPoint topL,botR;// top left and Bottom right
    boundingBox(topL,botR);
    if((botR-topL).manhattanLength() >0)
    {
        qTree = new quadTree(topL,botR);// initialise the quadTree
        qTree->minimalSize = 10;//set the minimal size of the cell
        foreach (QPoint *p, sommets)
        {
            plan<<*p;// initialise the polygone
        }
        qTree->plan->addPolygon(plan);// add the polygon to the qpainterpath
        QPoint center((topL.x()+botR.x())/2,(topL.y()+botR.y())/2);// get the center point
        Noeud *n = new Noeud (center, qTree->plan->intersects(QRect(topL,botR)));// initalise the node
        qTree->insert(n);// insert the node to the quadTree
    }

我与你分享四叉树类。

//quadtree.h
#ifndef QUADTREE_H
#define QUADTREE_H
#include <QPoint>
#include <QRect>
#include <QPainterPath>
#include <QDebug>
struct Noeud
{
    bool occupied;
    QPoint pos;
    Noeud(QPoint p, bool o)
    {
        occupied = o;
        pos = p;
    }
    Noeud()
    {
        occupied = false;
    }
};
class quadTree
{
public:
    QPoint topLeft;
    QPoint botRight;
    Noeud *node;
    QPainterPath *plan;
    quadTree *northEast;
    quadTree *northWest;
    quadTree *southEast;
    quadTree *southWest;
    int minimalSize;
    quadTree();
    quadTree(QPoint topLeft, QPoint botRight);
    void insert(Noeud *n);
    Noeud* search(QPoint p);
    bool inBoundary(QPoint p);
private:
    void initQuads();
    QPoint findMiddle(QPoint topL, QPoint botR);
};
#endif // QUADTREE_H


//quadtree.cpp
#include "quadtree.h"

quadTree::quadTree()
{
    topLeft = QPoint(0,0);
    botRight = QPoint(0,0);

    node = nullptr;
    northEast = nullptr;
    northWest = nullptr;
    southEast = nullptr;
    southWest = nullptr;
    minimalSize = 15;
    plan = new QPainterPath;
}

quadTree::quadTree(QPoint topLeft, QPoint botRight)
{
    this->topLeft = topLeft;
    this->botRight = botRight;

    node = nullptr;

    northEast = nullptr;
    northWest = nullptr;
    southEast = nullptr;
    southWest = nullptr;

    minimalSize = 15;
    plan = new QPainterPath;
}

void quadTree::insert(Noeud *n)
{
    if(n == nullptr)
    {return;}
    if(qAbs(topLeft.x()-botRight.x()) <=minimalSize &&
       qAbs(topLeft.y()-botRight.y()) <=minimalSize)
    {
            node = n;
        return;
    }

    if(n->occupied)
    {
        initQuads();
        QPoint pNE (findMiddle(northEast->topLeft, northEast->botRight));
        QRect rNE(northEast->topLeft, northEast->botRight);
        bool isOccupied = plan->intersects(rNE);
        Noeud *NE = new Noeud(pNE,isOccupied);
        northEast->insert(NE);/**/

        QPoint pNW (findMiddle(northWest->topLeft, northWest->botRight));
        QRect rNW(northWest->topLeft, northWest->botRight);
        isOccupied = plan->intersects(rNW);
        Noeud *NW = new Noeud(pNW,isOccupied);
        northWest->insert(NW);

        QPoint pSE (findMiddle(southEast->topLeft, southEast->botRight));
        QRect rSE(southEast->topLeft, southEast->botRight);
        isOccupied = plan->intersects(rSE);
        Noeud *SE = new Noeud(pSE,isOccupied);
        southEast->insert(SE);

        QPoint pSW (findMiddle(southWest->topLeft, southWest->botRight));
        QRect rSW(southWest->topLeft, southWest->botRight);
        isOccupied = plan->intersects(rSW);
        Noeud *SW = new Noeud(pSW,isOccupied);
        southWest->insert(SW);
    }
    else
    {
        node = n;
    }
}

Noeud *quadTree::search(QPoint p)
{
    if(!inBoundary(p))
        return nullptr;
    if(node != nullptr)
        return node;

    return node;

}

bool quadTree::inBoundary(QPoint p)
{
    bool inX = p.x()>=topLeft.x() && p.x()<= botRight.x();
    bool inY = p.y()>=topLeft.y() && p.y()<= botRight.y();
    return inX && inY;
}

void quadTree::initQuads()
{
    QPoint midPoint = findMiddle(topLeft, botRight);
    if (northEast == nullptr)
    {
        northEast = new quadTree(topLeft, midPoint);
        northEast->plan = plan;
        northEast->minimalSize = minimalSize;
    }
    if (southEast == nullptr)
    {
        southEast = new quadTree(
                    QPoint(topLeft.x(),
                           midPoint.y()),
                    QPoint(midPoint.x(),
                           botRight.y()));
        southEast->plan = plan;
        southEast->minimalSize = minimalSize;
    }
    if (northWest == nullptr)
    {
        northWest = new quadTree(
                    QPoint(midPoint.x(),
                           topLeft.y()),
                    QPoint(botRight.x(),
                           midPoint.y()));
        northWest->plan = plan;
        northWest->minimalSize = minimalSize;
    }
    if (southWest == nullptr)
    {
        southWest = new quadTree(midPoint,botRight);
        southWest->plan = plan;
        southWest->minimalSize = minimalSize;
    }
}
QPoint quadTree::findMiddle(QPoint topL, QPoint botR)
{
   return QPoint ((topL.x()+botR.x())/2,(topL.y()+botR.y())/2);

}
4

0 回答 0