十一長假后，同學們陸續開始做題，現在月底了，擴展題“-二值矩陣避障最短路徑演算法”只有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();
}