项目里,有个场景比较绕,比较远的目标寻路只会寻路到一半的地方就停下来, 终点是可达的,但unity的navmesh寻路算法本身因素,遍历深度有限制造成的。
(c++实现的recastnavigation 也有这个问题, dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) maxNodes 决定了寻路算法时最多的寻路缓存节点数,太多会慢,太少有些绕路的大寻不到。)
1. 一种思路是寻路对象那边处理, 遇到寻路有结果但不能到达终点(寻路得到的NavMeshPath status一般是 PathPartial的情况), 可以定时或者快到本次路径的终点时再触发一次寻路。
2.方案2,改一些逻辑比较多或者比较麻烦的时候,且不介意中途绕路的情况,则可采用迭代寻路的方案, 就是寻路的接口自己再封装一下: 没法直接寻路到终点,把本次得到的最后一个路点当起点,继续寻路,然后把若干段路径拼接起来。
这种方案的问题就是 中间的点很可能走到一个沟沟里,然后又跑出来。。。
unity 这边目前实现的代码:
/// <summary>
/// 可以拼接的寻路路点
/// </summary>
public class NavMeshLongPath
{
public NavMeshLongPath()
{
m_pcorners = m_listCorners.ToArray();
}
~NavMeshLongPath() { }
/// <summary>
/// Erase all corner points from path.
/// </summary>
public void ClearCorners()
{
m_listCorners.Clear();
m_pcorners = m_listCorners.ToArray();
}
public void MergeFrom(Vector3[] otherCorners, int startIndex = 0)
{
if (startIndex > 0)
{
for (int i = startIndex; i < otherCorners.Length; ++i)
{
m_listCorners.Add(otherCorners[i]);
}
}
else
{
m_listCorners.AddRange(otherCorners);
}
m_pcorners = m_listCorners.ToArray();
}
public Vector3[] corners
{
get
{
return m_pcorners;
}
}
public NavMeshPathStatus status = NavMeshPathStatus.PathInvalid;
protected List<Vector3> m_listCorners = new List<Vector3>();
protected Vector3[] m_pcorners = null;
}
// 另外封装一个函数替换原本调用 NavMesh.CalculatePath的地方
public static bool CalculateLongPath(Vector3 sourcePosition, Vector3 targetPosition, int areaMask, NavMeshLongPath path)
{
//if(null == path)
//{
// path = new NavMeshLongPath();
//}
NavMeshPath subPath = new NavMeshPath();
// 需要把终点作为起点再次寻路
Vector3 newStart = sourcePosition;
int loopCount = 0; // 防止意外死循环
while (loopCount++ < 5)
{
subPath.ClearCorners();
if (NavMesh.CalculatePath(newStart, targetPosition, areaMask, subPath))
{
path.status = subPath.status; // 状态用最后一个
if (subPath.corners.Length > 1)
{
if (subPath.corners[0] == subPath.corners[subPath.corners.Length - 1])
{
// 寻路到的还是这个点本身,则应该结束
if (1 == loopCount)
{
path.MergeFrom(subPath.corners);
}
break;
}
else
{
// 把剩余的点合并到path里
path.MergeFrom(subPath.corners, (1 == loopCount ? 0 : 1));
if (NavMeshPathStatus.PathComplete == subPath.status)
{
// 寻路结束
break;
}
else
{
// 还是没寻路到终点则继续找一遍
newStart = subPath.corners[subPath.corners.Length - 1];
}
}
}
else
{
// 寻路到的还是这个点本身,则应该结束
if (1 == loopCount)
{
path.MergeFrom(subPath.corners);
}
break;
}
}
else
{
if (1 == loopCount)
// 第一次就寻路失败则返回false,否则按理不需要
return false;
else
break;
}
}
return true;
}
C++ 另外一个项目的实现,仅供参考, 思路差不多的,寻路发现未达终点,尝试若干次迭代, 如果得到的路点只有1个 或者 终点就是起点 或者 寻路失败,则说明没法继续寻路停止迭代。
bool CPathFinding::findPath(dtNavMeshQuery* navQuery, const MapPos &posStart, const MapPos &posEnd, vecPbClientPath &vecPbPath) {
if(nullptr == navQuery) {
LOG_E("navQuery is null");
return false;
}
float straight_path[(MAX_POLYS + 1) * 3];
static unsigned char straight_pathflags[MAX_POLYS];
static dtPolyRef straight_path_polys[MAX_POLYS];
int straight_path_num = 0;
static PbcPosition clientPos;
// 查询的次数
int32 n32Count = 0;
auto _posStart = posStart;
bool bNeedReverse = false;
const int32 N32_MAX_TRY_NUM = 8;
// 是否需要循环继续下一个查询
bool bNotEnd = false;
while(n32Count < N32_MAX_TRY_NUM) {
++n32Count;
bNotEnd = false;
if (!CPathFinding::findPathInner(navQuery, _posStart, posEnd, straight_path_num, straight_path, straight_pathflags, straight_path_polys, n32Count, bNotEnd)) {
return false;
}
if (straight_path_num > 0) {
for (int32_t i = 0; i < straight_path_num; ++i) {
if (n32Count > 1 && 0 == i) {
// 重新寻路的第一个点跳过
continue;
}
clientPos.set_x((int32_t)(straight_path[i * 3] * 100));
clientPos.set_y((int32_t)(straight_path[i * 3 + 2] * 100));
*vecPbPath.Add() = clientPos;
}
if (bNotEnd) {
if (1 == straight_path_num) {
bNeedReverse = true; // 有可能虽然两个点很近但绕了很远的路, 可以尝试反向寻路,或许能成功。 如果都不行, 则就认命了。
break;
}
// 一次没有寻路完,则多段寻路
MapPos posLast = MapPos((int32_t)(straight_path[(straight_path_num - 1) * 3] * 100), (int32_t)(straight_path[(straight_path_num - 1) * 3 + 2] * 100));
_posStart = posLast;
} else {
break;
}
} else {
break;
}
}
if (bNotEnd && N32_MAX_TRY_NUM <= n32Count) {
// 没找到最终的点?尝试反向
bNeedReverse = true;
}
if (bNeedReverse) {
bNeedReverse = false;
_posStart = posEnd;
auto _posEnd = posStart;
vector<PbcPosition> vecRPos;
straight_path_num = 0;
n32Count = 0;
while (n32Count < N32_MAX_TRY_NUM) {
++n32Count;
bNotEnd = false;
if (!CPathFinding::findPathInner(navQuery, _posStart, _posEnd, straight_path_num, straight_path, straight_pathflags, straight_path_polys, n32Count, bNotEnd)) {
return false;
}
if (straight_path_num > 0) {
for (int32_t i = 0; i < straight_path_num; ++i) {
if (n32Count > 1 && 0 == i) {
// 重新寻路的第一个点跳过
continue;
}
clientPos.set_x((int32_t)(straight_path[i * 3] * 100));
clientPos.set_y((int32_t)(straight_path[i * 3 + 2] * 100));
vecRPos.emplace_back(clientPos);
}
if (bNotEnd) {
if (1 == straight_path_num) {
bNeedReverse = true; // 还是寻不到,则按失败处理, vecPbPath 仍旧存了之前寻路结果。
break;
}
// 一次没有寻路完,则多段寻路
MapPos posLast = MapPos((int32_t)(straight_path[(straight_path_num - 1) * 3] * 100), (int32_t)(straight_path[(straight_path_num - 1) * 3 + 2] * 100));
_posStart = posLast;
}
else {
break;
}
}
else {
break;
}
}
if (!bNeedReverse && vecRPos.size() > 0) {
if (bNotEnd && N32_MAX_TRY_NUM <= n32Count) {
// 还是没有找到完整路径,则不覆盖
} else {
// 没有出现问题, 用反向寻路出来的数据,返回
vecPbPath.Clear();
int n32Size = static_cast<int>(vecRPos.size());
vecPbPath.Reserve(n32Size);
for(int i = n32Size - 1; i >= 0; --i) {
*vecPbPath.Add() = vecRPos[i];
}
}
}
}
return true;
}
bool CPathFinding::findPathInner(dtNavMeshQuery* navQuery
, const MapPos& start
, const MapPos& end
, int& m_nstraightPath
, float* m_straightPath
, unsigned char* m_straightPathFlags
, dtPolyRef* m_straightPathPolys
, int32 n32Round
, bool& bNotEnd) {
if(nullptr == navQuery) {
LOG_E("navQuery is null");
return false;
}
if(nullptr == m_straightPath) {
LOG_E("m_straightPath is null");
return false;
}
if(nullptr == m_straightPathFlags) {
LOG_E("m_straightPathFlags is null");
return false;
}
if(nullptr == m_straightPathPolys) {
LOG_E("m_straim_straightPathPolysghtPath is null");
return false;
}
bNotEnd = false;
dtPolyRef m_startRef;
dtPolyRef m_endRef;
float m_spos[3];
float m_epos[3];
m_spos[0] = start.n32X / 100.0f;
m_spos[1] = 0.0f; // 0 在 0.0f 1 在10.0f
m_spos[2] = start.n32Y / 100.0f;
m_epos[0] = end.n32X / 100.0f;
m_epos[1] = 0.0f;
m_epos[2] = end.n32Y / 100.0f;
dtQueryFilter m_filter;
// m_filter.setIncludeFlags(0xffff^0x10);
// m_filter.setIncludeFlags(m_scene->get_nav_query_filter_include_flags());
m_filter.setExcludeFlags(SAMPLE_POLYFLAGS_DISABLED);
float m_polyPickExt[3];
m_polyPickExt[0] = 25.6f; // 不可走区域 容错 10x10格子左右
m_polyPickExt[1] = 1;
m_polyPickExt[2] = 25.6f;
dtPolyRef m_polys[MAX_POLYS];
int m_npolys = 0;
static float nearestPt[3];
bool isOverPoly = false;
navQuery->findNearestPoly(m_epos, m_polyPickExt, &m_filter, &m_endRef, nearestPt, &isOverPoly);
if (!isOverPoly) {
// 终点在障碍物里, 则Ref 必须取靠近spos的
float fDist = dtVdist2D(m_epos, m_spos);
if (fDist > 0) {
float npos[3];
float t = std::fmin(1.0f, 0.64f / fDist); // 1.28 偏移可能会穿墙 所以改0.64偏移
dtVlerp(npos, m_epos, m_spos, t);
navQuery->findNearestPoly(npos, m_polyPickExt, &m_filter, &m_endRef, m_epos); // 刷新 m_epos点
}
}
bool isStartOverPoly = false; // 点是否在多边形里,否的话说明点在障碍物里
navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, nearestPt, &isStartOverPoly);
if (!isStartOverPoly && 1 == n32Round) {
// 起点在障碍物里, 则Ref 必须取靠近epos的. 注:只有初始时要这么处理,若循环的第二次就不需要
float fDist = dtVdist2D(m_epos, m_spos);
if (fDist > 0) {
float npos[3];
float t = std::fmin(1.0f, 0.64f / fDist); // 1.28 偏移可能会穿墙 所以改0.64偏移
dtVlerp(npos, m_spos, m_epos, t);
navQuery->findNearestPoly(npos, m_polyPickExt, &m_filter, &m_startRef, m_spos); // 刷新 m_spos点
}
}
{
// 直线可达的优化
float t = 0;
float hitNormal[3];// , hitPos[3];
navQuery->raycast(m_startRef, m_spos, m_epos, &m_filter, &t, hitNormal, m_polys, &m_npolys, MAX_POLYS);
if (t > 1) {
// No hit
// dtVcopy(hitPos, m_epos);
m_nstraightPath = 2;
m_straightPath[0] = m_spos[0];
m_straightPath[1] = m_spos[1];
m_straightPath[2] = m_spos[2];
m_straightPath[3] = m_epos[0];
m_straightPath[4] = m_epos[1];
m_straightPath[5] = m_epos[2];
return true;
}
else {
// Hit, 但是射线的终点就是目标的polyid, 则说明也能直线到,
if (m_npolys > 1 && m_endRef == m_polys[m_npolys - 1]) {
m_nstraightPath = 2;
m_straightPath[0] = m_spos[0];
m_straightPath[1] = m_spos[1];
m_straightPath[2] = m_spos[2];
dtVlerp(&m_straightPath[3], m_spos, m_epos, t);
return true;
}
}
}
auto nDtStatus = navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
m_nstraightPath = 0;
if (m_npolys)
{
// In case of partial path, make sure the end point is clamped to the last polygon.
float epos[3];
dtVcopy(epos, m_epos);
if (m_polys[m_npolys - 1] != m_endRef && m_npolys > 1) {
// 寻路出来的路径没到终点, 且 非 (DT_SUCCESS & DT_PARTIAL_RESULT) 而是 (DT_SUCCESS & DT_PARTIAL_RESULT & DT_OUT_OF_NODES) 则后面需接上寻路
bNotEnd = true;
navQuery->closestPointOnPoly(m_polys[m_npolys - 1], m_epos, epos, 0);
}
auto pStraightPath = m_straightPath;
navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys, pStraightPath, m_straightPathFlags,
m_straightPathPolys, &m_nstraightPath, MAX_POLYS, DT_STRAIGHTPATH_AREA_CROSSINGS);
m_nstraightPath = FixedPath(m_nstraightPath, pStraightPath, m_straightPathFlags, m_straightPathPolys);
if (1 == m_nstraightPath && m_startRef == m_endRef) {
m_nstraightPath = 2;
m_straightPath[3] = m_straightPath[0];
m_straightPath[4] = m_straightPath[1];
m_straightPath[5] = m_straightPath[2];
}
}
else
{
return false;
}
return true;
}
// 修复Recast导航折返问题
static int FixedPath(
int ncorners,
float* cornerVerts,
unsigned char* cornerFlags,
dtPolyRef* cornerPolys) {
if (ncorners <= 2) {
return ncorners;
}
// 过滤共线折返路点
const float* p0 = cornerVerts;
const float* p1 = cornerVerts + 3;
const float* p2 = cornerVerts + 6;
// 起点不覆盖
for (int i = 1; i < ncorners - 1;)
{
if (cornerFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) {
break;
}
// 共线
if (dtAbs(dtTriArea2D(p0, p1, p2)) < dtSqr(0.1f)) {
// 反向
if ((p1[0] - p0[1]) * (p2[0] - p1[0]) < 0
|| (p1[2] - p0[2]) * (p2[2] - p1[2]) < 0) {
--ncorners;
memmove(cornerFlags + i, cornerFlags + i + 1, sizeof(unsigned char) * (ncorners - i));
memmove(cornerPolys + i, cornerPolys + i + 1, sizeof(dtPolyRef) * (ncorners - i));
memmove(cornerVerts + 3 * i, cornerVerts + 3 * (i + 1), sizeof(float) * 3 * (ncorners - i));
continue;
}
}
++i;
p0 = p1;
p1 = p2;
p2 += 3;
}
return ncorners;
}