【recast-navigation/源码解析】findStraightPath详解以及寻路结果贴边优化

说在前面

• recast-navigation版本：1.6.0

叉积cross product

• 正常来讲，叉乘为：
  $$|\vec{A}\times \vec{B}|=\left|\begin{array}{cc}{x}_A& y_A\\ x_B& y_B\end{array}\right|=x_A\cdot y_B-x_B\cdot y_A$$
• 例如：
  
• 但是在recast-navigation库中：

  /// Derives the signed xz-plane area of the triangle ABC, 
  /// or the relationship of line AB to point C.
  ///  @param[in]		a		Vertex A. [(x, y, z)]
  ///  @param[in]		b		Vertex B. [(x, y, z)]
  ///  @param[in]		c		Vertex C. [(x, y, z)]
  /// @return The signed xz-plane area of the triangle.
  inline float dtTriArea2D(const float* a, const float* b, const float* c)
  {
  	const float abx = b[0] - a[0];
  	const float abz = b[2] - a[2];
  	const float acx = c[0] - a[0];
  	const float acz = c[2] - a[2];
  	return acx*abz - abx*acz;
  }
  

  这应该是因为x轴与正常坐标系相反

findStraightPath

• 在使用findStraightPath前，我们通常会先使用findPath进行A*搜索，得到一个基于多边形的路径，假设findPath的结果如下图：
  

• 而findStraightPath的目的则是从这几个多边形中寻找一条路径出来，接下来我们一步步的分解这个过程

  1. 首先我们对一些计算过程中使用到的临时变量进行初始化， 主要为portalApex、portalLeft、portalRight；

     float portalApex[3], portalLeft[3], portalRight[3];
     dtVcopy(portalApex, closestStartPos);
     dtVcopy(portalLeft, portalApex);
     dtVcopy(portalRight, portalApex);
     

     可以看到这三个变量初始化为start点的值
     

  2. 我们从起始点(start)所在的多边形开始遍历，遍历次数(至少)为所有多边形的数量，在我们的例子中，数量为3；

     for (int i = 0; i < pathSize; ++i)
     {
     

  3. 在每次循环中，我们首先需要计算得到当前多边形与下一个多边形的公共边，得到公共边的两个点；假设当前多边形为绿色，下一个多边形为紫色，计算得到的两个点分别为left、right，如下图
     

     if (i+1 < pathSize)
     {
     	unsigned char fromType; // fromType is ignored.
     
     	// Next portal.
     	if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType)))
     	{
     

  4. 然后我们使用三角形的有向面积(signed area of triangle、叉乘)对几个点的位置进行判定

     /// Derives the signed xz-plane area of the triangle ABC, or the relationship of line AB to point C.
     ///  @param[in]		a		Vertex A. [(x, y, z)]
     ///  @param[in]		b		Vertex B. [(x, y, z)]
     ///  @param[in]		c		Vertex C. [(x, y, z)]
     /// @return The signed xz-plane area of the triangle.
     inline float dtTriArea2D(const float* a, const float* b, const float* c)
     {
     	const float abx = b[0] - a[0];
     	const float abz = b[2] - a[2];
     	const float acx = c[0] - a[0];
     	const float acz = c[2] - a[2];
     	return acx*abz - abx*acz;
     }
     

     首先是点portalApex、portalRight、right，由于此时portalApex、portalRight相同，所以结果为0；
     

     这个时候我们将点portalRight置为点right；同理，对于点portalApex、portalLeft、left，将点portalLeft置为点left；
     

     // Right vertex.
     if (dtTriArea2D(portalApex, portalRight, right) <= 0.0f)
     {
     	if (dtVequal(portalApex, portalRight) || dtTriArea2D(portalApex, portalLeft, right) > 0.0f)
     	{
     		dtVcopy(portalRight, right);
     		rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
     		rightPolyType = toType;
     		rightIndex = i;
     	}
     	else
     	{
     // ......
     // Left vertex.
     if (dtTriArea2D(portalApex, portalLeft, left) >= 0.0f)
     {
     	if (dtVequal(portalApex, portalLeft) || dtTriArea2D(portalApex, portalRight, left) < 0.0f)
     	{
     		dtVcopy(portalLeft, left);
     		leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
     		leftPolyType = toType;
     		leftIndex = i;
     	}
     	else
     	{
     

     至此，第一个循环结束

  5. 第二个循环，我们挪动第二个多边形，即当前多边形为第二个，计算第二、第三多边形的公共边，得到新的left、right；如下图，当前多边形为绿色，下一个多边形为紫色，红色为公共边
     

  6. 同样，先对点portalApex、portalRight、right以及点portalApex、portalLeft、left进行计算，这一步实际上是在判定边(left-right)是否在夹角(portalLeft-portalApex-portalRight)之内
     
     首先是点right，由于点right在向量portalApex-portalRight左侧，不再进行下一步判定；即以下条件不满足：

     // Right vertex.
     if (dtTriArea2D(portalApex, portalRight, right) <= 0.0f)
     {
     

     接着是点left，由于点left在向量portalApex-portalLeft左侧，满足条件，继续下一步判定；

     // Left vertex.
     if (dtTriArea2D(portalApex, portalLeft, left) >= 0.0f)
     {
     

     继续判定点left是否在向量portalApex-portalRight左侧，结果满足条件，所以点portalRight是拐点

     if (dtVequal(portalApex, portalLeft) || 
     	dtTriArea2D(portalApex, portalRight, left) < 0.0f)
     {
     	//......
     }
     else
     {
     	//....
     	dtVcopy(portalApex, portalRight);
     

     得到拐点之后，将拐点置为新的portalApex、portalRight、portalLeft，同时，下一次循环将继续从绿色多边形开始
     
     此时，我们就得到了部分路径(上图中绿色点)

  7. 继续下一次循环，同理可得到下述结果：
     

  8. 下一次循环，由于此时已经没有下一个多边形了，我们将left、right置为点end
     
     同样，先对点portalApex、portalRight、right以及点portalApex、portalLeft、left进行计算，
     
     最终得到拐点portalRight
     

  9. 下一次循环，同样，由于此时已经没有下一个多边形了，我们将left、right置为点end
     
     继续对点portalApex、portalRight、right以及点portalApex、portalLeft、left进行计算，
     
     此时循环结束，我们将点end加入路径

     }
     
     // Ignore status return value as we're just about to return anyway.
     appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0,
     					straightPath, straightPathFlags, straightPathRefs,
     					straightPathCount, maxStraightPath);
     

  10. 最终结果，如下图绿色路径
      

寻路结果贴边优化的一种方式

• 通过上面的解释，我们知道寻路结果上的拐点都是多边形的边上的顶点，也就是left、right点，所以想要让结果不那么贴边，可以将两点向内偏移，即：

  // Next portal.
  if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType)))
  {
  	//......
  }
  
  if (!dtVequal(left, right)) {
  	float _left[3], _right[3];
  	dtVcopy(_left, left);
  	dtVcopy(_right, right);
  
  	dtVlerp(left, _left, _right, 0.1);
  	dtVlerp(right, _right, _left, 0.1);
  }				
  

• 优化后