void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 const& endPos)
{
// *** getting start/end poly logic ***
float distToStartPoly, distToEndPoly;
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);
// 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 == INVALID_POLYREF || endPoly == INVALID_POLYREF)
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (startPoly == 0 || endPoly == 0)\n");
BuildShortcut();
bool path = _sourceUnit->GetTypeId() == TYPEID_UNIT && _sourceUnit->ToCreature()->CanFly();
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 < _pathPoints.size(); ++i)
{
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)
{
waterPath = false;
break;
}
}
}
_type = (path || waterPath) ? 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)
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: farFromPoly distToStartPoly=%.3f distToEndPoly=%.3f\n", distToStartPoly, distToEndPoly);
bool buildShotrcut = false;
if (_sourceUnit->GetTypeId() == TYPEID_UNIT)
{
Creature* owner = (Creature*)_sourceUnit;
G3D::Vector3 const& p = (distToStartPoly > 7.0f) ? startPos : endPos;
if (_sourceUnit->GetBaseMap()->IsUnderWater(p.x, p.y, p.z))
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: underWater case\n");
if (owner->CanSwim())
buildShotrcut = true;
}
else
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: flying case\n");
if (owner->CanFly())
buildShotrcut = true;
}
}
if (buildShotrcut)
{
BuildShortcut();
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
return;
}
else
{
float closestPoint[VERTEX_SIZE];
// we may want to use closestPointOnPolyBoundary instead
if (dtStatusSucceed(_navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint, NULL)))
{
dtVcopy(endPoint, closestPoint);
SetActualEndPosition(G3D::Vector3(endPoint[2], endPoint[0], endPoint[1]));
}
_type = PATHFIND_INCOMPLETE;
}
}
// *** poly path generating logic ***
// start and end are on same polygon
// just need to move in straight line
if (startPoly == endPoly)
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (startPoly == endPoly)\n");
BuildShortcut();
_pathPolyRefs[0] = startPoly;
_polyLength = 1;
_type = farFromPoly ? PATHFIND_INCOMPLETE : PATHFIND_NORMAL;
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: path type %d\n", _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 = 0;
uint32 pathEndIndex = 0;
if (_polyLength)
{
for (; pathStartIndex < _polyLength; ++pathStartIndex)
{
// here to catch few bugs
if (_pathPolyRefs[pathStartIndex] == INVALID_POLYREF)
{
TC_LOG_ERROR("maps", "Invalid poly ref in BuildPolyPath. _polyLength: %u, pathStartIndex: %u,"
" startPos: %s, endPos: %s, mapid: %u",
_polyLength, pathStartIndex, startPos.toString().c_str(), endPos.toString().c_str(),
_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)
{
TC_LOG_DEBUG("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;
memmove(_pathPolyRefs, _pathPolyRefs + pathStartIndex, _polyLength * sizeof(dtPolyRef));
}
else if (startPolyFound && !endPolyFound)
{
TC_LOG_DEBUG("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
uint32 prefixPolyLength = uint32(_polyLength * 0.8f + 0.5f);
memmove(_pathPolyRefs, _pathPolyRefs+pathStartIndex, prefixPolyLength * sizeof(dtPolyRef));
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 (dtStatusFailed(_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, NULL)))
{
// 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 (dtStatusFailed(_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, NULL)))
{
// suffixStartPoly is still invalid, error state
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
}
// generate suffix
uint32 suffixPolyLength = 0;
dtStatus dtResult;
if (_straightLine)
{
float hit = 0;
float hitNormal[3];
memset(hitNormal, 0, sizeof(hitNormal));
dtResult = _navMeshQuery->raycast(
suffixStartPoly,
suffixEndPoint,
endPoint,
&_filter,
&hit,
hitNormal,
_pathPolyRefs + prefixPolyLength - 1,
(int*)&suffixPolyLength,
MAX_PATH_LENGTH - prefixPolyLength);
// raycast() sets hit to FLT_MAX if there is a ray between start and end
if (hit != FLT_MAX)
{
// the ray hit something, return no path instead of the incomplete one
_type = PATHFIND_NOPATH;
return;
}
}
else
{
dtResult = _navMeshQuery->findPath(
suffixStartPoly, // start polygon
endPoly, // end polygon
suffixEndPoint, // start position
endPoint, // end position
&_filter, // polygon search filter
_pathPolyRefs + prefixPolyLength - 1, // [out] path
(int*)&suffixPolyLength,
MAX_PATH_LENGTH - prefixPolyLength); // max number of polygons in output path
}
if (!suffixPolyLength || 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
TC_LOG_ERROR("maps", "%u's Path Build failed: 0 length path", _sourceUnit->GetGUIDLow());
}
TC_LOG_DEBUG("maps", "++ m_polyLength=%u prefixPolyLength=%u suffixPolyLength=%u \n", _polyLength, prefixPolyLength, suffixPolyLength);
// new path = prefix + suffix - overlap
_polyLength = prefixPolyLength + suffixPolyLength - 1;
}
else
{
TC_LOG_DEBUG("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();
dtStatus dtResult;
if (_straightLine)
{
float hit = 0;
float hitNormal[3];
memset(hitNormal, 0, sizeof(hitNormal));
dtResult = _navMeshQuery->raycast(
startPoly,
startPoint,
endPoint,
&_filter,
&hit,
hitNormal,
_pathPolyRefs,
(int*)&_polyLength,
MAX_PATH_LENGTH);
// raycast() sets hit to FLT_MAX if there is a ray between start and end
if (hit != FLT_MAX)
{
// the ray hit something, return no path instead of the incomplete one
_type = PATHFIND_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
(int*)&_polyLength,
MAX_PATH_LENGTH); // max number of polygons in output path
}
if (!_polyLength || dtStatusFailed(dtResult))
{
// only happens if we passed bad data to findPath(), or navmesh is messed up
TC_LOG_ERROR("maps", "%u's Path Build failed: 0 length path", _sourceUnit->GetGUIDLow());
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
}
// by now we know what type of path we can get
if (_pathPolyRefs[_polyLength - 1] == endPoly && !(_type & PATHFIND_INCOMPLETE))
_type = PATHFIND_NORMAL;
else
_type = PATHFIND_INCOMPLETE;
// generate the point-path out of our up-to-date poly-path
BuildPointPath(startPoint, endPoint);
}
