QuadTree 优化版

 

QuadTree.h

#pragma once
#include <iostream>
#include <string>
#include <stdlib.h>
#include <ctime>
#include <vector>
#include <algorithm>
using namespace std;

#define MAX_ELE_NUM 300   //每块区域的最大点数
#define QUADRANT_RU 1
#define QUADRANT_LU 2
#define QUADRANT_LB 3
#define QUADRANT_RB 4

//extern int maxDepth;

//矩形区域
struct Region
{
    double bottom;
    double up;
    double left;
    double right;
};
   
//点结构体
struct ElePoint
{
    int idx{ -1 };
    float x;   //x坐标
    float y;   //y坐标
    float z{ 0 };   //y坐标
};

//四叉树结点
struct QuadTreeNode
{
    int depth;   //结点的深度
    int is_leaf;   //是否是叶子节点
    struct Region region;   //区域范围
    struct QuadTreeNode *LU;  //左上子节点指针
    struct QuadTreeNode *LB;   //左下子节点指针
    struct QuadTreeNode *RU;   //右上子节点指针
    struct QuadTreeNode *RB;   //右下子节点指针
    int ele_num;     //矩形区域中位置点数
    struct ElePoint *ele_list[MAX_ELE_NUM];   //矩形区域中位置点列表
};

struct neiInfo
{
    ElePoint pt;
    float dis{ -1 };
};

struct  prioBranchQueueEntry
{
    QuadTreeNode node;
    float dis{ 9999999 };
};

struct  prioPointQueueEntry
{
    ElePoint pt;
    float dis{ -1 };
};


class  QuadTree
{
public:
    QuadTree()
    {
    
    }

    ~QuadTree()
    {

    }

public:
    void setInputPoints(std::vector<ElePoint>&points1);
    bool initialTree();
    float knnSearch(ElePoint ele, int k, vector<int>&idxVec, vector<float>&disVec);
    float radiusSearch(ElePoint ele, float radius, vector<int>&idxVec, vector<float>&disVec);

private:
    bool getRegion();
    bool isBuildTreeSuccess{false};

public:
    std::vector<ElePoint> points;
    QuadTreeNode root;
    Region root_region;
    int treeMaxDepth{0};

    float minX{ (float)DBL_MAX };
    float maxX{ -(float)DBL_MAX };
    float minY{ (float)DBL_MAX };
    float maxY{ -(float)DBL_MAX };
};

 

QuadTree.cpp

#include "stdafx.h"
#include "QuadTree.h"
#include <cmath>

int maxDepth = 0;
float knnMinDis = 9999999;
std::vector<prioPointQueueEntry> searchResult;
//std::vector<neiInfo>ElePointSearchVec;

//函数声明
void initNode(QuadTreeNode *node, int depth, Region region);
void insertEle(QuadTreeNode *node, ElePoint ele);
void splitNode(QuadTreeNode *node);
void queryEle(QuadTreeNode tree, ElePoint ele);
void initRegion(Region *region, double up, double bottom, double left, double right);
QuadTreeNode *createChildNode(QuadTreeNode *node, double bottom, double up, double left, double right);

bool ptIsInRegion(ElePoint pt, double bottom, double up, double left, double right)
{
    if (pt.x > left && pt.x<right && pt.y>bottom && pt.y < up)
    {
        return true;
    }
    return false;
}

float twoPtDis(ElePoint p1, ElePoint p2)
{
    return abs(sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2)));
}

/**
* 插入元素
* 1.判断是否已分裂，已分裂的选择适合的子结点，插入；
* 2.未分裂的查看是否过载，过载的分裂结点，重新插入；
* 3.未过载的直接添加
*
* @param node
* @param ele
* todo 使用元素原地址，避免重新分配内存造成的效率浪费
*/
void insertEle(QuadTreeNode *node, ElePoint ele)
{
    //是叶子结点
    if (1 == node->is_leaf)
    {
        if (node->ele_num + 1 > MAX_ELE_NUM)//大于100个点就要分裂成2个节点
        {
            splitNode(node);

            //分裂后的 node 不是叶子节点，所以新插入的元素会插入到 node 的子节点上
            insertEle(node, ele);  //将新插入的元素插入到node的子节点上
        }
        else
        {
            ElePoint *ele_ptr = (ElePoint *)malloc(sizeof(ElePoint));
            ele_ptr->y = ele.y;
            ele_ptr->x = ele.x;
            ele_ptr->idx = ele.idx;
            //将新插入的点加入到父节点的位置点列表中
            node->ele_list[node->ele_num] = ele_ptr;
            node->ele_num++;
        }

        return;
    }

    //不是叶子结点
    double mid_vertical = (node->region.up + node->region.bottom) / 2;
    double mid_horizontal = (node->region.left + node->region.right) / 2;
    if (ele.y > mid_vertical)
    {
        if (ele.x > mid_horizontal)
        {
            insertEle(node->RU, ele);
        }
        else
        {
            insertEle(node->LU, ele);
        }
    }
    else
    {
        if (ele.x > mid_horizontal)
        {
            insertEle(node->RB, ele);
        }
        else
        {
            insertEle(node->LB, ele);
        }
    }
}

//创建子节点
QuadTreeNode *createChildNode(struct QuadTreeNode *node, double bottom, double up, double left, double right)
{
    int depth = node->depth + 1;
    QuadTreeNode *childNode = (QuadTreeNode *)malloc(sizeof(QuadTreeNode));
    Region *region = (Region *)malloc(sizeof(Region));
    initRegion(region, bottom, up, left, right);
    initNode(childNode, depth, *region);

    maxDepth = std::max(maxDepth, depth);
    return childNode;
}

//初始化四叉树结点
void initNode(QuadTreeNode *node, int depth, Region region)
{
    node->depth = depth;
    node->is_leaf = 1;
    node->ele_num = 0;
    node->region = region;
}

//初始化矩形区域
void initRegion(Region *region, double yMin, double yMax, double xMin, double xMax)
{
    region->bottom = yMin;
    region->up = yMax;
    region->left = xMin;
    region->right = xMax;
}

/**
* 分裂结点
* 1.通过父结点获取子结点的深度和范围
* 2.生成四个结点，挂载到父结点下
*
* @param node
*/
void splitNode(struct QuadTreeNode *node)
{
    double mid_vertical = (node->region.up + node->region.bottom) / 2;  //垂直放向的中间线
    double mid_horizontal = (node->region.left + node->region.right) / 2; //水平方向的中间线

    node->is_leaf = 0;

    //生成四个孩子结点
    node->RU = createChildNode(node, mid_vertical, node->region.up, mid_horizontal, node->region.right);
    node->LU = createChildNode(node, mid_vertical, node->region.up, node->region.left, mid_horizontal);
    node->RB = createChildNode(node, node->region.bottom, mid_vertical, mid_horizontal, node->region.right);
    node->LB = createChildNode(node, node->region.bottom, mid_vertical, node->region.left, mid_horizontal);

    for (int i = 0; i < node->ele_num; i++)
    {
        //此时插入的时候，node不是叶子节点，此时执行 insert 函数，会将元素插入到孩子节点上
        insertEle(node, *node->ele_list[i]);   // 将父节点元素 插入到子节点
        free(node->ele_list[i]);   //释放父节点元素

                                   //node->ele_num--;//这里每次循环的时候i在增加，而node.ele_num在减少，会导致有的点没有插进去
                                   //所以直接在循环结束后，将node.ele_num置为0即可
    }
    node->ele_num = 0;
}

//区域查询  输出该点所在的矩形区域的所有点
void queryEle(struct QuadTreeNode node, struct ElePoint ele)
{
    //是叶子结点
    if (node.is_leaf == 1 /*&& ptIsInRegion(ele, node.region.bottom, node.region.up, node.region.left, node.region.right)*/)
    {
        cout << "miny" << node.region.bottom << endl;
        cout << "maxy" << node.region.up << endl;
        cout << "minx" << node.region.left << endl;
        cout << "maxx" << node.region.right << endl;

        cout << "附近点有" << node.ele_num << "个,分别是：" << endl;
        for (int j = 0; j < node.ele_num; j++)
        {
            cout << "(" << node.ele_list[j]->x << "," << node.ele_list[j]->y << ")" << endl;
        }
        return;
    }

    //不是叶子节点
    double mid_vertical = (node.region.up + node.region.bottom) / 2;
    double mid_horizontal = (node.region.left + node.region.right) / 2;

    if (ele.y > mid_vertical)
    {
        if (ele.x > mid_horizontal)
        {
            queryEle(*node.RU, ele);
        }
        else
        {
            queryEle(*node.LU, ele);
        }
    }
    else
    {
        if (ele.x > mid_horizontal)
        {
            queryEle(*node.RB, ele);
        }
        else
        {
            queryEle(*node.LB, ele);
        }
    }
}

//点查询
void queryPoint(QuadTreeNode node, ElePoint &ele)
{
    //是叶子节点
    if (node.is_leaf == 1)
    {
        for (int i = 0; i < node.ele_num; i++)
        {
            if (ele.x == node.ele_list[i]->x&&ele.y == node.ele_list[i]->y)
            {
                cout << "(" << node.ele_list[i]->x << "," << node.ele_list[i]->y << ") 位于第" << node.depth << "层" << endl;
                return;
            }
        }
        cout << "未找到该点！" << endl;
        return;
    }

    //不是叶子结点
    double mid_vertical = (node.region.up + node.region.bottom) / 2;
    double mid_horizontal = (node.region.left + node.region.right) / 2;


    if (ele.x > mid_horizontal)
    {
        if (ele.y > mid_vertical)
        {
            //queryPoint(QTList[node.RU_0], ele);
            queryPoint(*node.RU, ele);
        }
        else
        {
            //queryPoint(QTList[node.RB_3], ele);
            queryPoint(*node.RB, ele);
        }
    }
    else
    {
        if (ele.y > mid_vertical)
        {
            //queryPoint(QTList[node.LU_1], ele);
            queryPoint(*node.LU, ele);
        }
        else
        {
            //queryPoint(QTList[node.LB_2], ele);
            queryPoint(*node.LB, ele);
        }
    }
}

//任意区域查询
void queryArea(QuadTreeNode *node, Region *region)
{    //是叶子节点
    if (node->is_leaf == 1)
    {
        //遍历叶子节点中的所有点
        for (int i = 0; i < node->ele_num; i++)
        {
            //如果叶子节点中有点在该矩形区域中，就输出该点坐标
            if (node->ele_list[i]->x >= region->left &&
                node->ele_list[i]->x <= region->right &&
                node->ele_list[i]->y >= region->bottom &&
                node->ele_list[i]->y <= region->up)
            {
                cout << "(" << node->ele_list[i]->x << "," << node->ele_list[i]->y << ") ";
            }
        }
        cout << endl;
        return;
    }

    //不是叶子结点 ， 递归查找与矩形区域有交集的叶子结点
    double mid_vertical = (node->region.up + node->region.bottom) / 2;
    double mid_horizontal = (node->region.left + node->region.right) / 2;

    //讨论矩形区域的9种分布情况
    if (region->bottom > mid_vertical) {
        if (region->left > mid_horizontal) {
            //如果矩形区域的下边界大，左边界大，就在右上区域查询
            queryArea(node->RU, region);
        }
        else if (region->right < mid_horizontal) {
            queryArea(node->LU, region);
        }
        else {
            //将该矩形区域分成两块，逐块递归判断其所属的子区域
            Region *region1 = (Region *)malloc(sizeof(Region));
            *region1 = { region->bottom,region->up,region->left,mid_horizontal };
            queryArea(node->LU, region1);

            Region *region2 = (Region *)malloc(sizeof(Region));
            *region2 = { region->bottom,region->up,mid_horizontal,region->right };
            queryArea(node->RU, region2);
        }
    }

    else if (region->up < mid_vertical) {
        if (region->right < mid_horizontal) {
            queryArea(node->LB, region);
        }
        else if (region->left > mid_horizontal) {
            queryArea(node->RB, region);
        }
        else {
            Region *region1 = (Region *)malloc(sizeof(Region));
            *region1 = { region->bottom, region->up,region->left,mid_horizontal };
            queryArea(node->LB, region1);

            Region *region2 = (Region *)malloc(sizeof(Region));
            *region2 = { region->bottom,region->up,mid_horizontal,region->right };
            queryArea(node->RB, region2);
        }
    }

    else {
        if (region->right < mid_horizontal) {
            Region *region1 = (Region *)malloc(sizeof(Region));
            *region1 = { mid_vertical,region->up,region->left,region->right };
            queryArea(node->LU, region1);

            Region *region2 = (Region *)malloc(sizeof(Region));
            *region2 = { region->bottom,mid_vertical,region->left,region->right };
            queryArea(node->LB, region2);

        }
        else if (region->left > mid_horizontal) {
            Region *region1 = (Region *)malloc(sizeof(Region));
            *region1 = { mid_vertical,region->up,region->left,region->right };
            queryArea(node->RU, region1);

            Region *region2 = (Region *)malloc(sizeof(Region));
            *region2 = { region->bottom,mid_vertical,region->left,region->right };
            queryArea(node->RB, region2);
        }
        else {
            Region *region1 = (Region *)malloc(sizeof(Region));
            *region1 = { mid_vertical,region->up,region->left,mid_horizontal };
            queryArea(node->LU, region1);

            Region *region2 = (Region *)malloc(sizeof(Region));
            *region2 = { mid_vertical,region->up,mid_horizontal,region->right };
            queryArea(node->RU, region2);

            Region *region3 = (Region *)malloc(sizeof(Region));
            *region3 = { region->bottom,mid_vertical,region->left,mid_horizontal };
            queryArea(node->LB, region3);

            Region *region4 = (Region *)malloc(sizeof(Region));
            *region4 = { region->bottom, mid_vertical,mid_horizontal,region->right };
            queryArea(node->RB, region4);
        }
    }
}

float knnSearchEx(QuadTreeNode node, ElePoint ele, int k)
{
    std::vector<prioBranchQueueEntry> search_heap;

    float curNodeXlen = node.region.right - node.region.left;
    float curNodeYlen = node.region.up - node.region.bottom;

    //先将节点分成4个节点
    if (node.is_leaf != 1)
    {
        ElePoint curCentPt;
        prioBranchQueueEntry prioBranchQueueEntryT;

        node.RU;
        curCentPt.x = (node.RU->region.left + node.RU->region.right) / 2;
        curCentPt.y = (node.RU->region.bottom + node.RU->region.up) / 2;
        prioBranchQueueEntryT.node = *node.RU;
        prioBranchQueueEntryT.dis = twoPtDis(ele, curCentPt);
        search_heap.push_back(prioBranchQueueEntryT);

        node.LU;
        curCentPt.x = (node.LU->region.left + node.LU->region.right) / 2;
        curCentPt.y = (node.LU->region.bottom + node.LU->region.up) / 2;
        prioBranchQueueEntryT.node = *node.LU;
        prioBranchQueueEntryT.dis = twoPtDis(ele, curCentPt);
        search_heap.push_back(prioBranchQueueEntryT);

        node.RB;
        curCentPt.x = (node.RB->region.left + node.RB->region.right) / 2;
        curCentPt.y = (node.RB->region.bottom + node.RB->region.up) / 2;
        prioBranchQueueEntryT.node = *node.RB;
        prioBranchQueueEntryT.dis = twoPtDis(ele, curCentPt);
        search_heap.push_back(prioBranchQueueEntryT);

        node.LB;
        curCentPt.x = (node.LB->region.left + node.LB->region.right) / 2;
        curCentPt.y = (node.LB->region.bottom + node.LB->region.up) / 2;
        prioBranchQueueEntryT.node = *node.LB;
        prioBranchQueueEntryT.dis = twoPtDis(ele, curCentPt);
        search_heap.push_back(prioBranchQueueEntryT);
    }

    std::sort(search_heap.begin(), search_heap.end(), [](prioBranchQueueEntry& p1, prioBranchQueueEntry& p2) {return p1.dis > p2.dis; });

    int aa = 1;

    while (
        !search_heap.empty()
        /*&&(search_heap.back().dis < (knnMinDis+max(curNodeXlen,curNodeYlen) ) )*/
        )
    {
        curNodeXlen = search_heap.back().node.region.right - search_heap.back().node.region.left;
        curNodeYlen = search_heap.back().node.region.up - search_heap.back().node.region.bottom;

        float dis1 = search_heap.back().dis;
        float disT = sqrt(pow(curNodeXlen / 2, 2) + pow(curNodeYlen / 2, 2));
        float dis2 = knnMinDis + disT/* max(curNodeXlen, curNodeYlen)*/;

        if (dis1 < dis2)
        {
            if (search_heap.back().node.is_leaf != 1)
            {
                knnMinDis = knnSearchEx(search_heap.back().node, ele, k);
            }
            else
            {
                QuadTreeNode& nodeT = search_heap.back().node;
                for (int i = 0; i < nodeT.ele_num; i++)
                {
                    float dis = twoPtDis(*nodeT.ele_list[i], ele);
                    if (dis < knnMinDis)
                    {
                        prioPointQueueEntry point_entry;
                        point_entry.pt = *nodeT.ele_list[i];
                        point_entry.dis = dis;
                        searchResult.push_back(point_entry);
                    }
                }

                std::sort(searchResult.begin(), searchResult.end(), [](prioPointQueueEntry& p1, prioPointQueueEntry& p2) {return p1.dis < p2.dis; });
                if (searchResult.size() > k)
                    searchResult.resize(k);
                knnMinDis = searchResult.back().dis;
            }
        }

        search_heap.pop_back();
    }

    return knnMinDis;
}

float radiusSearchEx(QuadTreeNode node, ElePoint ele, float radius)
{
    std::vector<prioBranchQueueEntry> search_heap;

    float curNodeXlen = node.region.right - node.region.left;
    float curNodeYlen = node.region.up - node.region.bottom;

    //先将节点分成4个节点
    if (node.is_leaf != 1)
    {
        ElePoint curCentPt;
        prioBranchQueueEntry prioBranchQueueEntryT;

        node.RU;
        curCentPt.x = (node.RU->region.left + node.RU->region.right) / 2;
        curCentPt.y = (node.RU->region.bottom + node.RU->region.up) / 2;
        prioBranchQueueEntryT.node = *node.RU;
        prioBranchQueueEntryT.dis = twoPtDis(ele, curCentPt);
        search_heap.push_back(prioBranchQueueEntryT);

        node.LU;
        curCentPt.x = (node.LU->region.left + node.LU->region.right) / 2;
        curCentPt.y = (node.LU->region.bottom + node.LU->region.up) / 2;
        prioBranchQueueEntryT.node = *node.LU;
        prioBranchQueueEntryT.dis = twoPtDis(ele, curCentPt);
        search_heap.push_back(prioBranchQueueEntryT);

        node.RB;
        curCentPt.x = (node.RB->region.left + node.RB->region.right) / 2;
        curCentPt.y = (node.RB->region.bottom + node.RB->region.up) / 2;
        prioBranchQueueEntryT.node = *node.RB;
        prioBranchQueueEntryT.dis = twoPtDis(ele, curCentPt);
        search_heap.push_back(prioBranchQueueEntryT);

        node.LB;
        curCentPt.x = (node.LB->region.left + node.LB->region.right) / 2;
        curCentPt.y = (node.LB->region.bottom + node.LB->region.up) / 2;
        prioBranchQueueEntryT.node = *node.LB;
        prioBranchQueueEntryT.dis = twoPtDis(ele, curCentPt);
        search_heap.push_back(prioBranchQueueEntryT);
    }

    std::sort(search_heap.begin(), search_heap.end(), [](prioBranchQueueEntry& p1, prioBranchQueueEntry& p2) {return p1.dis < p2.dis; });

#define useFunc1 1
#ifdef useFunc1
    for (size_t i = 0; i < search_heap.size(); i++)
    {
        curNodeXlen = search_heap[i].node.region.right - search_heap[i].node.region.left;
        curNodeYlen = search_heap[i].node.region.up - search_heap[i].node.region.bottom;

        float dis1 = search_heap[i].dis;
        float disT = sqrt(pow(curNodeXlen / 2, 2) + pow(curNodeYlen / 2, 2));
        float dis2 = radius + disT/* max(curNodeXlen, curNodeYlen)*/;

        if (dis1 < dis2)
        {
            if (search_heap[i].node.is_leaf != 1)
            {
                radiusSearchEx(search_heap[i].node, ele, radius);
            }
            else
            {
                QuadTreeNode& nodeT = search_heap[i].node;
                for (int i = 0; i < nodeT.ele_num; i++)
                {
                    float dis = twoPtDis(*nodeT.ele_list[i], ele);
                    if (dis <= radius)
                    {
                        prioPointQueueEntry point_entry;
                        point_entry.pt = *nodeT.ele_list[i];
                        point_entry.dis = dis;
                        searchResult.push_back(point_entry);
                    }
                }
            }
        }
    }
    std::sort(searchResult.begin(), searchResult.end(), [](prioPointQueueEntry& p1, prioPointQueueEntry& p2) {return p1.dis < p2.dis; });
    //方法二
#else
    while (!search_heap.empty())
    {
        curNodeXlen = search_heap.back().node.region.right - search_heap.back().node.region.left;
        curNodeYlen = search_heap.back().node.region.up - search_heap.back().node.region.bottom;

        float dis1 = search_heap.back().dis;
        float disT = sqrt(pow(curNodeXlen / 2, 2) + pow(curNodeYlen / 2, 2));
        float dis2 = radius + disT/* max(curNodeXlen, curNodeYlen)*/;

        if (dis1 < dis2)
        {
            if (search_heap.back().node.is_leaf != 1)
            {
                radiusSearch(search_heap.back().node, ele, radius);
            }
            else
            {
                QuadTreeNode& nodeT = search_heap.back().node;
                for (int i = 0; i < nodeT.ele_num; i++)
                {
                    float dis = twoPtDis(*nodeT.ele_list[i], ele);
                    if (dis <= radius)
                    {
                        prioPointQueueEntry point_entry;
                        point_entry.pt = *nodeT.ele_list[i];
                        point_entry.dis = dis;
                        searchResult.push_back(point_entry);
                    }
                }

                std::sort(searchResult.begin(), searchResult.end(), [](prioPointQueueEntry& p1, prioPointQueueEntry& p2) {return p1.dis < p2.dis; });
            }
        }

        search_heap.pop_back();
    }
#endif

    return knnMinDis;
}

void QuadTree::setInputPoints(std::vector<ElePoint>&points1)
{
    points = points1;
}

bool QuadTree::getRegion()
{
    for (int i=0;i<points.size();i++)
    {
        minX = std::min(points[i].x, minX);
        maxX = std::max(points[i].x, maxX);

        minY = std::min(points[i].y, minY);
        maxY = std::max(points[i].y, maxY);
    }

    return isfinite(minX)&& isfinite(maxX)&& isfinite(minY)&& isfinite(maxY);
}

bool QuadTree::initialTree()
{
    if (getRegion())
    {
        initRegion(&root_region, minY,maxY, minX, maxX);
        initNode(&root, 1, root_region);

        for (int i=0;i<points.size();i++)
        {
            insertEle(&root, points[i]);
        }

        treeMaxDepth = maxDepth;
        isBuildTreeSuccess = true;
        return true;
    }
    else
    {
        isBuildTreeSuccess = false;
        return false;
    }
}

float QuadTree::knnSearch(ElePoint ele, int k, vector<int>&idxVec, vector<float>&disVec)
{
    if (!isBuildTreeSuccess)
    {
        return -1;
    }

    knnMinDis=knnSearchEx(root,ele,k);

    for (int i = 0; i < searchResult.size(); i++)
    {
        idxVec.push_back(searchResult[i].pt.idx);
        disVec.push_back(searchResult[i].dis);
    }

    return knnMinDis;
}

float QuadTree::radiusSearch(ElePoint ele, float radius, vector<int>&idxVec, vector<float>&disVec)
{
    if (!isBuildTreeSuccess)
    {
        return -1;
    }

    knnMinDis = radiusSearchEx(root, ele, radius);

    for (int i = 0; i < searchResult.size(); i++)
    {
        idxVec.push_back(searchResult[i].pt.idx);
        disVec.push_back(searchResult[i].dis);
    }

    return knnMinDis;
}

 

main

#include "stdafx.h"
#include <iostream>
#include <fstream>
#include "QuadTree.h"
using namespace std;

int main()
{
    std::vector<ElePoint>points1;
    
    ifstream inFile("Building_Test_12_p3_2_测试四叉树.txt",ios::in);
    int num = 0;
    while (!inFile.eof())
    {
        ElePoint pt;
        float x, y, z;
        inFile >> x >> y >> z;
        pt.x = x;
        pt.y = y;
        pt.z = z;
        pt.idx = num;
        num++;
        points1.push_back(pt);
    }

    QuadTree QuadTreeT;
    QuadTreeT.setInputPoints(points1);
    QuadTreeT.initialTree();

    ElePoint ele;
    ele.x = -70.15;
    ele.y = -30.15;
    vector<int>idxVec;
    vector<float>disVec;
    //QuadTreeT.knnSearch(ele, 80, idxVec, disVec);
    QuadTreeT.radiusSearch(ele,1, idxVec, disVec);
    ofstream outFile("knnNearst_r1.txt", ios::out);
    for (int i=0;i<idxVec.size();i++)
    {
        int curIdx = idxVec[i];
        outFile << points1[curIdx].x << " " << points1[curIdx].y << " " << points1[curIdx].z << endl;
    }
    outFile.close();
}