十一长假后，同学们陆续开始做题，现在月底了，扩展题“-二值矩阵避障最短路径算法”只有7人上交了作业，其中能够运行的有2人，分别是电子18级邵华薇同学、软工18级唐宇。这里提出表扬。
题目可能是有些难度，是我的责任。不过，即使做不完，大家以后还是应该提交一个版本的代码，让老师们知道大家已经在思考了。如果不抓住本科难得的时间，没有工作压力的情况下专心思考，今后步入公司后，压力会更大。
题目：一个迷宫，用64x64的二维数组表示，值为1的元素表示平地，值为0的表示砖墙。现在，给定起点、终点，要求从起点走到终点，尽可能走最短距离。走位要求：在空白区域，可以朝着任意方向走，只要不碰到砖墙即可。
我们可以把这个题目分为两部分。第一部分，是如何走到终点；第二部分，是距离尽可能的短。对于如何走到终点的问题，可以使用启发搜索的算法；第二部分，在第一部分的基础上，做直线归并。
我把两位同学的代码整合为一个demo，参见 https://codechina.csdn.net/coloreaglestdio/qtcpp_demo/-/tree/master/floodfill_mdf 。
1. 找到一条路径
这里采用邵华薇同学的算法稍加改进，分为反向探路、前向回溯两步。
反向探路：
1、首先从终点开始，朝着东南西北四个方向步进，每次只走一格。
2、每个走到的空白格子里，填写当前的步数。
3、以当前走到的位置为种子，递增步数，重复上述过程，直到找到起点为止。
记录步数的矩阵需要独立设置，障碍物、没有走到的区域全部初始化为无穷大。
前向回溯：
从起点开始，从当前位置沿着8个方向（东南西北、东南、东北、西北、西南）寻找步数最小的一个格子，记录下来，并挪动当前位置到该格子，直到回到终点。
通过这两步，就可以找到一条路径，如下图所示：
2.归并直线路径
由于上述寻找的方向只是8个方向，显然路径中存在更短的捷径。找直线的方法，可以直接使用唐宇的思路，即穷尽法。当然，还有更有启发性的梯度下降，但同学们首先还是应该掌握简单的方法。
原理：不断的试探路线上的两点连线会不会被方块阻碍。如不会，则合并连线，产生新的轨迹。程序中有几个小的优化，用于在障碍周围精细地规避，以及避免整形舍入的误差。
3. 代码
这里只贴出矩阵处理的关键代码。绘图代码从 仓库 签出。
/*!
* \brief mdf_rev_fill 反向搜索，从终点处找起点
* \param v_mat 障碍地形，1是平地，0是墙
* \param startx 开始位置x
* \param starty 开始位置y
* \param endx 结束位置x
* \param endy 结束位置y
* \param p_rev 保存搜索步长的v_mat等尺寸矩阵
* \return 能否找到起点
*/
bool mdf_rev_fill(
const std::vector<std::vector<char> > & v_mat ,
const int startx,
const int starty,
const int endx,
const int endy,
std::vector<std::vector<unsigned int> > * p_rev
)
{
std::vector<std::vector<unsigned int> > & v_rev = *p_rev;
p_rev->clear();
const int rows = v_mat.size();
const int cols = v_mat[0].size();
for (int i=0;i<rows;++i)
{
std::vector<unsigned int> row;
row.resize(cols,0xffffffffu);
p_rev->push_back(std::move(row));
}
//反向着色
std::list<int> currX,currY;
unsigned int step = 1;
currX.push_back(endx);
currY.push_back(endy);
v_rev[endy][endx] = 0;
bool arrival = false;
while (currX.size() && !arrival)
{
const int rev_dirt[4][2] = {
{
1,0},{
0,1},{
-1,0},{
0,-1}
};
const int tasks = currX.size();
for (int t = 0; t< tasks&& !arrival; ++t)
{
const int cx = *currX.begin();
const int cy = *currY.begin();
currX.pop_front();
currY.pop_front();
for (int i=0;i<4&& !arrival;++i)
{
const int nx = cx + rev_dirt[i][0];
const int ny = cy + rev_dirt[i][1];
if (nx == startx && ny == starty)
arrival = true;
if (nx >= cols || nx < 0)
continue;
if (ny >= rows || ny < 0)
continue;
if (v_mat[ny][nx]==0)
continue;
if (v_rev[ny][nx]<0xffffffffu)
continue;
v_rev[ny][nx] = step;
currX.push_back(nx);
currY.push_back(ny);
}//end for (int i=0;i<4&& !arrival;++i)
}//end for (int t = 0; t< tasks&& !arrival; ++t)
++step;
}//end while (currX.size() && !arrival)
return arrival;
}
/*!
* \brief mdf_path_find 前向搜索路径
* \param v_rev 反向寻找后生成的距离矩阵
* \param startx 起点x
* \param starty 起点y
* \param cx 存储路径坐标的x向量
* \param cy 存储路径坐标的y向量
* \return 是否成功搜索生成路径
*/
bool mdf_path_find(
std::vector<std::vector<unsigned int> > & v_rev,
const int startx,
const int starty,
std::vector<int> * cx,
std::vector<int> * cy
)
{
const int rows = v_rev.size();
const int cols = v_rev[0].size();
unsigned int v = v_rev[starty][startx];
cx->clear();
cy->clear();
cx->push_back(startx);
cy->push_back(starty);
int tx = startx, ty= starty;
v_rev[ty][tx] = 0xffffffffu;
while (v>0 && v < 0xffffffffu)
{
const int rev_dirt[8][2] = {
{
1,0},{
0,1},{
-1,0},{
0,-1},
{
1,1},{
-1,1},{
1,-1},{
-1,-1}
};
unsigned int minv = 0xffffffffu;
int mind = 0;
for (int i=0;i<8;++i)
{
const int nx = tx + rev_dirt[i][0];
const int ny = ty + rev_dirt[i][1];
if (nx >= cols || nx < 0)
continue;
if (ny >= rows || ny < 0)
continue;
if (v_rev[ny][nx]<minv)
{
minv = v_rev[ny][nx];
mind = i;
}
}
v = minv;
tx += rev_dirt[mind][0];
ty += rev_dirt[mind][1];
cx->push_back(tx);
cy->push_back(ty);
assert(tx>=0 && tx < cols);
assert(ty>=0 && ty < rows);
}
return (v==0);
}
/*!
* \brief min_dis_opt 归并路径缩短距离
* \param v_mat 障碍地形，1是平地，0是墙
* \param rx 非最优路径x（直接搜索出来的）
* \param ry 非最优路径y（直接搜索出来的）
* \param cx 较优路径x
* \param cy 较优路径y
* \param pidx 关键waypoint下表(相对于cx,cy)，连接两个下标的cx,cy的为直线。
* \return 优化后路径大小
*/
int min_dis_opt(
const std::vector<std::vector<char> > & v_mat ,
const std::vector<int> & rx,
const std::vector<int> & ry,
std::vector<int> * cx,
std::vector<int> * cy,
std::vector<int> * pidx
)
{
const int rows = v_mat.size();
const int cols = v_mat[0].size();
std::vector<int> lx = rx, ly = ry;
std::set<int> important ;
size_t test_begin = 0;
//本次最差的目标
int opt_tar = test_begin + 2;
important.insert(test_begin);
while (test_begin < lx.size()-2)
{
int test_cur = test_begin + 2;
const int pns = lx.size();
bool good = true;
while (test_cur < pns && good)
{
const int x1 = lx[test_begin], y1 = ly[test_begin],x2 = lx[test_cur], y2 = ly[test_cur];
const int dx1 = (x2 - x1), dy1 = (y2 - y1);
const int absx = dx1>=0?dx1:-dx1, absy = dy1>=0?dy1:-dy1;
const int maxp = (absx + absy) * 3;
for (int i=0;i<maxp && good;++i)
{
//为不打擦边球，要求周围1格子也没有障碍才能优化。
for (int d = 0; d< 5 && good; ++d)
{
const int rev_dirt[9][2] = {
{
0,0},
{
1,0},{
0,1},{
-1,0},{
0,-1},
{
1,1},{
-1,1},{
1,-1},{
-1,-1}
};
const int tx = x1 + (i * dx1 ) / (maxp-1) + rev_dirt[d][0];
const int ty = y1 + (i * dy1 ) / (maxp-1) + rev_dirt[d][1];
if (tx >= cols || tx < 0)
continue;
if (ty >= rows || ty < 0)
continue;
if (v_mat[ty][tx] == 0)
good = false;
if (tx ==x1 && ty==y1)
break;
if (tx ==x2 && ty==y2)
break;
}
}
if (!good)
break;
++test_cur;
}
if (test_cur > opt_tar)
{
important.insert(test_begin);
std::vector<int> newx, newy;
for (size_t i=0;i<test_begin;++i)
{
newx.push_back(lx[i]);
newy.push_back(ly[i]);
}
const int x1 = lx[test_begin], y1 = ly[test_begin],x2 = lx[test_cur-1], y2 = ly[test_cur-1];
const int dx1 = (x2 - x1), dy1 = (y2 - y1);
const int absx = dx1>=0?dx1:-dx1, absy = dy1>=0?dy1:-dy1;
const int maxp = (absx + absy) * 3;
int last_x = -1, last_y = -1;
for (int i=0;i<maxp;++i)
{
const int tx = x1 + (i * dx1) / (maxp-1);
const int ty = y1 + (i * dy1) / (maxp-1);
if (tx != last_x || ty !=last_y)
{
last_x = tx;
last_y = ty;
assert(tx>=0 && tx < cols);
assert(ty>=0 && ty < rows);
assert(v_mat[ty][tx]);
newx.push_back(tx);
newy.push_back(ty);
}
}
//下次务必从这里开始
opt_tar = newx.size();
for (int i=test_cur;i<pns;++i)
{
newx.push_back(lx[i]);
newy.push_back(ly[i]);
}
lx = std::move(newx);
ly = std::move(newy);
}
if (good)
break;
++test_begin;
}
important.insert(lx.size()-1);
*cx = std::move(lx);
*cy = std::move(ly);
pidx->clear();
for (auto p : important)
{
pidx->push_back(p);
}
return cx->size();
}
/*!
* \brief min_distance_find 避障路径搜索函数
* \param v_mat 障碍地形，1是平地，0是墙
* \param startx 起点x
* \param starty 起点y
* \param endx 终点x
* \param endy 终点y
* \param x 路径x
* \param y 路径y
* \param pidx 关键waypoint下表(相对于cx,cy)，连接两个下标的cx,cy的为直线。
* \param join 是否做直线归并优化。
* \return cx大小
*/
int min_distance_find(
const std::vector<std::vector<char> > & v_mat ,
const int startx,
const int starty,
const int endx,
const int endy,
std::vector<int> *x,
std::vector<int> *y,
std::vector<int> * pidx,
bool join
)
{
std::vector<std::vector<unsigned int> > v_rev ;
if (!mdf_rev_fill(v_mat,startx,starty,endx,endy, &v_rev))
return false;
std::vector<int> cx,cy;
if (!mdf_path_find(v_rev,startx,starty,&cx,&cy))
return false;
if (join)
return min_dis_opt(v_mat,cx,cy,x,y,pidx);
size_t sz = cx.size();
pidx->clear();
for (size_t i=0;i<sz;++i)
pidx->push_back(i);
*x = std::move(cx);
*y = std::move(cy);
return x->size();
}