static bool getSteerTarget(Detour.dtNavMeshQuery navQuery, float[] startPos, float[] endPos,
float minTargetDist,
dtPolyRef[] path, int pathSize,
float[] steerPos, ref byte steerPosFlag, ref dtPolyRef steerPosRef,
ref float[] outPoints, ref int outPointCount)
{
// Find steer target.
const int MAX_STEER_POINTS = 3;
float[] steerPath = new float[MAX_STEER_POINTS * 3];
byte[] steerPathFlags = new byte[MAX_STEER_POINTS];
dtPolyRef[] steerPathPolys = new dtPolyRef[MAX_STEER_POINTS];
int nsteerPath = 0;
navQuery.findStraightPath(startPos, endPos, path, pathSize,
steerPath, steerPathFlags, steerPathPolys, ref nsteerPath, MAX_STEER_POINTS, 0);
if (nsteerPath == 0)
{
return(false);
}
//if (outPoints && outPointCount)
//{
outPointCount = nsteerPath;
for (int i = 0; i < nsteerPath; ++i)
{
Detour.dtVcopy(outPoints, i * 3, steerPath, i * 3);
}
//}
// Find vertex far enough to steer to.
int ns = 0;
while (ns < nsteerPath)
{
// Stop at Off-Mesh link or when point is further than slop away.
if ((steerPathFlags[ns] & (byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ||
!inRange(steerPath, ns * 3, startPos, 0, minTargetDist, 1000.0f))
{
break;
}
ns++;
}
// Failed to find good point to steer to.
if (ns >= nsteerPath)
{
return(false);
}
Detour.dtVcopy(steerPos, 0, steerPath, ns * 3);
steerPos[1] = startPos[1];
steerPosFlag = steerPathFlags[ns];
steerPosRef = steerPathPolys[ns];
return(true);
}
public static StraightPath ComputeStraightPath(Detour.dtNavMeshQuery navQuery, float[] startPos, float[] endPos, float distance = 10)
{
//m_ComputedPathType = PathType.Straight;
StraightPath path = new StraightPath();
float[] extents = new float[3];
for (int i = 0; i < 3; ++i)
{
extents[i] = distance;
}
dtPolyRef startRef = 0;
dtPolyRef endRef = 0;
float[] startPt = new float[3];
float[] endPt = new float[3];
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
navQuery.findNearestPoly(startPos, extents, filter, ref startRef, ref startPt);
navQuery.findNearestPoly(endPos, extents, filter, ref endRef, ref endPt);
int pathCount = -1;
navQuery.findPath(startRef, endRef, startPt, endPt, filter, path.m_RawPathPolys, ref pathCount, StraightPath.MAX_POLYS);
path.m_RawPathLength = pathCount;
if (pathCount > 0)
{
// In case of partial path, make sure the end point is clamped to the last polygon.
float[] epos = new float[3];
Detour.dtVcopy(epos, endPt);
if (path.m_RawPathPolys[pathCount - 1] != endRef)
{
bool posOverPoly = false;
navQuery.closestPointOnPoly(path.m_RawPathPolys[pathCount - 1], endPt, epos, ref posOverPoly);
}
navQuery.findStraightPath(startPt, endPt, path.m_RawPathPolys, pathCount,
path.m_straightPath, path.m_straightPathFlags,
path.m_straightPathPolys, ref path.m_straightPathCount,
StraightPath.MAX_POLYS, path.m_straightPathOptions);
}
return(path);
}
bool GetSteerTarget(float[] startPos, float[] endPos, float minTargetDist, ulong[] path, uint pathSize, out float[] steerPos, out Detour.dtStraightPathFlags steerPosFlag, out ulong steerPosRef)
{
steerPosRef = 0;
steerPos = new float[3];
steerPosFlag = 0;
// Find steer target.
float[] steerPath = new float[3 * 3];
byte[] steerPathFlags = new byte[3];
ulong[] steerPathPolys = new ulong[3];
int nsteerPath = 0;
uint dtResult = _navMeshQuery.findStraightPath(startPos, endPos, path, (int)pathSize, steerPath, steerPathFlags, steerPathPolys, ref nsteerPath, 3, 0);
if (nsteerPath == 0 || Detour.dtStatusFailed(dtResult))
{
return(false);
}
// Find vertex far enough to steer to.
uint ns = 0;
while (ns < nsteerPath)
{
Span <float> span = steerPath;
// Stop at Off-Mesh link or when point is further than slop away.
if ((steerPathFlags[ns].HasAnyFlag((byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) ||
!InRangeYZX(span.Slice((int)ns * 3).ToArray(), startPos, minTargetDist, 1000.0f)))
{
break;
}
ns++;
}
// Failed to find good point to steer to.
if (ns >= nsteerPath)
{
return(false);
}
Detour.dtVcopy(steerPos, 0, steerPath, (int)ns * 3);
steerPos[1] = startPos[1]; // keep Z value
steerPosFlag = (Detour.dtStraightPathFlags)steerPathFlags[ns];
steerPosRef = steerPathPolys[ns];
return(true);
}
uint FindSmoothPath(float[] startPos, float[] endPos, ulong[] polyPath, uint polyPathSize, out float[] smoothPath, out int smoothPathSize, uint maxSmoothPathSize)
{
smoothPathSize = 0;
int nsmoothPath = 0;
smoothPath = new float[74 * 3];
ulong[] polys = new ulong[74];
Array.Copy(polyPath, polys, polyPathSize);
uint npolys = polyPathSize;
float[] iterPos = new float[3];
float[] targetPos = new float[3];
if (Detour.dtStatusFailed(_navMeshQuery.closestPointOnPolyBoundary(polys[0], startPos, iterPos)))
{
return(Detour.DT_FAILURE);
}
if (Detour.dtStatusFailed(_navMeshQuery.closestPointOnPolyBoundary(polys[npolys - 1], endPos, targetPos)))
{
return(Detour.DT_FAILURE);
}
Detour.dtVcopy(smoothPath, nsmoothPath * 3, iterPos, 0);
nsmoothPath++;
// Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path.
while (npolys != 0 && nsmoothPath < maxSmoothPathSize)
{
// Find location to steer towards.
float[] steerPos;
Detour.dtStraightPathFlags steerPosFlag;
ulong steerPosRef = 0;
if (!GetSteerTarget(iterPos, targetPos, 0.3f, polys, npolys, out steerPos, out steerPosFlag, out steerPosRef))
{
break;
}
bool endOfPath = steerPosFlag.HasAnyFlag(Detour.dtStraightPathFlags.DT_STRAIGHTPATH_END);
bool offMeshConnection = steerPosFlag.HasAnyFlag(Detour.dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION);
// Find movement delta.
float[] delta = new float[3];
Detour.dtVsub(delta, steerPos, iterPos);
float len = (float)Math.Sqrt(Detour.dtVdot(delta, delta));
// If the steer target is end of path or off-mesh link, do not move past the location.
if ((endOfPath || offMeshConnection) && len < 4.0f)
{
len = 1.0f;
}
else
{
len = 4.0f / len;
}
float[] moveTgt = new float[3];
Detour.dtVmad(moveTgt, iterPos, delta, len);
// Move
float[] result = new float[3];
int MAX_VISIT_POLY = 16;
ulong[] visited = new ulong[MAX_VISIT_POLY];
int nvisited = 0;
_navMeshQuery.moveAlongSurface(polys[0], iterPos, moveTgt, _filter, result, visited, ref nvisited, MAX_VISIT_POLY);
npolys = FixupCorridor(polys, npolys, 74, visited, nvisited);
_navMeshQuery.getPolyHeight(polys[0], result, ref result[1]);
result[1] += 0.5f;
Detour.dtVcopy(iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && InRangeYZX(iterPos, steerPos, 0.3f, 1.0f))
{
// Reached end of path.
Detour.dtVcopy(iterPos, targetPos);
if (nsmoothPath < maxSmoothPathSize)
{
Detour.dtVcopy(smoothPath, nsmoothPath * 3, iterPos, 0);
nsmoothPath++;
}
break;
}
else if (offMeshConnection && InRangeYZX(iterPos, steerPos, 0.3f, 1.0f))
{
// Advance the path up to and over the off-mesh connection.
ulong prevRef = 0;
ulong polyRef = polys[0];
uint npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (uint i = npos; i < npolys; ++i)
{
polys[i - npos] = polys[i];
}
npolys -= npos;
// Handle the connection.
float[] connectionStartPos = new float[3];
float[] connectionEndPos = new float[3];
if (Detour.dtStatusSucceed(_navMesh.getOffMeshConnectionPolyEndPoints(prevRef, polyRef, connectionStartPos, connectionEndPos)))
{
if (nsmoothPath < maxSmoothPathSize)
{
Detour.dtVcopy(smoothPath, nsmoothPath * 3, connectionStartPos, 0);
nsmoothPath++;
}
// Move position at the other side of the off-mesh link.
Detour.dtVcopy(iterPos, connectionEndPos);
_navMeshQuery.getPolyHeight(polys[0], iterPos, ref iterPos[1]);
iterPos[1] += 0.5f;
}
}
// Store results.
if (nsmoothPath < maxSmoothPathSize)
{
Detour.dtVcopy(smoothPath, nsmoothPath * 3, iterPos, 0);
nsmoothPath++;
}
}
smoothPathSize = nsmoothPath;
// this is most likely a loop
return(nsmoothPath < 74 ? Detour.DT_SUCCESS : Detour.DT_FAILURE);
}
void BuildPolyPath(Vector3 startPos, Vector3 endPos)
{
// *** getting start/end poly logic ***
float distToStartPoly = 0;
float distToEndPoly = 0;
float[] startPoint = { startPos.Y, startPos.Z, startPos.X };
float[] endPoint = { endPos.Y, endPos.Z, endPos.X };
ulong startPoly = GetPolyByLocation(startPoint, ref distToStartPoly);
ulong endPoly = GetPolyByLocation(endPoint, ref distToEndPoly);
// we have a hole in our mesh
// make shortcut path and mark it as NOPATH ( with flying and swimming exception )
// its up to caller how he will use this info
if (startPoly == 0 || endPoly == 0)
{
Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . (startPoly == 0 || endPoly == 0)\n");
BuildShortcut();
bool path = _sourceUnit.IsTypeId(TypeId.Unit) && _sourceUnit.ToCreature().CanFly();
bool waterPath = _sourceUnit.IsTypeId(TypeId.Unit) && _sourceUnit.ToCreature().CanSwim();
if (waterPath)
{
// Check both start and end points, if they're both in water, then we can *safely* let the creature move
for (uint i = 0; i < _pathPoints.Length; ++i)
{
ZLiquidStatus status = _sourceUnit.GetMap().getLiquidStatus(_pathPoints[i].X, _pathPoints[i].Y, _pathPoints[i].Z, MapConst.MapAllLiquidTypes);
// One of the points is not in the water, cancel movement.
if (status == ZLiquidStatus.NoWater)
{
waterPath = false;
break;
}
}
}
pathType = (path || waterPath) ? (PathType.Normal | PathType.NotUsingPath) : PathType.NoPath;
return;
}
// we may need a better number here
bool farFromPoly = (distToStartPoly > 7.0f || distToEndPoly > 7.0f);
if (farFromPoly)
{
Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . farFromPoly distToStartPoly={0:F3} distToEndPoly={1:F3}\n", distToStartPoly, distToEndPoly);
bool buildShotrcut = false;
if (_sourceUnit.IsTypeId(TypeId.Unit))
{
Creature owner = _sourceUnit.ToCreature();
Vector3 p = (distToStartPoly > 7.0f) ? startPos : endPos;
if (_sourceUnit.GetMap().IsUnderWater(p.X, p.Y, p.Z))
{
Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . underWater case\n");
if (owner.CanSwim())
{
buildShotrcut = true;
}
}
else
{
Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . flying case\n");
if (owner.CanFly())
{
buildShotrcut = true;
}
}
}
if (buildShotrcut)
{
BuildShortcut();
pathType = (PathType.Normal | PathType.NotUsingPath);
return;
}
else
{
float[] closestPoint = new float[3];
// we may want to use closestPointOnPolyBoundary instead
bool posOverPoly = false;
if (Detour.dtStatusSucceed(_navMeshQuery.closestPointOnPoly(endPoly, endPoint, closestPoint, ref posOverPoly)))
{
Detour.dtVcopy(endPoint, closestPoint);
SetActualEndPosition(new Vector3(endPoint[2], endPoint[0], endPoint[1]));
}
pathType = PathType.Incomplete;
}
}
// *** poly path generating logic ***
// start and end are on same polygon
// just need to move in straight line
if (startPoly == endPoly)
{
Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . (startPoly == endPoly)\n");
BuildShortcut();
_pathPolyRefs[0] = startPoly;
_polyLength = 1;
pathType = farFromPoly ? PathType.Incomplete : PathType.Normal;
Log.outDebug(LogFilter.Maps, "BuildPolyPath . path type {0}\n", pathType);
return;
}
// look for startPoly/endPoly in current path
/// @todo we can merge it with getPathPolyByPosition() loop
bool startPolyFound = false;
bool endPolyFound = false;
uint pathStartIndex = 0;
uint pathEndIndex = 0;
if (_polyLength != 0)
{
for (; pathStartIndex < _polyLength; ++pathStartIndex)
{
// here to carch few bugs
if (_pathPolyRefs[pathStartIndex] == 0)
{
Log.outError(LogFilter.Maps, "Invalid poly ref in BuildPolyPath. _polyLength: {0}, pathStartIndex: {1}," +
" startPos: {2}, endPos: {3}, mapid: {4}", _polyLength, pathStartIndex, startPos, endPos, _sourceUnit.GetMapId());
break;
}
if (_pathPolyRefs[pathStartIndex] == startPoly)
{
startPolyFound = true;
break;
}
}
for (pathEndIndex = _polyLength - 1; pathEndIndex > pathStartIndex; --pathEndIndex)
{
if (_pathPolyRefs[pathEndIndex] == endPoly)
{
endPolyFound = true;
break;
}
}
}
if (startPolyFound && endPolyFound)
{
Log.outDebug(LogFilter.Maps, "BuildPolyPath : (startPolyFound && endPolyFound)\n");
// we moved along the path and the target did not move out of our old poly-path
// our path is a simple subpath case, we have all the data we need
// just "cut" it out
_polyLength = pathEndIndex - pathStartIndex + 1;
Array.Copy(_pathPolyRefs, pathStartIndex, _pathPolyRefs, 0, _polyLength);
}
else if (startPolyFound && !endPolyFound)
{
Log.outDebug(LogFilter.Maps, "BuildPolyPath : (startPolyFound && !endPolyFound)\n");
// we are moving on the old path but target moved out
// so we have atleast part of poly-path ready
_polyLength -= pathStartIndex;
// try to adjust the suffix of the path instead of recalculating entire length
// at given interval the target cannot get too far from its last location
// thus we have less poly to cover
// sub-path of optimal path is optimal
// take ~80% of the original length
/// @todo play with the values here
uint prefixPolyLength = (uint)(_polyLength * 0.8f + 0.5f);
Array.Copy(_pathPolyRefs, pathStartIndex, _pathPolyRefs, 0, prefixPolyLength);
ulong suffixStartPoly = _pathPolyRefs[prefixPolyLength - 1];
// we need any point on our suffix start poly to generate poly-path, so we need last poly in prefix data
float[] suffixEndPoint = new float[3];
bool posOverPoly = false;
if (Detour.dtStatusFailed(_navMeshQuery.closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, ref posOverPoly)))
{
// we can hit offmesh connection as last poly - closestPointOnPoly() don't like that
// try to recover by using prev polyref
--prefixPolyLength;
suffixStartPoly = _pathPolyRefs[prefixPolyLength - 1];
if (Detour.dtStatusFailed(_navMeshQuery.closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, ref posOverPoly)))
{
// suffixStartPoly is still invalid, error state
BuildShortcut();
pathType = PathType.NoPath;
return;
}
}
// generate suffix
uint suffixPolyLength = 0;
uint dtResult;
if (_straightLine)
{
float hit = 0;
float[] hitNormal = new float[3];
dtResult = _navMeshQuery.raycast(
suffixStartPoly,
suffixEndPoint,
endPoint,
_filter,
ref hit,
hitNormal,
_pathPolyRefs,
ref suffixPolyLength,
74 - (int)prefixPolyLength);
// raycast() sets hit to FLT_MAX if there is a ray between start and end
if (hit != float.MaxValue)
{
// the ray hit something, return no path instead of the incomplete one
pathType = PathType.NoPath;
return;
}
}
else
{
dtResult = _navMeshQuery.findPath(
suffixStartPoly, // start polygon
endPoly, // end polygon
suffixEndPoint, // start position
endPoint, // end position
_filter, // polygon search filter
_pathPolyRefs,
ref suffixPolyLength,
74 - (int)prefixPolyLength);
}
if (suffixPolyLength == 0 || Detour.dtStatusFailed(dtResult))
{
// this is probably an error state, but we'll leave it
// and hopefully recover on the next Update
// we still need to copy our preffix
Log.outError(LogFilter.Maps, "{0}'s Path Build failed: 0 length path", _sourceUnit.GetGUID().ToString());
}
Log.outDebug(LogFilter.Maps, "m_polyLength={0} prefixPolyLength={1} suffixPolyLength={2} \n", _polyLength, prefixPolyLength, suffixPolyLength);
// new path = prefix + suffix - overlap
_polyLength = prefixPolyLength + suffixPolyLength - 1;
}
else
{
Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . (!startPolyFound && !endPolyFound)\n");
// either we have no path at all . first run
// or something went really wrong . we aren't moving along the path to the target
// just generate new path
// free and invalidate old path data
Clear();
uint dtResult;
if (_straightLine)
{
float hit = 0;
float[] hitNormal = new float[3];
dtResult = _navMeshQuery.raycast(
startPoly,
startPoint,
endPoint,
_filter,
ref hit,
hitNormal,
_pathPolyRefs,
ref _polyLength,
74);
// raycast() sets hit to FLT_MAX if there is a ray between start and end
if (hit != float.MaxValue)
{
// the ray hit something, return no path instead of the incomplete one
pathType = PathType.NoPath;
return;
}
}
else
{
dtResult = _navMeshQuery.findPath(
startPoly, // start polygon
endPoly, // end polygon
startPoint, // start position
endPoint, // end position
_filter, // polygon search filter
_pathPolyRefs, // [out] path
ref _polyLength,
74); // max number of polygons in output path
}
if (_polyLength == 0 || Detour.dtStatusFailed(dtResult))
{
// only happens if we passed bad data to findPath(), or navmesh is messed up
Log.outError(LogFilter.Maps, "{0}'s Path Build failed: 0 length path", _sourceUnit.GetGUID().ToString());
BuildShortcut();
pathType = PathType.NoPath;
return;
}
}
// by now we know what type of path we can get
if (_pathPolyRefs[_polyLength - 1] == endPoly && !pathType.HasAnyFlag(PathType.Incomplete))
{
pathType = PathType.Normal;
}
else
{
pathType = PathType.Incomplete;
}
// generate the point-path out of our up-to-date poly-path
BuildPointPath(startPoint, endPoint);
}
public static SmoothPath ComputeSmoothPath(Detour.dtNavMeshQuery navQuery, float[] startWorldPos, float[] endWorldPos, float distance = 10)
{
SmoothPath smoothPath = new SmoothPath();
if (navQuery == null)
{
return(smoothPath);
}
float[] extents = new float[3];
for (int i = 0; i < 3; ++i)
{
extents[i] = distance;
}
dtPolyRef startRef = 0;
dtPolyRef endRef = 0;
float[] startPt = new float[3];
float[] endPt = new float[3];
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
navQuery.findNearestPoly(startWorldPos, extents, filter, ref startRef, ref startPt);
navQuery.findNearestPoly(endWorldPos, extents, filter, ref endRef, ref endPt);
const int maxPath = SmoothPath.MAX_POLYS;
dtPolyRef[] path = new dtPolyRef[maxPath];
int pathCount = -1;
navQuery.findPath(startRef, endRef, startPt, endPt, filter, path, ref pathCount, maxPath);
smoothPath.m_nsmoothPath = 0;
if (pathCount > 0)
{
// Iterate over the path to find smooth path on the detail mesh surface.
dtPolyRef[] polys = new dtPolyRef[SmoothPath.MAX_POLYS];
for (int i = 0; i < pathCount; ++i)
{
polys[i] = path[i];
}
int npolys = pathCount;
float[] iterPos = new float[3];
float[] targetPos = new float[3];
bool posOverPoly_dummy = false;
navQuery.closestPointOnPoly(startRef, startPt, iterPos, ref posOverPoly_dummy);
navQuery.closestPointOnPoly(polys[npolys - 1], endPt, targetPos, ref posOverPoly_dummy);
const float STEP_SIZE = 0.5f;
const float SLOP = 0.01f;
smoothPath.m_nsmoothPath = 0;
Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, iterPos, 0);
smoothPath.m_nsmoothPath++;
// Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path.
while (npolys != 0 && smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH)
{
// Find location to steer towards.
float[] steerPos = new float[3];
byte steerPosFlag = 0;
dtPolyRef steerPosRef = 0;
if (!getSteerTarget(navQuery, iterPos, targetPos, SLOP,
polys, npolys, steerPos, ref steerPosFlag, ref steerPosRef))
{
break;
}
bool endOfPath = (steerPosFlag & (byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_END) != 0 ? true : false;
bool offMeshConnection = (steerPosFlag & (byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ? true : false;
// Find movement delta.
float[] delta = new float[3]; //, len;
float len = .0f;
Detour.dtVsub(delta, steerPos, iterPos);
len = (float)Mathf.Sqrt(Detour.dtVdot(delta, delta));
// If the steer target is end of path or off-mesh link, do not move past the location.
if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
{
len = 1;
}
else
{
len = STEP_SIZE / len;
}
float[] moveTgt = new float[3];
Detour.dtVmad(moveTgt, iterPos, delta, len);
// Move
float[] result = new float[3];
dtPolyRef[] visited = new dtPolyRef[16];
int nvisited = 0;
navQuery.moveAlongSurface(polys[0], iterPos, moveTgt, filter,
result, visited, ref nvisited, 16);
npolys = fixupCorridor(polys, npolys, SmoothPath.MAX_POLYS, visited, nvisited);
npolys = fixupShortcuts(polys, npolys, navQuery);
float h = 0;
dtStatus getHeightStatus = navQuery.getPolyHeight(polys[0], result, ref h);
result[1] = h;
if ((getHeightStatus & Detour.DT_FAILURE) != 0)
{
Debug.LogError("Failed to getPolyHeight " + polys[0] + " pos " + result[0] + " " + result[1] + " " + result[2] + " h " + h);
}
Detour.dtVcopy(iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRange(iterPos, 0, steerPos, 0, SLOP, 1.0f))
{
// Reached end of path.
Detour.dtVcopy(iterPos, targetPos);
if (smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH)
{
Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, iterPos, 0);
smoothPath.m_nsmoothPath++;
}
break;
}
else if (offMeshConnection && inRange(iterPos, 0, steerPos, 0, SLOP, 1.0f))
{
// Reached off-mesh connection.
float[] startPos = new float[3]; //, endPos[3];
float[] endPos = new float[3];
// Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = 0, polyRef = polys[0];
int npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (int i = npos; i < npolys; ++i)
{
polys[i - npos] = polys[i];
}
npolys -= npos;
// Handle the connection.
dtStatus status = navQuery.getAttachedNavMesh().getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos);
if (Detour.dtStatusSucceed(status))
{
if (smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH)
{
Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, startPos, 0);
smoothPath.m_nsmoothPath++;
// Hack to make the dotted path not visible during off-mesh connection.
if ((smoothPath.m_nsmoothPath & 1) != 0)
{
Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, startPos, 0);
smoothPath.m_nsmoothPath++;
}
}
// Move position at the other side of the off-mesh link.
Detour.dtVcopy(iterPos, endPos);
float eh = 0.0f;
navQuery.getPolyHeight(polys[0], iterPos, ref eh);
iterPos[1] = eh;
}
}
// Store results.
if (smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH)
{
Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, iterPos, 0);
smoothPath.m_nsmoothPath++;
}
}
}
return(smoothPath);
}
void BuildPointPath(float[] startPoint, float[] endPoint)
{
float[] pathPoints = new float[74 * 3];
int pointCount = 0;
uint dtResult;
if (_straightLine)
{
dtResult = Detour.DT_SUCCESS;
pointCount = 1;
Array.Copy(startPoint, pathPoints, 3); // first point
// path has to be split into polygons with dist SMOOTH_PATH_STEP_SIZE between them
Vector3 startVec = new(startPoint[0], startPoint[1], startPoint[2]);
Vector3 endVec = new(endPoint[0], endPoint[1], endPoint[2]);
Vector3 diffVec = (endVec - startVec);
Vector3 prevVec = startVec;
float len = diffVec.Length();
diffVec *= 4.0f / len;
while (len > 4.0f)
{
len -= 4.0f;
prevVec += diffVec;
pathPoints[3 * pointCount + 0] = prevVec.X;
pathPoints[3 * pointCount + 1] = prevVec.Y;
pathPoints[3 * pointCount + 2] = prevVec.Z;
++pointCount;
}
Array.Copy(endPoint, 0, pathPoints, 3 * pointCount, 3); // last point
++pointCount;
}
else if (_useStraightPath)
{
dtResult = _navMeshQuery.findStraightPath(
startPoint, // start position
endPoint, // end position
_pathPolyRefs,
(int)_polyLength,
pathPoints, // [out] path corner points
null, // [out] flags
null, // [out] shortened path
ref pointCount,
(int)_pointPathLimit,
0); // maximum number of points/polygons to use
}
else
{
dtResult = FindSmoothPath(
startPoint, // start position
endPoint, // end position
_pathPolyRefs, // current path
_polyLength, // length of current path
out pathPoints, // [out] path corner points
out pointCount,
_pointPathLimit); // maximum number of points
}
// Special case with start and end positions very close to each other
if (_polyLength == 1 && pointCount == 1)
{
// First point is start position, append end position
Detour.dtVcopy(pathPoints, 1 * 3, endPoint, 0);
pointCount++;
}
else if (pointCount < 2 || Detour.dtStatusFailed(dtResult))
{
// only happens if pass bad data to findStraightPath or navmesh is broken
// single point paths can be generated here
// @todo check the exact cases
Log.outDebug(LogFilter.Maps, "++ PathGenerator.BuildPointPath FAILED! path sized {0} returned\n", pointCount);
BuildShortcut();
pathType |= PathType.NoPath;
return;
}
else if (pointCount == _pointPathLimit)
{
Log.outDebug(LogFilter.Maps, "++ PathGenerator.BuildPointPath FAILED! path sized {0} returned, lower than limit set to {1}\n", pointCount, _pointPathLimit);
BuildShortcut();
pathType |= PathType.Short;
return;
}
_pathPoints = new Vector3[pointCount];
for (uint i = 0; i < pointCount; ++i)
{
_pathPoints[i] = new Vector3(pathPoints[i * 3 + 2], pathPoints[i * 3], pathPoints[i * 3 + 1]);
}
NormalizePath();
// first point is always our current location - we need the next one
SetActualEndPosition(_pathPoints[pointCount - 1]);
// force the given destination, if needed
if (_forceDestination && (!pathType.HasAnyFlag(PathType.Normal) || !InRange(GetEndPosition(), GetActualEndPosition(), 1.0f, 1.0f)))
{
// we may want to keep partial subpath
if (Dist3DSqr(GetActualEndPosition(), GetEndPosition()) < 0.3f * Dist3DSqr(GetStartPosition(), GetEndPosition()))
{
SetActualEndPosition(GetEndPosition());
_pathPoints[^ 1] = GetEndPosition();
void BuildPointPath(float[] startPoint, float[] endPoint)
{
float[] pathPoints = new float[74 * 3];
int pointCount = 0;
uint dtResult;
if (_useRaycast)
{
// _straightLine uses raycast and it currently doesn't support building a point path, only a 2-point path with start and hitpoint/end is returned
Log.outError(LogFilter.Maps, $"PathGenerator::BuildPointPath() called with _useRaycast for unit {_source.GetGUID()}");
BuildShortcut();
pathType = PathType.NoPath;
return;
}
else if (_useStraightPath)
{
dtResult = _navMeshQuery.findStraightPath(
startPoint, // start position
endPoint, // end position
_pathPolyRefs,
(int)_polyLength,
pathPoints, // [out] path corner points
null, // [out] flags
null, // [out] shortened path
ref pointCount,
(int)_pointPathLimit,
0); // maximum number of points/polygons to use
}
else
{
dtResult = FindSmoothPath(
startPoint, // start position
endPoint, // end position
_pathPolyRefs, // current path
_polyLength, // length of current path
out pathPoints, // [out] path corner points
out pointCount,
_pointPathLimit); // maximum number of points
}
// Special case with start and end positions very close to each other
if (_polyLength == 1 && pointCount == 1)
{
// First point is start position, append end position
Detour.dtVcopy(pathPoints, 1 * 3, endPoint, 0);
pointCount++;
}
else if (pointCount < 2 || Detour.dtStatusFailed(dtResult))
{
// only happens if pass bad data to findStraightPath or navmesh is broken
// single point paths can be generated here
// @todo check the exact cases
Log.outDebug(LogFilter.Maps, "++ PathGenerator.BuildPointPath FAILED! path sized {0} returned\n", pointCount);
BuildShortcut();
pathType |= PathType.NoPath;
return;
}
else if (pointCount == _pointPathLimit)
{
Log.outDebug(LogFilter.Maps, "++ PathGenerator.BuildPointPath FAILED! path sized {0} returned, lower than limit set to {1}\n", pointCount, _pointPathLimit);
BuildShortcut();
pathType |= PathType.Short;
return;
}
_pathPoints = new Vector3[pointCount];
for (uint i = 0; i < pointCount; ++i)
{
_pathPoints[i] = new Vector3(pathPoints[i * 3 + 2], pathPoints[i * 3], pathPoints[i * 3 + 1]);
}
NormalizePath();
// first point is always our current location - we need the next one
SetActualEndPosition(_pathPoints[pointCount - 1]);
// force the given destination, if needed
if (_forceDestination && (!pathType.HasAnyFlag(PathType.Normal) || !InRange(GetEndPosition(), GetActualEndPosition(), 1.0f, 1.0f)))
{
// we may want to keep partial subpath
if (Dist3DSqr(GetActualEndPosition(), GetEndPosition()) < 0.3f * Dist3DSqr(GetStartPosition(), GetEndPosition()))
{
SetActualEndPosition(GetEndPosition());
_pathPoints[^ 1] = GetEndPosition();