dtPolyRef PathInfo::getPolyByLocation(const float* point, float *distance)
{
// 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);
if(polyRef != INVALID_POLYREF)
return polyRef;
// we don't have it in our old path
// try to get it by findNearestPoly()
// 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);
if(DT_SUCCESS == result && polyRef != INVALID_POLYREF)
{
*distance = dtVdist(closestPoint, point);
return polyRef;
}
// still nothing ..
// try with bigger search box
extents[1] = 200.0f;
result = m_navMeshQuery->findNearestPoly(point, extents, &m_filter, &polyRef, closestPoint);
if(DT_SUCCESS == result && polyRef != INVALID_POLYREF)
{
*distance = dtVdist(closestPoint, point);
return polyRef;
}
return INVALID_POLYREF;
}
void PathInfo::BuildPolyPath(PathNode startPos, PathNode endPos)
{
// *** getting start/end poly logic ***
float distToStartPoly, distToEndPoly;
// 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 startPoly = getPathPolyByPosition(m_pathPolyRefs, m_polyLength, startPos, &distToStartPoly);
dtPolyRef endPoly = getPathPolyByPosition(m_pathPolyRefs, m_polyLength, endPos, &distToEndPoly);
float startPoint[VERTEX_SIZE] = {startPos.y, startPos.z, startPos.x};
float endPoint[VERTEX_SIZE] = {endPos.y, endPos.z, endPos.x};
// we don't have it in our old path
// try to get it by findNearestPoly()
// use huge vertical range here
if (startPoly == INVALID_POLYREF || endPoly == INVALID_POLYREF)
{
float extents[VERTEX_SIZE] = {3.f, 200.f, 3.f}; // bounds of poly search area
dtQueryFilter filter = createFilter();
float closestPoint[VERTEX_SIZE];
if (startPoly == INVALID_POLYREF)
{
if (DT_SUCCESS == m_navMeshQuery->findNearestPoly(startPoint, extents, &filter, &startPoly, closestPoint))
distToStartPoly = dtVdist(closestPoint, startPoint);
}
if (endPoly == INVALID_POLYREF)
{
if (DT_SUCCESS == m_navMeshQuery->findNearestPoly(endPoint, extents, &filter, &endPoly, closestPoint))
distToEndPoly = dtVdist(closestPoint, endPoint);
}
}
// we have a hole in our mesh
// make shortcut path and mark it as NOPATH ( with flying exception )
// its up to caller how he will use this info
if (startPoly == INVALID_POLYREF || endPoly == INVALID_POLYREF)
{
PATH_DEBUG("++ BuildPolyPath :: (startPoly == 0 || endPoly == 0)\n");
BuildShortcut();
m_type = (m_sourceUnit->GetTypeId() == TYPEID_UNIT && ((Creature*)m_sourceUnit)->IsLevitating())
? PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH) : PATHFIND_NOPATH;
return;
}
// we may need a better number here
bool farFromPoly = (distToStartPoly > 7.0f || distToEndPoly > 7.0f);
if (farFromPoly)
{
PATH_DEBUG("++ BuildPolyPath :: farFromPoly distToStartPoly=%.3f distToEndPoly=%.3f\n", distToStartPoly, distToEndPoly);
bool buildShotrcut = false;
if (m_sourceUnit->GetTypeId() == TYPEID_UNIT)
{
Creature* owner = (Creature*)m_sourceUnit;
if (m_sourceUnit->GetBaseMap()->IsUnderWater(endPos.x, endPos.y, endPos.z))
{
PATH_DEBUG("++ BuildPolyPath :: underWater case\n");
if (owner->canSwim())
buildShotrcut = true;
}
else
{
PATH_DEBUG("++ BuildPolyPath :: flying case\n");
if (owner->IsLevitating())
buildShotrcut = true;
}
}
if (buildShotrcut)
{
BuildShortcut();
m_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))
{
dtVcopy(endPoint, closestPoint);
setActualEndPosition(PathNode(endPoint[2],endPoint[0],endPoint[1]));
}
m_type = PATHFIND_INCOMPLETE;
}
}
// *** poly path generating logic ***
// start and end are on same polygon
// just need to move in straight line
if (startPoly == endPoly)
{
PATH_DEBUG("++ BuildPolyPath :: (startPoly == endPoly)\n");
BuildShortcut();
m_pathPolyRefs = new dtPolyRef[1];
m_pathPolyRefs[0] = startPoly;
m_polyLength = 1;
m_type = farFromPoly ? PATHFIND_INCOMPLETE : PATHFIND_NORMAL;
PATH_DEBUG("++ BuildPolyPath :: path type %d\n", m_type);
return;
}
// look for startPoly/endPoly in current path
// TODO: we can merge it with getPathPolyByPosition() loop
bool startPolyFound = false;
bool endPolyFound = false;
uint32 pathStartIndex, pathEndIndex;
if (m_polyLength)
{
for (pathStartIndex = 0; pathStartIndex < m_polyLength; ++pathStartIndex)
{
if (m_pathPolyRefs[pathStartIndex] == startPoly)
{
startPolyFound = true;
break;
}
for (pathEndIndex = m_polyLength-1; pathEndIndex > pathStartIndex; --pathEndIndex)
{
if (m_pathPolyRefs[pathEndIndex] == endPoly)
{
endPolyFound = true;
break;
}
}
}
}
if (startPolyFound && endPolyFound)
{
PATH_DEBUG("++ 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
m_polyLength = pathEndIndex - pathStartIndex + 1;
dtPolyRef* newPolyRefs = new dtPolyRef[m_polyLength];
memcpy(newPolyRefs, m_pathPolyRefs+pathStartIndex, m_polyLength*sizeof(dtPolyRef));
delete [] m_pathPolyRefs;
m_pathPolyRefs = newPolyRefs;
}
else if (startPolyFound && !endPolyFound)
{
PATH_DEBUG("++ BuildPolyPath :: (startPolyFound && !endPolyFound)\n");
// we are moving on the old path but target moved out
// so we have atleast part of poly-path ready
m_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
uint32 prefixPolyLength = uint32(m_polyLength*0.8f + 0.5f);
dtPolyRef prefixPathPolys[MAX_PATH_LENGTH];
memcpy(prefixPathPolys, m_pathPolyRefs+pathStartIndex, prefixPolyLength*sizeof(dtPolyRef));
dtPolyRef suffixStartPoly = prefixPathPolys[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))
{
// suffixStartPoly is invalid somehow, or the navmesh is broken => error state
sLog->outError("%u's Path Build failed: invalid polyRef in path", m_sourceUnit->GetGUID());
BuildShortcut();
m_type = PATHFIND_NOPATH;
return;
}
// generate suffix
dtQueryFilter filter = createFilter();
dtPolyRef suffixPathPolys[MAX_PATH_LENGTH];
uint32 suffixPolyLength = 0;
dtStatus dtResult = m_navMeshQuery->findPath(
suffixStartPoly, // start polygon
endPoly, // end polygon
suffixEndPoint, // start position
endPoint, // end position
&filter, // polygon search filter
suffixPathPolys, // [out] path
(int*)&suffixPolyLength,
MAX_PATH_LENGTH-prefixPolyLength); // max number of polygons in output path
if (!suffixPolyLength || dtResult != DT_SUCCESS)
{
// 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("%u's Path Build failed: 0 length path", m_sourceUnit->GetGUID());
}
PATH_DEBUG("++ m_polyLength=%u prefixPolyLength=%u suffixPolyLength=%u \n",m_polyLength, prefixPolyLength, suffixPolyLength);
// new path = prefix + suffix - overlap
m_polyLength = prefixPolyLength + suffixPolyLength - 1;
delete [] m_pathPolyRefs;
m_pathPolyRefs = new dtPolyRef[m_polyLength];
// copy the part of the old path we keep - prefix
memcpy(m_pathPolyRefs, prefixPathPolys, prefixPolyLength*sizeof(dtPolyRef));
// copy the newly created suffix - skip first poly, we have it at prefix end
if (suffixPathPolys)
memcpy(m_pathPolyRefs+prefixPolyLength, suffixPathPolys+1, (suffixPolyLength-1)*sizeof(dtPolyRef));
}
else
{
PATH_DEBUG("++ 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();
dtQueryFilter filter = createFilter(); // use special filter so we use proper terrain types
dtPolyRef pathPolys[MAX_PATH_LENGTH];
m_polyLength = 0;
dtStatus dtResult = m_navMeshQuery->findPath(
startPoly, // start polygon
endPoly, // end polygon
startPoint, // start position
endPoint, // end position
&filter, // polygon search filter
pathPolys, // [out] path
(int*)&m_polyLength,
MAX_PATH_LENGTH); // max number of polygons in output path
if (!m_polyLength || dtResult != DT_SUCCESS)
{
// only happens if we passed bad data to findPath(), or navmesh is messed up
sLog->outError("%u's Path Build failed: 0 length path", m_sourceUnit->GetGUID());
BuildShortcut();
m_type = PATHFIND_NOPATH;
return;
}
m_pathPolyRefs = new dtPolyRef[m_polyLength];
memcpy(m_pathPolyRefs, pathPolys, m_polyLength*sizeof(dtPolyRef));
}
// 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;
else
m_type = PATHFIND_INCOMPLETE;
// generate the point-path out of our up-to-date poly-path
BuildPointPath(startPoint, endPoint);
}
void PathInfo::Update(const float destX, const float destY, const float destZ)
{
float x, y, z;
// update start and end
m_sourceObject->GetPosition(x, y, z);
setStartPosition(x, y, z);
setEndPosition(destX, destY, destZ);
// make sure navMesh works
if(!m_navMesh)
{
m_sourceObject->GetPosition(x, y, z);
m_navMesh = m_sourceObject->GetMap()->GetNavMesh();
if(!m_navMesh)
{
// can't pathfind if navmesh doesn't exist
shortcut();
return;
}
}
if(!m_pathPolyRefs)
{
// path was not built before, most likely because navmesh wasn't working
// start from scratch, then return
Build();
return;
}
// should be safe to update path now
bool startOffPath = false;
bool endOffPath = false;
// find start and end poly
// navMesh.findNearestPoly is expensive, so first we check just the current path
getStartPosition(x, y, z);
dtPolyRef startPoly = getPathPolyByPosition(x, y, z);
getEndPosition(x, y, z);
dtPolyRef endPoly = getPathPolyByPosition(x, y, z);
if(startPoly != 0 && endPoly != 0)
trim(startPoly, endPoly);
else
{
// start or end is off the path, need to find the polygon
float extents[3] = {2.f, 4.f, 2.f}; // bounds of poly search area
dtQueryFilter filter = dtQueryFilter(); // filter for poly search
if(!startPoly)
{
getStartPosition(x, y, z);
float startPos[3] = {y, z, x};
startOffPath = true;
startPoly = m_navMesh->findNearestPoly(startPos, extents, &filter, 0);
}
if(!endPoly)
{
getEndPosition(x, y, z);
float endPos[3] = {y, z, x};
endOffPath = true;
endPoly = m_navMesh->findNearestPoly(endPos, extents, &filter, 0);
}
if(startPoly == 0 || endPoly == 0)
{
// source or dest not near navmesh polygons:
// flying, falling, swimming, or navmesh has a hole
// ignore obstacles/terrain is better than giving up
// PATHFIND TODO: prevent walking/swimming mobs from flying into the air
shortcut();
return;
}
}
if(startPoly == endPoly)
{
// start and end are on same polygon
// just need to move in straight line
// PATHFIND TODO: prevent walking/swimming mobs from flying into the air
clear();
m_pathPolyRefs = new dtPolyRef[1];
m_pathPolyRefs[0] = startPoly;
m_length = 1;
getEndPosition(x, y, z);
setNextPosition(x, y, z);
m_type = PathType(m_type | PATHFIND_NORMAL);
return;
}
if(startOffPath)
{
bool adjacent = false;
int i;
for(i = 0; i < DT_VERTS_PER_POLYGON; ++i)
if(startPoly == m_navMesh->getPolyByRef(m_pathPolyRefs[0])->neis[i])
{
adjacent = true;
break;
}
if(adjacent)
{
// startPoly is adjacent to the path, we can add it to the start of the path
// 50th poly will fall off of path, shouldn't be an issue because most paths aren't that long
m_length = m_length < MAX_PATH_LENGTH ? m_length + 1 : m_length;
dtPolyRef* temp = new dtPolyRef[m_length];
temp[0] = startPoly;
for(i = 1; i < m_length; ++i)
temp[i] = m_pathPolyRefs[i - 1];
delete [] m_pathPolyRefs;
m_pathPolyRefs = temp;
}
else
{
// waste of time to optimize, just find brand new path
Build(startPoly, endPoly);
return;
}
}
if(endOffPath)
{
bool adjacent = false;
int i;
for(i = 0; i < DT_VERTS_PER_POLYGON; ++i)
if(startPoly == m_navMesh->getPolyByRef(m_pathPolyRefs[0])->neis[i])
{
adjacent = true;
break;
}
if(adjacent)
{
if(m_length < MAX_PATH_LENGTH)
{
// endPoly is adjacent to the path, and we have enough room to add it to the end
dtPolyRef* temp = new dtPolyRef[m_length + 1];
for(i = 0; i < m_length; ++i)
temp[i] = m_pathPolyRefs[i];
temp[i] = endPoly;
delete [] m_pathPolyRefs;
m_pathPolyRefs = temp;
}
//else
// ; // endPoly is adjacent to the path, we just don't have room to store it
}
else
{
// waste of time to optimize, just find brand new path
Build(startPoly, endPoly);
return;
}
}
updateNextPosition();
}