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 = _source.IsTypeId(TypeId.Unit) && _source.ToCreature().CanFly();
bool waterPath = _source.IsTypeId(TypeId.Unit) && _source.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 = _source.GetMap().GetLiquidStatus(_source.GetPhaseShift(), _pathPoints[i].X, _pathPoints[i].Y, _pathPoints[i].Z, LiquidHeaderTypeFlags.AllLiquids, _source.GetCollisionHeight());
// 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;
var p = (distToStartPoly > 7.0f) ? startPos : endPos;
if (_source.GetMap().IsUnderWater(_source.GetPhaseShift(), p.X, p.Y, p.Z))
{
Log.outDebug(LogFilter.Maps, "++ BuildPolyPath :: underWater case");
Unit _sourceUnit = _source.ToUnit();
if (_sourceUnit != null)
{
if (_sourceUnit.CanSwim())
{
buildShotrcut = true;
}
}
}
else
{
Log.outDebug(LogFilter.Maps, "++ BuildPolyPath :: flying case");
Unit _sourceUnit = _source.ToUnit();
if (_sourceUnit != null)
{
if (_sourceUnit.CanFly())
{
buildShotrcut = true;
}
// Allow to build a shortcut if the unit is falling and it's trying to move downwards towards a target (i.e. charging)
else if (_sourceUnit.IsFalling() && endPos.Z < startPos.Z)
{
buildShotrcut = true;
}
}
}
if (buildShotrcut)
{
BuildShortcut();
pathType = PathType.Normal | PathType.NotUsingPath | PathType.FarFromPoly;
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 | PathType.FarFromPoly;
}
}
// *** poly path generating logic ***
// start and end are on same polygon
// handle this case as if they were 2 different polygons, building a line path split in some few points
if (startPoly == endPoly)
{
Log.outDebug(LogFilter.Maps, "++ BuildPolyPath . (startPoly == endPoly)\n");
_pathPolyRefs[0] = startPoly;
_polyLength = 1;
pathType = farFromPoly ? PathType.Incomplete | PathType.FarFromPoly : PathType.Normal;
BuildPointPath(startPoint, endPoint);
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, _source.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;
ulong[] tempPolyRefs = new ulong[_pathPolyRefs.Length];
uint dtResult;
if (_straightLine)
{
float hit = 0;
float[] hitNormal = new float[3];
dtResult = _navMeshQuery.raycast(
suffixStartPoly,
suffixEndPoint,
endPoint,
_filter,
ref hit,
hitNormal,
tempPolyRefs,
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
Clear();
_polyLength = 2;
Array.Resize(ref _pathPoints, 2);
_pathPoints[0] = GetStartPosition();
float[] hitPos = new float[3];
Detour.dtVlerp(hitPos, startPoint, endPoint, hit);
_pathPoints[1] = new Vector3(hitPos[2], hitPos[0], hitPos[1]);
pathType = PathType.Incomplete;
return;
}
}
else
{
dtResult = _navMeshQuery.findPath(
suffixStartPoly, // start polygon
endPoly, // end polygon
suffixEndPoint, // start position
endPoint, // end position
_filter, // polygon search filter
tempPolyRefs,
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, $"Path Build failed\n{_source.GetDebugInfo()}");
}
Log.outDebug(LogFilter.Maps, "m_polyLength={0} prefixPolyLength={1} suffixPolyLength={2} \n", _polyLength, prefixPolyLength, suffixPolyLength);
for (var i = 0; i < _pathPolyRefs.Length - (prefixPolyLength - 1); ++i)
{
_pathPolyRefs[(prefixPolyLength - 1) + i] = tempPolyRefs[i];
}
// 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
Clear();
_polyLength = 2;
Array.Resize(ref _pathPoints, 2);
_pathPoints[0] = GetStartPosition();
float[] hitPos = new float[3];
Detour.dtVlerp(hitPos, startPoint, endPoint, hit);
_pathPoints[1] = new Vector3(hitPos[2], hitPos[0], hitPos[1]);
pathType = PathType.Incomplete;
return;
}
else
{
_navMeshQuery.getPolyHeight(_pathPolyRefs[_polyLength - 1], endPoint, ref endPoint[1]);
}
}
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", _source.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;
}
if (farFromPoly)
{
pathType |= PathType.FarFromPoly;
}
// generate the point-path out of our up-to-date poly-path
BuildPointPath(startPoint, endPoint);
}