//使用现有路径从起点到目标点建立标准的直接路径
public NavStatus BuildStraightPath(NavmeshPoint start, NavmeshPoint goal, NavmeshQuery query)
{
int iStart = FindPolyRef(0, start.polyRef);
int iGoal = -1;
if (iStart != -1)
{
iGoal = FindPolyRefReverse(iStart, goal.polyRef);
}
if (iGoal == -1)
{
return(NavStatus.Failure | NavStatus.InvalidParam);
}
//NavmeshQuery是直通C++的底层寻路
Array.Clear(straightPoints, 0, straightPoints.Length);
NavStatus status = query.GetStraightPath(start.point, goal.point
, path, iStart, iGoal - iStart + 1
, straightPoints, straightFlags, straightPath, out straightCount);
if (straightCount == 0)
{
return(NavStatus.Failure);
}
return(status);
}
/*
* public float GetPathDistance(Vector3 start, Vector3 end)
* {
* List<Vector3> path = GeneratePath(start, end).;
* float distance = 0;
*
* for (int i = 0; i < path.Count - 1; i++)
* {
* distance += path[i].DistanceFrom(path[i + 1]);
* }
*
* return distance;
* }
*/
private oVector3[] StraightenPath(oVector3 start, oVector3 end, uint[] path, int pathCount)
{
oVector3[] straightPath = new oVector3[200];
int count = 0;
if (NavUtil.Failed(_query.GetStraightPath(start, end, path, 0, pathCount, straightPath, null, null, out count)))
{
throw new Exception("Failed to straighten path.");
}
return(straightPath.Take(count).ToArray());
}
/// <summary>
/// Builds a standard straight path from the start to the goal point using the exiting path.
/// </summary>
/// <remarks>
/// <para>Limits:</para>
/// <ul>
/// <li>The path must exist and both the start and goal points must be within the path.</li>
/// <li>
/// The goal's polygon reference must be in or after the start points polygon reference.
/// </li>
/// </ul>
/// </remarks>
/// <param name="start">The start point.</param>
/// <param name="goal">The goal point.</param>
/// <returns>The status of the operation.</returns>
public NavStatus BuildStraightPath(NavmeshPoint start, NavmeshPoint goal, NavmeshQuery query)
{
int iStart = FindPolyRef(0, start.polyRef);
int iGoal = -1;
if (iStart != -1)
iGoal = FindPolyRefReverse(iStart, goal.polyRef);
if (iGoal == -1)
return (NavStatus.Failure | NavStatus.InvalidParam);
NavStatus status = query.GetStraightPath(start.point, goal.point
, path, iStart, iGoal - iStart + 1
, straightPoints, straightFlags, straightPath, out straightCount);
if (straightCount == 0)
return NavStatus.Failure;
return status;
}
//用递归的方式生成路径点，因为直接用MoveAlongSurface寻路找不到太复杂的路径
private void generatePointPath(NavmeshPoint startPoint, NavmeshPoint endPoint)
{
int nVisitedCount;
Vector3 targetPos;
NavStatus status = query.MoveAlongSurface(startPoint, endPoint.point, mCrowdManager.QueryFilter
, out targetPos, polyResultBuffer, out nVisitedCount);
float dis = Vector3.Distance(targetPos, endPoint.point);
if (Vector3.Distance(targetPos, endPoint.point) > mMaxPointDis)
{
int nPolyCount;
//点路径过为复杂，计算失败，根据三角形路径拆为两段递归计算
status = query.FindPath(startPoint, endPoint
, mCrowdManager.QueryFilter, polyResultBuffer
, out nPolyCount);
if (nPolyCount > 2)
{
uint midPoly = polyResultBuffer[nPolyCount / 2];
NavmeshTile tile;
NavmeshPoly poly;
navmesh.GetTileAndPoly(midPoly, out tile, out poly);
if (tile.GetTileRef() != tileBufferRef)
{
tile.GetVerts(tileBufferPoints);
}
int minIndex = -1;
float minDistance = 0.0f;
for (int i = 0; i < poly.vertCount; i++)
{
ushort index = poly.indices[i];
Vector3 pos = tileBufferPoints[index];
//Debug.Log("pos=" + pos.ToString());
float distance = Vector3.Distance(pos, endPoint.point);
if (minIndex < 0 || distance < minDistance)
{
minDistance = distance;
minIndex = index;
}
}
Vector3 midPos = tileBufferPoints[minIndex];
NavmeshPoint midPoint;
query.GetNearestPoint(midPos, Vector3.one, mCrowdManager.QueryFilter, out midPoint);
generatePointPath(startPoint, midPoint);
generatePointPath(midPoint, endPoint);
return;
}
else if (nPolyCount == 1)
{
uint PolyIndex = polyResultBuffer[0];
NavmeshTile tile;
NavmeshPoly poly;
navmesh.GetTileAndPoly(PolyIndex, out tile, out poly);
//if (poly.Type == NavmeshPolyType.OffMeshConnection)
{
Debug.Log(poly.Type.ToString() + ":" + startPoint.point.ToString() + endPoint.point.ToString());
}
int minIndex = -1;
float minDistance = 0.0f;
for (int i = 0; i < poly.vertCount; i++)
{
ushort index = poly.indices[i];
Vector3 pos = tileBufferPoints[index];
Debug.Log("pos=" + pos.ToString());
float distance = Vector3.Distance(pos, endPoint.point);
if (minIndex < 0 || distance < minDistance)
{
minDistance = distance;
minIndex = index;
}
}
}
else
{
//中间没有多个多边形，直接过去
Debug.Log("go stright:" + startPoint.point.ToString() + endPoint.point.ToString());
if ((pointResultCount < 1) || (Vector3.Distance(startPoint.point, pointResult[pointResultCount - 1]) > mMaxPointDis))
{
pointResult[pointResultCount] = startPoint.point;
pointResultCount++;
}
pointResult[pointResultCount] = endPoint.point;
pointResultCount++;
}
}
else
{
//已经找到简单路径，输出
int pathCount;
query.GetStraightPath(startPoint.point, targetPos
, polyResultBuffer, 0, nVisitedCount
, pointResultBuffer
, null, null, out pathCount);
if ((pointResultCount < 1) || (Vector3.Distance(pointResultBuffer[0], pointResult[pointResultCount - 1]) > mMaxPointDis))
{
pointResult[pointResultCount] = pointResultBuffer[0];
pointResultCount++;
}
for (int i = 1; i < pathCount; i++)
{
pointResult[pointResultCount] = pointResultBuffer[i];
pointResultCount++;
}
}
}
