Backups/GTASource
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
c971d568a112e5bec2e45ab389375b07c6914a6e
GTASource/game/pathserver/PathServer_PathMisc.cpp
expvintl 419f2e4752 init
2025-02-23 17:40:52 +08:00

1937 lines
68 KiB
C++
Raw Blame History

#include "PathServer\PathServer.h"
// Rage headers
#include "atl\inmap.h"
#include "system/xtl.h"
// Framework headers
#include "ai/navmesh/priqueue.h"
#include "fwmaths\Vector.h"
#include "fwmaths/random.h"
NAVMESH_OPTIMISATIONS()
#define MAX_NUM_TESSELLATED_FRAGMENTS 768 // was 512 // was 256 // was 64
atArray<TNavMeshPoly*> startPolys(0,MAX_NUM_TESSELLATED_FRAGMENTS);
atArray<TNavMeshPoly*> tessellatedStartPolys(0,MAX_NUM_TESSELLATED_FRAGMENTS);
// The distances from adjacent waypoints towards the corner-point, for vCtrlPt2 and vCtrlPt3
//const float g_fPathSplineDists[PATHSPLINE_NUM_TEST_DISTANCES] = { 0.0f, 0.25f, 0.5f, 0.9f };
const float g_fPathSplineDists[PATHSPLINE_NUM_TEST_DISTANCES] = { 0.1f, 0.25f, 0.5f, 0.9f };
// The magnitude that the outer control pts are pulled back (scales the length of the line-seg)
const float g_fOuterCtrlPtDists[PATHSPLINE_NUM_TEST_DISTANCES] = { 1.0f, 0.75f, 0.5f, 0.25f };
namespace
{
bool CalcCornerPushDir(Vector3& vPushDir, const Vector3& vPrevPt, const Vector3& vMidPt, const Vector3& vNextPt, float fPushThreshold)
{
Vector3 vEntry = vMidPt - vPrevPt;
vEntry.z = 0.f;
vEntry.Normalize();
Vector3 vExit = vMidPt - vNextPt;
vExit.z = 0.f;
vExit.Normalize();
if (abs(vEntry.Dot(vExit)) < fPushThreshold)
{
vPushDir = vEntry + vExit;
vPushDir.Normalize();
return true;
}
return false;
}
/* Not sued in the code base. Orbis complains about it
TNavMeshPoly* DigOutPolyBelowPoint(const Vector3& vNewPathPos, aiNavDomain domain)
{
const u32 iStartNavMesh = CPathServer::GetNavMeshIndexFromPosition(vNewPathPos, domain);
if (iStartNavMesh == NAVMESH_NAVMESH_INDEX_NONE)
return NULL;
CNavMesh * pStartNavMesh = CPathServer::GetNavMeshFromIndex(iStartNavMesh, domain);
if (!pStartNavMesh)
return NULL;
Vector3 vIntersectPoint;
const u32 iStartPolyIndex = pStartNavMesh->GetPolyBelowPoint(vNewPathPos + ZAXIS, vIntersectPoint, 5.0f);
if (iStartPolyIndex == NAVMESH_POLY_INDEX_NONE)
return NULL;
return pStartNavMesh->GetPoly(iStartPolyIndex);
}
*/
bool IsPointsMakingAQuirk(const Vector3& vPrevPrevPt, const Vector3& vPrevPt, const Vector3& vThisPt, const Vector3& vNextPt, float UNUSED_PARAM(fTolerance))
{
Vector3 vToPrev = vPrevPt - vPrevPrevPt;
vToPrev.z = 0.f;
vToPrev.Normalize();
Vector3 vToThis = vThisPt - vPrevPt;
vToThis.z = 0.f;
vToThis.Normalize();
Vector3 vToNext = vNextPt - vThisPt;
vToNext.z = 0.f;
vToNext.Normalize();
// if (vToPrev.Dot(vToThis) < fTolerance && vToThis.Dot(vToNext) < fTolerance)
{
// Change of direction?
if ((int)Sign(-CrossProduct(vToThis, vToNext).z) == (int)Sign(-CrossProduct(vToPrev, vToThis).z))
return false;
return true;
}
// return false;
}
}
//*************************************************************************************************
// This function tessellates polys around the path start, and adds them to the priority queue
// ready for the pathfinding algorithm to use. It only does this if there are any dynamic
// objects in the vicinity of the start position. The original (untessellated) start poly is
// already in the priority queue at this time, and if it becomes tessellated here it will be
// marked as "replaced by tessellation" & subsequently ignored by the pathfinder when encountered.
//*************************************************************************************************
bool CPathServerThread::MaybeTessellateAndMarkPolysSurroundingPathStart(CNavMesh * pNavMesh, CPathRequest * pPathRequest, const float fInitialCost)
{
dev_float fNearbyPolyRadius = (pPathRequest->m_bExpandStartEndPolyTessellationRadius ? 2.5f : 0.25f);
static const float fStartPosRadius = 0.25f;
static const float fZSize = 5.0f;
// This tesselation stuff is only for the normal navigation domain for now.
const aiNavDomain domain = kNavDomainRegular;
const bool bUseMultipleStartPolys = DoesMinMaxIntersectAnyDynamicObjectsApproximate(m_Vars.m_pStartPoly->m_MinMax);
if(bUseMultipleStartPolys)
{
TShortMinMax startPosMinMax;
startPosMinMax.SetFloat(
pPathRequest->m_vPathStart.x - fStartPosRadius, pPathRequest->m_vPathStart.y - fStartPosRadius, pPathRequest->m_vPathStart.z - fZSize,
pPathRequest->m_vPathStart.x + fStartPosRadius, pPathRequest->m_vPathStart.y + fStartPosRadius, pPathRequest->m_vPathStart.z + fZSize
);
startPolys.clear();
startPolys.Append()=m_Vars.m_pStartPoly;
const float fAlternativeStartPenalty = 50.0f;
// Get a list of polys which are very close to the start position.
GetMultipleNavMeshPolysForRouteEndPoints(pPathRequest, m_Vars.m_pStartPoly, pNavMesh, pPathRequest->m_vPathStart, fNearbyPolyRadius, startPolys, domain);
for(int u=0; u<startPolys.GetCount(); u++)
{
TNavMeshPoly * pNavMeshPoly = startPolys[u];
CPathServerThread::OnFirstVisitingPoly(pNavMeshPoly);
if(pNavMeshPoly->TestFlags(NAVMESHPOLY_ALTERNATIVE_STARTING_POLY) ||
pNavMeshPoly->GetReplacedByTessellation())
continue;
s32 tp;
tessellatedStartPolys.clear();
tessellatedStartPolys.Append() = pNavMeshPoly;
for(tp=0; tp<tessellatedStartPolys.GetCount(); tp++)
{
TNavMeshPoly * pTessPoly = tessellatedStartPolys[tp];
// This should never return FALSE, as we will never reach max visited polys at this early stage
OnFirstVisitingPoly(pTessPoly);
Assert(!(pTessPoly->TestFlags(NAVMESHPOLY_ALTERNATIVE_STARTING_POLY)));
Assert(!pTessPoly->GetReplacedByTessellation());
CNavMesh * pTessNavMesh = CPathServer::GetNavMeshFromIndex(pTessPoly->GetNavMeshIndex(), domain);
if(fwPathServer::CanTessellateThisPoly(pTessPoly) &&
fwPathServer::IsRoomToTessellateThisPoly(pTessPoly))
{
CPathServerThread::ms_dynamicObjectsIntersectingPolygons.clear();
CDynamicObjectsContainer::GetObjectsIntersectingRegion(pTessPoly->m_MinMax, CPathServerThread::ms_dynamicObjectsIntersectingPolygons, TDynamicObject::FLAG_IGNORE_OPENABLE|TDynamicObject::FLAG_IGNORE_NOT_OBSTACLE);
if(CPathServerThread::ms_dynamicObjectsIntersectingPolygons.GetCount())
{
Assert(!(pTessPoly->TestFlags(NAVMESHPOLY_ALTERNATIVE_STARTING_POLY)));
const bool bOk = TessellateNavMeshPolyAndCreateDegenerateConnections(
pTessNavMesh, pTessPoly,
pTessNavMesh->GetPolyIndex(pTessPoly),
(pTessPoly == m_Vars.m_pStartPoly), // don't bother updating m_pStartPoly when we're using multiple starting polys
(pTessPoly == m_Vars.m_pEndPoly), // but we do want to update the m_pEndPoly
NULL, NAVMESH_NAVMESH_INDEX_NONE, NAVMESH_POLY_INDEX_NONE,
&tessellatedStartPolys
);
NAVMESH_OPTIMISATIONS_OFF_ONLY( Assert(bOk); );
if(!bOk)
{
if(!m_PathSearchPriorityQueue->GetNodeCount())
{
Assert(m_Vars.m_pStartPoly);
if(!m_Vars.m_pStartPoly)
return false;
m_PathSearchPriorityQueue->Insert(fInitialCost, 0.0f, m_Vars.m_pStartPoly, pPathRequest->m_vPathStart, m_Vars.m_vDirFromPrevious, 0, NAVMESH_POLY_INDEX_NONE);
}
return true;
}
// This should never return FALSE, as we will never reach max visited polys at this early stage
OnFirstVisitingPoly(pTessPoly);
tessellatedStartPolys.Delete(tp);
tp--;
if(!m_Vars.m_pStartPoly)
{
float fClosestEdgeDistSqr = FLT_MAX;
TNavMeshPoly * pClosestPoly = NULL;
Vector3 vClosestPt;
for(tp=0; tp<tessellatedStartPolys.GetCount(); tp++)
{
pTessPoly = tessellatedStartPolys[tp];
if(pTessPoly->GetIsTessellatedFragment())
{
const s32 iEdge = fwPathServer::GetTessellationNavMesh()->GetPolyClosestEdgeMidToPoint(
pPathRequest->m_vPathStart, pTessPoly, &vClosestPt);
if(iEdge >= 0)
{
const float fDistSqr = (vClosestPt-pPathRequest->m_vPathStart).XYMag2();
if(fDistSqr < fClosestEdgeDistSqr)
{
fClosestEdgeDistSqr = fDistSqr;
pClosestPoly = pTessPoly;
}
}
}
}
// There's a chance this still isn't found (eg. if all polygons were removed);
m_Vars.m_pStartPoly = pClosestPoly;
Assertf(m_Vars.m_pStartPoly, "Starting poly must have been clipped to nothing");
if(!m_Vars.m_pStartPoly)
return false;
}
}
}
}
for(tp=0; tp<tessellatedStartPolys.GetCount(); tp++)
{
TNavMeshPoly * pTessPoly = tessellatedStartPolys[tp];
if(pTessPoly == m_Vars.m_pStartPoly)
continue;
if(!pTessPoly->m_MinMax.Intersects(startPosMinMax))
continue;
// This should never return FALSE, as we will never reach max visited polys at this early stage
OnFirstVisitingPoly(pTessPoly);
Assert(!(pTessPoly->TestFlags(NAVMESHPOLY_ALTERNATIVE_STARTING_POLY)));
// We are about to see whether this poly could be a valid alternative starting poly for the pathsearch.
// If so then will be added to the binheap, and thus when the pathfind algorithm traces back the
// route to the start, it could end up at this polygon.
// Therefore its important that we chose a point-enum for this poly from which the path startpos can be
// seen in an unobstructed line.
pTessPoly->m_PathParentPoly = NULL;
CNavMesh * pTessNavMesh = CPathServer::GetNavMeshFromIndex(pTessPoly->GetNavMeshIndex(), domain);
u32 v;
for(v=0; v<pTessPoly->GetNumVertices(); v++)
{
pTessNavMesh->GetVertex( pTessNavMesh->GetPolyVertexIndex(pTessPoly, v), m_Vars.m_vPolyPts[v] );
}
const u32 iNumCandidatePts = CreatePointsInPoly(
pTessNavMesh,
pTessPoly,
m_Vars.m_vPolyPts,
ShouldUseMorePointsForPoly(*pTessPoly, pPathRequest) ? POINTSINPOLY_EXTRA : POINTSINPOLY_NORMAL,
m_Vars.g_vClosestPtsInPoly
);
for(v=0; v<iNumCandidatePts; v++)
{
if(TestDynamicObjectLOS(pPathRequest->m_vPathStart, m_Vars.g_vClosestPtsInPoly[v]))
break;
}
// If we have found a point enum value which can see the start position, then we can safely use this poly
// and point-enum as an alternative starting point for the pathsearch - thus increasing the chance of the
// pathsearch completing successfully.
if(v != iNumCandidatePts)
{
pTessPoly->OrFlags(NAVMESHPOLY_ALTERNATIVE_STARTING_POLY);
pTessPoly->SetPointEnum(v);
float fDist = (m_Vars.g_vClosestPtsInPoly[v] - pPathRequest->m_vPathStart).Mag();
const float fCost = fInitialCost + fAlternativeStartPenalty;
m_PathSearchPriorityQueue->Insert(fCost, fDist, pTessPoly, m_Vars.g_vClosestPtsInPoly[v], m_Vars.m_vDirFromPrevious, 0, NAVMESH_POLY_INDEX_NONE);
}
}
}
// If the m_pStartPoly didn't get added added to the binheap, then make sure it is - we don't want to end up with an empty binheap!
Assert(!(m_Vars.m_pStartPoly->TestFlags(NAVMESHPOLY_ALTERNATIVE_STARTING_POLY)));
m_PathSearchPriorityQueue->Insert(fInitialCost, 0.0f, m_Vars.m_pStartPoly, pPathRequest->m_vPathStart, m_Vars.m_vDirFromPrevious, 0, NAVMESH_POLY_INDEX_NONE);
}
else
{
m_Vars.m_pStartPoly->m_PathParentPoly = NULL;
m_PathSearchPriorityQueue->Insert(fInitialCost, 0.0f, m_Vars.m_pStartPoly, pPathRequest->m_vPathStart, m_Vars.m_vDirFromPrevious, 0, NAVMESH_POLY_INDEX_NONE);
}
// Finally make sure that the actual m_pStartPoly itself doesn't have the NAVMESHPOLY_ALTERNATIVE_STARTING_POLY flag set.
// This is important, because if our route started on a polygon marked with this flag - then we will automatically
// prepend the m_pStartPoly itself.
m_Vars.m_pStartPoly->AndFlags(~NAVMESHPOLY_ALTERNATIVE_STARTING_POLY);
return true;
}
void
CPathServerThread::MaybeTessellatePolysSurroundingPathEnd(CNavMesh * pNavMesh, CPathRequest * pPathRequest)
{
Assert(m_Vars.m_pEndPoly);
const float fOriginalEdgeThreshold = CNavMesh::ms_fSmallestTessellatedEdgeThresholdSqr;
static dev_float fSmallEdgeThreshold = 2.0f * 2.0f;
dev_float fNearbyPolyRadius = (pPathRequest->m_bExpandStartEndPolyTessellationRadius ? 2.5f : 0.25f);
const bool bUseMultipleEndPolys = DoesMinMaxIntersectAnyDynamicObjectsApproximate(m_Vars.m_pEndPoly->m_MinMax);
// This tesselation stuff is only for the normal navigation domain for now.
const aiNavDomain domain = kNavDomainRegular;
if(bUseMultipleEndPolys)
{
startPolys.clear();
startPolys.Append() = m_Vars.m_pEndPoly;
// Get a list of polys which are very close to the end position.
GetMultipleNavMeshPolysForRouteEndPoints(pPathRequest, m_Vars.m_pEndPoly, pNavMesh, pPathRequest->m_vPathEnd, fNearbyPolyRadius, startPolys, domain);
s32 u;
for(u=0; u<startPolys.GetCount(); u++)
{
TNavMeshPoly * pNavMeshPoly = startPolys[u];
// This should never return FALSE, as we will never reach max visited polys at this early stage
OnFirstVisitingPoly(pNavMeshPoly);
if(pNavMeshPoly->TestFlags(NAVMESHPOLY_ALTERNATIVE_STARTING_POLY) ||
pNavMeshPoly->GetReplacedByTessellation() ||
pNavMeshPoly == m_Vars.m_pStartPoly)
continue;
s32 tp;
tessellatedStartPolys.clear();
tessellatedStartPolys.Append() = pNavMeshPoly;
for(tp=0; tp<tessellatedStartPolys.GetCount(); tp++)
{
TNavMeshPoly * pTessPoly = tessellatedStartPolys[tp];
// This should never return FALSE, as we will never reach max visited polys at this early stage
OnFirstVisitingPoly(pTessPoly);
Assert(!pTessPoly->GetReplacedByTessellation());
CNavMesh * pTessNavMesh = CPathServer::GetNavMeshFromIndex(pTessPoly->GetNavMeshIndex(), domain);
if(pTessPoly == m_Vars.m_pEndPoly)
{
CNavMesh::ms_fSmallestTessellatedEdgeThresholdSqr = fSmallEdgeThreshold;
}
if(fwPathServer::CanTessellateThisPoly(pTessPoly) &&
fwPathServer::IsRoomToTessellateThisPoly(pTessPoly))
{
CPathServerThread::ms_dynamicObjectsIntersectingPolygons.clear();
CDynamicObjectsContainer::GetObjectsIntersectingRegion(pTessPoly->m_MinMax, CPathServerThread::ms_dynamicObjectsIntersectingPolygons, TDynamicObject::FLAG_IGNORE_OPENABLE|TDynamicObject::FLAG_IGNORE_NOT_OBSTACLE);
if(CPathServerThread::ms_dynamicObjectsIntersectingPolygons.GetCount())
{
bool bOk = TessellateNavMeshPolyAndCreateDegenerateConnections(
pTessNavMesh, pTessPoly,
pTessNavMesh->GetPolyIndex(pTessPoly),
false, // don't bother updating m_pStartPoly when we're using multiple starting polys
(pTessPoly == m_Vars.m_pEndPoly), // but we do want to update the m_pEndPoly
NULL, NAVMESH_NAVMESH_INDEX_NONE, NAVMESH_POLY_INDEX_NONE,
&tessellatedStartPolys
);
Assert(bOk);
if(bOk)
{
// This should never return FALSE, as we will never reach max visited polys at this early stage
OnFirstVisitingPoly(pTessPoly);
tessellatedStartPolys.Delete(tp);
tp--;
}
}
}
CNavMesh::ms_fSmallestTessellatedEdgeThresholdSqr = fOriginalEdgeThreshold;
}
float fClosestEdgeDistSqr = FLT_MAX;
TNavMeshPoly * pClosestPoly = NULL;
Vector3 vClosestPt;
for(tp=0; tp<tessellatedStartPolys.GetCount(); tp++)
{
TNavMeshPoly * pTessPoly = tessellatedStartPolys[tp];
OnFirstVisitingPoly(pTessPoly);
pTessPoly->m_PathParentPoly = NULL;
// If no replacement end poly was found underneath the end pos,
// select the one whose edge is closest to the end pos
if(!m_Vars.m_pEndPoly && pTessPoly->GetIsTessellatedFragment())
{
const s32 iEdge = fwPathServer::GetTessellationNavMesh()->GetPolyClosestEdgeMidToPoint(
pPathRequest->m_vPathEnd, pTessPoly, &vClosestPt);
if(iEdge >= 0)
{
const float fDistSqr = (vClosestPt-pPathRequest->m_vPathEnd).XYMag2();
if(fDistSqr < fClosestEdgeDistSqr)
{
fClosestEdgeDistSqr = fDistSqr;
pClosestPoly = pTessPoly;
}
}
}
}
// There's a chance this still isn't found (eg. if all polygons were removed);
if(!m_Vars.m_pEndPoly)
m_Vars.m_pEndPoly = pClosestPoly;
}
}
CNavMesh::ms_fSmallestTessellatedEdgeThresholdSqr = fOriginalEdgeThreshold;
}
//******************************************************************************************************************
// AddWaypointsToPreserveHeightChanges
// After finding & refining (string-pulling) the path, we are left with a set of straight line segments. The
// string-pulling operates in 2d & ignores height changes on the polys underneath. If we need to have waypoints
// placed at significant changes in *slope* in the navmesh underneath the path - then use this function to
// traverse path inserting waypoints wherever the slope changes.
//
//******************************************************************************************************************
void
CPathServerThread::AddWaypointsToPreserveHeightChanges(CPathRequest * pPathRequest)
{
#if !__FINAL
m_PerfTimer->Reset();
m_PerfTimer->Start();
#endif
#if __ASSERT
if( pPathRequest->m_iNumPoints < 2 )
{
pPathRequest->m_pSavePathFileName = "x:\\gta5\\build\\dev\\807928.prob";
Displayf( "Less than 2 points in path..\n");
Displayf( "m_vPathStart (%.2f, %.2f, %.2f)\n", pPathRequest->m_vPathStart.x, pPathRequest->m_vPathStart.y, pPathRequest->m_vPathStart.z);
Displayf( "m_vPathEnd (%.2f, %.2f, %.2f)\n", pPathRequest->m_vPathEnd.x, pPathRequest->m_vPathEnd.y, pPathRequest->m_vPathEnd.z);
Displayf( "m_iNumPoints %i \n", pPathRequest->m_iNumPoints);
Displayf( "m_PathPolys[0] 0x%p", pPathRequest->m_PathPolys[0]);
Displayf( "m_PathPolys[1] 0x%p", pPathRequest->m_PathPolys[1]);
Displayf( "m_iNumInitiallyFoundPolys %i", pPathRequest->m_iNumInitiallyFoundPolys);
Displayf( "m_WaypointFlags[0] %u", pPathRequest->m_WaypointFlags[0].GetSpecialActionFlags());
Displayf( "m_WaypointFlags[1] %u", pPathRequest->m_WaypointFlags[1].GetSpecialActionFlags());
Assertf(pPathRequest->m_iNumPoints >= 2, "see (url:bugstar:807928). Please include the TTY as it will have some more details, and attach the file \"%s\" after closing this dialog box.", pPathRequest->m_pSavePathFileName);
}
#endif
TNavMeshPoly * pHeightChangePoly = NULL;
Vector3 vHeightChangePos;
const Vector3 vEpsilon(0.25f, 0.25f, 0.5f);
static Vector3 vFinalPoints[PRESERVE_HEIGHT_CHANGE_MAX_NUM_PTS];
static TNavMeshPoly * pFinalPolys[PRESERVE_HEIGHT_CHANGE_MAX_NUM_PTS];
static TNavMeshWaypointFlag iFinalWaypointFlags[PRESERVE_HEIGHT_CHANGE_MAX_NUM_PTS];
vFinalPoints[0] = pPathRequest->m_PathPoints[0];
pFinalPolys[0] = pPathRequest->m_PathPolys[0];
iFinalWaypointFlags[0] = pPathRequest->m_WaypointFlags[0];
u32 iNumFinalPoints=1;
s32 i;
const aiNavDomain domain = pPathRequest->GetMeshDataSet();
for(i=0; i<pPathRequest->m_iNumPoints-1; i++)
{
Vector3 & vVert = pPathRequest->m_PathPoints[i];
const Vector3 & vNextVert = pPathRequest->m_PathPoints[i+1];
TNavMeshPoly * pPoly = pPathRequest->m_PathPolys[i];
TNavMeshPoly * pNextPoly = pPathRequest->m_PathPolys[i+1];
if(pPoly && pNextPoly)
{
// If any of these polygons are tessellated, then replace with the original polygons
// for the purpose of our tests. This is in line with our strategy for navmesh LOS
// queries: it results in considerably less processing, and less complicated code than
// having to deal with connection polygons, etc.
if(pPoly->GetNavMeshIndex() == NAVMESH_INDEX_TESSELLATION)
{
const u32 iPoly = CPathServer::GetTessellationNavMesh()->GetPolyIndex(pPoly);
TTessInfo * pTess = CPathServer::GetTessInfo(iPoly);
Assert(pTess);
CNavMesh * pOriginalNavMesh = CPathServer::GetNavMeshFromIndex(pTess->m_iNavMeshIndex, kNavDomainRegular);
NAVMESH_OPTIMISATIONS_OFF_ONLY( Assert(pOriginalNavMesh); )
if(pOriginalNavMesh)
pPoly = pOriginalNavMesh->GetPoly(pTess->m_iPolyIndex);
else
pPoly = NULL;
}
if(pNextPoly->GetNavMeshIndex() == NAVMESH_INDEX_TESSELLATION)
{
const u32 iPoly = CPathServer::GetTessellationNavMesh()->GetPolyIndex(pNextPoly);
TTessInfo * pTess = CPathServer::GetTessInfo(iPoly);
Assert(pTess);
CNavMesh * pOriginalNavMesh = CPathServer::GetNavMeshFromIndex(pTess->m_iNavMeshIndex, kNavDomainRegular);
NAVMESH_OPTIMISATIONS_OFF_ONLY( Assert(pOriginalNavMesh); )
if(pOriginalNavMesh)
pNextPoly = pOriginalNavMesh->GetPoly(pTess->m_iPolyIndex);
else
pNextPoly = NULL;
}
if(pPoly && pNextPoly)
{
Vector3 vVecToNext = vNextVert - vVert;
vVecToNext.Normalize();
IncTimeStamp();
bool bHasHeightChange = FindHeightChangePosAlongLine(vVert, vNextVert, pNextPoly, pPoly, pHeightChangePoly, vHeightChangePos, false, domain);
while(bHasHeightChange)
{
Assert(pHeightChangePoly && pHeightChangePoly->GetNavMeshIndex()!=NAVMESH_INDEX_TESSELLATION);
const bool bAlmostSamePositionAsVert1 = vHeightChangePos.IsClose(vVert, vEpsilon);
const bool bAlmostSamePositionAsVert2 = vHeightChangePos.IsClose(vNextVert, vEpsilon);
if(!bAlmostSamePositionAsVert1 && !bAlmostSamePositionAsVert2)
{
vFinalPoints[iNumFinalPoints] = vHeightChangePos;
pFinalPolys[iNumFinalPoints] = pHeightChangePoly;
iFinalWaypointFlags[iNumFinalPoints].Clear();
//**************************
// Preserve certain flags
if(((pPathRequest->m_WaypointFlags[i].m_iSpecialActionFlags) & WAYPOINT_FLAG_IS_INTERIOR) &&
((pPathRequest->m_WaypointFlags[i+1].m_iSpecialActionFlags) & WAYPOINT_FLAG_IS_INTERIOR))
iFinalWaypointFlags[iNumFinalPoints].m_iSpecialActionFlags |= WAYPOINT_FLAG_IS_INTERIOR;
if(((pPathRequest->m_WaypointFlags[i].m_iSpecialActionFlags) & WAYPOINT_FLAG_IS_ON_WATER_SURFACE) &&
((pPathRequest->m_WaypointFlags[i+1].m_iSpecialActionFlags) & WAYPOINT_FLAG_IS_ON_WATER_SURFACE))
iFinalWaypointFlags[iNumFinalPoints].m_iSpecialActionFlags |= WAYPOINT_FLAG_IS_ON_WATER_SURFACE;
iNumFinalPoints++;
if(iNumFinalPoints==PRESERVE_HEIGHT_CHANGE_MAX_NUM_PTS-1)
goto __BreakOut;
}
pPoly = pHeightChangePoly;
vVert = vHeightChangePos;
vVecToNext = vNextVert - vVert;
vVecToNext.Normalize();
bHasHeightChange = FindHeightChangePosAlongLine(vVert, vNextVert, pNextPoly, pPoly, pHeightChangePoly, vHeightChangePos, false, domain);
}
}
}
vFinalPoints[iNumFinalPoints] = vNextVert;
pFinalPolys[iNumFinalPoints] = pNextPoly;
iFinalWaypointFlags[iNumFinalPoints] = pPathRequest->m_WaypointFlags[i+1];
iNumFinalPoints++;
if(iNumFinalPoints==PRESERVE_HEIGHT_CHANGE_MAX_NUM_PTS-1)
break;
}
__BreakOut:
pPathRequest->m_PathPoints[0] = vFinalPoints[0];
pPathRequest->m_PathPolys[0] = pFinalPolys[0];
pPathRequest->m_WaypointFlags[0] = iFinalWaypointFlags[0];
vFinalPoints[iNumFinalPoints] = pPathRequest->m_PathPoints[pPathRequest->m_iNumPoints-1];
pFinalPolys[iNumFinalPoints] = pPathRequest->m_PathPolys[pPathRequest->m_iNumPoints-1];
iFinalWaypointFlags[iNumFinalPoints] = pPathRequest->m_WaypointFlags[pPathRequest->m_iNumPoints-1];
iNumFinalPoints++;
pPathRequest->m_iNumPoints = 1;
static const float fMaxDot = rage::Cosf(( DtoR * 1.0f));
// Now go through points, and remove any between which there is no significant gradient change
u32 p;
for(p=1; p<iNumFinalPoints-1; p++)
{
const Vector3 & vLastPt = vFinalPoints[p-1];
const Vector3 & vPt = vFinalPoints[p];
const Vector3 & vNextPt = vFinalPoints[p+1];
Vector3 vFromLast = vPt - vLastPt;
Vector3 vToNext = vNextPt - vPt;
vFromLast.Normalize();
vToNext.Normalize();
const float fDot = DotProduct(vFromLast, vToNext);
// Gradients are similar, so central point can be removed
if(fDot > fMaxDot && (iFinalWaypointFlags[p-1].GetSpecialAction() == iFinalWaypointFlags[p].GetSpecialAction()
&& iFinalWaypointFlags[p].GetSpecialAction() == iFinalWaypointFlags[p+1].GetSpecialAction()))
{
}
else
{
pPathRequest->m_PathPoints[pPathRequest->m_iNumPoints] = vFinalPoints[p];
pPathRequest->m_PathPolys[pPathRequest->m_iNumPoints] = pFinalPolys[p];
pPathRequest->m_WaypointFlags[pPathRequest->m_iNumPoints] = iFinalWaypointFlags[p];
pPathRequest->m_iNumPoints++;
}
if(pPathRequest->m_iNumPoints == MAX_NUM_PATH_POINTS-1)
break;
}
#if __ASSERT
for(s32 t=0; t<pPathRequest->m_iNumPoints; t++)
{
Assert( rage::FPIsFinite(pPathRequest->m_PathPoints[t].x) && rage::FPIsFinite(pPathRequest->m_PathPoints[t].y) && rage::FPIsFinite(pPathRequest->m_PathPoints[t].z) );
}
#endif
#if !__FINAL
m_PerfTimer->Stop();
pPathRequest->m_fMillisecsToPostProcessZHeights = (float) m_PerfTimer->GetTimeMS();
#endif
}
//********************************************************************
// MinimisePathDistance
// This function minimises the distance of the path by examining
// each set of 3 nodes, and moving the center one within its
// polygon to find a position where the total length of the two
// connected line segments are least - and the LOS is maintained
// to each of the adjoined points.
//********************************************************************
#define MINIMISE_PATH_RANDDIST_SQR 36.0f
void CPathServerThread::MinimisePathDistance(CPathRequest * pPathRequest)
{
if(pPathRequest->m_iNumPoints <= 2)
return;
Assert(pPathRequest->m_iNumPoints > 2); // Try to catch weird num-points bug
float fRandErrorRangeSqr = 0.0f;
if(pPathRequest->m_bRandomisePoints)
{
m_RandomNumGen.Reset(pPathRequest->m_iPedRandomSeed);
}
#if !__FINAL
m_PerfTimer->Reset();
m_PerfTimer->Start();
#endif
static Vector3 vPolyPts[NAVMESHPOLY_MAX_NUM_VERTICES];
s32 p;
u32 v;
u32 n;
Vector3 * pPrevPt, * pCurrentPt, * pNextPt;
TNavMeshPoly * pPrevPolyPt, * pNextPolyPt;
CNavMesh * pNavMesh;
TNavMeshPoly * pPoly;
float fPrevToCurrDist;
float fCurrToNextDist;
float fPrevToCurrDistSqr;
float fCurrToNextDistSqr;
float fTrialPrevToCurrDist;
float fTrialCurrToNextDist;
float fTrialPrevToCurrDistSqr;
float fTrialCurrToNextDistSqr;
static const float fSmallDistance = 0.1f;
static const float fSmallDistanceSqr = 0.1f * 0.1f;
float fBestTotalDiff, fDiff;
s32 iNumPoints = pPathRequest->m_iNumPoints;
Assert( ((iNumPoints-1) > 0) && ((iNumPoints-1) < (s32)pPathRequest->m_iNumPoints)); // Try to catch weird num-points bug
const aiNavDomain domain = pPathRequest->GetMeshDataSet();
for(p=1; p<iNumPoints-1; p++)
{
Assert(iNumPoints > 2); // Try to catch weird num-points bug
pPoly = pPathRequest->m_PathPolys[p];
// If there was no poly set for this point, it means it was either the start/end point (but not in this case)
// or was a point generated along the edge between 2 polys for which there was no LOS between.
// For now, we shall skip the processing of these cases - hoping that the generated point is okay.
if(!pPoly)
continue;
const u32 iPointEnum = pPoly->GetPointEnum();
// Assert(iPointEnum != NAVMESHPOLY_POINTENUM_SPECIAL_LINK_ENDPOS);
if((iPointEnum == NAVMESHPOLY_POINTENUM_CENTROID /*|| iPointEnum == NAVMESHPOLY_POINTENUM_UNUSED*/ )
&& !pPathRequest->m_bRandomisePoints)
continue;
if(pPathRequest->m_bRandomisePoints)
fRandErrorRangeSqr = m_RandomNumGen.GetVaried(MINIMISE_PATH_RANDDIST_SQR);
pNavMesh = CPathServer::GetNavMeshFromIndex(pPoly->GetNavMeshIndex(), domain);
if(!pNavMesh)
continue;
Assert(p+1 < iNumPoints);
pPrevPt = &pPathRequest->m_PathPoints[p-1];
pCurrentPt = &pPathRequest->m_PathPoints[p];
pNextPt = &pPathRequest->m_PathPoints[p+1];
Assert( rage::FPIsFinite(pPrevPt->x) && rage::FPIsFinite(pPrevPt->y) && rage::FPIsFinite(pPrevPt->z) );
Assert( rage::FPIsFinite(pCurrentPt->x) && rage::FPIsFinite(pCurrentPt->y) && rage::FPIsFinite(pCurrentPt->z) );
Assert( rage::FPIsFinite(pNextPt->x) && rage::FPIsFinite(pNextPt->y) && rage::FPIsFinite(pNextPt->z) );
pPrevPolyPt = pPathRequest->m_PathPolys[p-1];
pNextPolyPt = pPathRequest->m_PathPolys[p+1];
fPrevToCurrDistSqr = (*pCurrentPt - *pPrevPt).Mag2() + fRandErrorRangeSqr;
fCurrToNextDistSqr = (*pNextPt - *pCurrentPt).Mag2() + fRandErrorRangeSqr;
// Calc the actual distances from the squared distances
if(fPrevToCurrDistSqr > fSmallDistanceSqr)
{
Assert(fPrevToCurrDistSqr > 0.0f);
fPrevToCurrDist = 1.0f / invsqrtf_fast(fPrevToCurrDistSqr);
}
else
{
fPrevToCurrDist = fSmallDistance;
}
if(fCurrToNextDistSqr > fSmallDistanceSqr)
{
Assert(fCurrToNextDistSqr > 0.0f);
fCurrToNextDist = 1.0f / invsqrtf_fast(fCurrToNextDistSqr);
}
else
{
fCurrToNextDist = fSmallDistance;
}
for(v=0; v<pPoly->GetNumVertices(); v++)
{
pNavMesh->GetVertex( pNavMesh->GetPolyVertexIndex(pPoly, v), vPolyPts[v]);
}
const u32 iNumClosePts = CreatePointsInPoly(
pNavMesh,
pPoly,
vPolyPts,
ShouldUseMorePointsForPoly(*pPoly, pPathRequest) ? POINTSINPOLY_EXTRA : POINTSINPOLY_NORMAL,
m_Vars.g_vClosestPtsInPoly);
Assert(iNumClosePts < NUM_CLOSE_PTS_IN_POLY);
if(pPathRequest->m_bRandomisePoints && iNumClosePts > 1)
{
for(n=0; n<iNumClosePts; n++)
{
if(n != iPointEnum)
{
m_Vars.g_vClosestPtsInPoly[n] = CreateRandomPointInPoly(pPoly, vPolyPts);
}
}
}
// To start with we'll set our best choice as out existing (current) point
fBestTotalDiff = 0.0f;
for(n=0; n<iNumClosePts; n++)
{
// Don't bother processing for the point which we already have
if(n == pPoly->GetPointEnum())
continue;
if(pPathRequest->m_bRandomisePoints)
fRandErrorRangeSqr = m_RandomNumGen.GetVaried(MINIMISE_PATH_RANDDIST_SQR);
fTrialPrevToCurrDistSqr = (m_Vars.g_vClosestPtsInPoly[n] - *pPrevPt).Mag2() + fRandErrorRangeSqr;
fTrialCurrToNextDistSqr = (*pNextPt - m_Vars.g_vClosestPtsInPoly[n]).Mag2() + fRandErrorRangeSqr;
// We can early-out by comparing both the squared distances
if(fTrialPrevToCurrDistSqr > fPrevToCurrDistSqr && fTrialCurrToNextDistSqr > fCurrToNextDistSqr)
continue;
// Calc the actual distances from the squared distances
if(fTrialPrevToCurrDistSqr > fSmallDistanceSqr)
{
Assert(fTrialPrevToCurrDistSqr > 0.0f);
fTrialPrevToCurrDist = 1.0f / invsqrtf_fast(fTrialPrevToCurrDistSqr);
}
else
{
fTrialPrevToCurrDist = fSmallDistance;
}
if(fTrialCurrToNextDistSqr > fSmallDistanceSqr)
{
Assert(fTrialCurrToNextDistSqr > 0.0f);
fTrialCurrToNextDist = 1.0f / invsqrtf_fast(fTrialCurrToNextDistSqr);
}
else
{
fTrialCurrToNextDist = fSmallDistance;
}
// If this is a better path, fDiff will be a positive value
fDiff = (fPrevToCurrDist + fCurrToNextDist) - (fTrialPrevToCurrDist + fTrialCurrToNextDist);
// If this point placement is better than our current best effort, see if the LOS is clear
if(fDiff > fBestTotalDiff)
{
// If there was no previous poly, it indicates that the prev point was a waypoint
// inserted on the polygon edge in order to see round a corner - so it is guaranteed
// to be visible from anywhere in this poly
if(pPrevPolyPt)
{
// Test LOS from current point to last point
IncTimeStamp();
if(!TestNavMeshLOS(m_Vars.g_vClosestPtsInPoly[n], *pPrevPt, pPrevPolyPt, pPoly, NULL, m_Vars.m_iLosFlags, domain))
{
continue;
}
}
// If there is no next poly, it indicates that the next point was a waypoint
// inserted on the polygon edge in order to see round a corner - so it is guaranteed
// to be visible from anywhere in this poly
if(pNextPolyPt)
{
// Test LOS from current point to next point
IncTimeStamp();
if(!TestNavMeshLOS(m_Vars.g_vClosestPtsInPoly[n], *pNextPt, pNextPolyPt, pPoly, NULL, m_Vars.m_iLosFlags, domain))
{
continue;
}
}
// If we're considering dynamic objects, then test those as well
if(!TestDynamicObjectLOS(m_Vars.g_vClosestPtsInPoly[n], *pPrevPt))
{
continue;
}
// If we're considering dynamic objects, then test those as well
if(!TestDynamicObjectLOS(m_Vars.g_vClosestPtsInPoly[n], *pNextPt))
{
continue;
}
// The distance is shorter, and all the LOS tests have passed - so this is a better point for the route.
fBestTotalDiff = fDiff;
Assert(p < MAX_NUM_PATH_POINTS+1);
Assert(n < NUM_CLOSE_PTS_IN_POLY);
pPathRequest->m_PathPoints[p] = m_Vars.g_vClosestPtsInPoly[n];
pPoly->SetPointEnum(n);
fPrevToCurrDistSqr = fTrialPrevToCurrDistSqr;
fCurrToNextDistSqr = fTrialCurrToNextDistSqr;
fPrevToCurrDist = fTrialPrevToCurrDist;
fCurrToNextDist = fTrialCurrToNextDist;
}
}
}
#ifdef GTA_ENGINE
#if AI_OPTIMISATIONS_OFF || NAVMESH_OPTIMISATIONS_OFF
Assertf(pPathRequest->m_iNumPoints > 2, "path handle:%p", pPathRequest->m_hHandle); // Try to catch weird num-points bug
#endif
#endif
#if !__FINAL
m_PerfTimer->Stop();
pPathRequest->m_fMillisecsToMinimisePathLength = (float) m_PerfTimer->GetTimeMS();
#endif
}
void CPathServerThread::PullPathOutFromEdges(CPathRequest * pPathRequest, bool bDoStringPull)
{
if (pPathRequest->m_iNumPoints <= 2 && pPathRequest->m_iNumPoints < MAX_NUM_PATH_POINTS)
return;
#if !__FINAL
m_PerfTimer->Reset();
m_PerfTimer->Start();
#endif
const float fPushDist = (pPathRequest->m_bPullFromEdgeExtra ? CPathServerThread::ms_fPullOutFromEdgesDistanceExtra : CPathServerThread::ms_fPullOutFromEdgesDistance);
static const float fPushThreshold = 0.999f;
Vector3 vCacheVec;
const aiNavDomain domain = pPathRequest->GetMeshDataSet();
Vector3* pCachePushDir = NULL;
int nPoints = pPathRequest->m_iNumPoints - 1; // We check one node ahead
for (int i = 1; i < nPoints; ++i) // and one behind
{
Vector3 vPushDir;
const Vector3 vPrevPt = pPathRequest->m_PathPoints[i-1];
const Vector3 vThisPt = pPathRequest->m_PathPoints[i];
const Vector3 vNextPt = pPathRequest->m_PathPoints[i+1];
bool bSkip = false;
bool bFirstFrame = (i == 1);
if (pCachePushDir)
vPushDir = *pCachePushDir;
else if (!bFirstFrame)
bSkip = true;
//Scan ahead 2 nodes and cache that direction to push since if we push without caching
//we will be pushing in the wrong direction since the last push will then move the "prev vector"
if (i < nPoints - 1)
{
const Vector3 vNextNextPt = pPathRequest->m_PathPoints[i+2];
if (CalcCornerPushDir(vCacheVec, vThisPt, vNextPt, vNextNextPt, fPushThreshold))
pCachePushDir = &vCacheVec;
else
pCachePushDir = NULL;
}
if (bFirstFrame && !CalcCornerPushDir(vPushDir, vPrevPt, vThisPt, vNextPt, fPushThreshold))
continue;
else if (bSkip)
continue; // The cache was invalid (straight)
// Not having valid polys might cause a crash?
TNavMeshPoly* pPrevPoly = pPathRequest->m_PathPolys[i-1];
TNavMeshPoly* pThisPoly = pPathRequest->m_PathPolys[i];
TNavMeshPoly* pNextPoly = pPathRequest->m_PathPolys[i+1];
if (!pPrevPoly || !pThisPoly || !pNextPoly)
continue;
// Don't pull and stuff if we got any special action near, such as climbs
if (pPathRequest->m_WaypointFlags[i-1].GetSpecialAction() ||
pPathRequest->m_WaypointFlags[i].GetSpecialAction() ||
pPathRequest->m_WaypointFlags[i+1].GetSpecialAction())
{
continue;
}
//// If we pull points too close to each other and one of them fail the angle between them will be quite bad
//if (vPrevPt.Dist2(vThisPt) < rage::square(fPushDist * 2.f) ||
// vNextPt.Dist2(vThisPt) < rage::square(fPushDist * 2.f))
//{
// continue;
//}
// Do those LOS tests...
// Consider a table like this: [7,6,5,4 (middle) 3,2,1,0], where middle is our current position
// We loop through and test from middle and see how far we got LOS on each side
// The left in the table represent pulling the corner point out, the right is pulling it in
// We do consider LOS distance on each side, and try to find a good position to pull or push our position to
// We skew this algorithm slightly towards pushing out from corners as that should be better
Vector3 vIntersectPoint;
const Vector2 vLosDir = Vector2(vPushDir * fPushDist, Vector2::kXY);
int nIn, nOut;
float fIn = 0.f;
float fOut = 0.f;
const float fPushTableScale[5] = {1.0f, 0.75f, 0.5f, 0.25f, 0.0f};
// We first check LOS distance how close we are to the corner
for (nIn = 0; nIn < 4; ++nIn, fIn += 1.f)
{
IncTimeStamp();
if (TestNavMeshLOS(vThisPt, -vLosDir * fPushTableScale[nIn], &vIntersectPoint, pThisPoly, m_Vars.m_iLosFlags, domain) &&
TestDynamicObjectLOS(vThisPt, vThisPt - vPushDir * fPushDist * fPushTableScale[nIn]))
{
break;
}
}
// And how far we can push out from that corner
for (nOut = 0; nOut < 4; ++nOut, fOut += 1.f)
{
IncTimeStamp();
if (TestNavMeshLOS(vThisPt, vLosDir * fPushTableScale[nOut], &vIntersectPoint, pThisPoly, m_Vars.m_iLosFlags, domain) &&
TestDynamicObjectLOS(vThisPt, vThisPt + vPushDir * fPushDist * fPushTableScale[nOut]))
{
break;
}
}
// Full LOS to either side, no need to move this path position around
if (nIn == 0 && nOut ==0)
continue;
// Map the values to match the table [8,7,6,5 (4) 3,2,1,0], this should be skewed by adding the middle as a value, and round up
const int iAvg = (int)ceil(((fIn + (8.f - fOut)) * 0.5f) + 0.5f);
if (nIn > 0)
pPathRequest->m_WaypointFlags[i].m_iSpecialActionFlags |= WAYPOINT_FLAG_IS_NARROW_IN;
if (nOut > 0)
pPathRequest->m_WaypointFlags[i].m_iSpecialActionFlags |= WAYPOINT_FLAG_IS_NARROW_OUT;
// Map the values to match the table [7,6,5,4 (middle) 3,2,1,0], should not be skewed or we won't favor pushing the corner out
const bool bIsPullingOut = iAvg > 4;
const int iPushIndex = (bIsPullingOut ? 7 - iAvg : iAvg);
//
const Vector3 vNewPathPos = vThisPt + vPushDir * fPushDist * fPushTableScale[iPushIndex] * (bIsPullingOut ? 1.f : -1.f);
IncTimeStamp();
const Vector2 vLosDirFinal = Vector2(vNewPathPos - vThisPt, Vector2::kXY);
TNavMeshPoly* pNewPoly = TestNavMeshLOS(vThisPt, vLosDirFinal, &vIntersectPoint, pThisPoly, m_Vars.m_iLosFlags, domain);
if (pNewPoly)
{
// And then we just test to see if this pull is a valid position to reach in the path
IncTimeStamp();
if (!TestNavMeshLOS(vNewPathPos, vPrevPt, pPrevPoly, pNewPoly, NULL, m_Vars.m_iLosFlags, domain))
continue;
if (!TestDynamicObjectLOS(vNewPathPos, vPrevPt))
continue;
IncTimeStamp();
if (!TestNavMeshLOS(vNewPathPos, vNextPt, pNextPoly, pNewPoly, NULL, m_Vars.m_iLosFlags, domain))
continue;
if (!TestDynamicObjectLOS(vNewPathPos, vNextPt))
continue;
pPathRequest->m_PathPoints[i] = vNewPathPos;
pPathRequest->m_PathPolys[i] = pNewPoly;
}
}
// We do another pass of string pull, in case we can get rid of smaller quirky points hidden behind corners
// This might defeat our purpose slightly though, we should only string pull quirks so we might need to
// improve this part. Maybe it is better to avoid the quirk in the pull out instead though.
if (bDoStringPull)
{
for (int i = 2; i < nPoints; ++i)
{
const Vector3 vPrevPrevPt = pPathRequest->m_PathPoints[i-2];
const Vector3 vPrevPt = pPathRequest->m_PathPoints[i-1];
const Vector3 vThisPt = pPathRequest->m_PathPoints[i];
const Vector3 vNextPt = pPathRequest->m_PathPoints[i+1];
// const Vector3 vNextNextPt = pPathRequest->m_PathPoints[i+2];
// Not having valid polys might cause a crash?
TNavMeshPoly* pPrevPrevPoly = pPathRequest->m_PathPolys[i-2];
TNavMeshPoly* pPrevPoly = pPathRequest->m_PathPolys[i-1];
TNavMeshPoly* pThisPoly = pPathRequest->m_PathPolys[i];
TNavMeshPoly* pNextPoly = pPathRequest->m_PathPolys[i+1];
if (!pPrevPoly || !pNextPoly)
continue;
// Don't pull and stuff if we got any special action near, such as climbs
if (pPathRequest->m_WaypointFlags[i-1].GetSpecialAction() || pPathRequest->m_WaypointFlags[i+1].GetSpecialAction())
continue;
// Pull out should not cause greater dist than this
const float fMinDistSqr = rage::square(fPushDist * 2.0f);
// Don't stringpull when points are not close as that could defeat our purpose
if (vThisPt.Dist2(vPrevPt) > fMinDistSqr && vThisPt.Dist2(vNextPt) > fMinDistSqr)
continue;
// And the angle has to make a sharp corner "quirk"
static const float fTolerance = 0.15f; // Almost 90deg
if (!IsPointsMakingAQuirk(vPrevPrevPt, vPrevPt, vThisPt, vNextPt, fTolerance) && vThisPt.Dist2(vPrevPt) > rage::square(0.1f))
continue;
int iPointToRemove = i;
// Do those LOS tests...
IncTimeStamp();
if (!TestNavMeshLOS(vPrevPt, vNextPt, pPrevPoly, pNextPoly, NULL, m_Vars.m_iLosFlags, domain) || !TestDynamicObjectLOS(vPrevPt, vNextPt))
{
// Not having valid polys might cause a crash
if (!pPrevPrevPoly || !pThisPoly)
continue;
// Shit ok, let's try with the previous one instead
// Don't pull and stuff if we got any special action near, such as climbs
if (pPathRequest->m_WaypointFlags[i-2].GetSpecialAction() || pPathRequest->m_WaypointFlags[i].GetSpecialAction())
continue;
// Don't stringpull when points are not close as that could defeat our purpose
if (vThisPt.Dist2(vPrevPt) > fMinDistSqr)
continue;
// Do those LOS tests...
IncTimeStamp();
if (!TestNavMeshLOS(vPrevPrevPt, vThisPt, pPrevPrevPoly, pThisPoly, NULL, m_Vars.m_iLosFlags, domain) || !TestDynamicObjectLOS(vPrevPrevPt, vThisPt))
continue;
// So remove the one behind us instead!
iPointToRemove = i - 1;
}
for (int j = iPointToRemove; j < nPoints; ++j)
{
pPathRequest->m_PathPoints[j] = pPathRequest->m_PathPoints[j+1];
pPathRequest->m_PathPolys[j] = pPathRequest->m_PathPolys[j+1];
pPathRequest->m_WaypointFlags[j] = pPathRequest->m_WaypointFlags[j+1];
}
--i;
--nPoints;
--pPathRequest->m_iNumPoints;
}
}
#if __ASSERT
for(s32 t=0; t<pPathRequest->m_iNumPoints; t++)
{
Assert( rage::FPIsFinite(pPathRequest->m_PathPoints[t].x) && rage::FPIsFinite(pPathRequest->m_PathPoints[t].y) && rage::FPIsFinite(pPathRequest->m_PathPoints[t].z) );
}
#endif
#if !__FINAL
m_PerfTimer->Stop();
pPathRequest->m_fMillisecsToPullOutFromEdges = (float) m_PerfTimer->GetTimeMS();
#endif
}
//****************************************************************************
// CalcPathControlPoints
// This is a new approach to the problem which SmoothPath() tries to fix.
// At each sharp bend in the path, a bezier curve is created to try and
// cut off as much of the sharp corner as possible. The bezier is evaluated
// to a fixed resolution, and a number of line-segments are tested against
// the world in an attempt to determine whether the new route is clear of
// obstacles. If so, then some information is stored with the path about
// the control points which were chosen. If obstacles do exist, then we can
// attempt again with a different set of control points - or abort the
// operation for this corner in the path.
void
CPathServerThread::CalcPathBezierControlPoints(CPathRequest * pPathRequest)
{
// If this is a straight line, then there's nothing to do..
if(pPathRequest->m_iNumPoints < 3)
{
return;
}
#if !__FINAL
m_PerfTimer->Reset();
m_PerfTimer->Start();
#endif
static const float fMinSmoothDist = 1.0f;
static const float fSplineStep = 1.0f / ((float)PATHSPLINE_NUM_TEST_SUBDIVISIONS);
// Use this variable to prevent spline overlaps
float fDistPreviousPt3ToNext = FLT_MAX;
const aiNavDomain domain = pPathRequest->GetMeshDataSet();
for(s32 iCurrent=1; iCurrent<pPathRequest->m_iNumPoints-1; iCurrent++)
{
const int iLast = iCurrent-1;
const int iNext = iCurrent+1;
const Vector3 & vLast = pPathRequest->m_PathPoints[iLast];
const Vector3 & vCurrent = pPathRequest->m_PathPoints[iCurrent];
const Vector3 & vNext = pPathRequest->m_PathPoints[iNext];
const Vector3 vLastToCurrent = vCurrent - vLast;
const Vector3 vCurrentToNext = vNext - vCurrent;
const float fDistLastToCurrent = vLastToCurrent.Mag();
const float fDistCurrentToNext = vCurrentToNext.Mag();
// If the current waypoint is a climb/drop/etc - then don't use splines here
const u32 iWpt = pPathRequest->m_WaypointFlags[iCurrent].GetSpecialActionFlags();
if(IsSpecialActionClimbOrDrop(iWpt))
{
fDistPreviousPt3ToNext = FLT_MAX;
continue;
}
if(fDistLastToCurrent < fMinSmoothDist || fDistCurrentToNext < fMinSmoothDist)
{
fDistPreviousPt3ToNext = FLT_MAX;
continue;
}
// If the angle between the two waypoints is shallow, then don't bother with using splines
Vector3 vLastToCurrentNormalized = vLastToCurrent;
Vector3 vCurrentToNextNormalized = vCurrentToNext;
NormalizeAndMag(vLastToCurrentNormalized);
vCurrentToNextNormalized.Normalize();
static const float fDeadAhead = rage::Cosf(( DtoR * 8.0f));
if(DotProduct(vLastToCurrentNormalized, vCurrentToNextNormalized) > fDeadAhead)
{
fDistPreviousPt3ToNext = FLT_MAX;
continue;
}
Vector3 vCtrlPt1, vCtrlPt2, vCtrlPt3, vCtrlPt4;
for(int s=0; s<PATHSPLINE_NUM_TEST_DISTANCES; s++)
{
// If the distance from vCtrlPt2->vCurrent is MORE than the previous vCtrlPt3->vNext
// Then we CANNOT use this enumerant value of 's' since it will OVERLAP the last spline,
// and cause awkward situations when following the path (such as doubling-back)
vCtrlPt2 = vLast + (vLastToCurrent * g_fPathSplineDists[s]);
const float fDistPt2ToCurrent = (vCurrent-vCtrlPt2).Mag();
if(fDistPt2ToCurrent > fDistPreviousPt3ToNext)
{
pPathRequest->m_PathResultInfo.m_fPathCtrlPt2TVals[iCurrent] = 1.0f - (fDistPreviousPt3ToNext/fDistLastToCurrent);
}
else
{
pPathRequest->m_PathResultInfo.m_fPathCtrlPt2TVals[iCurrent] = 0.0f;
}
CalcPathSplineCtrlPts(s, pPathRequest->m_PathResultInfo.m_fPathCtrlPt2TVals[iCurrent], vLast, vNext, vLastToCurrent, vCurrentToNext, vCtrlPt1, vCtrlPt2, vCtrlPt3, vCtrlPt4);
// Now create PATHSPLINE_NUM_TEST_SUBDIVISIONS points along this spline.
Vector3 vSplinePtA = vCtrlPt2;
Vector3 vEndPos;
float u;
bool bSplineOk = true;
TNavMeshPoly * pEndingPoly = NULL;
// Efficiently find the initial pEndingPoly.
// We already know which polygon vCurrent is upon, so we can do a navmesh LOS
// to vCtrlPt2 (the starting point for this spline curve) to obtain the polygon
// directly under that point. This should be hugely more efficient than doing
// a full polygon query in the navmesh.
TNavMeshPoly * pPolyUnderCurrent = pPathRequest->m_PathPolys[iCurrent];
// pPolyUnderCurrent may be NULL if this point was added before/after a non-standard adjacency
// such as a climb or drop-down, etc.
if(pPolyUnderCurrent)
{
IncTimeStamp();
const Vector2 vDirToCtrlPt2(vCtrlPt2.x - vCurrent.x, vCtrlPt2.y - vCurrent.y);
pEndingPoly = TestNavMeshLOS(vCurrent, vDirToCtrlPt2, &vEndPos, pPolyUnderCurrent, m_Vars.m_iLosFlags, domain);
if(pEndingPoly)
{
for(u=fSplineStep; u<=1.0f; u+=fSplineStep)
{
const Vector3 vSplinePtB = PathSpline(vCtrlPt1, vCtrlPt2, vCtrlPt3, vCtrlPt4, u);
const Vector2 vDir(vSplinePtB.x - vSplinePtA.x, vSplinePtB.y - vSplinePtA.y);
IncTimeStamp();
pEndingPoly = TestNavMeshLOS(vSplinePtA, vDir, &vEndPos, pEndingPoly, m_Vars.m_iLosFlags, domain);
if(pEndingPoly && !TestDynamicObjectLOS(vSplinePtA, vSplinePtB))
pEndingPoly = NULL;
if(!pEndingPoly)
{
// Fail!
bSplineOk = false;
break;
}
vSplinePtA = vSplinePtB;
}
// If this spline worked ok (ie. all the test segments had a clear LOS, then we'll use this)
// Otherwise the algorithm will test another spline for this corner, using different ctrl pts.
if(bSplineOk)
{
pPathRequest->m_PathResultInfo.m_iPathSplineDistances[iCurrent] = s;
break;
}
}
}
}
// We have a spline at this corner, so calculate this distance to use in the overlap test at the next corner
if(pPathRequest->m_PathResultInfo.m_iPathSplineDistances[iCurrent] != -1)
{
fDistPreviousPt3ToNext = (vNext - vCtrlPt3).Mag(); // xy?
}
// No spline at this corner, so reset this value
else
{
fDistPreviousPt3ToNext = FLT_MAX;
pPathRequest->m_PathResultInfo.m_fPathCtrlPt2TVals[iCurrent] = 0.0f;
}
}
#if __ASSERT
for(s32 t=0; t<pPathRequest->m_iNumPoints; t++)
{
Assert( rage::FPIsFinite(pPathRequest->m_PathPoints[t].x) && rage::FPIsFinite(pPathRequest->m_PathPoints[t].y) && rage::FPIsFinite(pPathRequest->m_PathPoints[t].z) );
}
#endif
#if !__FINAL
m_PerfTimer->Stop();
pPathRequest->m_fMillisecsToSmoothPath = (float) m_PerfTimer->GetTimeMS();
#endif // !__FINAL
}
//******************************************************************************
// SmoothPath
// Smooth the path by identifying sharp corners, and cutting the corners off.
// We do this by moving one vertex & inserting another.
// If this creates a path which is larger than 'iMaxNumPoints' then we will
// have to truncate the path up to where we ended processing it..
//******************************************************************************
// Add all the path points into a list
atArray<Vector3> newPoints(0, 32);
atArray<TNavMeshWaypointFlag> newWayPoints(0, 32);
void
CPathServerThread::SmoothPath(CPathRequest * pPathRequest)
{
// If this is a straight line, then there's nothing to do..
if(pPathRequest->m_iNumPoints < 3)
{
return;
}
#if !__FINAL
m_PerfTimer->Reset();
m_PerfTimer->Start();
#endif
static const float fCosSharpAngle = rage::Cosf(( DtoR * 67.5f));
static const float fSmallDistanceSqr = 0.35f * 0.35f;
static const int NUM_TESTS = 5;
float fLastToCurrTestDists[NUM_TESTS];
float fCurrToNextTestDists[NUM_TESTS];
Vector3 vTmp;
newPoints.clear();
newWayPoints.clear();
s32 p;
for(p=0; p<pPathRequest->m_iNumPoints; p++)
{
newPoints.Append() = pPathRequest->m_PathPoints[p];
newWayPoints.Append() = pPathRequest->m_WaypointFlags[p];
}
bool bMadeChanges = false;
int iCurrentPt = 1;
const aiNavDomain domain = pPathRequest->GetMeshDataSet();
while(iCurrentPt < newPoints.GetCount()-1 && iCurrentPt < MAX_NUM_PATH_POINTS)
{
// Can only cut corners which aren't special waypoints..
if(newWayPoints[iCurrentPt].m_iSpecialActionFlags != 0)
{
iCurrentPt++;
continue;
}
const Vector3 & vLastPt = newPoints[iCurrentPt-1];
const Vector3 & vCurrentPt = newPoints[iCurrentPt];
const Vector3 & vNextPt = newPoints[iCurrentPt+1];
Vector3 vLastToCurr = vCurrentPt - vLastPt;
Vector3 vCurrToNext = vNextPt - vCurrentPt;
float fLastToCurrDist2 = vLastToCurr.Mag2();
float fCurrToNextDist2 = vCurrToNext.Mag2();
if(fLastToCurrDist2 < fSmallDistanceSqr || fCurrToNextDist2 < fSmallDistanceSqr)
{
iCurrentPt++;
continue;
}
vLastToCurr.NormalizeFast();
vCurrToNext.NormalizeFast();
const float fDot = Dot(vLastToCurr, vCurrToNext);
if(fDot < fCosSharpAngle)
{
bool bGotLos=false;
Vector3 vMidPointFromLast, vMidPointToNext;
float fLastToCurrDist = Sqrtf(fLastToCurrDist2);
float fCurrToNextDist = Sqrtf(fCurrToNextDist2);
// Work out a number of test distances for each vector
fLastToCurrTestDists[0] = fLastToCurrDist * 0.5f;
fLastToCurrTestDists[1] = fLastToCurrDist * 0.75f;
fLastToCurrTestDists[2] = fLastToCurrDist * 0.85f;
fLastToCurrTestDists[3] = Max(fLastToCurrDist - 1.0f, 1.0f);
fLastToCurrTestDists[4] = Max(fLastToCurrDist - 0.5f, 1.0f);
fCurrToNextTestDists[0] = fCurrToNextDist * 0.5f;
fCurrToNextTestDists[1] = fCurrToNextDist * 0.25f;
fCurrToNextTestDists[2] = fCurrToNextDist * 0.15f;
fCurrToNextTestDists[3] = Min(1.0f, fCurrToNextDist);
fCurrToNextTestDists[4] = Min(0.5f, fCurrToNextDist);
for(int iTest=0; iTest<NUM_TESTS; iTest++)
{
vMidPointFromLast = vLastPt + (vLastToCurr * fLastToCurrTestDists[iTest]);
vMidPointToNext = vCurrentPt + (vCurrToNext * fCurrToNextTestDists[iTest]);
// Is there a clear navmesh LOS between these new midpoints?
u32 iStartNavMesh = CPathServer::GetNavMeshIndexFromPosition(vMidPointFromLast, domain);
if(iStartNavMesh == NAVMESH_NAVMESH_INDEX_NONE)
continue;
CNavMesh * pStartNavMesh = CPathServer::GetNavMeshFromIndex(iStartNavMesh, domain);
if(!pStartNavMesh)
continue;
u32 iStartPolyIndex = pStartNavMesh->GetPolyBelowPoint(vMidPointFromLast + Vector3(0, 0, 1.0f), vTmp, 5.0f);
if(iStartPolyIndex == NAVMESH_POLY_INDEX_NONE)
continue;
TNavMeshPoly * pStartPoly = pStartNavMesh->GetPoly(iStartPolyIndex);
Assert(pStartPoly);
u32 iEndNavMesh = CPathServer::GetNavMeshIndexFromPosition(vMidPointToNext, domain);
if(iEndNavMesh == NAVMESH_NAVMESH_INDEX_NONE)
continue;
CNavMesh * pEndNavMesh = CPathServer::GetNavMeshFromIndex(iEndNavMesh, domain);
if(!pEndNavMesh)
continue;
u32 iEndPolyIndex = pEndNavMesh->GetPolyBelowPoint(vMidPointToNext + Vector3(0, 0, 1.0f), vTmp, 5.0f);
if(iEndPolyIndex == NAVMESH_POLY_INDEX_NONE)
continue;
TNavMeshPoly * pEndPoly = pEndNavMesh->GetPoly(iEndPolyIndex);
Assert(pEndPoly);
IncTimeStamp();
bGotLos = TestNavMeshLOS(vMidPointFromLast, vMidPointToNext, pEndPoly, pStartPoly, NULL, m_Vars.m_iLosFlags, domain);
if(bGotLos)
{
bGotLos = TestDynamicObjectLOS(vMidPointFromLast, vMidPointToNext);
if(bGotLos)
break;
}
}
if(bGotLos)
{
newPoints[iCurrentPt] = vMidPointFromLast;
newWayPoints[iCurrentPt].Clear();
newPoints.Insert(iCurrentPt+1) = vMidPointToNext;
newWayPoints.Insert(iCurrentPt+1).Clear();
bMadeChanges = true;
}
}
iCurrentPt++;
}
// Now copy the new points back into the path request
if(bMadeChanges)
{
s32 iNum = Min(newPoints.GetCount(), MAX_NUM_PATH_POINTS);
for(p=0; p<iNum; p++)
{
pPathRequest->m_PathPoints[p] = newPoints[p];
pPathRequest->m_WaypointFlags[p] = newWayPoints[p];
pPathRequest->m_PathPolys[p] = NULL;
}
pPathRequest->m_iNumPoints = iNum;
}
#if !__FINAL
m_PerfTimer->Stop();
pPathRequest->m_fMillisecsToSmoothPath = (float) m_PerfTimer->GetTimeMS();
#endif
}
bool
CPathServerThread::GetSharedEdgeEndPoints(TNavMeshPoly * pPoly1, TNavMeshPoly * pPoly2, Vector3 & vPoint1, Vector3 & vPoint2, aiNavDomain domain)
{
CNavMesh * pPoly1NavMesh = CPathServer::GetNavMeshFromIndex(pPoly1->GetNavMeshIndex(), domain);
if(!pPoly1NavMesh)
return false;
CNavMesh * pPoly2NavMesh = CPathServer::GetNavMeshFromIndex(pPoly2->GetNavMeshIndex(), domain);
if(!pPoly2NavMesh)
return false;
// Go through pPoly1's edges
for(u32 v=0; v<pPoly1->GetNumVertices(); v++)
{
const TAdjPoly & adjPoly = pPoly1NavMesh->GetAdjacentPoly(pPoly1->GetFirstVertexIndex()+v);
// Adjacent poly is in same NavMesh as pPoly2
if(adjPoly.GetAdjacencyType()==ADJACENCY_TYPE_NORMAL && adjPoly.GetNavMeshIndex(pPoly1NavMesh->GetAdjacentMeshes()) == pPoly2->GetNavMeshIndex())
{
// Is this pPoly2 ?
if(pPoly2NavMesh->GetPoly(adjPoly.GetPolyIndex()) == pPoly2)
{
// Okay, so we've found the shared edge between the 2 polys.
// Now take the line defined by "vBestPointInPoly1 to vBestPointInPoly2" and test
// both endpoints of the shared edge against it. Boint endpoints should be
// to the same side of the line.
// The vertex result is to be the endpoint closest to the line.
const int nextv = (v+1 < pPoly1->GetNumVertices()) ? v+1 : 0;
pPoly1NavMesh->GetVertex( pPoly1NavMesh->GetPolyVertexIndex(pPoly1, v), vPoint1 );
pPoly1NavMesh->GetVertex( pPoly1NavMesh->GetPolyVertexIndex(pPoly1, nextv), vPoint2 );
return true;
}
}
}
return false;
}
// NAME : ObtainEntryEdgeViaConnectionPoly
// PURPOSE : Given pParentPoly and pChildPoly, which are separated by any number of connection polys -
// we wish to find which edge pChildPoly was entered by.
bool CPathServerThread::ObtainEntryEdgeViaConnectionPoly(
TNavMeshPoly * pStartPoly, CNavMesh * pStartNavMesh,
TNavMeshPoly * pTargetPoly, CNavMesh * pTargetNavMesh,
TNavMeshPoly * pRealOrConnectionPoly, CNavMesh * pNavMesh,
TNavMeshPoly * pParentPoly, CNavMesh * pParentNavMesh,
TNavMeshAndPoly & outPrevNavMeshAndPoly,
const Vector3 & vStartExitEdge, const Vector3 & vStartExitEdgeV1, const Vector3 & vStartExitEdgeV2, const aiNavDomain domain ) const
{
// Avoid possibility of infinite recursion by timestamping polygons we visit
pRealOrConnectionPoly->m_TimeStamp = m_iNavMeshPolyTimeStamp;
// When we recurse down to a real polygon, we see if this is the pChildPoly
if( !pRealOrConnectionPoly->GetIsDegenerateConnectionPoly())
{
if(pRealOrConnectionPoly == pTargetPoly)
{
Assert(pParentNavMesh && pParentPoly);
if(pParentNavMesh && pParentPoly)
{
outPrevNavMeshAndPoly.m_iNavMeshIndex = pParentNavMesh->GetIndexOfMesh();
outPrevNavMeshAndPoly.m_iPolyIndex = pParentNavMesh->GetPolyIndex(pParentPoly);
}
return true;
}
}
// For connection polygons, we find all edges pointing in opposite direction to the exit edge
else
{
s32 lasta = pRealOrConnectionPoly->GetNumVertices()-1;
Vector3 vLast, vVert, vAdjacentEdge;
s32 lastvi = pNavMesh->GetPolyVertexIndex(pRealOrConnectionPoly, lasta);
pNavMesh->GetVertex( lastvi, vLast);
for( s32 a=0; a<pRealOrConnectionPoly->GetNumVertices(); a++ )
{
const s32 vi = pNavMesh->GetPolyVertexIndex(pRealOrConnectionPoly, a);
const TAdjPoly & adjacentPoly = pNavMesh->GetAdjacentPoly(lastvi);
const TNavMeshIndex iNavMesh = adjacentPoly.GetNavMeshIndex( pNavMesh->GetAdjacentMeshes() );
if( iNavMesh != NAVMESH_NAVMESH_INDEX_NONE && adjacentPoly.GetAdjacencyType()==ADJACENCY_TYPE_NORMAL )
{
pNavMesh->GetVertex( vi, vVert);
vAdjacentEdge = vVert - vLast;
NAVMESH_OPTIMISATIONS_OFF_ONLY( Assert(vAdjacentEdge.Mag2() > SMALL_FLOAT); )
if(vAdjacentEdge.Mag2() > SMALL_FLOAT)
{
vAdjacentEdge.NormalizeFast();
const float fDot = (vAdjacentEdge.x * vStartExitEdge.x) + (vAdjacentEdge.y * vStartExitEdge.y);
NAVMESH_OPTIMISATIONS_OFF_ONLY( Assert(Abs(fDot) > 0.707f); )
if( fDot > 0.0f )
{
// Test for interval overlap
float fExitEdgeMin = vAdjacentEdge.Dot( vStartExitEdgeV1 );
float fExitEdgeMax = vAdjacentEdge.Dot( vStartExitEdgeV2 );
if(fExitEdgeMin > fExitEdgeMax)
SwapEm(fExitEdgeMin, fExitEdgeMax);
float fAdjacentEdgeMin = vAdjacentEdge.Dot( vLast );
float fAdjacentEdgeMax = vAdjacentEdge.Dot( vVert );
if(fAdjacentEdgeMin > fAdjacentEdgeMax)
SwapEm(fAdjacentEdgeMin, fAdjacentEdgeMax);
const float fIntervalDistance = (fExitEdgeMin < fAdjacentEdgeMin) ?
fAdjacentEdgeMin - fExitEdgeMax : fExitEdgeMin - fAdjacentEdgeMax;
// Overlap exists, so this is an edge leaving the connection poly on the opposite side to the
// exit edge of the previous polyg) and with overlapping extents : ie. a candidate to visit
if(fIntervalDistance < 0.0f)
{
CNavMesh * pAdjNavMesh = CPathServer::GetNavMeshFromIndex(iNavMesh, domain);
if(pAdjNavMesh)
{
TNavMeshPoly * pNextPoly = pAdjNavMesh->GetPoly( pNavMesh->GetAdjacentPoly(lastvi).GetPolyIndex() );
// Only visit a polygon we've not already visited
if( pNextPoly->m_TimeStamp != m_iNavMeshPolyTimeStamp )
{
if( ObtainEntryEdgeViaConnectionPoly( pStartPoly, pStartNavMesh, pTargetPoly, pTargetNavMesh, pNextPoly, pAdjNavMesh, pRealOrConnectionPoly, pNavMesh, outPrevNavMeshAndPoly, vStartExitEdge, vStartExitEdgeV1, vStartExitEdgeV2, domain ) )
{
return true;
}
}
}
}
}
}
vLast = vVert;
lasta = a;
lastvi = vi;
}
}
}
return false;
}
const float g_fEdgeOffsetDist = 0.1f;
bool CPathServerThread::GetPointAlongSharedEdge(
const Vector3 & vBestPointInParentPoly,
const Vector3 & vBestPointInChildPoly,
TNavMeshPoly * pParentPoly,
TNavMeshPoly * pChildPoly,
float fEntityRadius,
Vector3 & vPoint,
bool & bOut_LineActuallyIntersectsEdge,
aiNavDomain domain)
{
if(pChildPoly->m_PathParentPoly==NULL)
{
Assert(pChildPoly->TestFlags(NAVMESHPOLY_ALTERNATIVE_STARTING_POLY));
return false;
}
CNavMesh * pParentNavMesh = CPathServer::GetNavMeshFromIndex(pParentPoly->GetNavMeshIndex(), domain);
Assert(pParentNavMesh);
CNavMesh * pChildNavMesh = CPathServer::GetNavMeshFromIndex(pChildPoly->GetNavMeshIndex(), domain);
Assert(pChildNavMesh);
// Find out if we reached the pChildPoly via a connection polygon.
// If so we will have to perform some more complex logic, if not then we
// can simply traverse the child's edges to find the parent.
const s32 iParentEdge = pChildPoly->m_Struct4.m_iParentExitEdge;
const TAdjPoly adjPoly = pParentNavMesh->GetAdjacentPoly( pParentPoly->GetFirstVertexIndex()+iParentEdge );
const u32 iAdjNavMesh = adjPoly.GetNavMeshIndex( pParentNavMesh->GetAdjacentMeshes() );
if(iAdjNavMesh==NAVMESH_NAVMESH_INDEX_NONE)
return false;
CNavMesh * pAdjNavMesh = CPathServer::GetNavMeshFromIndex(iAdjNavMesh, domain);
if(!pAdjNavMesh)
return false;
TNavMeshPoly * pAdjPoly = pAdjNavMesh->GetPoly(adjPoly.GetPolyIndex());
Vector3 vert1, vert2;
if(pAdjPoly->GetIsDegenerateConnectionPoly())
{
TNavMeshAndPoly prevNavMeshAndPoly;
prevNavMeshAndPoly.Reset();
Vector3 vParentV1, vParentV2, vParentEdge;
pParentNavMesh->GetVertex( pParentNavMesh->GetPolyVertexIndex( pParentPoly, iParentEdge ), vParentV1 );
pParentNavMesh->GetVertex( pParentNavMesh->GetPolyVertexIndex( pParentPoly, (iParentEdge+1)%pParentPoly->GetNumVertices() ), vParentV2 );
vParentEdge = vParentV2 - vParentV1;
vParentEdge.Normalize();
IncTimeStamp();
if( !ObtainEntryEdgeViaConnectionPoly( pParentPoly, pParentNavMesh, pChildPoly, pChildNavMesh, pAdjPoly, pAdjNavMesh, NULL, NULL, prevNavMeshAndPoly, vParentEdge, vParentV1, vParentV2, domain ) )
{
#if NAVMESH_OPTIMISATIONS_OFF
//Assert(false);
#endif
return false;
}
if(prevNavMeshAndPoly.m_iNavMeshIndex == NAVMESH_NAVMESH_INDEX_NONE)
return false;
CNavMesh * pPriorNavMesh = CPathServer::GetNavMeshFromIndex(prevNavMeshAndPoly.m_iNavMeshIndex, domain);
Assert(pPriorNavMesh && pPriorNavMesh->GetIndexOfMesh()==NAVMESH_INDEX_TESSELLATION);
TNavMeshPoly * pPriorPoly = pPriorNavMesh->GetPoly(prevNavMeshAndPoly.m_iPolyIndex);
if(!GetSharedEdgeEndPoints(pPriorPoly, pChildPoly, vert1, vert2, domain))
return false;
}
else
{
if(!GetSharedEdgeEndPoints(pParentPoly, pChildPoly, vert1, vert2, domain))
return false;
}
Vector3 vEdge = vert2 - vert1;
if(vEdge.Mag2() > SMALL_FLOAT)
{
Assert(fEntityRadius >= PATHSERVER_PED_RADIUS);
fEntityRadius -= PATHSERVER_PED_RADIUS;
// Test to see if the line segments actually intersect.
// If so then we'll just return the intersection point.
if(CNavMesh::LineSegsIntersect2D(vBestPointInParentPoly, vBestPointInChildPoly, vert1, vert2, &vPoint)==SEGMENTS_INTERSECT)
{
bOut_LineActuallyIntersectsEdge = true;
// If we have a non-standard entity radius, we may have to offset this point along the edge
// to account for the difference between the entity's radius & the PATHSERVER_PED_RADIUS.
if(fEntityRadius > 0.0f)
{
vEdge.NormalizeFast();
Vector3 vVert1ToPoint = vPoint - vert1;
Vector3 vVert2ToPoint = vPoint - vert2;
const float fV1PLength = NormalizeAndMag(vVert1ToPoint);
const float fV2PLength = NormalizeAndMag(vVert2ToPoint);
if(fV1PLength < fEntityRadius)
vPoint += vEdge * (fEntityRadius-fV1PLength);
else if(fV2PLength < fEntityRadius)
vPoint -= vEdge * (fEntityRadius-fV2PLength);
}
return true;
}
}
bOut_LineActuallyIntersectsEdge = false;
// This vector is used to nudge the chosen point away from the poly edge a bit
Vector3 vPolyEdgeVec = vert2 - vert1;
vPolyEdgeVec.Normalize();
Vector3 vLineVec = vBestPointInChildPoly - vBestPointInParentPoly;
vLineVec.Normalize();
Vector3 vEdgeNormal;
vEdgeNormal.Cross(vLineVec, Vector3(0, 0, 1.0f));
float fEdgePlaneDist = - Dot(vEdgeNormal, vBestPointInParentPoly);
float fVert1Dist = Dot(vEdgeNormal, vert1) + fEdgePlaneDist;
float fVert2Dist = Dot(vEdgeNormal, vert2) + fEdgePlaneDist;
if(rage::Abs(fVert1Dist) < rage::Abs(fVert2Dist))
{
vPoint = vert1;
vPoint += vPolyEdgeVec * (fEntityRadius + g_fEdgeOffsetDist);
}
else
{
vPoint = vert2;
vPoint -= vPolyEdgeVec * (fEntityRadius + g_fEdgeOffsetDist);
}
return true;
}
bool CPathServerThread::GetPointWithDynamicObjectVisiblityAlongSharedEdge(const Vector3 & vBestPointInPoly1, const Vector3 & vBestPointInPoly2, TNavMeshPoly * pPoly1, TNavMeshPoly * pPoly2, float fEntityRadius, Vector3 & vPoint, aiNavDomain domain)
{
if(pPoly2->m_PathParentPoly==NULL)
{
NAVMESH_OPTIMISATIONS_OFF_ONLY( Assert(pPoly2->TestFlags(NAVMESHPOLY_ALTERNATIVE_STARTING_POLY)); )
return false;
}
CNavMesh * pPoly1NavMesh = CPathServer::GetNavMeshFromIndex(pPoly1->GetNavMeshIndex(), domain);
Assert(pPoly1NavMesh);
CNavMesh * pPoly2NavMesh = CPathServer::GetNavMeshFromIndex(pPoly2->GetNavMeshIndex(), domain);
Assert(pPoly2NavMesh);
// Find out if we reached the pChildPoly via a connection polygon.
// If so we will have to perform some more complex logic, if not then we
// can simply traverse the child's edges to find the parent.
const s32 iParentEdge = pPoly2->m_Struct4.m_iParentExitEdge;
const TAdjPoly adjPoly = pPoly1NavMesh->GetAdjacentPoly( pPoly1->GetFirstVertexIndex()+iParentEdge );
const u32 iAdjNavMesh = adjPoly.GetNavMeshIndex( pPoly1NavMesh->GetAdjacentMeshes() );
if(iAdjNavMesh == NAVMESH_NAVMESH_INDEX_NONE)
return false;
CNavMesh * pAdjNavMesh = CPathServer::GetNavMeshFromIndex(iAdjNavMesh, domain);
if(!pAdjNavMesh)
return false;
TNavMeshPoly * pAdjPoly = pAdjNavMesh->GetPoly(adjPoly.GetPolyIndex());
Vector3 vEdgeVert1, vEdgeVert2;
if(pAdjPoly->GetIsDegenerateConnectionPoly())
{
TNavMeshAndPoly prevNavMeshAndPoly;
prevNavMeshAndPoly.Reset();
Vector3 vParentV1, vParentV2, vParentEdge;
pPoly1NavMesh->GetVertex( pPoly1NavMesh->GetPolyVertexIndex( pPoly1, iParentEdge ), vParentV1 );
pPoly1NavMesh->GetVertex( pPoly1NavMesh->GetPolyVertexIndex( pPoly1, (iParentEdge+1)%pPoly1->GetNumVertices() ), vParentV2 );
vParentEdge = vParentV2 - vParentV1;
vParentEdge.Normalize();
IncTimeStamp();
if( !ObtainEntryEdgeViaConnectionPoly( pPoly1, pPoly1NavMesh, pPoly2, pPoly2NavMesh, pAdjPoly, pAdjNavMesh, NULL, NULL, prevNavMeshAndPoly, vParentEdge, vParentV1, vParentV2, domain ) )
{
#if NAVMESH_OPTIMISATIONS_OFF
//Assert(false);
#endif
return false;
}
if(prevNavMeshAndPoly.m_iNavMeshIndex == NAVMESH_NAVMESH_INDEX_NONE)
return false;
CNavMesh * pPriorNavMesh = CPathServer::GetNavMeshFromIndex(prevNavMeshAndPoly.m_iNavMeshIndex, domain);
Assert(pPriorNavMesh && pPriorNavMesh->GetIndexOfMesh()==NAVMESH_INDEX_TESSELLATION);
TNavMeshPoly * pPriorPoly = pPriorNavMesh->GetPoly(prevNavMeshAndPoly.m_iPolyIndex);
if(!GetSharedEdgeEndPoints(pPriorPoly, pPoly2, vEdgeVert1, vEdgeVert2, domain))
return false;
}
else
{
if(!GetSharedEdgeEndPoints(pPoly1, pPoly2, vEdgeVert1, vEdgeVert2, domain))
return false;
}
Vector3 vEdge = vEdgeVert2 - vEdgeVert1;
// If we have a non-standard actor radius then we must move edge points in by the
// difference between the radius and the default radius which is baked into the navmesh
if(fEntityRadius != PATHSERVER_PED_RADIUS)
{
Assert(fEntityRadius > PATHSERVER_PED_RADIUS);
// Subtract the PATHSERVER_PED_RADIUS, which is baked into the navmesh
fEntityRadius -= PATHSERVER_PED_RADIUS;
// If this edge is too small for us to offset points by radius - they just give up on this
if(vEdge.Mag2() < PATHSERVER_PED_RADIUS*PATHSERVER_PED_RADIUS)
return false;
vEdge.NormalizeFast();
vEdge *= fEntityRadius;
vEdgeVert1 += vEdge;
vEdgeVert2 -= vEdge;
}
else
{
vEdge.NormalizeFast();
}
Vector3 vPointsToTry[3] = { vEdgeVert1 + (vEdge * g_fEdgeOffsetDist), vEdgeVert2 - (vEdge * g_fEdgeOffsetDist), (vEdgeVert1+vEdgeVert2)*0.5f };
float fDists[3] =
{
((vPointsToTry[0] - vBestPointInPoly1).Mag() + (vPointsToTry[0] - vBestPointInPoly2).Mag()),
((vPointsToTry[1] - vBestPointInPoly1).Mag() + (vPointsToTry[1] - vBestPointInPoly2).Mag()),
((vPointsToTry[2] - vBestPointInPoly1).Mag() + (vPointsToTry[2] - vBestPointInPoly2).Mag())
};
// Cheesy bubble-sort by total distance.
// NB: This is actually flawed, need to take a look...
if(fDists[2] < fDists[1])
{
SwapEm(fDists[1], fDists[2]);
SwapEm(vPointsToTry[1], vPointsToTry[2]);
}
if(fDists[1] < fDists[0])
{
SwapEm(fDists[0], fDists[1]);
SwapEm(vPointsToTry[0], vPointsToTry[1]);
}
for(int t=0; t<3; t++)
{
if(TestDynamicObjectLOS(vBestPointInPoly1, vPointsToTry[t]) && TestDynamicObjectLOS(vPointsToTry[t], vBestPointInPoly2))
{
vPoint = vPointsToTry[t];
return true;
}
}
// None of the vPointsToTry had visibility.
return false;
}
void CPathServer::ModifyMovementCosts(CPathFindMovementCosts* pMovementCosts, bool bWander, bool bFlee)
{
if(bWander)
{
pMovementCosts->m_fClimbHighPenalty *= CPathServerThread::ms_fClimbMultiplier_Wander;
pMovementCosts->m_fClimbLowPenalty *= CPathServerThread::ms_fClimbMultiplier_Wander;
}
else if(bFlee)
{
pMovementCosts->m_fClimbHighPenalty *= CPathServerThread::ms_fClimbMultiplier_Flee;
pMovementCosts->m_fClimbLowPenalty *= CPathServerThread::ms_fClimbMultiplier_Flee;
pMovementCosts->m_fEnterWaterPenalty *= CPathServerThread::ms_fWaterMultiplier_Flee;
pMovementCosts->m_fLeaveWaterPenalty *= CPathServerThread::ms_fWaterMultiplier_Flee;
pMovementCosts->m_fBeInWaterPenalty *= CPathServerThread::ms_fWaterMultiplier_Flee;
}
else
{
pMovementCosts->m_fClimbHighPenalty *= CPathServerThread::ms_fClimbMultiplier_ShortestPath;
pMovementCosts->m_fClimbLowPenalty *= CPathServerThread::ms_fClimbMultiplier_ShortestPath;
}
}
Reference in New Issue View Git Blame Copy Permalink