以有效的方式在 Circlepath 上查找网格的每个单元格

Finding every Cell of Grid on Circlepath in an efficient way

假设我们有一个圆心 (float_x,float_y) 和半径 float_r 的圆。它位于像平面这样的网格或数组上,我们想要找到与圆相交的每个 Cell(int i,int j)。

这些单元格应根据它们与 x 轴的角度排列在数组中(x 轴向右水平正向,y 垂直向上方向正向)。

问题是圆心不必正好在单元格的中间,我想这否定了单元格的“点”对称性的使用。

我想假装有一个单元格对称圆并迭代 90° 并将偏移量添加到我正在计算的每个点,但我不确定这是否会产生好的结果。

如果有任何帮助或想法,我将不胜感激

谢谢

编辑:

以下代码用于查找第一象限 (x+,y+) 中的每个点我还没有尝试过,但我很确定它会起作用。 也可以应用于其他象限,但必须更改 x/y 迭代顺序和方向。由于我们从 xmax/pt_y 开始,所以点将按角度排序。

一旦我测试了这个问题,我就会将这个问题标记为已解决。

float pt_x,pt_y为圆心坐标

float searchradius 为圆的半径

float map_info.scale 是网格中一个单元格的大小

int maxx=round((pt_x+searchradius)/map_info.scale) 获取最大可能的单元格

            j=round(pt_y/map_info.scale);
            for (i=maxx; i >round(pt_x/map_info.scale); i--) {
                while(true)
                {
                    Point.x=i;
                    Point.y=j;
                    Points.push_back(Point);
                    if(sqrt(pow(i*map_info.scale,2)+pow((j+1)*map_info.scale,2))<=searchradius)
                    {
                        j++;
                    }
                    else break;
                }
            }

所以让我们测试 与在 floats 上计算的网格线 的交点(正如我在评论中建议的那样)。我首先将圆分成 4 个部分(“象限”),如下所示:

在每个部分中只有一个轴是主要的(总是与另一个轴有更大或相等的差异)所以我们可以通过递增直接迭代它(不会在曲线上创建孔)。然后使用圆方程计算短轴:

y = sqrt( r*r - (x-x0)*(x-x0)  )
x = sqrt( r*r - (y-y0)*(y-y0)  )

所以只需按 4x(未嵌套)for 循环的顺序循环主轴并添加点。然而,每当计算的短轴不直接在网格上时,我们需要添加 2 个点,其中第二个点已递减长轴。这可能会导致重复点,因此为了避免重复,我们还应该检查添加的点是否尚未添加到列表的末尾。在上一节的末尾,我们还应该检查列表的开头(或删除最后几个重复的点)我认为最多会有 4 个点,例如 max.

在 C++/VCL 中将所有这些放在一起时,它看起来像这样:

//---------------------------------------------------------------------------
// global grid
float gx0=0.0;      // cell origin
float gy0=0.0;
float gxs=30.0;     // cell size
float gys=20.0;
//---------------------------------------------------------------------------
//  grid -> screen     |  screen -> grid
//  sx = gx0 + gx*gxs  |  gx = (sx-gx0)/gxs
//  sy = gy0 + gy*gys  |  gy = (sy-gy0)/gys
//---------------------------------------------------------------------------
void draw_cell(TCanvas *scr,int x,int y,DWORD col)  // screen, cell, color
    {
    x=floor(gx0+(float(x)*gxs));
    y=floor(gy0+(float(y)*gys));
    scr->Pen->Color=clDkGray;
    scr->Brush->Color=TColor(col);
    scr->Rectangle(x,y,x+gxs,y+gys);
    }
//---------------------------------------------------------------------------
void draw_grid(TCanvas *scr,int xs,int ys,DWORD col)    // screen, its resolution, color
    {
    int x0,y0,x1,y1,x,y;
    // visible grid
    x0=floor((float( 0)-gx0)/gxs);
    y0=floor((float( 0)-gy0)/gys);
    x1= ceil((float(xs)-gx0)/gxs);
    y1= ceil((float(ys)-gy0)/gys);
    for (y=y0;y<=y1;y++)
     for (x=x0;x<=x1;x++)
      draw_cell(scr,x,y,col);
    }
//---------------------------------------------------------------------------
void draw_circle(TCanvas *scr,float cx,float cy,float r,DWORD col)  // screen, circle, color
    {
    int i,ix,iy;
    int *pnt,pnts=0;            // output list of cells { x0,y0, x1,y1, ... }
    float x,y,x0,y0,x1,y1,xx,yy,rr=r*r;
    // ranges where x or y is major axis
    x0=floor(cx-(r*sqrt(0.5)));
    y0=floor(cy-(r*sqrt(0.5)));
    x1=ceil (cx+(r*sqrt(0.5)));
    y1=ceil (cy+(r*sqrt(0.5)));
    // prepare list
    pnt=new int[4*(x1-x0+1)];
    // this adds point (px,py) if the pnt[pi] ... pnt[pi-6] in list is not the same as (px,py)
    #define pnt_add(pi,px,py)                                         \
        {                                                             \
        int e=1;                                                      \
        if (pi>=0)                                                    \
            {                                                         \
            if ((pi+1<pnts)&&((pnt[pi+0]==px)&&(pnt[pi+1]==py))) e=0; \
            if ((pi-1<pnts)&&((pnt[pi-2]==px)&&(pnt[pi-1]==py))) e=0; \
            if ((pi-3<pnts)&&((pnt[pi-4]==px)&&(pnt[pi-3]==py))) e=0; \
            if ((pi-5<pnts)&&((pnt[pi-6]==px)&&(pnt[pi-5]==py))) e=0; \
            }                                                         \
        if (e){ pnt[pnts]=px; pnts++; pnt[pnts]=py; pnts++; }         \
        }
    // rasterize "quadrants" in order so no sorting is needed later
    for (x=x0;x<x1;x++)
        {
        xx=x-cx; xx*=xx; y=rr-xx; if (y<0.0) continue;
        y=sqrt(y); iy=float(cy-y); ix=x;
        if (y-floor(y)>1e-10) pnt_add(pnts-2,ix-1,iy);
                              pnt_add(pnts-4,ix  ,iy);
        }
    for (y=y0;y<y1;y++)
        {
        yy=y-cy; yy*=yy; x=rr-yy; if (x<0.0) continue;
        x=sqrt(x); ix=float(cx+x); iy=y;
        if (x-floor(x)>1e-10) pnt_add(pnts-2,ix,iy-1);
                              pnt_add(pnts-4,ix,iy  );
        }
    for (x=x1;x>x0;x--)
        {
        xx=x-cx; xx*=xx; y=rr-xx; if (y<0.0) continue;
        y=sqrt(y); iy=float(cy+y); ix=x;
                              pnt_add(pnts-2,ix  ,iy);
        if (y-floor(y)>1e-10) pnt_add(pnts-4,ix-1,iy);
        }
    for (y=y1;y>y0;y--)
        {
        yy=y-cy; yy*=yy; x=rr-yy; if (x<0.0) continue;
        x=sqrt(x); ix=float(cx-x); iy=y;
                              pnt_add(pnts-2,ix,iy  );
        if (x-floor(x)>1e-10) pnt_add(pnts-4,ix,iy-1);
        }
    // check duplicity between pnt start and end
    for (i=pnts-8;i+1<pnts;i+=2)
     if (pnt[0]==pnt[i+0])
      if (pnt[1]==pnt[i+1])
        {
        pnts=i;
        break;
        }

    // render the list (continuously change color to show points order)
    for (i=0;i+1<pnts;i+=2) draw_cell(scr,pnt[i+0],pnt[i+1],0x010000*(25+((230*i)/pnts)));

    // ---- you can ignore all the rest of the code its my debug rendering -----
    // debug numbers
    scr->Font->Color=TColor(0x0000FF00);
    scr->Font->Height=gys;
    scr->Brush->Style=bsClear;
    for (i=0;i+1<pnts;i+=2)
        {
        x=gx0+float(pnt[i+0])*gxs;
        y=gy0+float(pnt[i+1])*gys;
        scr->TextOutA(x,y,i>>1);
        }
    scr->Brush->Style=bsSolid;
    // debug circle overlay (ignore this)
    if (1)
        {
        int x,y,rx,ry,w=8;
        x =floor(gx0+(cx*gxs));
        y =floor(gy0+(cy*gys));
        rx=floor(r*gxs);
        ry=floor(r*gys);
        scr->Pen->Color=clYellow;
        scr->Brush->Style=bsClear;
        scr->Ellipse(x-rx,y-ry,x+rx,y+ry);
        scr->MoveTo(x-w,y);
        scr->LineTo(x+w,y);
        scr->MoveTo(x,y-w);
        scr->LineTo(x,y+w);
        scr->Brush->Style=bsSolid;
        }

    // free the list you should store/copy/export it somwhere
    delete[] pnt;
    #undef pnt_add
    }
//---------------------------------------------------------------------------

这是输出(包括我的调试渲染):

结果在 pnt[pnts] 数组中,按顺序保存 x,y 网格坐标 ...

这远未优化,例如:

  • 每个 sqrt 被计算两次你可以记住它们...
  • 每个 sqr 刚增加的值可以是

好的,第二次尝试。 我试图将更复杂的数学的使用限制在最低限度,但我相信这可以优化。

首先是迭代的代码部分;

        double minx=round((pt_x-searchradius)/map_info.scale);
        double maxx=round((pt_x+searchradius)/map_info.scale);
        double miny=round((pt_y-searchradius)/map_info.scale);
        double maxy=round((pt_y+searchradius)/map_info.scale);


        //since circle wont ever be point symetrical we need to iterate through all quadrants
        //this is caused since we wont shift the circlecenter to the middle of the nearest field otherwise symmetry could be used
        Points.clear();
        //first quadrant
        int i,j;
        j=round(pt_y/map_info.scale);
        for (i=maxx; i >round(pt_x/map_info.scale); i--) {
            while(checkcellincircle(i,j,pt_x,pt_y,map_info,Points))
            {
                j++;
            }
            j--;
        }
        //second quadrant
        i=round(pt_x/map_info.scale);
        for (j=maxy; j >round(pt_y/map_info.scale); j--) {
            while(checkcellincircle(i,j,pt_x,pt_y,map_info,Points))
            {
                i--;
            }
            i++;
        }
        //third quadrant
        j=round(pt_y/map_info.scale);
        for (i=minx; i<round(pt_x/map_info.scale); i++) {
            while(checkcellincircle(i,j,pt_x,pt_y,map_info,Points))
            {
                j--;
            }
            j++;
        }
        //fourth quadrant
        i=round(pt_x/map_info.scale);
        for (j=miny; j <round(pt_y/map_info.scale); j++) {
            while(checkcellincircle(i,j,pt_x,pt_y,map_info,Points))
            {
                i++;
            }
            i--;
        }    

第二部分处理单元格是否属于圆圈的天气:

bool checkcellincircle(int &i, int &j,double &pt_x,double &pt_y, map_inf &map,std::vector<geometry_msgs::Point32> &Points)
{
    //this function checks the shortest distance from pt_x/pt_y to the given cell
    //if the distance is <= the searchradius the point gets added to the Pointsvector and the function returns true
    //else the function returns false
    double cminx,cminy,dx,dy;
    geometry_msgs::Point32 Point;
    if(i*map.scale>pt_x)
        dx=abs(pt_x-i*map.scale-map.scale/2);
    else if(i*map.scale<pt_x)
        dx=abs(pt_x-i*map.scale+map.scale/2);
    else dx=abs(pt_x-i*map.scale);
    if(j*map.scale>pt_y)
        dy=abs(pt_y-j*map.scale-map.scale/2);
    else if(j*map.scale<pt_y)
        dy=abs(pt_y-j*map.scale+map.scale/2);
    else dy=abs(pt_y-j*map.scale);

    //getting coordinates of closest point on cell edge
    if((abs(dx)>=abs(dy))||dy==0)
    {
        cminx=map.scale;
        cminy=(abs(dy)/abs(dx))*map.scale;
    }

    else if((abs(dx)<abs(dy))||dx==0)
    {
        cminy=map.scale;
        cminx=(abs(dx)/abs(dy))*map.scale;
    }
    if(sqrt(pow(dx-cminx,2)+pow(dy-cminy,2))<=searchradius)
    {
        Point.x=i;
        Point.y=j;
        Points.push_back(Point);
        return true;

    }
    else return false;
}

最终结果的图片 1m rad,网格为 1m,地图分辨率为 0.05m: