|
|
|
|
@@ -28,69 +28,69 @@
|
|
|
|
|
|
|
|
|
|
////////////////// PathGenerator //////////////////
|
|
|
|
|
PathGenerator::PathGenerator(const Unit* owner) :
|
|
|
|
|
m_polyLength(0), m_type(PATHFIND_BLANK),
|
|
|
|
|
m_useStraightPath(false), m_forceDestination(false), m_pointPathLimit(MAX_POINT_PATH_LENGTH),
|
|
|
|
|
m_sourceUnit(owner), m_navMesh(NULL), m_navMeshQuery(NULL), m_endPosition(Vector3::zero())
|
|
|
|
|
_polyLength(0), _type(PATHFIND_BLANK),
|
|
|
|
|
_useStraightPath(false), _forceDestination(false), _pointPathLimit(MAX_POINT_PATH_LENGTH),
|
|
|
|
|
_sourceUnit(owner), _navMesh(NULL), _navMeshQuery(NULL), _endPosition(Vector3::zero())
|
|
|
|
|
{
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::PathGenerator for %u \n", m_sourceUnit->GetGUIDLow());
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::PathGenerator for %u \n", _sourceUnit->GetGUIDLow());
|
|
|
|
|
|
|
|
|
|
uint32 mapId = m_sourceUnit->GetMapId();
|
|
|
|
|
uint32 mapId = _sourceUnit->GetMapId();
|
|
|
|
|
if (MMAP::MMapFactory::IsPathfindingEnabled(mapId))
|
|
|
|
|
{
|
|
|
|
|
MMAP::MMapManager* mmap = MMAP::MMapFactory::createOrGetMMapManager();
|
|
|
|
|
m_navMesh = mmap->GetNavMesh(mapId);
|
|
|
|
|
m_navMeshQuery = mmap->GetNavMeshQuery(mapId, m_sourceUnit->GetInstanceId());
|
|
|
|
|
_navMesh = mmap->GetNavMesh(mapId);
|
|
|
|
|
_navMeshQuery = mmap->GetNavMeshQuery(mapId, _sourceUnit->GetInstanceId());
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
createFilter();
|
|
|
|
|
CreateFilter();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
PathGenerator::~PathGenerator()
|
|
|
|
|
{
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::~PathGenerator() for %u \n", m_sourceUnit->GetGUIDLow());
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::~PathGenerator() for %u \n", _sourceUnit->GetGUIDLow());
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool PathGenerator::CalculatePath(float destX, float destY, float destZ, bool forceDest)
|
|
|
|
|
{
|
|
|
|
|
float x, y, z;
|
|
|
|
|
m_sourceUnit->GetPosition(x, y, z);
|
|
|
|
|
_sourceUnit->GetPosition(x, y, z);
|
|
|
|
|
|
|
|
|
|
if (!Trinity::IsValidMapCoord(destX, destY, destZ) || !Trinity::IsValidMapCoord(x, y, z))
|
|
|
|
|
return false;
|
|
|
|
|
|
|
|
|
|
Vector3 oldDest = getEndPosition();
|
|
|
|
|
Vector3 oldDest = GetEndPosition();
|
|
|
|
|
Vector3 dest(destX, destY, destZ);
|
|
|
|
|
setEndPosition(dest);
|
|
|
|
|
SetEndPosition(dest);
|
|
|
|
|
|
|
|
|
|
Vector3 start(x, y, z);
|
|
|
|
|
setStartPosition(start);
|
|
|
|
|
SetStartPosition(start);
|
|
|
|
|
|
|
|
|
|
m_forceDestination = forceDest;
|
|
|
|
|
_forceDestination = forceDest;
|
|
|
|
|
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::CalculatePath() for %u \n", m_sourceUnit->GetGUIDLow());
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::CalculatePath() for %u \n", _sourceUnit->GetGUIDLow());
|
|
|
|
|
|
|
|
|
|
// make sure navMesh works - we can run on map w/o mmap
|
|
|
|
|
// check if the start and end point have a .mmtile loaded (can we pass via not loaded tile on the way?)
|
|
|
|
|
if (!m_navMesh || !m_navMeshQuery || m_sourceUnit->HasUnitState(UNIT_STATE_IGNORE_PATHFINDING) ||
|
|
|
|
|
if (!_navMesh || !_navMeshQuery || _sourceUnit->HasUnitState(UNIT_STATE_IGNORE_PATHFINDING) ||
|
|
|
|
|
!HaveTile(start) || !HaveTile(dest))
|
|
|
|
|
{
|
|
|
|
|
BuildShortcut();
|
|
|
|
|
m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
|
|
|
|
|
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
updateFilter();
|
|
|
|
|
UpdateFilter();
|
|
|
|
|
|
|
|
|
|
// check if destination moved - if not we can optimize something here
|
|
|
|
|
// we are following old, precalculated path?
|
|
|
|
|
float dist = m_sourceUnit->GetObjectSize();
|
|
|
|
|
if (oldDest != Vector3::zero() && inRange(oldDest, dest, dist, dist) && m_pathPoints.size() > 2)
|
|
|
|
|
float dist = _sourceUnit->GetObjectSize();
|
|
|
|
|
if (oldDest != Vector3::zero() && InRange(oldDest, dest, dist, dist) && _pathPoints.size() > 2)
|
|
|
|
|
{
|
|
|
|
|
// our target is not moving - we just coming closer
|
|
|
|
|
// we are moving on precalculated path - enjoy the ride
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::CalculatePath:: precalculated path\n");
|
|
|
|
|
|
|
|
|
|
m_pathPoints.erase(m_pathPoints.begin());
|
|
|
|
|
_pathPoints.erase(_pathPoints.begin());
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
@@ -101,7 +101,7 @@ bool PathGenerator::CalculatePath(float destX, float destY, float destZ, bool fo
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
dtPolyRef PathGenerator::getPathPolyByPosition(const dtPolyRef *polyPath, uint32 polyPathSize, const float* point, float *distance) const
|
|
|
|
|
dtPolyRef PathGenerator::GetPathPolyByPosition(dtPolyRef const* polyPath, uint32 polyPathSize, float const* point, float* distance) const
|
|
|
|
|
{
|
|
|
|
|
if (!polyPath || !polyPathSize)
|
|
|
|
|
return INVALID_POLYREF;
|
|
|
|
|
@@ -113,7 +113,7 @@ dtPolyRef PathGenerator::getPathPolyByPosition(const dtPolyRef *polyPath, uint32
|
|
|
|
|
for (uint32 i = 0; i < polyPathSize; ++i)
|
|
|
|
|
{
|
|
|
|
|
float closestPoint[VERTEX_SIZE];
|
|
|
|
|
if (DT_SUCCESS != m_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint))
|
|
|
|
|
if (DT_SUCCESS != _navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint))
|
|
|
|
|
continue;
|
|
|
|
|
|
|
|
|
|
float d = dtVdist2DSqr(point, closestPoint);
|
|
|
|
|
@@ -134,12 +134,12 @@ dtPolyRef PathGenerator::getPathPolyByPosition(const dtPolyRef *polyPath, uint32
|
|
|
|
|
return (minDist2d < 3.0f) ? nearestPoly : INVALID_POLYREF;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
dtPolyRef PathGenerator::getPolyByLocation(const float* point, float *distance) const
|
|
|
|
|
dtPolyRef PathGenerator::GetPolyByLocation(float const* point, float* distance) const
|
|
|
|
|
{
|
|
|
|
|
// first we check the current path
|
|
|
|
|
// if the current path doesn't contain the current poly,
|
|
|
|
|
// we need to use the expensive navMesh.findNearestPoly
|
|
|
|
|
dtPolyRef polyRef = getPathPolyByPosition(m_pathPolyRefs, m_polyLength, point, distance);
|
|
|
|
|
dtPolyRef polyRef = GetPathPolyByPosition(_pathPolyRefs, _polyLength, point, distance);
|
|
|
|
|
if (polyRef != INVALID_POLYREF)
|
|
|
|
|
return polyRef;
|
|
|
|
|
|
|
|
|
|
@@ -148,7 +148,7 @@ dtPolyRef PathGenerator::getPolyByLocation(const float* point, float *distance)
|
|
|
|
|
// first try with low search box
|
|
|
|
|
float extents[VERTEX_SIZE] = {3.0f, 5.0f, 3.0f}; // bounds of poly search area
|
|
|
|
|
float closestPoint[VERTEX_SIZE] = {0.0f, 0.0f, 0.0f};
|
|
|
|
|
dtStatus result = m_navMeshQuery->findNearestPoly(point, extents, &m_filter, &polyRef, closestPoint);
|
|
|
|
|
dtStatus result = _navMeshQuery->findNearestPoly(point, extents, &_filter, &polyRef, closestPoint);
|
|
|
|
|
if (DT_SUCCESS == result && polyRef != INVALID_POLYREF)
|
|
|
|
|
{
|
|
|
|
|
*distance = dtVdist(closestPoint, point);
|
|
|
|
|
@@ -158,7 +158,7 @@ dtPolyRef PathGenerator::getPolyByLocation(const float* point, float *distance)
|
|
|
|
|
// still nothing ..
|
|
|
|
|
// try with bigger search box
|
|
|
|
|
extents[1] = 200.0f;
|
|
|
|
|
result = m_navMeshQuery->findNearestPoly(point, extents, &m_filter, &polyRef, closestPoint);
|
|
|
|
|
result = _navMeshQuery->findNearestPoly(point, extents, &_filter, &polyRef, closestPoint);
|
|
|
|
|
if (DT_SUCCESS == result && polyRef != INVALID_POLYREF)
|
|
|
|
|
{
|
|
|
|
|
*distance = dtVdist(closestPoint, point);
|
|
|
|
|
@@ -168,7 +168,7 @@ dtPolyRef PathGenerator::getPolyByLocation(const float* point, float *distance)
|
|
|
|
|
return INVALID_POLYREF;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos)
|
|
|
|
|
void PathGenerator::BuildPolyPath(Vector3 const& startPos, Vector3 const& endPos)
|
|
|
|
|
{
|
|
|
|
|
// *** getting start/end poly logic ***
|
|
|
|
|
|
|
|
|
|
@@ -176,8 +176,8 @@ void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos
|
|
|
|
|
float startPoint[VERTEX_SIZE] = {startPos.y, startPos.z, startPos.x};
|
|
|
|
|
float endPoint[VERTEX_SIZE] = {endPos.y, endPos.z, endPos.x};
|
|
|
|
|
|
|
|
|
|
dtPolyRef startPoly = getPolyByLocation(startPoint, &distToStartPoly);
|
|
|
|
|
dtPolyRef endPoly = getPolyByLocation(endPoint, &distToEndPoly);
|
|
|
|
|
dtPolyRef startPoly = GetPolyByLocation(startPoint, &distToStartPoly);
|
|
|
|
|
dtPolyRef endPoly = GetPolyByLocation(endPoint, &distToEndPoly);
|
|
|
|
|
|
|
|
|
|
// we have a hole in our mesh
|
|
|
|
|
// make shortcut path and mark it as NOPATH ( with flying and swimming exception )
|
|
|
|
|
@@ -186,15 +186,15 @@ void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos
|
|
|
|
|
{
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ BuildPolyPath :: (startPoly == 0 || endPoly == 0)\n");
|
|
|
|
|
BuildShortcut();
|
|
|
|
|
bool path = m_sourceUnit->GetTypeId() == TYPEID_UNIT && m_sourceUnit->ToCreature()->CanFly();
|
|
|
|
|
bool path = _sourceUnit->GetTypeId() == TYPEID_UNIT && _sourceUnit->ToCreature()->CanFly();
|
|
|
|
|
|
|
|
|
|
bool waterPath = m_sourceUnit->GetTypeId() == TYPEID_UNIT && m_sourceUnit->ToCreature()->canSwim();
|
|
|
|
|
bool waterPath = _sourceUnit->GetTypeId() == 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 (uint32 i = 0; i < m_pathPoints.size(); ++i)
|
|
|
|
|
for (uint32 i = 0; i < _pathPoints.size(); ++i)
|
|
|
|
|
{
|
|
|
|
|
ZLiquidStatus status = m_sourceUnit->GetBaseMap()->getLiquidStatus(m_pathPoints[i].x, m_pathPoints[i].y, m_pathPoints[i].z, MAP_ALL_LIQUIDS, NULL);
|
|
|
|
|
ZLiquidStatus status = _sourceUnit->GetBaseMap()->getLiquidStatus(_pathPoints[i].x, _pathPoints[i].y, _pathPoints[i].z, MAP_ALL_LIQUIDS, NULL);
|
|
|
|
|
// One of the points is not in the water, cancel movement.
|
|
|
|
|
if (status == LIQUID_MAP_NO_WATER)
|
|
|
|
|
{
|
|
|
|
|
@@ -204,7 +204,7 @@ void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
m_type = (path || waterPath) ? PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH) : PATHFIND_NOPATH;
|
|
|
|
|
_type = (path || waterPath) ? PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH) : PATHFIND_NOPATH;
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@@ -215,12 +215,12 @@ void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ BuildPolyPath :: farFromPoly distToStartPoly=%.3f distToEndPoly=%.3f\n", distToStartPoly, distToEndPoly);
|
|
|
|
|
|
|
|
|
|
bool buildShotrcut = false;
|
|
|
|
|
if (m_sourceUnit->GetTypeId() == TYPEID_UNIT)
|
|
|
|
|
if (_sourceUnit->GetTypeId() == TYPEID_UNIT)
|
|
|
|
|
{
|
|
|
|
|
Creature* owner = (Creature*)m_sourceUnit;
|
|
|
|
|
Creature* owner = (Creature*)_sourceUnit;
|
|
|
|
|
|
|
|
|
|
Vector3 p = (distToStartPoly > 7.0f) ? startPos : endPos;
|
|
|
|
|
if (m_sourceUnit->GetBaseMap()->IsUnderWater(p.x, p.y, p.z))
|
|
|
|
|
if (_sourceUnit->GetBaseMap()->IsUnderWater(p.x, p.y, p.z))
|
|
|
|
|
{
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ BuildPolyPath :: underWater case\n");
|
|
|
|
|
if (owner->canSwim())
|
|
|
|
|
@@ -237,20 +237,20 @@ void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos
|
|
|
|
|
if (buildShotrcut)
|
|
|
|
|
{
|
|
|
|
|
BuildShortcut();
|
|
|
|
|
m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
|
|
|
|
|
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
float closestPoint[VERTEX_SIZE];
|
|
|
|
|
// we may want to use closestPointOnPolyBoundary instead
|
|
|
|
|
if (DT_SUCCESS == m_navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint))
|
|
|
|
|
if (DT_SUCCESS == _navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint))
|
|
|
|
|
{
|
|
|
|
|
dtVcopy(endPoint, closestPoint);
|
|
|
|
|
setActualEndPosition(Vector3(endPoint[2],endPoint[0],endPoint[1]));
|
|
|
|
|
SetActualEndPosition(Vector3(endPoint[2], endPoint[0], endPoint[1]));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
m_type = PATHFIND_INCOMPLETE;
|
|
|
|
|
_type = PATHFIND_INCOMPLETE;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@@ -264,11 +264,11 @@ void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos
|
|
|
|
|
|
|
|
|
|
BuildShortcut();
|
|
|
|
|
|
|
|
|
|
m_pathPolyRefs[0] = startPoly;
|
|
|
|
|
m_polyLength = 1;
|
|
|
|
|
_pathPolyRefs[0] = startPoly;
|
|
|
|
|
_polyLength = 1;
|
|
|
|
|
|
|
|
|
|
m_type = farFromPoly ? PATHFIND_INCOMPLETE : PATHFIND_NORMAL;
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ BuildPolyPath :: path type %d\n", m_type);
|
|
|
|
|
_type = farFromPoly ? PATHFIND_INCOMPLETE : PATHFIND_NORMAL;
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ BuildPolyPath :: path type %d\n", _type);
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@@ -279,22 +279,22 @@ void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos
|
|
|
|
|
uint32 pathStartIndex = 0;
|
|
|
|
|
uint32 pathEndIndex = 0;
|
|
|
|
|
|
|
|
|
|
if (m_polyLength)
|
|
|
|
|
if (_polyLength)
|
|
|
|
|
{
|
|
|
|
|
for (; pathStartIndex < m_polyLength; ++pathStartIndex)
|
|
|
|
|
for (; pathStartIndex < _polyLength; ++pathStartIndex)
|
|
|
|
|
{
|
|
|
|
|
// here to carch few bugs
|
|
|
|
|
ASSERT(m_pathPolyRefs[pathStartIndex] != INVALID_POLYREF);
|
|
|
|
|
ASSERT(_pathPolyRefs[pathStartIndex] != INVALID_POLYREF);
|
|
|
|
|
|
|
|
|
|
if (m_pathPolyRefs[pathStartIndex] == startPoly)
|
|
|
|
|
if (_pathPolyRefs[pathStartIndex] == startPoly)
|
|
|
|
|
{
|
|
|
|
|
startPolyFound = true;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
for (pathEndIndex = m_polyLength-1; pathEndIndex > pathStartIndex; --pathEndIndex)
|
|
|
|
|
if (m_pathPolyRefs[pathEndIndex] == endPoly)
|
|
|
|
|
for (pathEndIndex = _polyLength-1; pathEndIndex > pathStartIndex; --pathEndIndex)
|
|
|
|
|
if (_pathPolyRefs[pathEndIndex] == endPoly)
|
|
|
|
|
{
|
|
|
|
|
endPolyFound = true;
|
|
|
|
|
break;
|
|
|
|
|
@@ -309,8 +309,8 @@ void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos
|
|
|
|
|
// our path is a simple subpath case, we have all the data we need
|
|
|
|
|
// just "cut" it out
|
|
|
|
|
|
|
|
|
|
m_polyLength = pathEndIndex - pathStartIndex + 1;
|
|
|
|
|
memmove(m_pathPolyRefs, m_pathPolyRefs+pathStartIndex, m_polyLength*sizeof(dtPolyRef));
|
|
|
|
|
_polyLength = pathEndIndex - pathStartIndex + 1;
|
|
|
|
|
memmove(_pathPolyRefs, _pathPolyRefs + pathStartIndex, _polyLength * sizeof(dtPolyRef));
|
|
|
|
|
}
|
|
|
|
|
else if (startPolyFound && !endPolyFound)
|
|
|
|
|
{
|
|
|
|
|
@@ -319,7 +319,7 @@ void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos
|
|
|
|
|
// we are moving on the old path but target moved out
|
|
|
|
|
// so we have atleast part of poly-path ready
|
|
|
|
|
|
|
|
|
|
m_polyLength -= pathStartIndex;
|
|
|
|
|
_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
|
|
|
|
|
@@ -328,37 +328,37 @@ void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos
|
|
|
|
|
|
|
|
|
|
// take ~80% of the original length
|
|
|
|
|
// TODO : play with the values here
|
|
|
|
|
uint32 prefixPolyLength = uint32(m_polyLength*0.8f + 0.5f);
|
|
|
|
|
memmove(m_pathPolyRefs, m_pathPolyRefs+pathStartIndex, prefixPolyLength*sizeof(dtPolyRef));
|
|
|
|
|
uint32 prefixPolyLength = uint32(_polyLength * 0.8f + 0.5f);
|
|
|
|
|
memmove(_pathPolyRefs, _pathPolyRefs+pathStartIndex, prefixPolyLength * sizeof(dtPolyRef));
|
|
|
|
|
|
|
|
|
|
dtPolyRef suffixStartPoly = m_pathPolyRefs[prefixPolyLength-1];
|
|
|
|
|
dtPolyRef 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[VERTEX_SIZE];
|
|
|
|
|
if (DT_SUCCESS != m_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint))
|
|
|
|
|
if (DT_SUCCESS != _navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint))
|
|
|
|
|
{
|
|
|
|
|
// we can hit offmesh connection as last poly - closestPointOnPoly() don't like that
|
|
|
|
|
// try to recover by using prev polyref
|
|
|
|
|
--prefixPolyLength;
|
|
|
|
|
suffixStartPoly = m_pathPolyRefs[prefixPolyLength-1];
|
|
|
|
|
if (DT_SUCCESS != m_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint))
|
|
|
|
|
suffixStartPoly = _pathPolyRefs[prefixPolyLength-1];
|
|
|
|
|
if (DT_SUCCESS != _navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint))
|
|
|
|
|
{
|
|
|
|
|
// suffixStartPoly is still invalid, error state
|
|
|
|
|
BuildShortcut();
|
|
|
|
|
m_type = PATHFIND_NOPATH;
|
|
|
|
|
_type = PATHFIND_NOPATH;
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// generate suffix
|
|
|
|
|
uint32 suffixPolyLength = 0;
|
|
|
|
|
dtStatus dtResult = m_navMeshQuery->findPath(
|
|
|
|
|
dtStatus dtResult = _navMeshQuery->findPath(
|
|
|
|
|
suffixStartPoly, // start polygon
|
|
|
|
|
endPoly, // end polygon
|
|
|
|
|
suffixEndPoint, // start position
|
|
|
|
|
endPoint, // end position
|
|
|
|
|
&m_filter, // polygon search filter
|
|
|
|
|
m_pathPolyRefs + prefixPolyLength - 1, // [out] path
|
|
|
|
|
&_filter, // polygon search filter
|
|
|
|
|
_pathPolyRefs + prefixPolyLength - 1, // [out] path
|
|
|
|
|
(int*)&suffixPolyLength,
|
|
|
|
|
MAX_PATH_LENGTH-prefixPolyLength); // max number of polygons in output path
|
|
|
|
|
|
|
|
|
|
@@ -367,13 +367,13 @@ void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos
|
|
|
|
|
// 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
|
|
|
|
|
sLog->outError(LOG_FILTER_MAPS, "%u's Path Build failed: 0 length path", m_sourceUnit->GetGUIDLow());
|
|
|
|
|
sLog->outError(LOG_FILTER_MAPS, "%u's Path Build failed: 0 length path", _sourceUnit->GetGUIDLow());
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ m_polyLength=%u prefixPolyLength=%u suffixPolyLength=%u \n",m_polyLength, prefixPolyLength, suffixPolyLength);
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ m_polyLength=%u prefixPolyLength=%u suffixPolyLength=%u \n", _polyLength, prefixPolyLength, suffixPolyLength);
|
|
|
|
|
|
|
|
|
|
// new path = prefix + suffix - overlap
|
|
|
|
|
m_polyLength = prefixPolyLength + suffixPolyLength - 1;
|
|
|
|
|
_polyLength = prefixPolyLength + suffixPolyLength - 1;
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
@@ -384,33 +384,33 @@ void PathGenerator::BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos
|
|
|
|
|
// just generate new path
|
|
|
|
|
|
|
|
|
|
// free and invalidate old path data
|
|
|
|
|
clear();
|
|
|
|
|
Clear();
|
|
|
|
|
|
|
|
|
|
dtStatus dtResult = m_navMeshQuery->findPath(
|
|
|
|
|
dtStatus dtResult = _navMeshQuery->findPath(
|
|
|
|
|
startPoly, // start polygon
|
|
|
|
|
endPoly, // end polygon
|
|
|
|
|
startPoint, // start position
|
|
|
|
|
endPoint, // end position
|
|
|
|
|
&m_filter, // polygon search filter
|
|
|
|
|
m_pathPolyRefs, // [out] path
|
|
|
|
|
(int*)&m_polyLength,
|
|
|
|
|
&_filter, // polygon search filter
|
|
|
|
|
_pathPolyRefs, // [out] path
|
|
|
|
|
(int*)&_polyLength,
|
|
|
|
|
MAX_PATH_LENGTH); // max number of polygons in output path
|
|
|
|
|
|
|
|
|
|
if (!m_polyLength || dtResult != DT_SUCCESS)
|
|
|
|
|
if (!_polyLength || dtResult != DT_SUCCESS)
|
|
|
|
|
{
|
|
|
|
|
// only happens if we passed bad data to findPath(), or navmesh is messed up
|
|
|
|
|
sLog->outError(LOG_FILTER_MAPS, "%u's Path Build failed: 0 length path", m_sourceUnit->GetGUIDLow());
|
|
|
|
|
sLog->outError(LOG_FILTER_MAPS, "%u's Path Build failed: 0 length path", _sourceUnit->GetGUIDLow());
|
|
|
|
|
BuildShortcut();
|
|
|
|
|
m_type = PATHFIND_NOPATH;
|
|
|
|
|
_type = PATHFIND_NOPATH;
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// by now we know what type of path we can get
|
|
|
|
|
if (m_pathPolyRefs[m_polyLength - 1] == endPoly && !(m_type & PATHFIND_INCOMPLETE))
|
|
|
|
|
m_type = PATHFIND_NORMAL;
|
|
|
|
|
if (_pathPolyRefs[_polyLength - 1] == endPoly && !(_type & PATHFIND_INCOMPLETE))
|
|
|
|
|
_type = PATHFIND_NORMAL;
|
|
|
|
|
else
|
|
|
|
|
m_type = PATHFIND_INCOMPLETE;
|
|
|
|
|
_type = PATHFIND_INCOMPLETE;
|
|
|
|
|
|
|
|
|
|
// generate the point-path out of our up-to-date poly-path
|
|
|
|
|
BuildPointPath(startPoint, endPoint);
|
|
|
|
|
@@ -421,29 +421,29 @@ void PathGenerator::BuildPointPath(const float *startPoint, const float *endPoin
|
|
|
|
|
float pathPoints[MAX_POINT_PATH_LENGTH*VERTEX_SIZE];
|
|
|
|
|
uint32 pointCount = 0;
|
|
|
|
|
dtStatus dtResult = DT_FAILURE;
|
|
|
|
|
if (m_useStraightPath)
|
|
|
|
|
if (_useStraightPath)
|
|
|
|
|
{
|
|
|
|
|
dtResult = m_navMeshQuery->findStraightPath(
|
|
|
|
|
dtResult = _navMeshQuery->findStraightPath(
|
|
|
|
|
startPoint, // start position
|
|
|
|
|
endPoint, // end position
|
|
|
|
|
m_pathPolyRefs, // current path
|
|
|
|
|
m_polyLength, // lenth of current path
|
|
|
|
|
_pathPolyRefs, // current path
|
|
|
|
|
_polyLength, // lenth of current path
|
|
|
|
|
pathPoints, // [out] path corner points
|
|
|
|
|
NULL, // [out] flags
|
|
|
|
|
NULL, // [out] shortened path
|
|
|
|
|
(int*)&pointCount,
|
|
|
|
|
m_pointPathLimit); // maximum number of points/polygons to use
|
|
|
|
|
_pointPathLimit); // maximum number of points/polygons to use
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
dtResult = findSmoothPath(
|
|
|
|
|
dtResult = FindSmoothPath(
|
|
|
|
|
startPoint, // start position
|
|
|
|
|
endPoint, // end position
|
|
|
|
|
m_pathPolyRefs, // current path
|
|
|
|
|
m_polyLength, // length of current path
|
|
|
|
|
_pathPolyRefs, // current path
|
|
|
|
|
_polyLength, // length of current path
|
|
|
|
|
pathPoints, // [out] path corner points
|
|
|
|
|
(int*)&pointCount,
|
|
|
|
|
m_pointPathLimit); // maximum number of points
|
|
|
|
|
_pointPathLimit); // maximum number of points
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (pointCount < 2 || dtResult != DT_SUCCESS)
|
|
|
|
|
@@ -453,81 +453,80 @@ void PathGenerator::BuildPointPath(const float *startPoint, const float *endPoin
|
|
|
|
|
// TODO : check the exact cases
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::BuildPointPath FAILED! path sized %d returned\n", pointCount);
|
|
|
|
|
BuildShortcut();
|
|
|
|
|
m_type = PATHFIND_NOPATH;
|
|
|
|
|
_type = PATHFIND_NOPATH;
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
else if (pointCount == m_pointPathLimit)
|
|
|
|
|
else if (pointCount == _pointPathLimit)
|
|
|
|
|
{
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::BuildPointPath FAILED! path sized %d returned, lower than limit set to %d\n", pointCount, m_pointPathLimit);
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::BuildPointPath FAILED! path sized %d returned, lower than limit set to %d\n", pointCount, _pointPathLimit);
|
|
|
|
|
BuildShortcut();
|
|
|
|
|
m_type = PATHFIND_SHORT;
|
|
|
|
|
_type = PATHFIND_SHORT;
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
m_pathPoints.resize(pointCount);
|
|
|
|
|
_pathPoints.resize(pointCount);
|
|
|
|
|
for (uint32 i = 0; i < pointCount; ++i)
|
|
|
|
|
m_pathPoints[i] = Vector3(pathPoints[i*VERTEX_SIZE+2], pathPoints[i*VERTEX_SIZE], pathPoints[i*VERTEX_SIZE+1]);
|
|
|
|
|
_pathPoints[i] = Vector3(pathPoints[i*VERTEX_SIZE+2], pathPoints[i*VERTEX_SIZE], pathPoints[i*VERTEX_SIZE+1]);
|
|
|
|
|
|
|
|
|
|
NormalizePath();
|
|
|
|
|
|
|
|
|
|
// first point is always our current location - we need the next one
|
|
|
|
|
setActualEndPosition(m_pathPoints[pointCount-1]);
|
|
|
|
|
SetActualEndPosition(_pathPoints[pointCount-1]);
|
|
|
|
|
|
|
|
|
|
// force the given destination, if needed
|
|
|
|
|
if(m_forceDestination &&
|
|
|
|
|
(!(m_type & PATHFIND_NORMAL) || !inRange(getEndPosition(), getActualEndPosition(), 1.0f, 1.0f)))
|
|
|
|
|
if (_forceDestination &&
|
|
|
|
|
(!(_type & PATHFIND_NORMAL) || !InRange(GetEndPosition(), GetActualEndPosition(), 1.0f, 1.0f)))
|
|
|
|
|
{
|
|
|
|
|
// we may want to keep partial subpath
|
|
|
|
|
if(dist3DSqr(getActualEndPosition(), getEndPosition()) <
|
|
|
|
|
0.3f * dist3DSqr(getStartPosition(), getEndPosition()))
|
|
|
|
|
if (Dist3DSqr(GetActualEndPosition(), GetEndPosition()) < 0.3f * Dist3DSqr(GetStartPosition(), GetEndPosition()))
|
|
|
|
|
{
|
|
|
|
|
setActualEndPosition(getEndPosition());
|
|
|
|
|
m_pathPoints[m_pathPoints.size()-1] = getEndPosition();
|
|
|
|
|
SetActualEndPosition(GetEndPosition());
|
|
|
|
|
_pathPoints[_pathPoints.size()-1] = GetEndPosition();
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
setActualEndPosition(getEndPosition());
|
|
|
|
|
SetActualEndPosition(GetEndPosition());
|
|
|
|
|
BuildShortcut();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
|
|
|
|
|
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::BuildPointPath path type %d size %d poly-size %d\n", m_type, pointCount, m_polyLength);
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::BuildPointPath path type %d size %d poly-size %d\n", _type, pointCount, _polyLength);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void PathGenerator::NormalizePath()
|
|
|
|
|
{
|
|
|
|
|
for (uint32 i = 0; i < m_pathPoints.size(); ++i)
|
|
|
|
|
m_sourceUnit->UpdateAllowedPositionZ(m_pathPoints[i].x, m_pathPoints[i].y, m_pathPoints[i].z);
|
|
|
|
|
for (uint32 i = 0; i < _pathPoints.size(); ++i)
|
|
|
|
|
_sourceUnit->UpdateAllowedPositionZ(_pathPoints[i].x, _pathPoints[i].y, _pathPoints[i].z);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void PathGenerator::BuildShortcut()
|
|
|
|
|
{
|
|
|
|
|
sLog->outDebug(LOG_FILTER_MAPS, "++ BuildShortcut :: making shortcut\n");
|
|
|
|
|
|
|
|
|
|
clear();
|
|
|
|
|
Clear();
|
|
|
|
|
|
|
|
|
|
// make two point path, our curr pos is the start, and dest is the end
|
|
|
|
|
m_pathPoints.resize(2);
|
|
|
|
|
_pathPoints.resize(2);
|
|
|
|
|
|
|
|
|
|
// set start and a default next position
|
|
|
|
|
m_pathPoints[0] = getStartPosition();
|
|
|
|
|
m_pathPoints[1] = getActualEndPosition();
|
|
|
|
|
_pathPoints[0] = GetStartPosition();
|
|
|
|
|
_pathPoints[1] = GetActualEndPosition();
|
|
|
|
|
|
|
|
|
|
NormalizePath();
|
|
|
|
|
|
|
|
|
|
m_type = PATHFIND_SHORTCUT;
|
|
|
|
|
_type = PATHFIND_SHORTCUT;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void PathGenerator::createFilter()
|
|
|
|
|
void PathGenerator::CreateFilter()
|
|
|
|
|
{
|
|
|
|
|
uint16 includeFlags = 0;
|
|
|
|
|
uint16 excludeFlags = 0;
|
|
|
|
|
|
|
|
|
|
if (m_sourceUnit->GetTypeId() == TYPEID_UNIT)
|
|
|
|
|
if (_sourceUnit->GetTypeId() == TYPEID_UNIT)
|
|
|
|
|
{
|
|
|
|
|
Creature* creature = (Creature*)m_sourceUnit;
|
|
|
|
|
Creature* creature = (Creature*)_sourceUnit;
|
|
|
|
|
if (creature->canWalk())
|
|
|
|
|
includeFlags |= NAV_GROUND; // walk
|
|
|
|
|
|
|
|
|
|
@@ -535,37 +534,37 @@ void PathGenerator::createFilter()
|
|
|
|
|
if (creature->canSwim())
|
|
|
|
|
includeFlags |= (NAV_WATER | NAV_MAGMA | NAV_SLIME); // swim
|
|
|
|
|
}
|
|
|
|
|
else if (m_sourceUnit->GetTypeId() == TYPEID_PLAYER)
|
|
|
|
|
else if (_sourceUnit->GetTypeId() == TYPEID_PLAYER)
|
|
|
|
|
{
|
|
|
|
|
// perfect support not possible, just stay 'safe'
|
|
|
|
|
includeFlags |= (NAV_GROUND | NAV_WATER);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
m_filter.setIncludeFlags(includeFlags);
|
|
|
|
|
m_filter.setExcludeFlags(excludeFlags);
|
|
|
|
|
_filter.setIncludeFlags(includeFlags);
|
|
|
|
|
_filter.setExcludeFlags(excludeFlags);
|
|
|
|
|
|
|
|
|
|
updateFilter();
|
|
|
|
|
UpdateFilter();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void PathGenerator::updateFilter()
|
|
|
|
|
void PathGenerator::UpdateFilter()
|
|
|
|
|
{
|
|
|
|
|
// allow creatures to cheat and use different movement types if they are moved
|
|
|
|
|
// forcefully into terrain they can't normally move in
|
|
|
|
|
if (m_sourceUnit->IsInWater() || m_sourceUnit->IsUnderWater())
|
|
|
|
|
if (_sourceUnit->IsInWater() || _sourceUnit->IsUnderWater())
|
|
|
|
|
{
|
|
|
|
|
uint16 includedFlags = m_filter.getIncludeFlags();
|
|
|
|
|
includedFlags |= getNavTerrain(m_sourceUnit->GetPositionX(),
|
|
|
|
|
m_sourceUnit->GetPositionY(),
|
|
|
|
|
m_sourceUnit->GetPositionZ());
|
|
|
|
|
uint16 includedFlags = _filter.getIncludeFlags();
|
|
|
|
|
includedFlags |= GetNavTerrain(_sourceUnit->GetPositionX(),
|
|
|
|
|
_sourceUnit->GetPositionY(),
|
|
|
|
|
_sourceUnit->GetPositionZ());
|
|
|
|
|
|
|
|
|
|
m_filter.setIncludeFlags(includedFlags);
|
|
|
|
|
_filter.setIncludeFlags(includedFlags);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
NavTerrain PathGenerator::getNavTerrain(float x, float y, float z)
|
|
|
|
|
NavTerrain PathGenerator::GetNavTerrain(float x, float y, float z)
|
|
|
|
|
{
|
|
|
|
|
LiquidData data;
|
|
|
|
|
m_sourceUnit->GetBaseMap()->getLiquidStatus(x, y, z, MAP_ALL_LIQUIDS, &data);
|
|
|
|
|
_sourceUnit->GetBaseMap()->getLiquidStatus(x, y, z, MAP_ALL_LIQUIDS, &data);
|
|
|
|
|
|
|
|
|
|
switch (data.type_flags)
|
|
|
|
|
{
|
|
|
|
|
@@ -581,16 +580,16 @@ NavTerrain PathGenerator::getNavTerrain(float x, float y, float z)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool PathGenerator::HaveTile(const Vector3 &p) const
|
|
|
|
|
bool PathGenerator::HaveTile(Vector3 const& p) const
|
|
|
|
|
{
|
|
|
|
|
int tx, ty;
|
|
|
|
|
float point[VERTEX_SIZE] = {p.y, p.z, p.x};
|
|
|
|
|
|
|
|
|
|
m_navMesh->calcTileLoc(point, &tx, &ty);
|
|
|
|
|
return (m_navMesh->getTileAt(tx, ty) != NULL);
|
|
|
|
|
_navMesh->calcTileLoc(point, &tx, &ty);
|
|
|
|
|
return (_navMesh->getTileAt(tx, ty) != NULL);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
uint32 PathGenerator::fixupCorridor(dtPolyRef* path, uint32 npath, uint32 maxPath, dtPolyRef const* visited, uint32 nvisited)
|
|
|
|
|
uint32 PathGenerator::FixupCorridor(dtPolyRef* path, uint32 npath, uint32 maxPath, dtPolyRef const* visited, uint32 nvisited)
|
|
|
|
|
{
|
|
|
|
|
int32 furthestPath = -1;
|
|
|
|
|
int32 furthestVisited = -1;
|
|
|
|
|
@@ -635,8 +634,8 @@ uint32 PathGenerator::fixupCorridor(dtPolyRef* path, uint32 npath, uint32 maxPat
|
|
|
|
|
return req+size;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool PathGenerator::getSteerTarget(const float* startPos, const float* endPos,
|
|
|
|
|
float minTargetDist, const dtPolyRef* path, uint32 pathSize,
|
|
|
|
|
bool PathGenerator::GetSteerTarget(float const* startPos, float const* endPos,
|
|
|
|
|
float minTargetDist, dtPolyRef const* path, uint32 pathSize,
|
|
|
|
|
float* steerPos, unsigned char& steerPosFlag, dtPolyRef& steerPosRef)
|
|
|
|
|
{
|
|
|
|
|
// Find steer target.
|
|
|
|
|
@@ -645,7 +644,7 @@ bool PathGenerator::getSteerTarget(const float* startPos, const float* endPos,
|
|
|
|
|
unsigned char steerPathFlags[MAX_STEER_POINTS];
|
|
|
|
|
dtPolyRef steerPathPolys[MAX_STEER_POINTS];
|
|
|
|
|
uint32 nsteerPath = 0;
|
|
|
|
|
dtStatus dtResult = m_navMeshQuery->findStraightPath(startPos, endPos, path, pathSize,
|
|
|
|
|
dtStatus dtResult = _navMeshQuery->findStraightPath(startPos, endPos, path, pathSize,
|
|
|
|
|
steerPath, steerPathFlags, steerPathPolys, (int*)&nsteerPath, MAX_STEER_POINTS);
|
|
|
|
|
if (!nsteerPath || DT_SUCCESS != dtResult)
|
|
|
|
|
return false;
|
|
|
|
|
@@ -656,7 +655,7 @@ bool PathGenerator::getSteerTarget(const float* startPos, const float* endPos,
|
|
|
|
|
{
|
|
|
|
|
// Stop at Off-Mesh link or when point is further than slop away.
|
|
|
|
|
if ((steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ||
|
|
|
|
|
!inRangeYZX(&steerPath[ns*VERTEX_SIZE], startPos, minTargetDist, 1000.0f))
|
|
|
|
|
!InRangeYZX(&steerPath[ns*VERTEX_SIZE], startPos, minTargetDist, 1000.0f))
|
|
|
|
|
break;
|
|
|
|
|
ns++;
|
|
|
|
|
}
|
|
|
|
|
@@ -672,8 +671,8 @@ bool PathGenerator::getSteerTarget(const float* startPos, const float* endPos,
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
dtStatus PathGenerator::findSmoothPath(const float* startPos, const float* endPos,
|
|
|
|
|
const dtPolyRef* polyPath, uint32 polyPathSize,
|
|
|
|
|
dtStatus PathGenerator::FindSmoothPath(float const* startPos, float const* endPos,
|
|
|
|
|
dtPolyRef const* polyPath, uint32 polyPathSize,
|
|
|
|
|
float* smoothPath, int* smoothPathSize, uint32 maxSmoothPathSize)
|
|
|
|
|
{
|
|
|
|
|
*smoothPathSize = 0;
|
|
|
|
|
@@ -684,10 +683,10 @@ dtStatus PathGenerator::findSmoothPath(const float* startPos, const float* endPo
|
|
|
|
|
uint32 npolys = polyPathSize;
|
|
|
|
|
|
|
|
|
|
float iterPos[VERTEX_SIZE], targetPos[VERTEX_SIZE];
|
|
|
|
|
if (DT_SUCCESS != m_navMeshQuery->closestPointOnPolyBoundary(polys[0], startPos, iterPos))
|
|
|
|
|
if (DT_SUCCESS != _navMeshQuery->closestPointOnPolyBoundary(polys[0], startPos, iterPos))
|
|
|
|
|
return DT_FAILURE;
|
|
|
|
|
|
|
|
|
|
if (DT_SUCCESS != m_navMeshQuery->closestPointOnPolyBoundary(polys[npolys-1], endPos, targetPos))
|
|
|
|
|
if (DT_SUCCESS != _navMeshQuery->closestPointOnPolyBoundary(polys[npolys-1], endPos, targetPos))
|
|
|
|
|
return DT_FAILURE;
|
|
|
|
|
|
|
|
|
|
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos);
|
|
|
|
|
@@ -702,7 +701,7 @@ dtStatus PathGenerator::findSmoothPath(const float* startPos, const float* endPo
|
|
|
|
|
unsigned char steerPosFlag;
|
|
|
|
|
dtPolyRef steerPosRef = INVALID_POLYREF;
|
|
|
|
|
|
|
|
|
|
if (!getSteerTarget(iterPos, targetPos, SMOOTH_PATH_SLOP, polys, npolys, steerPos, steerPosFlag, steerPosRef))
|
|
|
|
|
if (!GetSteerTarget(iterPos, targetPos, SMOOTH_PATH_SLOP, polys, npolys, steerPos, steerPosFlag, steerPosRef))
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END);
|
|
|
|
|
@@ -727,15 +726,15 @@ dtStatus PathGenerator::findSmoothPath(const float* startPos, const float* endPo
|
|
|
|
|
dtPolyRef visited[MAX_VISIT_POLY];
|
|
|
|
|
|
|
|
|
|
uint32 nvisited = 0;
|
|
|
|
|
m_navMeshQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &m_filter, result, visited, (int*)&nvisited, MAX_VISIT_POLY);
|
|
|
|
|
npolys = fixupCorridor(polys, npolys, MAX_PATH_LENGTH, visited, nvisited);
|
|
|
|
|
_navMeshQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &_filter, result, visited, (int*)&nvisited, MAX_VISIT_POLY);
|
|
|
|
|
npolys = FixupCorridor(polys, npolys, MAX_PATH_LENGTH, visited, nvisited);
|
|
|
|
|
|
|
|
|
|
m_navMeshQuery->getPolyHeight(polys[0], result, &result[1]);
|
|
|
|
|
_navMeshQuery->getPolyHeight(polys[0], result, &result[1]);
|
|
|
|
|
result[1] += 0.5f;
|
|
|
|
|
dtVcopy(iterPos, result);
|
|
|
|
|
|
|
|
|
|
// Handle end of path and off-mesh links when close enough.
|
|
|
|
|
if (endOfPath && inRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 1.0f))
|
|
|
|
|
if (endOfPath && InRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 1.0f))
|
|
|
|
|
{
|
|
|
|
|
// Reached end of path.
|
|
|
|
|
dtVcopy(iterPos, targetPos);
|
|
|
|
|
@@ -746,7 +745,7 @@ dtStatus PathGenerator::findSmoothPath(const float* startPos, const float* endPo
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
else if (offMeshConnection && inRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 1.0f))
|
|
|
|
|
else if (offMeshConnection && InRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 1.0f))
|
|
|
|
|
{
|
|
|
|
|
// Advance the path up to and over the off-mesh connection.
|
|
|
|
|
dtPolyRef prevRef = INVALID_POLYREF;
|
|
|
|
|
@@ -766,7 +765,7 @@ dtStatus PathGenerator::findSmoothPath(const float* startPos, const float* endPo
|
|
|
|
|
|
|
|
|
|
// Handle the connection.
|
|
|
|
|
float startPos[VERTEX_SIZE], endPos[VERTEX_SIZE];
|
|
|
|
|
if (DT_SUCCESS == m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos))
|
|
|
|
|
if (DT_SUCCESS == _navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos))
|
|
|
|
|
{
|
|
|
|
|
if (nsmoothPath < maxSmoothPathSize)
|
|
|
|
|
{
|
|
|
|
|
@@ -775,7 +774,7 @@ dtStatus PathGenerator::findSmoothPath(const float* startPos, const float* endPo
|
|
|
|
|
}
|
|
|
|
|
// Move position at the other side of the off-mesh link.
|
|
|
|
|
dtVcopy(iterPos, endPos);
|
|
|
|
|
m_navMeshQuery->getPolyHeight(polys[0], iterPos, &iterPos[1]);
|
|
|
|
|
_navMeshQuery->getPolyHeight(polys[0], iterPos, &iterPos[1]);
|
|
|
|
|
iterPos[1] += 0.5f;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
@@ -794,21 +793,21 @@ dtStatus PathGenerator::findSmoothPath(const float* startPos, const float* endPo
|
|
|
|
|
return nsmoothPath < MAX_POINT_PATH_LENGTH ? DT_SUCCESS : DT_FAILURE;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool PathGenerator::inRangeYZX(const float* v1, const float* v2, float r, float h) const
|
|
|
|
|
bool PathGenerator::InRangeYZX(const float* v1, const float* v2, float r, float h) const
|
|
|
|
|
{
|
|
|
|
|
const float dx = v2[0] - v1[0];
|
|
|
|
|
const float dy = v2[1] - v1[1]; // elevation
|
|
|
|
|
const float dz = v2[2] - v1[2];
|
|
|
|
|
return (dx*dx + dz*dz) < r*r && fabsf(dy) < h;
|
|
|
|
|
return (dx * dx + dz * dz) < r * r && fabsf(dy) < h;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool PathGenerator::inRange(const Vector3 &p1, const Vector3 &p2, float r, float h) const
|
|
|
|
|
bool PathGenerator::InRange(Vector3 const& p1, Vector3 const& p2, float r, float h) const
|
|
|
|
|
{
|
|
|
|
|
Vector3 d = p1-p2;
|
|
|
|
|
return (d.x*d.x + d.y*d.y) < r*r && fabsf(d.z) < h;
|
|
|
|
|
Vector3 d = p1 - p2;
|
|
|
|
|
return (d.x * d.x + d.y * d.y) < r * r && fabsf(d.z) < h;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float PathGenerator::dist3DSqr(const Vector3 &p1, const Vector3 &p2) const
|
|
|
|
|
float PathGenerator::Dist3DSqr(Vector3 const& p1, Vector3 const& p2) const
|
|
|
|
|
{
|
|
|
|
|
return (p1-p2).squaredLength();
|
|
|
|
|
return (p1 - p2).squaredLength();
|
|
|
|
|
}
|
|
|
|
|
|