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

975 lines
38 KiB
C++
Raw Permalink Blame History

#include "PathServer\PathServer.h"
// Rage headers
#include "atl/inmap.h"
#include "system/xtl.h"
#include "vector/geometry.h"
// Framework headers
#include "fwmaths/Vector.h"
#include "fwmaths/random.h"
NAVMESH_OPTIMISATIONS()
//******************************************************************************************************************
// RefinePathAndCreateNodes
// This function takes the list of polygons (and the chosen waypoints within these polygons) which were found
// by the A* pathfind function. It then preforms the process known as "string-pulling" which removes kinks in
// the path, and reduces it to a small number of straight line-segments with visibility between them.
// These become the final waypoints which make up the path.
//******************************************************************************************************************
#define __BREAK_ON_POS 0
bool CPathServerThread::ConvertPointsInPolyToVertexList(
CPathRequest * pPathRequest,
const Vector3 & vStartPos,
const Vector3 & vEndPos,
Vector3 & vOut_StartWaypoint,
TNavMeshPoly *& pOut_StartPoly,
Vector3 & vOut_EndWaypoint,
TNavMeshPoly *& pOut_EndPoly,
s32 & iOut_NumPointsToTest)
{
if(m_Vars.m_iNumPathPolys == 0)
{
// If m_iNumPathPolys was zero, and pPathRequest->m_iNumPoints is 2 - it means
// we resolved the path with a simple LOS.
if(pPathRequest->m_iNumPoints == 2)
return false;
// Otherwise, we couldn't find a path - so reset the number of points
pPathRequest->m_iNumPoints = 0;
}
pPathRequest->m_iNumPoints = 0;
Vector3 vPolyPts[NAVMESHPOLY_MAX_NUM_VERTICES];
pOut_StartPoly = m_Vars.m_PathPolys[0];
pOut_EndPoly = m_Vars.m_PathPolys[ m_Vars.m_iNumPathPolys-1 ];
vOut_StartWaypoint = vStartPos;
vOut_EndWaypoint = vEndPos;
static const Vector3 vRaiseVecForVolumePolys(0.0f, 0.0f, UNDERWATER_DEFAULT_RAISE_UP_Z_AMT);
m_Vars.m_PathPolys[m_Vars.m_iNumPathPolys] = pOut_EndPoly;
//const float fEntityRadius = pPathRequest->m_fEntityRadius;
const aiNavDomain domain = pPathRequest->GetMeshDataSet();
u32 v;
u32 t;
int iStartOfPts;
CNavMesh * pNavMesh;
// If the start poly in the path is actually the m_pStartPoly, then use the vStartWaypoint
// for the point-in-poly. m_pStartPoly won't have a valid point enum.
if(m_Vars.m_PathPolys[0]==m_Vars.m_pStartPoly)
{
m_Vars.m_vClosestPointInPathPolys[0] = vOut_StartWaypoint;
iStartOfPts = 1;
}
// Otherwise we must have started from a poly added during the tessellation around the start-pos.
// In this case the vStartWaypoint may be well outside of the first polygon, so use its pointenum.
// NB : Will poly[0] even have a correct iPointEnum - its never set for starting polys..
else
{
iStartOfPts = 0;
}
for(t=iStartOfPts; t<m_Vars.m_iNumPathPolys; t++)
{
pNavMesh = CPathServer::GetNavMeshFromIndex(m_Vars.m_PathPolys[t]->GetNavMeshIndex(), domain);
Assert(pNavMesh);
Assert(!m_Vars.m_PathPolys[t]->GetIsDegenerateConnectionPoly());
if(m_Vars.m_PathPolys[t]->GetPointEnum() == NAVMESHPOLY_POINTENUM_CENTROID)
{
if( pNavMesh->GetIndexOfMesh() != NAVMESH_INDEX_TESSELLATION)
{
pNavMesh->GetPolyCentroidQuick(m_Vars.m_PathPolys[t], m_Vars.m_vClosestPointInPathPolys[t]);
}
else
{
pNavMesh->GetPolyCentroid( pNavMesh->GetPolyIndex(m_Vars.m_PathPolys[t]), m_Vars.m_vClosestPointInPathPolys[t]);
}
}
else
{
for(v=0; v<m_Vars.m_PathPolys[t]->GetNumVertices(); v++)
{
pNavMesh->GetVertex( pNavMesh->GetPolyVertexIndex(m_Vars.m_PathPolys[t], v), vPolyPts[v] );
}
bool bAddedLinkPos = false;
const s32 iPointEnum = m_Vars.m_PathPolys[t]->GetPointEnum();
if(iPointEnum == NAVMESHPOLY_POINTENUM_SPECIAL_LINK_ENDPOS)
{
Assert(t > 0);
if(t > 0)
{
// Find the special link from the pPathParent poly, to this poly
TNavMeshPoly * pSpecialLinkStartPoly = m_Vars.m_PathPolys[t-1];
if(pSpecialLinkStartPoly)
{
CNavMesh * pSpecialLinkStartNavMesh = CPathServer::GetNavMeshFromIndex(pSpecialLinkStartPoly->GetNavMeshIndex(), pPathRequest->GetMeshDataSet());
TNavMeshPoly * pSpecialLinkEndPoly = m_Vars.m_PathPolys[t];
CNavMesh * pSpecialLinkEndNavMesh = CPathServer::GetNavMeshFromIndex(pSpecialLinkEndPoly->GetNavMeshIndex(), pPathRequest->GetMeshDataSet());
const u32 iStartNavMesh = pSpecialLinkStartNavMesh->GetIndexOfMesh();
const u32 iEndNavMesh = pSpecialLinkEndNavMesh->GetIndexOfMesh();
const u32 iStartPoly = pSpecialLinkStartNavMesh->GetPolyIndex( pSpecialLinkStartPoly );
const u32 iEndPoly = pSpecialLinkEndNavMesh->GetPolyIndex( pSpecialLinkEndPoly );
if(pSpecialLinkStartPoly->GetIsTessellatedFragment())
{
pSpecialLinkStartNavMesh = CPathServer::GetNavMeshFromIndex( CPathServer::GetTessInfo(pSpecialLinkStartPoly)->m_iNavMeshIndex, pPathRequest->GetMeshDataSet() );
pSpecialLinkStartPoly = pSpecialLinkStartNavMesh->GetPoly( CPathServer::GetTessInfo(pSpecialLinkStartPoly)->m_iPolyIndex );
}
if(pSpecialLinkEndPoly->GetIsTessellatedFragment())
{
pSpecialLinkEndNavMesh = CPathServer::GetNavMeshFromIndex( CPathServer::GetTessInfo(pSpecialLinkEndPoly)->m_iNavMeshIndex, pPathRequest->GetMeshDataSet() );
pSpecialLinkEndPoly = pSpecialLinkEndNavMesh->GetPoly( CPathServer::GetTessInfo(pSpecialLinkEndPoly)->m_iPolyIndex );
}
s32 iLinkLookupIndex = pSpecialLinkStartPoly->GetSpecialLinksStartIndex();
for(s32 s=0; s<pSpecialLinkStartPoly->GetNumSpecialLinks(); s++)
{
u16 iLinkIndex = pSpecialLinkStartNavMesh->GetSpecialLinkIndex(iLinkLookupIndex++);
Assert(iLinkIndex < pSpecialLinkStartNavMesh->GetNumSpecialLinks());
CSpecialLinkInfo & linkInfo = pSpecialLinkStartNavMesh->GetSpecialLinks()[iLinkIndex];
if( linkInfo.GetLinkFromNavMesh()==iStartNavMesh &&
linkInfo.GetLinkFromPoly()==iStartPoly &&
linkInfo.GetLinkToNavMesh()==iEndNavMesh &&
linkInfo.GetLinkToPoly()==iEndPoly )
{
Vector3 vLinkFromPos, vLinkToPos;
CNavMesh::DecompressVertex(vLinkFromPos, linkInfo.GetLinkFromPosX(), linkInfo.GetLinkFromPosY(), linkInfo.GetLinkFromPosZ(), pSpecialLinkStartNavMesh->GetQuadTree()->m_Mins, pSpecialLinkStartNavMesh->GetExtents());
CNavMesh::DecompressVertex(vLinkToPos, linkInfo.GetLinkToPosX(), linkInfo.GetLinkToPosY(), linkInfo.GetLinkToPosZ(), pSpecialLinkEndNavMesh->GetQuadTree()->m_Mins, pSpecialLinkEndNavMesh->GetExtents());
Vector3 vLinkVec = vLinkToPos - vLinkFromPos;
vLinkVec.z = 0.0f;
vLinkVec.Normalize();
Vector3 vPointInPolyOffset = vLinkVec * (pPathRequest->m_fEntityRadius + 0.1f);
m_Vars.m_vClosestPointInPathPolys[t] = vLinkToPos + vPointInPolyOffset;
bAddedLinkPos = true;
break;
}
}
}
}
if(!bAddedLinkPos)
{
m_Vars.m_PathPolys[t]->SetPointEnum(NAVMESHPOLY_POINTENUM_CENTROID);
if( pNavMesh->GetIndexOfMesh() != NAVMESH_INDEX_TESSELLATION)
{
pNavMesh->GetPolyCentroidQuick(m_Vars.m_PathPolys[t], m_Vars.m_vClosestPointInPathPolys[t]);
}
else
{
pNavMesh->GetPolyCentroid( pNavMesh->GetPolyIndex(m_Vars.m_PathPolys[t]), m_Vars.m_vClosestPointInPathPolys[t]);
}
bAddedLinkPos = true;
}
}
if(!bAddedLinkPos)
{
GetPointInPolyFromPointEnum(
pNavMesh,
m_Vars.m_PathPolys[t],
vPolyPts,
&m_Vars.m_vClosestPointInPathPolys[t],
iPointEnum,
ShouldUseMorePointsForPoly(*m_Vars.m_PathPolys[t], pPathRequest) );
}
}
#if __STORE_POLYS_IN_PATH_REQUEST
pPathRequest->m_InitiallyFoundPathPointInPolys[t] = m_Vars.m_vClosestPointInPathPolys[t];
#endif
}
#if __STORE_POLYS_IN_PATH_REQUEST
pPathRequest->m_InitiallyFoundPathPointInPolys[0] = vOut_StartWaypoint;
pPathRequest->m_InitiallyFoundPathPointInPolys[m_Vars.m_iNumPathPolys-1] = vOut_EndWaypoint;
#endif
if(!CPathServer::ms_bRefinePaths)
{
for(u32 f=1; f<MIN(m_Vars.m_iNumPathPolys,MAX_NUM_PATH_POINTS) ; f++)
{
#if __STORE_POLYS_IN_PATH_REQUEST
pPathRequest->m_InitiallyFoundPathPointInPolys[f] = m_Vars.m_vClosestPointInPathPolys[f];
#endif
pPathRequest->m_PathPoints[f] = m_Vars.m_vClosestPointInPathPolys[f];
pPathRequest->m_PathPolys[f] = m_Vars.m_PathPolys[f];
pPathRequest->m_WaypointFlags[f] = m_Vars.m_iPolyWaypointFlags[f];
}
pPathRequest->m_PathPolys[0] = m_Vars.m_PathPolys[0];
pPathRequest->m_PathPoints[0] = vOut_StartWaypoint;
pPathRequest->m_PathPolys[m_Vars.m_iNumPathPolys-1] = m_Vars.m_PathPolys[m_Vars.m_iNumPathPolys-1];
pPathRequest->m_PathPoints[m_Vars.m_iNumPathPolys-1] = vOut_EndWaypoint;
pPathRequest->m_iNumPoints = MIN(m_Vars.m_iNumPathPolys,MAX_NUM_PATH_POINTS);
return false;
}
iOut_NumPointsToTest = 0;
// If we're not fleeing, then the end point is taken explicitly from the path request
if(!pPathRequest->m_bFleeTarget && !pPathRequest->m_bWander)
{
// If we didn't actually get exactly to the endpoint, but terminated within the completion radius -
// then see how far off we were. If outside some set distance, then we shall use the endpoint
// we achieved instead of the actual path endpoint.
if(pPathRequest->m_PathResultInfo.m_bUsedCompletionRadius)
{
static const float fUseRealEndPointEpsSqr = 0.5f*0.5f;
Vector3 vDiffToActualEndPoint = m_Vars.m_vClosestPointInPathPolys[m_Vars.m_iNumPathPolys-1] - pPathRequest->m_vPathEnd;
if(vDiffToActualEndPoint.Mag2() < fUseRealEndPointEpsSqr)
{
m_Vars.m_vClosestPointInPathPolys[m_Vars.m_iNumPathPolys] = vOut_EndWaypoint;
}
else
{
vOut_EndWaypoint = m_Vars.m_vClosestPointInPathPolys[m_Vars.m_iNumPathPolys-1];
m_Vars.m_vClosestPointInPathPolys[m_Vars.m_iNumPathPolys] = vOut_EndWaypoint;
}
}
else
{
m_Vars.m_vClosestPointInPathPolys[m_Vars.m_iNumPathPolys] = vOut_EndWaypoint;
}
iOut_NumPointsToTest = m_Vars.m_iNumPathPolys;
}
// Otherwise, it is the position in the poly at which we terminated the flee path
else
{
vOut_EndWaypoint = m_Vars.m_vClosestPointInPathPolys[m_Vars.m_iNumPathPolys-1];
m_Vars.m_vClosestPointInPathPolys[m_Vars.m_iNumPathPolys] = vOut_EndWaypoint;
iOut_NumPointsToTest = m_Vars.m_iNumPathPolys-1;
}
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ] = vOut_StartWaypoint;
pPathRequest->m_PathPolys[ pPathRequest->m_iNumPoints ] = pOut_StartPoly;
pPathRequest->m_WaypointFlags[ pPathRequest->m_iNumPoints ] = m_Vars.m_iPolyWaypointFlags[0];
pPathRequest->m_iNumPoints++;
return true;
}
bool CPathServerThread::RefinePathAndCreateNodes(CPathRequest * pPathRequest, const Vector3 & vStartPos, const Vector3 & vEndPos, const s32 iMaxNumPoints)
{
#if !__FINAL
m_PerfTimer->Reset();
m_PerfTimer->Start();
#endif
#if __BREAK_ON_POS
static Vector3 vBreakOnPos(9999.9f,9999.9f,9999.9f);
static bool bPause = false;
#endif
//*************************************************************
Vector3 vStartWaypoint, vEndWaypoint;
TNavMeshPoly * pStartPoly, * pEndPoly;
s32 iNumPointsToTest;
if( !ConvertPointsInPolyToVertexList(pPathRequest, vStartPos, vEndPos, vStartWaypoint, pStartPoly, vEndWaypoint, pEndPoly, iNumPointsToTest))
{
return true;
}
//*************************************************************
const float fEntityRadius = pPathRequest->m_fEntityRadius;
const aiNavDomain domain = pPathRequest->GetMeshDataSet();
m_iNumRefinePathIterations = 0;
Vector3 vCurrentStartPos = vStartWaypoint;
TNavMeshPoly * pCurrentStartPoly = pStartPoly;
u32 iCurrentStartIndex = 0;
u32 iCurrentTestIndex = 1;
bool bLOS = false, bDynObjLOS = false, bNavMeshLOS = false;
DEV_ONLY(s32 iNumIterations=0;)
while(iCurrentTestIndex <= iNumPointsToTest)
{
#if __DEV
iNumIterations++;
Assertf(iNumIterations < 1000, "Possible infinite loop found in string pull (see url:bugstar:984258)");
#endif
if(CPathServer::m_bForceAbortCurrentRequest)
{
pPathRequest->m_iCompletionCode = PATH_ABORTED_DUE_TO_ANOTHER_THREAD;
return false;
}
// Early out if path is cancelled during processing
if(pPathRequest->m_iCompletionCode == PATH_CANCELLED)
{
return false;
}
//**************************************************************************************
TNavMeshPoly * pTestPoly = m_Vars.m_PathPolys[iCurrentTestIndex];
Vector3 vTestPos = m_Vars.m_vClosestPointInPathPolys[iCurrentTestIndex];
// Increment timestamp before LOS. This prevents infinite recursion in some tricky boundary cases.
IncTimeStamp();
m_bLosHitInfluenceBoundary = false;
m_bLosCrossedCoverBoundary = false;
bool bPointAdded = false;
static dev_bool bDoExtend = false;
static const float fExtendDist = 0.1f;
Vector3 vExtend = VEC3_ZERO;
if(bDoExtend)
{
vExtend = m_Vars.m_vClosestPointInPathPolys[iCurrentTestIndex] - m_Vars.m_vClosestPointInPathPolys[iCurrentStartIndex];
vExtend.Normalize();
vExtend *= fExtendDist;
}
bNavMeshLOS = TestNavMeshLOS(
m_Vars.m_vClosestPointInPathPolys[iCurrentStartIndex] - vExtend,
m_Vars.m_vClosestPointInPathPolys[iCurrentTestIndex] + vExtend,
m_Vars.m_PathPolys[iCurrentTestIndex],
pCurrentStartPoly,
NULL,
m_Vars.m_iLosFlags,
domain);
if(/*bNavMeshLOS && */CPathServer::m_eObjectAvoidanceMode != CPathServer::ENoObjectAvoidance)
{
bDynObjLOS = TestDynamicObjectLOS(
m_Vars.m_vClosestPointInPathPolys[iCurrentStartIndex],
m_Vars.m_vClosestPointInPathPolys[iCurrentTestIndex]
);
}
else
{
bDynObjLOS = true;
}
if(iCurrentTestIndex <= iCurrentStartIndex+1)
{
m_bLosHitInfluenceBoundary = false;
m_bLosCrossedCoverBoundary = false;
}
bLOS = (bNavMeshLOS && bDynObjLOS);
//***************************************************************************
// No line-of-sight at all, or we hit an openable/pushable/climbable obejct
if((!bLOS) || m_bLosHitInfluenceBoundary || m_bLosCrossedCoverBoundary)
{
//***********************************************************************************************************
// If the current poly was reached via a non-standard adjacency (ie. a climb-up, drop-down, etc), then
// we must ensure that one point is added exactly halfway along the edge from which this polygon was
// reached & another must be added as close to is as possible within this poly. It should ideally be
// exactly in the direction pointed by the previously traversed edge's normal. (NB: In fact, we could
// generate it manually since we're not restricted to the pointEnum scheme here...)
//***********************************************************************************************************
if(m_Vars.m_PathPolys[iCurrentTestIndex]->TestFlags(NAVMESHPOLY_REACHEDBY_MASK) &&
((iCurrentTestIndex <= iCurrentStartIndex + 1) || bDynObjLOS))
{
TNavMeshPoly * pCurrentTestPoly = m_Vars.m_PathPolys[iCurrentTestIndex];
CNavMesh * pCurrentNavMesh = CPathServer::GetNavMeshFromIndex(pCurrentTestPoly->GetNavMeshIndex(), domain);
// CL:3638784 (B* 1024539: If a climb point is blocked by a dynamic object in the path we now fail the path.)
// JB: Unless this is a special link
if (!bDynObjLOS && pCurrentTestPoly->TestFlags(NAVMESHPOLY_WAS_REACHED_VIA_SPECIAL_LINK)==false)
{
pPathRequest->m_iCompletionCode = PATH_NOT_FOUND;
return false;
}
//**********************************************************
// Special link type of adjacency led to this poly
if(pCurrentTestPoly->TestFlags(NAVMESHPOLY_WAS_REACHED_VIA_SPECIAL_LINK))
{
Assert(pCurrentTestPoly->m_PathParentPoly != NULL);
TNavMeshPoly * pParentPoly = pCurrentTestPoly->m_PathParentPoly;
CNavMesh * pParentNavMesh = CPathServer::GetNavMeshFromIndex(pParentPoly->GetNavMeshIndex(), domain);
// Find the special link from the previous poly to this poly
CNavMesh * pOriginalFromNavMesh, * pOriginalToNavMesh;
TNavMeshPoly * pOriginalFromPoly, * pOriginalToPoly;
// Find the original 'from' poly, if the pParentPoly is in the tessellation navmesh
if(pParentNavMesh->GetIndexOfMesh() == NAVMESH_INDEX_TESSELLATION)
{
const int iPolyIndexInTessMesh = pParentNavMesh->GetPolyIndex(pParentPoly);
TTessInfo & pTessInfo = CPathServer::m_PolysTessellatedFrom[iPolyIndexInTessMesh];
pOriginalFromNavMesh = CPathServer::GetNavMeshFromIndex(pTessInfo.m_iNavMeshIndex, domain);
pOriginalFromPoly = pOriginalFromNavMesh->GetPoly(pTessInfo.m_iPolyIndex);
}
else
{
pOriginalFromNavMesh = pParentNavMesh;
pOriginalFromPoly = pParentPoly;
}
// Find the original 'to' poly, if the pCurrentTestPoly is in the tessellation navmesh
if(pCurrentNavMesh->GetIndexOfMesh() == NAVMESH_INDEX_TESSELLATION)
{
int iPolyIndexInTessMesh = pCurrentNavMesh->GetPolyIndex(pCurrentTestPoly);
TTessInfo & pTessInfo = CPathServer::m_PolysTessellatedFrom[iPolyIndexInTessMesh];
pOriginalToNavMesh = CPathServer::GetNavMeshFromIndex(pTessInfo.m_iNavMeshIndex, domain);
pOriginalToPoly = pOriginalFromNavMesh->GetPoly(pTessInfo.m_iPolyIndex);
}
else
{
pOriginalToNavMesh = pCurrentNavMesh;
pOriginalToPoly = pCurrentTestPoly;
}
//s32 iLinkLookupIndex = pParentPoly->GetSpecialLinksStartIndex();
//for(s32 s=0; s<pParentPoly->GetNumSpecialLinks(); s++)
s32 iLinkLookupIndex = pOriginalFromPoly->GetSpecialLinksStartIndex();
for(s32 s=0; s<pOriginalFromPoly->GetNumSpecialLinks(); s++)
{
u16 iLinkIndex = pOriginalFromNavMesh->GetSpecialLinkIndex(iLinkLookupIndex++);
Assert(iLinkIndex < pOriginalFromNavMesh->GetNumSpecialLinks());
CSpecialLinkInfo & linkInfo = pOriginalFromNavMesh->GetSpecialLinks()[iLinkIndex];
// if(linkInfo.m_Struct1.m_iOriginalLinkFromNavMesh==pOriginalFromPoly->GetNavMeshIndex() &&
// linkInfo.m_iOriginalLinkFromPoly==pOriginalFromNavMesh->GetPolyIndex(pOriginalFromPoly) &&
// linkInfo.m_Struct2.m_iOriginalLinkToNavMesh==pOriginalToPoly->GetNavMeshIndex() &&
// linkInfo.m_iOriginalLinkToPoly==pOriginalToNavMesh->GetPolyIndex(pOriginalToPoly))
if( linkInfo.GetLinkFromNavMesh()==pParentNavMesh->GetIndexOfMesh() &&
linkInfo.GetLinkFromPoly()==pParentNavMesh->GetPolyIndex(pParentPoly) &&
linkInfo.GetLinkToNavMesh()==pCurrentNavMesh->GetIndexOfMesh() &&
linkInfo.GetLinkToPoly()==pCurrentNavMesh->GetPolyIndex(pCurrentTestPoly) )
{
Assert(linkInfo.GetAStarTimeStamp()==m_iAStarTimeStamp);
// This is the special link from the previous poly to this poly.
// We need to add the start & end point to the path
Vector3 vSpecialLinkStart, vSpecialLinkEnd;
pOriginalFromNavMesh->DecompressVertex(vSpecialLinkStart, linkInfo.GetLinkFromPosX(), linkInfo.GetLinkFromPosY(), linkInfo.GetLinkFromPosZ());
pOriginalToNavMesh->DecompressVertex(vSpecialLinkEnd, linkInfo.GetLinkToPosX(), linkInfo.GetLinkToPosY(), linkInfo.GetLinkToPosZ());
switch(linkInfo.GetLinkType())
{
case NAVMESH_LINKTYPE_CLIMB_LADDER:
case NAVMESH_LINKTYPE_DESCEND_LADDER:
case NAVMESH_LINKTYPE_CLIMB_OBJECT:
{
// Bail out if we don't have enough room to set up this climb
if(pPathRequest->m_iNumPoints + 3 >= iMaxNumPoints)
{
return true;
}
#ifdef GTA_ENGINE
// Locally turn off ladders if too many wants to use them within a short time
if ( !pPathRequest->m_bCoverFinderPath &&
(linkInfo.GetLinkType() == NAVMESH_LINKTYPE_CLIMB_LADDER ||
linkInfo.GetLinkType() == NAVMESH_LINKTYPE_DESCEND_LADDER))
{
m_BlockedLadders.UpdateUsage(vSpecialLinkStart.z < vSpecialLinkEnd.z ? vSpecialLinkStart : vSpecialLinkEnd);
}
#endif // GTA_ENGINE
// We have to add normally the point in the previous polygon we visited prior to
// this ladder climb's top/bottom points. This is because we cannot necessarily
// see the get-on point from the last route point we added.
if(iCurrentTestIndex > 0)
{
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ] = m_Vars.m_vClosestPointInPathPolys[iCurrentTestIndex-1];
pPathRequest->m_PathPolys[ pPathRequest->m_iNumPoints ] = m_Vars.m_PathPolys[iCurrentTestIndex-1];
// Prev point will have been standard
pPathRequest->m_WaypointFlags[pPathRequest->m_iNumPoints].Clear();
pPathRequest->m_iNumPoints++;
Assert(pPathRequest->m_iNumPoints < iMaxNumPoints);
}
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ] = vSpecialLinkStart;
pPathRequest->m_PathPolys[ pPathRequest->m_iNumPoints ] = NULL;
// Set the special action depending upon the link-type
switch(linkInfo.GetLinkType())
{
case NAVMESH_LINKTYPE_CLIMB_LADDER:
pPathRequest->m_WaypointFlags[pPathRequest->m_iNumPoints].m_iSpecialActionFlags = (u16)WAYPOINT_FLAG_CLIMB_LADDER;
break;
case NAVMESH_LINKTYPE_DESCEND_LADDER:
pPathRequest->m_WaypointFlags[pPathRequest->m_iNumPoints].m_iSpecialActionFlags = (u16)WAYPOINT_FLAG_DESCEND_LADDER;
break;
case NAVMESH_LINKTYPE_CLIMB_OBJECT:
pPathRequest->m_WaypointFlags[pPathRequest->m_iNumPoints].m_iSpecialActionFlags = (u16)WAYPOINT_FLAG_CLIMB_OBJECT;
break;
default:
Assert(false);
break;
}
// We must set the water flag or it gets lost and we get troubles navigating to climb points in water
if (m_Vars.m_iPolyWaypointFlags[iCurrentTestIndex-1].m_iSpecialActionFlags & WAYPOINT_FLAG_IS_ON_WATER_SURFACE)
pPathRequest->m_WaypointFlags[pPathRequest->m_iNumPoints].m_iSpecialActionFlags |= WAYPOINT_FLAG_IS_ON_WATER_SURFACE;
// NEW: Set the heading of this waypoint from the heading of the link (store in m_iLinkFlags)
pPathRequest->m_WaypointFlags[pPathRequest->m_iNumPoints].m_iHeading = (u8)linkInfo.GetLinkFlags();
pPathRequest->m_iNumPoints++;
Assert(pPathRequest->m_iNumPoints < iMaxNumPoints);
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ] = vSpecialLinkEnd;
pPathRequest->m_PathPolys[ pPathRequest->m_iNumPoints ] = NULL;
pPathRequest->m_WaypointFlags[pPathRequest->m_iNumPoints].Clear();
pPathRequest->m_iNumPoints++;
Assert(pPathRequest->m_iNumPoints < iMaxNumPoints);
break;
}
default:
{
Assert(0);
break;
}
}
// We've found the special link, so quit
break;
}
}
}
//****************************************************************************************************
// We arrived in this poly via a navmesh non-standard adjacency. In other words, the adjacency
// is encoded in the previous path-poly's TAdjPoly adjacency array.
else
{
// Find the edge on the previous poly which adjoined to this poly
if(iCurrentTestIndex > 0)
{
TNavMeshPoly * pPrevPoly = m_Vars.m_PathPolys[iCurrentTestIndex-1];
CNavMesh * pPrevNavMesh = CPathServer::GetNavMeshFromIndex(pPrevPoly->GetNavMeshIndex(), domain);
// If the parent polygon was a tessellated fragment, then look to the original untesselated polygon
// to locate the TAdjPoly which contains the adjacency info.
// NB: We need to do the same for the pCurrentTestPoly polygon & pCurrentNavMesh, since this might have been
// tessellated- and the nonstandard adjacency in the parent polygon will still point to the original polygon/navmesh
if(pPrevPoly->GetIsTessellatedFragment())
{
Assert(pPrevNavMesh->GetIndexOfMesh()==NAVMESH_INDEX_TESSELLATION);
TTessInfo * pTessInfo = CPathServer::GetTessInfo(pPrevPoly);
Assert(pTessInfo);
pPrevNavMesh = CPathServer::GetNavMeshFromIndex(pTessInfo->m_iNavMeshIndex, pPathRequest->GetMeshDataSet());
pPrevPoly = pPrevNavMesh->GetPoly(pTessInfo->m_iPolyIndex);
}
CNavMesh * pCurrentNavMesh = CPathServer::GetNavMeshFromIndex(pCurrentTestPoly->GetNavMeshIndex(), domain);
if(pPrevNavMesh && pCurrentNavMesh)
{
u32 e;
for(e=0; e<pPrevPoly->GetNumVertices(); e++)
{
const TAdjPoly & adjPoly = pPrevNavMesh->GetAdjacentPoly( pPrevPoly->GetFirstVertexIndex() + e );
if(adjPoly.GetNavMeshIndex(pPrevNavMesh->GetAdjacentMeshes()) == pCurrentTestPoly->GetNavMeshIndex())
{
TNavMeshPoly * pNextPoly = pCurrentNavMesh->GetPoly(adjPoly.GetPolyIndex());
if(pNextPoly == pCurrentTestPoly)
break;
}
}
//************************************************************************************
// Add a vertex just before the adjacency. The vertex is half-way along the
// previous poly's edge which we arrived in this poly from. (That is the per-edge
// location which is checked for jumps/drop-downs/etc during the navmesh creation)
if(e != pPrevPoly->GetNumVertices())
{
Vector3 vEdgeVec1, vEdgeVec2, vPointOnEdge;
pPrevNavMesh->GetVertex( pPrevNavMesh->GetPolyVertexIndex(pPrevPoly, e), vEdgeVec1);
pPrevNavMesh->GetVertex( pPrevNavMesh->GetPolyVertexIndex(pPrevPoly, ((e+1)%pPrevPoly->GetNumVertices()) ), vEdgeVec2);
vPointOnEdge = (vEdgeVec1 + vEdgeVec2) * 0.5f;
Vector3 vEdgeVec = vEdgeVec2 - vEdgeVec1;
vEdgeVec.Normalize();
Vector3 vEdgeNormal;
vEdgeNormal.Cross(vEdgeVec, Vector3(0,0,1.0f));
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ] = vPointOnEdge;
pPathRequest->m_PathPolys[ pPathRequest->m_iNumPoints ] = NULL;
// Prev point will have been standard
pPathRequest->m_WaypointFlags[ pPathRequest->m_iNumPoints ] = m_Vars.m_iPolyWaypointFlags[iCurrentTestIndex];
#if __BREAK_ON_POS
if(vBreakOnPos.IsClose(pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ], 0.25f))
bPause = true;
#endif
// Find the closest point in pCurrentTestPoly, to the just-added vertex
Vector3 vClosestPoint1;
Vector3 vPolyPts[NAVMESHPOLY_MAX_NUM_VERTICES];
u32 v;
for(v=0; v<pCurrentTestPoly->GetNumVertices(); v++)
{
pCurrentNavMesh->GetVertex( pCurrentNavMesh->GetPolyVertexIndex(pCurrentTestPoly, v), vPolyPts[v]);
}
// Firstly find the closest point on the polygon edge
float fLeastDist2 = FLT_MAX;
u32 lastv = pCurrentTestPoly->GetNumVertices()-1;
for(v=0; v<pCurrentTestPoly->GetNumVertices(); v++)
{
const Vector3 vVec = vPolyPts[v]-vPolyPts[lastv];
const float T = geomTValues::FindTValueSegToPoint(vPolyPts[lastv], vVec, vPointOnEdge);
const Vector3 vPos = vPolyPts[lastv] + (vVec * T);
const float fDiff2 = (vPos - vPointOnEdge).XYMag2();
if( fDiff2 < fLeastDist2 )
{
fLeastDist2 = fDiff2;
vClosestPoint1 = vPos;
}
lastv = v;
}
// Secondly, generate a set of candidate points within the polygon body
m_VisitPolyVars.m_pFromNavMesh = NULL;
m_VisitPolyVars.m_pFromPoly = NULL;
m_VisitPolyVars.m_pToNavMesh = pCurrentNavMesh;
m_VisitPolyVars.m_pToPoly = pCurrentTestPoly;
m_VisitPolyVars.m_pToPolyPoints = vPolyPts;
m_VisitPolyVars.m_vTargetPos = vPointOnEdge;
s32 iDummyPointEnum;
Vector3 vClosestPoint2;
GetClosestPointInPolyToTargetDistSqr(
m_VisitPolyVars,
TVisitPolyStruct::FLAG_CONSIDER_MAXIMUM_NUM_VERTICES,
&vClosestPoint2,
iDummyPointEnum);
const Vector3 vClosestPoint = (vClosestPoint1 - vPointOnEdge).XYMag2() < (vClosestPoint2 - vPointOnEdge).XYMag2() ? vClosestPoint1 : vClosestPoint2;
#ifdef GTA_ENGINE
Vector3 vVecToClosestPoint = vClosestPoint - vPointOnEdge;
//float fWptHeading = fwAngle::GetRadianAngleBetweenPoints(vClosestPoint.x + vEdgeNormal.x, vClosestPoint.y + vEdgeNormal.y, vPointOnEdge.x, vPointOnEdge.y);
float fWptHeading = fwAngle::GetRadianAngleBetweenPoints(vVecToClosestPoint.x, vVecToClosestPoint.y, 0.0f, 0.0f);
while(fWptHeading < 0.0f) fWptHeading += TWO_PI;
while(fWptHeading >= TWO_PI) fWptHeading -= TWO_PI;
pPathRequest->m_WaypointFlags[ pPathRequest->m_iNumPoints ].m_iHeading = (u8) ((fWptHeading / TWO_PI) * 255.0f);
#endif
pPathRequest->m_iNumPoints++;
if(pPathRequest->m_iNumPoints >= iMaxNumPoints)
return true;
// ( NB : Maybe move the point away from the edge, because if we're not careful we may end up climbing
// up the wall again in our attempt to get to this waypoint ? )
// Add this point too
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ] = vClosestPoint + vEdgeNormal;
pPathRequest->m_PathPolys[ pPathRequest->m_iNumPoints ] = NULL;
// The poly at 'iCurrentTestIndex' was connected to by a non-standard adjacency
pPathRequest->m_WaypointFlags[pPathRequest->m_iNumPoints].Clear();
#if __BREAK_ON_POS
if(vBreakOnPos.IsClose(pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ], 0.25f))
bPause = true;
#endif
pPathRequest->m_iNumPoints++;
if(pPathRequest->m_iNumPoints >= iMaxNumPoints)
return true;
}
}
}
}
// JB: No need for this, since it is done later at the end of the loop.
// Doing it here means that the progress gets advanced too far..
/*
iCurrentStartIndex = iCurrentTestIndex;
iCurrentTestIndex++;
pCurrentStartPoly = m_Vars.m_PathPolys[iCurrentStartIndex];
vCurrentStartPos = m_Vars.m_vClosestPointInPathPolys[iCurrentStartIndex];
*/
}
//**********************************************************************************************
// Otherwise - this is a normal poly->poly connection in the navmesh
//
// If the poly we are testing is further on than the one we are starting from.
// ie. the LOS is blocked, but not between adjacent polys (we'll handle that
// case in a sec).
else if((iCurrentTestIndex > iCurrentStartIndex + 1) || m_bLosHitInfluenceBoundary || m_bLosCrossedCoverBoundary)
{
// What we could do here to improve the path, is to adjust the point
// "m_vClosestPointInPathPolys[iCurrentStartIndex]" repeatedly until we get
// a better point. This should be a point which is closer to lying on the
// line between the last point and the next point ??
// In the case of an influence boundary, we will just start from the next poly.
// It's not important whether or not we have a direct LOS, as there is no physical obstruction
if(bLOS && (m_bLosHitInfluenceBoundary || m_bLosCrossedCoverBoundary))
{
iCurrentStartIndex = iCurrentTestIndex;
//iCurrentTestIndex++;
}
else
{
iCurrentStartIndex = iCurrentTestIndex - 1;
iCurrentTestIndex = iCurrentTestIndex - 1;
}
/*
pCurrentStartPoly = m_Vars.m_PathPolys[iCurrentStartIndex];
vCurrentStartPos = m_Vars.m_vClosestPointInPathPolys[iCurrentStartIndex];
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ] = vCurrentStartPos;
pPathRequest->m_PathPolys[ pPathRequest->m_iNumPoints ] = pCurrentStartPoly;
// Prev point will have been standard
pPathRequest->m_WaypointFlags[pPathRequest->m_iNumPoints].Clear();
#if __BREAK_ON_POS
if(vBreakOnPos.IsClose(pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ], 0.25f))
bPause = true;
#endif
pPathRequest->m_iNumPoints++;
if(pPathRequest->m_iNumPoints >= iMaxNumPoints)
return true;
*/
}
else
{
// We cannot see from one adjacent poly to the next poly.
// Add a point along the shared edge between each poly.
bool bFoundNewPoint = false;
bool bLineActuallyIntersectsEdge = false;
if(bNavMeshLOS && !bDynObjLOS)
{
// If the obstruction was purely due to a dynamic object, then choose a position along
// the shared edge for which there is visibility between the last two points.
bFoundNewPoint = GetPointWithDynamicObjectVisiblityAlongSharedEdge(
m_Vars.m_vClosestPointInPathPolys[iCurrentStartIndex],
m_Vars.m_vClosestPointInPathPolys[iCurrentTestIndex],
pCurrentStartPoly,
pTestPoly,
fEntityRadius,
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ],
domain
);
}
else
{
// Find which side of the line defined by the best points in "pCurrentStartPoly to pTestPoly",
// the two points defining the shared edge between the polys lies.
bFoundNewPoint = GetPointAlongSharedEdge(
m_Vars.m_vClosestPointInPathPolys[iCurrentStartIndex],
m_Vars.m_vClosestPointInPathPolys[iCurrentTestIndex],
pCurrentStartPoly,
pTestPoly,
fEntityRadius,
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ],
bLineActuallyIntersectsEdge,
domain
);
}
#if __BREAK_ON_POS
if(vBreakOnPos.IsClose(m_Vars.m_vClosestPointInPathPolys[iCurrentStartIndex], 0.25f))
bPause = true;
#endif
if(bLineActuallyIntersectsEdge)
{
// No need to add a new point here after all, since the line from start->test points
// in fact intersects the shared edge between the two polygons. By NOT adding the point
// here, we'll enable ourselves more useful path points later on in the route.
}
else if(!bFoundNewPoint)
{
// If the pTestPoly is an alternative starting polygon, then there is not necessarily an adjacency with the
// previous polygon (which will be the m_pStartPoly). In this case just assume we have LOS, because this was
// a prerequisive of having marked the polygon as 'NAVMESHPOLY_ALTERNATIVE_STARTING_POLY'
if( pTestPoly->TestFlags(NAVMESHPOLY_ALTERNATIVE_STARTING_POLY) && (pTestPoly->m_PathParentPoly == m_Vars.m_pStartPoly || pTestPoly->m_PathParentPoly == NULL))
{
}
else
{
#if NAVMESH_OPTIMISATIONS_OFF
//Assertf(false, "!bFoundNewPoint :-(");
#endif
// 7/12/05 - This line used to just return FALSE. However that was causing problems because the
// last point in the route might not be at/near the target & we'd get navmesh routes not completing.
// I've just added the midpoint between the two polys & will hope for the best.
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ] =
(m_Vars.m_vClosestPointInPathPolys[iCurrentStartIndex] +
m_Vars.m_vClosestPointInPathPolys[iCurrentTestIndex]) * 0.5f;
#if __BREAK_ON_POS
if(vBreakOnPos.IsClose(m_Vars.m_vClosestPointInPathPolys[iCurrentStartIndex], 0.25f))
bPause = true;
#endif
pPathRequest->m_iNumPoints++;
if(pPathRequest->m_iNumPoints >= iMaxNumPoints)
return true;
bPointAdded = m_Vars.InsertPoly(iCurrentStartIndex + 1,
pPathRequest->m_PathPoints[pPathRequest->m_iNumPoints -1],
pCurrentStartPoly,
m_Vars.m_iPolyWaypointFlags[iCurrentStartIndex]);
}
}
else
{
pPathRequest->m_PathPolys[ pPathRequest->m_iNumPoints ] = pCurrentStartPoly; //pTestPoly; //NULL;
pPathRequest->m_iNumPoints++;
if(pPathRequest->m_iNumPoints >= iMaxNumPoints)
return true;
bPointAdded = m_Vars.InsertPoly(iCurrentStartIndex + 1,
pPathRequest->m_PathPoints[pPathRequest->m_iNumPoints - 1],
pPathRequest->m_PathPolys[pPathRequest->m_iNumPoints - 1],
m_Vars.m_iPolyWaypointFlags[iCurrentStartIndex]);
}
}
iCurrentStartIndex = iCurrentTestIndex;
pCurrentStartPoly = m_Vars.m_PathPolys[iCurrentStartIndex];
vCurrentStartPos = m_Vars.m_vClosestPointInPathPolys[iCurrentStartIndex];
if(iCurrentTestIndex != 0 && !bPointAdded)
{
// Add a new point in the destination poly, this will become the new starting point
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ] = m_Vars.m_vClosestPointInPathPolys[iCurrentTestIndex];
// NB : we could set this to be either poly, and we allow us to modify the point later??
pPathRequest->m_PathPolys[ pPathRequest->m_iNumPoints ] = pCurrentStartPoly;
// Prev point will have been standard
pPathRequest->m_WaypointFlags[pPathRequest->m_iNumPoints].Clear();
pPathRequest->m_iNumPoints++;
if(pPathRequest->m_iNumPoints >= iMaxNumPoints)
return true;
}
iCurrentTestIndex++;
}
//************************************************************************
// Clear line-of-sight / didn't hit an openable/pushable/climbable object
else
{
iCurrentTestIndex++;
}
}
if( pPathRequest->m_iNumPoints == 1)
{
if( (pPathRequest->m_PathPoints[0] - m_Vars.m_vClosestPointInPathPolys[0]).Mag2() < 0.001f )
{
pPathRequest->m_PathPoints[1] = m_Vars.m_vClosestPointInPathPolys[m_Vars.m_iNumPathPolys-1];
pPathRequest->m_PathPolys[1] = m_Vars.m_PathPolys[m_Vars.m_iNumPathPolys-1];
pPathRequest->m_WaypointFlags[1] = m_Vars.m_iPolyWaypointFlags[m_Vars.m_iNumPathPolys-1];
pPathRequest->m_iNumPoints++;
}
}
//**************************************************************************
// Make sure that the final waypoint is added to the path, if not already
if( (pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints-1 ] - vEndWaypoint).XYMag2() > 0.01f )
{
//*********************************************************************************
// If we can directly see the final waypoint, from the penultimate point, then we
// just replace the final point rather than appending another..
if( pPathRequest->m_iNumPoints >= 2 )
{
bNavMeshLOS = TestNavMeshLOS(
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints-2 ],
vEndWaypoint,
pEndPoly,
m_Vars.m_PathPolys[ pPathRequest->m_iNumPoints-2 ],
NULL,
m_Vars.m_iLosFlags,
domain);
if(bNavMeshLOS && CPathServer::m_eObjectAvoidanceMode != CPathServer::ENoObjectAvoidance)
{
bDynObjLOS = TestDynamicObjectLOS(
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints-2 ],
vEndWaypoint
);
}
}
if( ! (bNavMeshLOS && bDynObjLOS) )
{
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints ] = vEndWaypoint;
pPathRequest->m_PathPolys[ pPathRequest->m_iNumPoints ] = pEndPoly;
pPathRequest->m_WaypointFlags[ pPathRequest->m_iNumPoints ] = m_Vars.m_iPolyWaypointFlags[m_Vars.m_iNumPathPolys-1];
pPathRequest->m_iNumPoints++;
}
else
{
pPathRequest->m_PathPoints[ pPathRequest->m_iNumPoints-1 ] = vEndWaypoint;
pPathRequest->m_PathPolys[ pPathRequest->m_iNumPoints-1 ] = pEndPoly;
}
}
return true;
}
Reference in New Issue View Git Blame Copy Permalink