void GridLineTraversal::gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line )
{
int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag;
int cnt = 0;
dx = abs(end.x-start.x); dy = abs(end.y-start.y);
if (dy <= dx) {
d = 2*dy - dx; incr1 = 2 * dy; incr2 = 2 * (dy - dx);
if (start.x > end.x) {
x = end.x; y = end.y;
ydirflag = (-1);
xend = start.x;
} else {
x = start.x; y = start.y;
ydirflag = 1;
xend = end.x;
}
line->points[cnt].x=x;
line->points[cnt].y=y;
cnt++;
if (((end.y - start.y) * ydirflag) > 0) {
while (x < xend) {
x++;
if (d <0) {
d+=incr1;
} else {
y++; d+=incr2;
}
line->points[cnt].x=x;
line->points[cnt].y=y;
cnt++;
}
} else {
while (x < xend) {
x++;
if (d <0) {
d+=incr1;
} else {
y--; d+=incr2;
}
line->points[cnt].x=x;
line->points[cnt].y=y;
cnt++;
}
}
} else {
d = 2*dx - dy;
incr1 = 2*dx; incr2 = 2 * (dx - dy);
if (start.y > end.y) {
y = end.y; x = end.x;
yend = start.y;
xdirflag = (-1);
} else {
y = start.y; x = start.x;
yend = end.y;
xdirflag = 1;
}
line->points[cnt].x=x;
line->points[cnt].y=y;
cnt++;
if (((end.x - start.x) * xdirflag) > 0) {
while (y < yend) {
y++;
if (d <0) {
d+=incr1;
} else {
x++; d+=incr2;
}
line->points[cnt].x=x;
line->points[cnt].y=y;
cnt++;
}
} else {
while (y < yend) {
y++;
if (d <0) {
d+=incr1;
} else {
x--; d+=incr2;
}
line->points[cnt].x=x;
line->points[cnt].y=y;
cnt++;
}
}
}
line->num_points = cnt;
}
//最終在外面被使用的bresenham畫線算法
void GridLineTraversal::gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) {
int i,j;
int half;
IntPoint v;
gridLineCore( start, end, line );
if ( start.x!=line->points[0].x ||
start.y!=line->points[0].y ) {
half = line->num_points/2;
for (i=0,j=line->num_points - 1;i<half; i++,j--) {
v = line->points[i];
line->points[i] = line->points[j];
line->points[j] = v;
}
}
}
代碼來源:openslam_gmapping/gridlinetraversal.h
原理:
二、直線Bresenham算法思想之一:
由於顯示直線的象素點只能取整數值座標,可以假設直線上第i個象素點座標爲(xi,yi),它是直線上點(xi,yi)的最佳近似,並且xi=xi(假設m<1),如下圖所示。那麼,直線上下一個象素點的可能位置是(xi+1,yi)或(xi+1,yi+1)。
由圖中可以知道,在x=xi+1處,直線上點的y值是y=m(xi+1)+b,該點離象素點(xi+1,yi)和象素點(xi+1,yi+1)的距離分別是d1和d2:
d1=y-yi=m(xi+1)+b-yi (2-8)
d2=(yi+1)-y=(yi+1)-m(xi+1)-b (2-9)
這兩個距離差是
d1-d2=2m(xi+1)-2yi+2b-1 (2-10)
我們來分析公式(2-10):
(1)當此值爲正時,d1>d2,說明直線上理論點離(xi+1,yi+1)象素較近,下一個象素點應取(xi+1,yi+1)。
(2)當此值爲負時,d1<d2,說明直線上理論點離(xi+1,yi)象素較近,則下一個象素點應取(xi+1,yi)。
(3)當此值爲零時,說明直線上理論點離上、下兩個象素點的距離相等,取哪個點都行,假設算法規定這種情況下取(xi+1,yi+1)作爲下一個象素點。
因此只要利用(d1-d2)的符號就可以決定下一個象素點的選擇。爲此,我們進一步定義一個新的判別式:
pi=△x×(d1-d2)=2△y·xi-2△x·yi+c (2-11)
式(2-11)中的△x=(x2-x1)>0,因此pi與(d1-d2)有相同的符號;
這裏△y=y2-y1,m=△y/△x;c=2△y+△x(2b-1)。
下面對式(2-11)作進一步處理,以便得出誤差判別遞推公式並消除常數c。
將式(2-11)中的下標i改寫成i+1,得到:
pi+1=2△y·xi+1-2△x·yi+1+c (2-12)
將式(2-12)減去(2-11),並利用xi+1=xi+1,可得:
pi+1= pi+2△y-2△x·(yi+1-yi) (2-13)
再假設直線的初始端點恰好是其象素點的座標,即滿足:
y1=mx1+b (2-14)
由式(2-11)和式(2-14)得到p1的初始值:
p1=2△y-△x (2-15)
這樣,我們可利用誤差判別變量,得到如下算法表示:
初始 p1=2△y-△x (2-16)
當pi≥0時: yi+1=yi+1,
xi+1=xi+1,
pi+1=pi+2(△y-△x)
否則: yi+1=yi,
xi+1=xi+1,
pi+1=pi+2△y
從式(2-16)可以看出,第i+1步的判別變量pi+1僅與第i步的判別變量pi、直線的兩個端點座標分量差△x和△y有關,運算中只含有整數相加和乘2運算,而乘2可利用算術左移一位來完成,因此這個算法速度快並易於硬件實現。
原理來源:https://blog.csdn.net/kakaxi2222/article/details/50708552?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task