以有效的方式在 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:
假设我们有一个圆心 (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: