RomkaZVO/FC1
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
main
FC1/CryAISystem/Graph.cpp
romkazvo 34d6c5d489 123
2023-08-07 19:29:24 +08:00

2964 lines
70 KiB
C++
Raw Permalink Blame History

// Graph.cpp: implementation of the CGraph class.
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "Graph.h"
#include "Heuristic.h"
#include "CAISystem.h"
#if !defined(LINUX)
#include <assert.h>
#endif
#include "Cry_Math.h"
#include <ISystem.h>
#include <Cry_Camera.h>
#include <IRenderer.h>
#include <ILog.h>
#include <I3DEngine.h>
#include <algorithm>
#include <IConsole.h>
#include "IPhysics.h"
#include "AIObject.h"
#include "VertexList.h"
#include <CryFile.h>
#if defined(WIN32) && defined(_DEBUG)
#include <crtdbg.h>
#define DEBUG_NEW_NORMAL_CLIENTBLOCK(file, line) new(_NORMAL_BLOCK, file, line)
#define new DEBUG_NEW_NORMAL_CLIENTBLOCK( __FILE__, __LINE__)
#endif
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
CGraph::CGraph(CAISystem *pSystem)
{
m_pFirst = CreateNewNode(true);//new GraphNode;
m_pSafeFirst = m_pFirst;
m_pCurrent = m_pFirst;
m_pCurrent->data.Reset();
m_pCurrent->link.clear();
nNodes = 0;
m_pHeuristic = 0;
m_nTagged = 0;
m_pPathBegin = 0;
m_pPathfinderCurrent = 0;
m_pWalkBackCurrent = 0;
m_bBeautifying = true;
m_pAISystem = pSystem;
m_lstTagTracker.reserve(1000);
m_lstMarkTracker.reserve(1000);
}
CGraph::~CGraph()
{
m_vNodes.clear();
DeleteGraph(m_pSafeFirst,0);
char str[255];
sprintf(str,"Released %d nodes\n",nNodes);
OutputDebugString(str);
m_mapEntrances.clear();
if (m_pHeuristic)
{
delete m_pHeuristic;
m_pHeuristic = 0;
}
}
GraphNode *CGraph::GetCurrent()
{
return m_pCurrent;
}
GraphNode *CGraph::GetEnclosing(const Vec3d &pos,GraphNode *pNode, bool bOutdoorOnly)
{
if (bOutdoorOnly)
{
if (!pNode)
DebugWalk(m_pFirst,pos);
else
DebugWalk(pNode,pos);
return m_pCurrent;
}
IVisArea *pGoalArea;
int nGoalBuilding;
if (!m_pAISystem->CheckInside(pos,nGoalBuilding,pGoalArea))
{
nGoalBuilding = -1;
pGoalArea = NULL;
}
if (nGoalBuilding<0)
{
if (!pNode)
DebugWalk(m_pFirst,pos);
else
DebugWalk(pNode,pos);
m_pFirst = m_pCurrent;
return m_pCurrent;
}
else
{
GraphNode *pEntrance = GetEntrance(nGoalBuilding,pos);
if (!pEntrance)
{
if (!pGoalArea)
AIWarning("No entrance for navigation area nr %d. The position is (%.3f,%.3f,%.3f)",nGoalBuilding,pos.x,pos.y,pos.z);
else
AIWarning("No entrance into some indoors or some visareas not connected trough portals (NR:%d).(%3f,%3f,%3f)",nGoalBuilding,pos.x,pos.y,pos.z);
}
else
{
if (pGoalArea)
IndoorDebugWalk(pEntrance,pos,pGoalArea);
else
IndoorDebugWalk(pEntrance,pos);
}
}
return m_pCurrent;
}
void CGraph::Connect(GraphNode *one, GraphNode *two)
{
if (one==two)
return;
if (!one || !two)
return;
VectorOfLinks::iterator vi;
for (vi=one->link.begin();vi!=one->link.end();vi++)
{
if ( (*vi).pLink == two)
return;
}
GraphLink NewLink;
NewLink.pLink = two;
if ((one==m_pSafeFirst) || (two==m_pSafeFirst))
NewLink.fMaxRadius = -1.f; // do not EVER go to the safe first node while tracing
else
NewLink.fMaxRadius = 100.f; // default big value
// if (one->link.size()==3)
// int a=5;
one->link.push_back(NewLink);
two->AddRef();
// connect two to one
Connect(two,one);
if (m_pCurrent == m_pSafeFirst)
{
// connect dummy first node to graph
if (one->nBuildingID==-1)
{
Connect(m_pSafeFirst,one);
m_pCurrent=one;
m_pFirst = m_pCurrent;
}
}
}
void CGraph::WriteToFile(const char *pname)
{
#ifdef __MWERKS__
#warning Code not implemented under CodeWarrior
#else
m_vLinks.clear();
m_vLinksDesc.clear();
m_vBuffer.clear();
m_lstSaveStack.clear();
m_vBuffer.reserve(100000);
GraphNode *pNext;
GraphNode *pCurrent = m_pSafeFirst;
while (pNext = WriteLine(pCurrent)) pCurrent=pNext;
CCryFile file;
if( false != file.Open( pname, "wb" ) )
{
int iNumber = m_vBuffer.size();
int nFileVersion = BAI_FILE_VERSION;
int nRandomNr = 0;
file.Write( &nFileVersion, sizeof( int ) );
file.Write( &m_vBBoxMin.x, sizeof( float ) );
file.Write( &m_vBBoxMin.y, sizeof( float ) );
file.Write( &m_vBBoxMin.z, sizeof( float ) );
file.Write( &m_vBBoxMax.x, sizeof( float ) );
file.Write( &m_vBBoxMax.y, sizeof( float ) );
file.Write( &m_vBBoxMax.z, sizeof( float ) );
// write the triangle descriptors
file.Write( &iNumber, sizeof( int ) );
file.Write( &m_vBuffer[ 0 ], iNumber * sizeof( NodeDescriptor ) );
//iNumber = m_vLinks.size();
//WriteFile(hf,&iNumber,sizeof(int),&written,NULL);
//WriteFile(hf,&m_vLinks[0],iNumber*sizeof(int),&written,NULL);
m_pAISystem->m_VertexList.WriteToFile( file );
iNumber = m_vLinksDesc.size();
file.Write( &iNumber, sizeof( int ) );
if (iNumber>0)
file.Write( &m_vLinksDesc[ 0 ], iNumber * sizeof( LinkDescriptor ) );
file.Close();
m_vLinks.clear();
m_vLinksDesc.clear();
m_vNodes.clear();
m_vBuffer.clear();
//m_mapEntrances.clear();
ClearMarks();
}
#endif
}
#ifdef __MWERKS__
#warning Code not implemented under CodeWarrior
#else
GraphNode *CGraph::WriteLine( GraphNode *pNode)
{
// write the id of the node
NodeDescriptor desc;
desc.id = (INT_PTR) pNode; //AMD Port
if (pNode == m_pSafeFirst) desc.id=1;
///desc.pArea = pNode->pArea;
desc.building = pNode->nBuildingID;
desc.data = pNode->data;
desc.bEntrance = false;
desc.bExit = false;
desc.bCreated = pNode->bCreated;
// check if this node is an entrance;
EntranceMap::iterator ei;
ei = m_mapEntrances.find(desc.building);
if (ei!=m_mapEntrances.end())
{
while ( (ei!=m_mapEntrances.end()) && (ei->first == desc.building))
{
if ( (ei->second) == pNode )
desc.bEntrance = true;
++ei;
}
}
else
{
ei=m_mapExits.find(desc.building);
if (ei!=m_mapExits.end())
{
while ( (ei!=m_mapExits.end()) && (ei->first == desc.building))
{
if ( (ei->second) == pNode )
desc.bExit = true;
++ei;
}
}
}
//desc.bEntrance = (m_mapEntrances.find(desc.building) != m_mapEntrances.end());
MarkNode(pNode);
if (!pNode->vertex.empty())
{
desc.nObstacles = pNode->vertex.size();
if (desc.nObstacles>10)
desc.nObstacles=10;
int i=0;
ObstacleIndexVector::iterator li;
for (li=pNode->vertex.begin();li!=pNode->vertex.end() && i<10 ;li++,i++)
{
desc.obstacle[i] = (*li);
//desc.obstacle[i].vPos = (*li).vPos;
//desc.obstacle[i].vDir = (*li).vDir;
}
if (i==10)
m_pAISystem->m_pSystem->GetILog()->Log("\003FOUND INDOOR WAYPOINT WITH MORE THAN 10 HIDEPOINTS LINKED TO IT. SURPLUS HIDEPOINTS IGNORED");
}
else
desc.nObstacles = 0;
m_vBuffer.push_back(desc);
// now write the links
// first size
/* int size = pNode->link.size();
m_vLinks.push_back(desc.id);
m_vLinks.push_back(size);
VectorOfLinks::iterator vi;
for (vi=pNode->link.begin();vi!=pNode->link.end();vi++)
{
GraphNode *pToPush = (*vi).pLink;
if (pToPush == m_pSafeFirst)
m_vLinks.push_back(1);
else
m_vLinks.push_back((int) pToPush);
m_lstSaveStack.remove(pToPush);
if (pToPush->mark)
continue;
m_lstSaveStack.push_back(pToPush);
}*/
VectorOfLinks::iterator vi;
for (vi=pNode->link.begin();vi!=pNode->link.end();vi++)
{
GraphNode *pToPush = (*vi).pLink;
LinkDescriptor ld;
ld.nSourceNode = desc.id;
if (pToPush == m_pSafeFirst)
ld.nTargetNode = 1;
else
ld.nTargetNode = (INT_PTR) pToPush;
ld.fMaxPassRadius = (*vi).fMaxRadius;
ld.nStartIndex = (*vi).nStartIndex;
ld.nEndIndex = (*vi).nEndIndex;
ld.vEdgeCenter = (*vi).vEdgeCenter;
ld.vWayOut = (*vi).vWayOut;
m_vLinksDesc.push_back(ld);
if (pToPush->mark)
{
m_lstSaveStack.remove(pToPush);
continue;
}
if (std::find(m_lstSaveStack.begin(),m_lstSaveStack.end(),pToPush) == m_lstSaveStack.end())
m_lstSaveStack.push_back(pToPush);
}
// Disconnect(pNode);
// if (pNode == m_pFirst)
// delete pNode;
if (m_lstSaveStack.empty())
return NULL;
else
{
GraphNode *pNext;
while (!m_lstSaveStack.empty())
{
pNext = m_lstSaveStack.front();
m_lstSaveStack.pop_front();
if (!pNext->mark)
break;
}
if (pNext->mark)
return NULL;
else
return pNext;
}
}
#endif
void CGraph::DebugWalk(GraphNode *pNode, const Vec3d &pos)
{
// get out of indoor
if (pNode->nBuildingID>=0)
{
pNode=GetEntrance(pNode->nBuildingID,pos);
VectorOfLinks::iterator vli,iend = pNode->link.end();
for (vli=pNode->link.begin();vli!=iend;++vli)
if ((*vli).pLink->nBuildingID<0)
{
pNode = (*vli).pLink;
break;
}
}
ClearMarks();
GraphNode *pNextNode = pNode;
GraphNode *pPrevNode = 0;
int iterations=0;
while (pPrevNode != pNextNode)
{
iterations++;
pPrevNode = pNextNode;
pNextNode = GREEDYStep(pPrevNode,pos);
}
m_pAISystem->f7+=iterations;
m_pAISystem->f7/=2.f;
m_pCurrent = pNextNode; // or pPrevNode, they are the same
ClearMarks();
m_mapGreedyWalkCandidates.clear();
if (!m_pCurrent)
CryError("[AIERROR] located in NULL graph node... Try regenerating triangulation, or submit a bug report.");
}
void CGraph::DrawPath(IRenderer *pRenderer)
{
//if (m_lstVisited.empty()) return;
ListNodes::iterator i;
CandidateMap::iterator ci;
pRenderer->SetMaterialColor(1,0,0,1);
//ci=m_lstVisited.begin();
GraphNode *pStart = (ci->second);
// for (ci++;ci!=m_lstVisited.end();ci++)
{
GraphNode *pEnd = (ci->second);
Vec3d stpos = pStart->data.m_pos;
Vec3d endpos = pEnd->data.m_pos;
stpos.z+=1;
endpos.z+=1;
pRenderer->DrawLine(stpos, endpos );
pStart = pEnd;
}
if (m_lstPath.empty()) return;
Vec3d vStartPos;
ListPositions::iterator pi;
pRenderer->ResetToDefault();
pi=m_lstPath.begin();
vStartPos = (*pi);
for (pi++;pi!=m_lstPath.end();pi++)
{
//GraphNode *pEnd = (*i);
Vec3d vEndPos = (*pi);
// Vec3d stpos = pStart->data.m_pos;
// Vec3d endpos = pEnd->data.m_pos;
vStartPos.z+=1;
vEndPos.z+=1;
pRenderer->DrawLine(vStartPos, vEndPos );
vStartPos = vEndPos;
}
}
void CGraph::GetFieldCenter(Vec3d &pos)
{
pos = m_pCurrent->data.m_pos;
}
void CGraph::DEBUG_DrawCenters(GraphNode *pNode, IRenderer *pRenderer, int dist)
{
return;
if (dist > 10 ) return;
//if (pNode->bDebug) return;
//pNode->bDebug = true;
pRenderer->ResetToDefault();
if (pNode->data.bWater)
pRenderer->SetMaterialColor(0,0,1.0,1.0);
else
pRenderer->SetMaterialColor(pNode->data.fSlope,(float)(1.0-pNode->data.fSlope),(float) (1.0-pNode->data.fSlope),1.0);
pRenderer->DrawBall(pNode->data.m_pos,1.5f);
VectorOfLinks::iterator vi;
for (vi=pNode->link.begin();vi!=pNode->link.end();vi++)
{
Vec3d start, end;
start = pNode->data.m_pos;
start.x+=0.5f;
start.y+=0.5f;
end = (*vi).pLink->data.m_pos;
end.y+=0.5f;
end.x+=0.5f;
pRenderer->DrawLine(start, end);
DEBUG_DrawCenters((*vi).pLink,pRenderer,dist+1);
}
}
int CGraph::WalkAStar(GraphNode *pBegin, GraphNode *pEnd, int &nIterations)
{
m_lstCurrentHistory.clear();
ClearPath(); // clear the previously generated path
// m_lstVisited.clear();
if (!ClearTags()) // clear all the tags in the diagram
return PATHFINDER_CLEANING_GRAPH;
if ((!pBegin) || (!pEnd)) return PATHFINDER_NOPATH;
// lets check if last generated path was similar to this one
if (!m_lstLastPath.empty())
{
if (m_lstLastPath.back() == pEnd)
{
if (CanReuseLastPath(pBegin))
return PATHFINDER_BEAUTIFYINGPATH;
}
}
m_pPathfinderCurrent = pBegin;
m_pPathBegin = pBegin;
m_nAStarDistance = 0;
m_pPathfinderCurrent->fDistance = 0;
//m_fDistance = (pBegin->data.m_pos - pEnd->data.m_pos).GetLength();
m_mapCandidates.clear();
m_pPathfinderCurrent = ASTARStep(pBegin, pEnd);
while (m_pPathfinderCurrent && !m_mapCandidates.empty() && (m_pPathfinderCurrent != pEnd) && (nIterations--))
m_pPathfinderCurrent = ASTARStep(m_pPathfinderCurrent, pEnd);
if (!m_pPathfinderCurrent)
return PATHFINDER_NOPATH;
if (m_pPathfinderCurrent == pEnd)
return PATHFINDER_WALKINGBACK;
return PATHFINDER_STILLTRACING;
}
GraphNode * CGraph::ASTARStep(GraphNode *pBegin, GraphNode *pEnd)
{
TagNode(pBegin);
if (pEnd == pBegin)
{
pEnd->fHeuristic = 10000 - (float)m_nAStarDistance;
return pEnd; // reached the end
}
// this piece evaluates simple distance heuristic for backtracking----------
///float thisdist = (pBegin->data.m_pos - m_pPathBegin->data.m_pos).GetLength();
//pBegin->fHeuristic = 1.f - (thisdist / m_fDistance);
//--------------------------------------------------------------------------
pBegin->fHeuristic = 10000 - (float)m_nAStarDistance;
m_nAStarDistance++;
VectorOfLinks::iterator vi;
for (vi=pBegin->link.begin();vi!=pBegin->link.end();vi++)
{
// if ((*vi).fMaxRadius >= 1.f)
if ((*vi).fMaxRadius >= m_pRequester->m_fPassRadius)
{
EvaluateNode( (*vi).pLink, pEnd, pBegin);
}
}
if (!m_mapCandidates.empty())
{
CandidateHistoryMap::reverse_iterator ci = m_mapCandidates.rbegin();
GraphNode *pNextNode = 0;
if (ci!=m_mapCandidates.rend())
{
pNextNode = (ci->second).pNode;
while ((pNextNode == pBegin)||(pNextNode->tag))
{
m_mapCandidates.erase((++ci).base());
ci = m_mapCandidates.rbegin();
if (ci==m_mapCandidates.rend())
break;
pNextNode = (ci->second).pNode;
}
}
if (ci==m_mapCandidates.rend())
return 0;
m_lstCurrentHistory = (ci->second).lstParents;
float f = ci->first;
m_mapCandidates.erase((++ci).base());
if (GetAISystem()->m_cvDrawPath->GetIVal()==2)
m_lstVisited.insert(CandidateMap::iterator::value_type(f,pNextNode));
return pNextNode;
}
else
return 0;
}
void CGraph::TagNode(GraphNode *pNode)
{
pNode->tag = true;
m_lstTagTracker.push_back(pNode);
}
bool CGraph::ClearTags()
{
int nIterations = PATHFINDER_ITERATIONS;
while (!m_lstTagTracker.empty() && (nIterations--))
{
GraphNode *pLastNode = m_lstTagTracker.back();
pLastNode->tag = false;
pLastNode->fHeuristic = -9999.f;
m_lstTagTracker.pop_back();
}
if (!m_lstTagTracker.empty())
return false; // we are still cleaning up
return true;
}
void CGraph::EvaluateNode(GraphNode *pNode,GraphNode *pEnd, GraphNode *pParent)
{
if (!pNode) return;
if (pNode->tag) return;
float desirability=0;
float thisdist = (pNode->data.m_pos - m_vRealPathfinderEnd).GetLength();
desirability = 1.f - (thisdist / m_fDistance) * 0.5f;
desirability += m_pHeuristic->Estimate(pNode, this) * 0.5f;
NodeWithHistory nwh;
nwh.pNode = pNode;
if (m_lstCurrentHistory.size() > 1 )
m_lstCurrentHistory.pop_back();
m_lstCurrentHistory.push_front(pParent);
nwh.lstParents = m_lstCurrentHistory;
m_mapCandidates.insert(CandidateHistoryMap::iterator::value_type(desirability,nwh));
}
int CGraph::ContinueAStar(GraphNode *pEnd, int &nIterations)
{
if (!pEnd) return PATHFINDER_NOPATH;
if (!m_pHeuristic) return PATHFINDER_NOPATH;
//int nIterations = PATHFINDER_ITERATIONS;
m_pPathfinderCurrent = ASTARStep(m_pPathfinderCurrent, pEnd);
while (!m_mapCandidates.empty() && (m_pPathfinderCurrent != pEnd) && (nIterations--))
m_pPathfinderCurrent = ASTARStep(m_pPathfinderCurrent, pEnd);
if (!m_pPathfinderCurrent)
return PATHFINDER_NOPATH;
if (m_pPathfinderCurrent == pEnd)
return PATHFINDER_WALKINGBACK;
return PATHFINDER_STILLTRACING;
}
int CGraph::WalkBack(GraphNode *pBegin, GraphNode *pEnd, int &nIterations)
{
//int nIterations = PATHFINDER_ITERATIONS;
//GraphNode *pCurrent;
if (!m_pWalkBackCurrent)
{
m_lstNodeStack.clear();
m_lstPath.clear();
m_pWalkBackCurrent = pBegin;
}
//else
//m_pWalkBackCurrent = m_pWalkBackCurrent;
float maxHeur = m_pWalkBackCurrent->fHeuristic;
while (m_pWalkBackCurrent!=pEnd && --nIterations)
{
m_lstPath.push_front(m_pWalkBackCurrent->data.m_pos); // push in path
m_lstNodeStack.push_front(m_pWalkBackCurrent); // push in nodestack
GraphNode *pNext = 0;
float maxheur = m_pWalkBackCurrent->fHeuristic;
VectorOfLinks::iterator vi;
for (vi=m_pWalkBackCurrent->link.begin(); vi!=m_pWalkBackCurrent->link.end(); vi++)
{
GraphNode *pLink = (*vi).pLink;
if (pLink->fHeuristic > maxheur && (*vi).fMaxRadius>=1.f)
{
maxheur = pLink->fHeuristic;
pNext = pLink;
}
}
m_pWalkBackCurrent->fHeuristic = -9999.f;
if (pNext)
m_pWalkBackCurrent = pNext;
else
{
// dead end hit... retrace
// try to continue moving with a revised heuristic
for (vi=m_pWalkBackCurrent->link.begin(); vi!=m_pWalkBackCurrent->link.end(); vi++)
{
GraphNode *pLink = (*vi).pLink;
if (pLink->fHeuristic > m_pWalkBackCurrent->fHeuristic)
{
maxheur = pLink->fHeuristic;
pNext = pLink;
}
}
if (pNext)
m_pWalkBackCurrent = pNext;
else
{
if (!m_lstPath.empty())
m_lstPath.pop_front();
if (!m_lstNodeStack.empty())
m_lstNodeStack.pop_front();
if (m_lstNodeStack.empty())
return PATHFINDER_NOPATH;;
m_pWalkBackCurrent = m_lstNodeStack.front();
if (!m_lstNodeStack.empty())
m_lstNodeStack.pop_front();
if (!m_lstPath.empty())
m_lstPath.pop_front();
}
}
// if (std::find(m_lstNodeStack.begin(),m_lstNodeStack.end(),pCurrent) != m_lstNodeStack.end())
// DEBUG_BREAK;
}
if (m_pWalkBackCurrent == pEnd)
{
m_pWalkBackCurrent = 0;
m_mapGreedyWalkCandidates.clear();
if (std::find(m_lstNodeStack.begin(),m_lstNodeStack.end(),pEnd)==m_lstNodeStack.end())
{
m_lstNodeStack.push_front(pEnd);
m_lstPath.push_front(pEnd->data.m_pos);
}
m_lstLastPath.clear();
m_lstLastPath.insert(m_lstLastPath.begin(),m_lstNodeStack.begin(),m_lstNodeStack.end());
return PATHFINDER_BEAUTIFYINGPATH;
}
else
return PATHFINDER_WALKINGBACK;
}
void CGraph::ClearPath()
{
m_lstPath.clear();
}
void CGraph::DeleteGraph(GraphNode *pNode, int depth)
{
VectorOfLinks::iterator vi;
m_lstDeleteStack.push_front(pNode);
while (!m_lstDeleteStack.empty())
{
GraphNode *pCurrentNode = m_lstDeleteStack.front();
m_lstDeleteStack.pop_front();
// put all links into the node stack
for (vi=pCurrentNode->link.begin();vi!=pCurrentNode->link.end();vi++)
{
GraphNode *pLink =(*vi).pLink;
if (std::find(m_lstDeleteStack.begin(),m_lstDeleteStack.end(),pLink) == m_lstDeleteStack.end())
m_lstDeleteStack.push_front(pLink);
}
Disconnect(pCurrentNode);
/*if (!pCurrentNode->link.empty())
{
// delink this node from all his adjacent nodes
for (vi=pCurrentNode->link.begin();vi!=pCurrentNode->link.end();vi++)
{
GraphNode *pLink = (*vi);
if ((found = std::find(pLink->link.begin(),pLink->link.end(),pCurrentNode)) != pLink->link.end())
{
(*found)->Release();
pLink->link.erase(found);
}
}
GraphNode *pNext= pCurrentNode->link.back();
m_lstNodeStack.push_front(pNext);
pCurrentNode->link.pop_back();
}
else
{
if (pCurrentNode != m_pFirst)
DeleteNode(pCurrentNode);
else
delete m_pFirst;
//if (pCurrentNode->Release())
//{
// delete pCurrentNode;
// nNodes++;
//}
m_lstNodeStack.pop_front();
}
*/
if (pCurrentNode == m_pSafeFirst)
m_pSafeFirst = 0;
}
}
void CGraph::ClearDebugFlag(GraphNode *pNode)
{
//if (!pNode->bDebug) return;
// pNode->bDebug = false;
//VectorOfLinks::iterator vi;
//for (vi=pNode->link.begin();vi!=pNode->link.end();vi++)
// ClearDebugFlag((*vi).pLink);
}
GraphNode * CGraph::CheckClosest(GraphNode *pCurrent, const Vec3d &pos)
{
if (!pCurrent)
{
DebugWalk(m_pCurrent,pos);
return m_pCurrent;
}
else
{
float dist = (pCurrent->data.m_pos - pos).GetLength();
GraphNode *pReturnNode = pCurrent;
VectorOfLinks::iterator si;
for (si=pCurrent->link.begin();si!=pCurrent->link.end();si++)
{
GraphNode *pNode = (*si).pLink;
float cmpdist = (pNode->data.m_pos - pos).GetLength();
if (cmpdist < dist)
pReturnNode = pNode;
}
if (pReturnNode != pCurrent)
return CheckClosest(pReturnNode, pos);
else
return pReturnNode;
}
}
void CGraph::Disconnect(GraphNode * pDisconnected, bool bDelete)
{
// if the node we are disconnecting is the current node, move the current
// to one of his links, or the root if it has no links
if (m_pSafeFirst)
{
if (m_pSafeFirst->link.empty())
return;
}
if (pDisconnected == m_pCurrent)
{
if (!pDisconnected->link.empty())
{
m_pCurrent = pDisconnected->link.front().pLink;
}
else
m_pCurrent = m_pSafeFirst;
}
// if its the root that is being disconnected, move it
if (m_pFirst == pDisconnected)
{
if (!pDisconnected->link.empty())
{
m_pFirst = pDisconnected->link.front().pLink;
}
else
{
if (m_pFirst!=m_pSafeFirst)
m_pFirst = m_pSafeFirst;
else
m_pFirst = 0;
}
}
// now disconnect this node from its links
if (!pDisconnected->link.empty())
{
VectorOfLinks::iterator vi;
for (vi=pDisconnected->link.begin();vi!=pDisconnected->link.end();vi++)
{
GraphNode *pLink = (*vi).pLink;
if (!pLink->link.empty())
{
VectorOfLinks::iterator li;
for (li=pLink->link.begin();li!=pLink->link.end();li++)
{
GraphNode *pBackLink = (*li).pLink;
if (pBackLink == pDisconnected)
{
pLink->link.erase(li);
pDisconnected->Release();
pLink->Release();
break;
}
} // li
}
} // vi
}
pDisconnected->link.clear();
if (bDelete)
DeleteNode(pDisconnected);
if (!m_pSafeFirst)
return;
if (pDisconnected != m_pSafeFirst && m_pSafeFirst->link.empty())
{
GraphNode *pFirst = m_pFirst;
// we have disconnected the link to the dummy safe node - relink it to any outdoor node of the graph
if (pFirst == m_pSafeFirst)
{
if (m_pCurrent == m_pSafeFirst)
{
// try any entrance
if (!m_mapEntrances.empty())
pFirst= (m_mapEntrances.begin()->second);
else
{
AIError("!Could not recover from deletion of Safe graph node. Try deleting .bai files and regenerating.");
return;
}
}
else
pFirst = m_pCurrent;
}
if (pFirst->nBuildingID==-1)
Connect(m_pSafeFirst,pFirst);
else
{
GraphNode *pEntrance = GetEntrance(pFirst->nBuildingID,Vec3d(0,0,0));
if (pEntrance)
{
VectorOfLinks::iterator vli,iend = pEntrance->link.end();
for (vli = pEntrance->link.begin();vli!=iend;++vli)
if ( (*vli).pLink->nBuildingID == -1 )
{
Connect(m_pSafeFirst,(*vli).pLink);
break;
}
}
}
}
}
// walk that will always produce a result, for indoors
void CGraph::IndoorDebugWalk(GraphNode * pNode, const Vec3d & pos, IVisArea *pTargetArea)
{
if (pNode->pArea)
{
float this_dist = (pos - pNode->data.m_pos).GetLength();
m_mapGreedyWalkCandidates.insert(CandidateMap::iterator::value_type(this_dist,pNode));
}
FillGreedyMap(pNode,pos,pTargetArea,false);
if (m_mapGreedyWalkCandidates.empty())
{
AIWarning("No nodes found for this indoor or navigation modifier area apart from entrance!!");
m_pCurrent = pNode;
}
else
{
m_pCurrent = (m_mapGreedyWalkCandidates.begin())->second;
ClearMarks();
m_mapGreedyWalkCandidates.clear();
}
if (!m_pCurrent)
CryError("[AIERROR] located in NULL graph node... Try regenerating triangulation, or submit a bug report.");
}
// Clears the tags of the graph without time-slicing the operation
void CGraph::ClearTagsNow(void)
{
while (!ClearTags());
}
// Check whether a position is within a node's triangle
bool CGraph::PointInTriangle(const Vec3d & pos, GraphNode * pNode)
{
if (pNode->vertex.empty())
return false;
bool bSide=false;
// check first and last
//Vec3d edge = (pNode->vertex.back()).vPos - (pNode->vertex.front()).vPos;
Vec3d vFront = m_pAISystem->m_VertexList.GetVertex(pNode->vertex.front()).vPos;
Vec3d edge = m_pAISystem->m_VertexList.GetVertex(pNode->vertex.back()).vPos - vFront;
Vec3d test = pos - vFront;
float cross = edge.x * test.y - edge.y * test.x;
if (cross>0)
bSide = true;
for (unsigned int i=1;i<pNode->vertex.size();i++)
{
Vec3d vI = m_pAISystem->m_VertexList.GetVertex(pNode->vertex[i]).vPos;
edge = m_pAISystem->m_VertexList.GetVertex(pNode->vertex[i-1]).vPos - vI;
test = pos - vI;
cross = edge.x * test.y - edge.y * test.x;
if ((cross<0) && bSide)
return false;
if ((cross>0) && !bSide)
return false;
}
return true;
}
// uses mark for internal graph operation without disturbing the pathfinder
void CGraph::MarkNode(GraphNode * pNode)
{
pNode->mark = true;
m_lstMarkTracker.push_back(pNode);
}
// clears the marked nodes
void CGraph::ClearMarks(bool bJustClear)
{
if (bJustClear)
{
m_lstMarkTracker.resize(0);
return;
}
while (!m_lstMarkTracker.empty())
{
m_lstMarkTracker.back()->mark = false;
m_lstMarkTracker.pop_back();
}
}
// iterative function to quickly converge on the target position in the graph
GraphNode * CGraph::GREEDYStep(GraphNode * pBegin, const Vec3d & pos, bool bIndoor)
{
MarkNode(pBegin);
if (bIndoor)
{
//indoor
float d_dist = (pos - pBegin->data.m_pos).GetLength();
if (d_dist < 1.f)
return pBegin;
VectorOfLinks::iterator vi;
if (pBegin->link.empty())
return pBegin;
for (vi=pBegin->link.begin();vi!=pBegin->link.end();vi++)
{
GraphNode *pLink = (*vi).pLink;
if ((pLink->mark) || (pLink->nBuildingID==-1)) // don't go outside
continue;
float this_dist = (pos - pLink->data.m_pos).GetLength();
m_mapGreedyWalkCandidates.insert(CandidateMap::iterator::value_type(this_dist,pLink));
}
if (!m_mapGreedyWalkCandidates.empty())
{
CandidateMap::iterator ci = m_mapGreedyWalkCandidates.end();
ci--;
GraphNode *pNextNode = (ci->second);
if (pNextNode == pBegin)
{
ci--;
pNextNode = (ci->second);
}
m_mapGreedyWalkCandidates.erase(ci);
return pNextNode;
}
else
{
return pBegin;
}
}
else
{
// outdoor
if (PointInTriangle(pos,pBegin))
return pBegin; // we have arrived
if (pBegin == m_pSafeFirst)
{
if (pBegin->link.empty())
return pBegin;
m_mapGreedyWalkCandidates.insert(CandidateMap::iterator::value_type(0,pBegin->link.front().pLink));
}
else
{
VectorOfLinks::iterator vi;
if (pBegin!=m_pSafeFirst || !pBegin->mark)
{
for (vi=pBegin->link.begin();vi!=pBegin->link.end();vi++)
{
GraphNode *pLink = (*vi).pLink;
if (pLink->mark || (pLink->nBuildingID!=-1)) // dont go in marked or indoors
continue;
// float this_dist = (pos - pLink->data.m_pos).GetLength();
if (pBegin->vertex.empty())
continue;
Vec3d midpoint = (m_pAISystem->m_VertexList.GetVertex(pBegin->vertex[(*vi).nStartIndex]).vPos + m_pAISystem->m_VertexList.GetVertex(pBegin->vertex[(*vi).nEndIndex]).vPos)/2.f;
midpoint.z = pos.z;
Vec3d dir1 = pos-midpoint;
Vec3d vWayOut = (*vi).vWayOut;
vWayOut.z = 0.f;//pos.z;
dir1.Normalize();
float this_dist = (1.f-dir1.Dot(vWayOut));
this_dist*= (pLink->data.m_pos - pos).GetLength()/30.f;
m_mapGreedyWalkCandidates.insert(CandidateMap::iterator::value_type(this_dist,pLink));
}
}
}
if (!m_mapGreedyWalkCandidates.empty())
{
GraphNode *pNextNode;
do {
if (m_mapGreedyWalkCandidates.empty())
{
AIWarning("Could not locate position (%.3f,%.3f,%.3f) in triangulation. Call Petar!",pos.x,pos.y,pos.z);
return pBegin;
}
pNextNode= (m_mapGreedyWalkCandidates.begin()->second);
m_mapGreedyWalkCandidates.erase(m_mapGreedyWalkCandidates.begin());
} while (pNextNode == pBegin);
return pNextNode;
}
else
return pBegin;
}
}
// adds an entrance for easy traversing later
void CGraph::AddIndoorEntrance(int nBuildingID, GraphNode* pNode, bool bExitOnly)
{
if (m_pSafeFirst)
{
if (m_pSafeFirst->link.empty())
return;
}
if (bExitOnly)
m_mapExits.insert(EntranceMap::iterator::value_type(nBuildingID,pNode));
else
m_mapEntrances.insert(EntranceMap::iterator::value_type(nBuildingID,pNode));
if (pNode->link.empty())
{
// it has to be connected to the outside
GraphNode *pOutsideNode = GetEnclosing(pNode->data.m_pos,0,true);
Connect(pNode,pOutsideNode);
}
else
{
VectorOfLinks::iterator gi;
bool bOutsideLink = false;
for (gi=pNode->link.begin();gi!=pNode->link.end();gi++)
{
if ( (*gi).pLink->nBuildingID == -1 )
bOutsideLink = true;
}
if (!bOutsideLink)
{
// it has to be connected to the outside
GraphNode *pOutsideNode = GetEnclosing(pNode->data.m_pos,0,true);
Connect(pNode,pOutsideNode);
}
}
if (bExitOnly)
{
VectorOfLinks::iterator gi;
for (gi=pNode->link.begin();gi!=pNode->link.end();gi++)
{
if ( (*gi).pLink->nBuildingID == -1 )
{
GraphNode *pOutNode = (*gi).pLink;
VectorOfLinks::iterator ii;
for (ii=pOutNode->link.begin();ii!=pOutNode->link.end();ii++)
{
if ( (*ii).pLink == pNode)
(*ii).fMaxRadius = 0;
}
}
}
}
}
// Reads the AI graph from a specified file
bool CGraph::ReadFromFile(const char * szName)
{
#ifdef __MWERKS__
#warning Code not implemented under CodeWarrior
#else
CCryFile file;;
if (file.Open( szName,"rb"))
{
ReadNodes( file );
return true;
}
//[Timur]
/*
HANDLE hf;
hf = CreateFile(szName,GENERIC_READ,0,NULL,OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0 );
if (hf != INVALID_HANDLE_VALUE)
{
ReadNodes(hf);
CloseHandle(hf);
return true;
}
*/
return false;
#endif
}
#ifdef __MWERKS__
#warning Code not implemented under CodeWarrior
#else
// reads all the nodes in a map
bool CGraph::ReadNodes( CCryFile &file )
{
//assert(_heapchk()==_HEAPOK);
//DWORD read;
int iNumber;
Vec3d mins,maxs;
m_pAISystem->m_pSystem->GetILog()->UpdateLoadingScreen("\003[AISYSTEM] Verifying BAI file version");
file.Read(&iNumber, sizeof(int) );
if (iNumber != BAI_FILE_VERSION)
{
m_pAISystem->m_pSystem->GetILog()->UpdateLoadingScreen("\002$3[AISYSTEM WARNING] Wrong BAI file version!! Delete them and regenerate them in the editor.");
return false;
}
m_pAISystem->m_pSystem->GetILog()->UpdateLoadingScreen("\003[AISYSTEM] Reading BBOX");
file.Read( &mins.x, sizeof(float) );
file.Read( &mins.y, sizeof(float) );
file.Read( &mins.z, sizeof(float) );
file.Read( &maxs.x, sizeof(float) );
file.Read( &maxs.y, sizeof(float) );
file.Read( &maxs.z, sizeof(float) );
SetBBox(mins,maxs);
m_pAISystem->m_pSystem->GetILog()->UpdateLoadingScreen("\003[AISYSTEM] Reading node descriptors");
file.Read( &iNumber, sizeof(int) );
if (iNumber>0)
{
m_vBuffer.resize(iNumber);
file.Read( &m_vBuffer[0], iNumber*sizeof(NodeDescriptor) );
}
m_vNodes.clear();
// m_vNodes.resize(iNumber);
m_pAISystem->m_pSystem->GetILog()->UpdateLoadingScreen("\003[AISYSTEM] Creating graph nodes");
I3DEngine *pEngine = m_pAISystem->m_pSystem->GetI3DEngine();
NodeBuffer::iterator ni;
int index=0;
for (ni=m_vBuffer.begin();ni!=m_vBuffer.end();ni++,index++)
{
NodeDescriptor buffer = (*ni);
GraphNode *pNode = CreateNewNode(!buffer.bCreated);//new GraphNode;//&m_vNodes[index];
if (buffer.bCreated)
m_pAISystem->CheckInside(buffer.data.m_pos,pNode->nBuildingID,pNode->pArea);
else
{
pNode->nBuildingID=-1;
pNode->pArea = 0;
}
pNode->data = buffer.data;
pNode->vertex.clear();
if (buffer.nObstacles)
{
pNode->vertex.reserve(buffer.nObstacles);
for (int i=0;i<buffer.nObstacles;i++)
{
int nVertexIndex = buffer.obstacle[i];
//ObstacleData od = buffer.obstacle[i];
pNode->vertex.push_back(nVertexIndex);
//pNode->vertex.push_back(buffer.obstacle[i]);
}
}
m_mapReadNodes.insert(EntranceMap::iterator::value_type((EntranceMap::key_type)buffer.id,pNode));
if (buffer.bEntrance)
m_mapEntrances.insert(EntranceMap::iterator::value_type(pNode->nBuildingID,pNode));
if (buffer.bExit)
m_mapExits.insert(EntranceMap::iterator::value_type(pNode->nBuildingID,pNode));
// if ((pNode->nBuildingID < 0) && (pNode->vertex.empty()) && buffer.id!=1)
// AIWarning("![AIERROR] A node was found that had buildingid:%d during export, but now it has buildingid:-1 [pos:(x:%.3f,y:%.3f,z:%.3f)]. Please make sure that all your navigation modifiers are correctly exported.",
// buffer.building,buffer.data.m_pos.x,buffer.data.m_pos.y,buffer.data.m_pos.z);
}
m_pAISystem->m_pSystem->GetILog()->UpdateLoadingScreen("\003[AISYSTEM] Reading vertex list");
m_pAISystem->m_VertexList.ReadFromFile(file);
m_pAISystem->m_pSystem->GetILog()->UpdateLoadingScreen("\003[AISYSTEM] Reading links");
/* file.Read( &iNumber, sizeof(int) );
m_vLinks.resize(iNumber);
file.Read( &m_vLinks[0], iNumber * sizeof(int) );
*/
file.Read( &iNumber, sizeof(int) );
if (iNumber>0)
{
m_vLinksDesc.resize(iNumber);
file.Read( &m_vLinksDesc[0], iNumber * sizeof(LinkDescriptor) );
}
EntranceMap::iterator ei,link;
ei = m_mapReadNodes.find(1);
if (ei==m_mapReadNodes.end())
{
m_pAISystem->m_pSystem->GetILog()->UpdateLoadingScreen("\001[AIERROR] FIRST NODE NOT FOUND !!!!");
return false;
}
else
{
Disconnect(m_pSafeFirst);
//delete m_pFirst;
m_pSafeFirst = (ei->second);
m_pFirst = m_pSafeFirst;
m_pCurrent = m_pSafeFirst;
}
m_pAISystem->m_pSystem->GetILog()->UpdateLoadingScreen("\003[AISYSTEM] Reconnecting links (might take some time:)");
if (!m_vLinksDesc.empty())
{
LinkDescBuffer::iterator iend = m_vLinksDesc.end();
for (LinkDescBuffer::iterator ldbi=m_vLinksDesc.begin();ldbi!=iend;++ldbi)
{
LinkDescriptor &ldb = (*ldbi);
ei=m_mapReadNodes.find((EntranceMap::key_type)ldb.nSourceNode);
GraphNode *pNode = ei->second;
link = m_mapReadNodes.find((EntranceMap::key_type)ldb.nTargetNode);
if (link==m_mapReadNodes.end() || ei==m_mapReadNodes.end())
AIError("!Read a link to a node which could not be found!");
else
{
Connect(pNode,link->second);
for (VectorOfLinks::iterator vli=pNode->link.begin();vli!=pNode->link.end();vli++)
if ( (*vli).pLink == link->second )
{
(*vli).fMaxRadius = ldb.fMaxPassRadius;
(*vli).nStartIndex = ldb.nStartIndex;
(*vli).nEndIndex = ldb.nEndIndex;
(*vli).vWayOut = ldb.vWayOut;
(*vli).vEdgeCenter = ldb.vEdgeCenter;
break;
}
}
}
}
/*int count = 0;
while (count < iNumber)
{
// get id of node
ei = m_mapReadNodes.find(m_vLinks[count++]);
GraphNode *pNode = (ei->second);
int nrlinks = m_vLinks[count++];
pNode->link.reserve(nrlinks);
while(nrlinks--)
{
link = m_mapReadNodes.find(m_vLinks[count++]);
Connect(pNode,(link->second));
ResolveLinkData(pNode,(link->second));
}
}
*/
m_vBuffer.clear();
m_mapReadNodes.clear();
m_vLinksDesc.clear();
//assert(_heapchk()==_HEAPOK);
return true;
}
#endif
// defines bounding rectangle of this graph
void CGraph::SetBBox(const Vec3d & min, const Vec3d & max)
{
m_vBBoxMin = min;
m_vBBoxMax = max;
}
// how is that for descriptive naming of functions ??
bool CGraph::OutsideOfBBox(const Vec3d & pos)
{
if (pos.x < m_vBBoxMin.x)
return true;
if (pos.x > m_vBBoxMax.x)
return true;
if (pos.y < m_vBBoxMin.y)
return true;
if (pos.y > m_vBBoxMax.y)
return true;
return false;
}
void CGraph::FillGreedyMap(GraphNode * pNode, const Vec3d &pos, IVisArea *pTargetArea, bool bStayInArea)
{
MarkNode(pNode);
if (pNode->link.empty())
return;
GraphNode *pNext = 0;
VectorOfLinks::iterator pi,piend = pNode->link.end();
for (pi=pNode->link.begin();pi!=piend;++pi)
{
GraphNode *pNow = (*pi).pLink;
if ( (pNow->mark) || (pNow->nBuildingID!=pNode->nBuildingID))
continue;
if (bStayInArea && (pNow->pArea != pTargetArea))
continue;
float thisdist = (pos - pNow->data.m_pos).GetLength();
m_mapGreedyWalkCandidates.insert(CandidateMap::iterator::value_type(thisdist,pNow));
// this snippet will make sure we only check all points inside the target area - not the whole building
if (pTargetArea && pNow->pArea == pTargetArea)
FillGreedyMap(pNow,pos,pTargetArea,true);
else
FillGreedyMap(pNow,pos,pTargetArea,false);
}
}
bool CGraph::RemoveEntrance(int nBuildingID, GraphNode * pNode)
{
EntranceMap::iterator ei= m_mapEntrances.find(nBuildingID);
if (ei!=m_mapEntrances.end())
{
while ((ei->first == nBuildingID) && ei!=m_mapEntrances.end())
{
if (ei->second == pNode)
{
m_mapEntrances.erase(ei);
return true;
}
ei++;
}
}
ei = m_mapExits.find(nBuildingID);
if (ei!=m_mapExits.end())
{
while ((ei->first == nBuildingID) && ei!=m_mapExits.end())
{
if (ei->second == pNode)
{
m_mapExits.erase(ei);
return true;
}
ei++;
}
}
return false;
}
void CGraph::RemoveIndoorNodes(void)
{
if (m_mapEntrances.empty())
return;
VectorOfLinks::iterator vi;
m_lstDeleteStack.push_front((m_mapEntrances.begin())->second);
while (!m_lstDeleteStack.empty())
{
GraphNode *pCurrentNode = m_lstDeleteStack.front();
m_lstDeleteStack.pop_front();
// put all links into the node stack only if they are indoor nodes
for (vi=pCurrentNode->link.begin();vi!=pCurrentNode->link.end();vi++)
{
GraphNode *pLink =(*vi).pLink;
if (pLink->nBuildingID>=0)
{
if (std::find(m_lstDeleteStack.begin(),m_lstDeleteStack.end(),pLink) == m_lstDeleteStack.end())
m_lstDeleteStack.push_front(pLink);
}
}
Disconnect(pCurrentNode);
}
m_mapEntrances.clear();
}
void CGraph::REC_RemoveNodes(GraphNode * pNode)
{
if (pNode->link.empty())
return;
if (pNode->nBuildingID == -1)
return;
MarkNode(pNode);
VectorOfLinks vecLinks;
vecLinks.insert(vecLinks.begin(),pNode->link.begin(),pNode->link.end());
VectorOfLinks::iterator i;
for (i=vecLinks.begin();i!=vecLinks.end();i++)
{
GraphNode *pLink = (*i).pLink;
if ((pLink->nBuildingID == pNode->nBuildingID) && (!pLink->mark))
REC_RemoveNodes(pLink);
}
Disconnect(pNode);
}
GraphNode * CGraph::CreateNewNode(bool bFromTriangulation)
{
nNodes++;
GraphNode *pRet = new GraphNode;
if (bFromTriangulation)
pRet->bCreated = false;
pRet->AddRef();
return pRet;
}
void CGraph::DeleteNode(GraphNode * pNode)
{
if (pNode->Release())
{
delete pNode;
nNodes--;
}
}
int CGraph::SelectNodesInSphere(const Vec3d & vCenter, float fRadius, GraphNode *pStart)
{
GraphNode *pNode = GetEnclosing(vCenter);
ClearMarks();
m_lstSelected.clear();
if (pNode->nBuildingID == -1)
SelectNodeRecursive(pNode,vCenter,fRadius);
else
{
SelectNodesRecursiveIndoors(pNode,vCenter,fRadius,0);
}
ClearMarks();
return m_lstSelected.size();
}
void CGraph::SelectNodeRecursive(GraphNode* pNode, const Vec3d & vCenter, float fRadius)
{
if (pNode->vertex.empty())
return;
if (pNode->mark)
return;
MarkNode(pNode);
bool stillin = false;
ObstacleIndexVector::iterator oi;
for (oi=pNode->vertex.begin();oi!=pNode->vertex.end();oi++)
{
ObstacleData od = m_pAISystem->m_VertexList.GetVertex((*oi));
float flength = GetLengthSquared((od.vPos - vCenter));
if (flength < (fRadius*fRadius))
{
if ( std::find(m_lstSelected.begin(),m_lstSelected.end(),od) == m_lstSelected.end())
m_lstSelected.push_back(od);
stillin = true;
}
}
if (!stillin)
return;
// go trough all the links of this one
VectorOfLinks::iterator gi;
for (gi=pNode->link.begin();gi!=pNode->link.end();gi++)
{
if ((*gi).pLink->nBuildingID<0)
SelectNodeRecursive((*gi).pLink,vCenter,fRadius);
}
}
void CGraph::SelectNodesRecursiveIndoors(GraphNode * pNode, const Vec3d & vCenter, float fRadius, float fDistance)
{
if (pNode->nBuildingID == -1)
return; // DO NOT go outdoors
if (fDistance>fRadius)
return;
if (pNode->mark)
return;
MarkNode(pNode);
bool stillin = false;
if ( GetLengthSquared((pNode->data.m_pos - vCenter)) < 2*(fRadius*fRadius))
stillin = true;
ObstacleIndexVector::iterator oi;
for (oi=pNode->vertex.begin();oi!=pNode->vertex.end();oi++)
{
ObstacleData od = m_pAISystem->m_VertexList.GetVertex((*oi));
float flength = GetLengthSquared((od.vPos - vCenter));
// float fPathLength = fDistance + (pNode->data.m_pos - od.vPos).GetLength();
// if (fPathLength*fPathLength > flength*2.f)
// continue;
if (flength < (fRadius*fRadius))
{
if ( std::find(m_lstSelected.begin(),m_lstSelected.end(),od) == m_lstSelected.end())
m_lstSelected.push_back(od);
stillin = true;
}
}
if (!stillin)
return;
// go trough all the links of this one
VectorOfLinks::iterator gi;
for (gi=pNode->link.begin();gi!=pNode->link.end();gi++)
{
GraphNode *pNextNode = (*gi).pLink;
SelectNodesRecursiveIndoors(pNextNode,vCenter,fRadius,fDistance+(pNode->data.m_pos - pNextNode->data.m_pos).GetLength());
}
}
void CGraph::AddHidePoint(GraphNode* pOwner, const Vec3d & pos, const Vec3d & dir)
{
if (!pOwner)
return;
ObstacleData od;
od.vPos = pos;
od.vDir = dir;
index_t i = m_pAISystem->m_VertexList.FindVertex(od);
if (i<0)
{
//pOwner->vertex.push_back(od);
pOwner->vertex.push_back(m_pAISystem->m_VertexList.AddVertex(od));
}
else
{
ObstacleIndexVector::iterator oi = std::find(pOwner->vertex.begin(),pOwner->vertex.end(),i);
if (oi!=pOwner->vertex.end())
{
m_pAISystem->m_VertexList.ModifyVertex(i).vPos = pos;
m_pAISystem->m_VertexList.ModifyVertex(i).vDir = dir;
}
else
pOwner->vertex.push_back(i);
}
}
void CGraph::RemoveHidePoint(GraphNode * pOwner, const Vec3d & pos, const Vec3d & dir)
{
if (!pOwner)
return;
ObstacleIndexVector::iterator oi;
for ( oi = pOwner->vertex.begin();oi != pOwner->vertex.end(); oi++ )
{
ObstacleData od = m_pAISystem->m_VertexList.GetVertex((*oi));
if ( ( (od.vPos - pos).GetLength() < 0.0001 ) )
{
pOwner->vertex.erase(oi);
break;
}
}
}
void CGraph::DisconnectUnreachable(void)
{
GraphNode *pCurrent = m_pFirst;
m_lstNodeStack.clear();
ClearMarks();
m_lstNodeStack.push_back(pCurrent);
ray_hit rh;
while (!m_lstNodeStack.empty())
{
MarkNode(pCurrent);
vectorf start = pCurrent->data.m_pos;
VectorOfLinks linkVector;
linkVector = pCurrent->link;
VectorOfLinks::iterator vi;
for (vi=linkVector.begin();vi!=linkVector.end();vi++)
{
if (!(*vi).pLink->mark)
{
if (std::find(m_lstNodeStack.begin(),m_lstNodeStack.end(),(*vi).pLink) == m_lstNodeStack.end())
m_lstNodeStack.push_front((*vi).pLink);
}
// check connectivity
vectorf end = (*vi).pLink->data.m_pos;
int cnt = m_pAISystem->GetPhysicalWorld()->RayWorldIntersection(start,end-start,ent_static, 0,&rh,1);
if (cnt)
DisconnectLink(pCurrent,(*vi).pLink);
}
pCurrent = m_lstNodeStack.front();
m_lstNodeStack.pop_front();
}
ClearMarks();
}
void CGraph::DisconnectLink(GraphNode * one, GraphNode * two, bool bOneWay)
{
if (!one || !two)
return;
VectorOfLinks::iterator vi;
for (vi=one->link.begin();vi!=one->link.end();vi++)
{
if ( (*vi).pLink == two)
{
one->link.erase(vi);
DeleteNode(two); // this will lower the ref count... will delete if appropriate
break;
}
}
if (bOneWay)
return;
for (vi=two->link.begin();vi!=two->link.end();vi++)
{
if ( (*vi).pLink == one)
{
two->link.erase(vi);
DeleteNode(one); // this will lower the ref count... will delete if appropriate
break;
}
}
}
int CGraph::BeautifyPath(int &nIterations, const Vec3d &start, const Vec3d &end)
{
//return PATHFINDER_PATHFOUND;
if (!m_pAISystem->m_cvBeautifyPath->GetIVal())
{
return PATHFINDER_PATHFOUND;
}
// try to skip unnecessary wiggling around
if (m_bBeautifying)
{
m_lstPath.clear();
if (m_lstNodeStack.size()==1)
{
m_lstPath.push_back(end);
return PATHFINDER_PATHFOUND;
}
m_iFirst = m_lstNodeStack.begin();
m_iThird = m_lstNodeStack.end();
m_bBeautifying = false;
m_vBeautifierStart = start;
//CryLogAlways("Start BEAUTIFY PATH for: %s ",m_pRequester->GetName());
//CryLogAlways("From position (%.3f,%.3f,%.3f)",start.x,start.y,start.z);
//CryLogAlways("To position (%.3f,%.3f,%.3f)",end.x,end.y,end.z);
}
//else
//{
// CryLogAlways("continuing beautification for: %s ",m_pRequester->GetName());
//}
while ((nIterations--) && (m_iFirst!=m_lstNodeStack.end()))
{
ListNodes::iterator iNextFirst = m_lstNodeStack.end();
m_iSecond = m_iFirst;
m_iSecond++;
Vec3d vRealStart,vRealEnd;
bool bSkipEval = false;
if ((*m_iFirst)->nBuildingID < 0)
{
while (m_iSecond != m_lstNodeStack.end())
{
GraphNode *pFirst = (*m_iFirst);
GraphNode *pSecond = (*m_iSecond);
if (pSecond->nBuildingID >= 0)
{
if (!bSkipEval)
{
bSkipEval = true;
iNextFirst = m_lstNodeStack.end();
}
// if (m_iThird!=m_lstNodeStack.end())
// {
// m_lstPath.push_back( (*m_iThird)->data.m_pos);
// m_iFirst = m_iThird;
// }
break;
}
// traverse list from iFirst to iSecond testing edges as you go
bool bPassable = true; // optimistic expectation
ListNodes::iterator iPairStart=m_iFirst;
for (;iPairStart!=m_iSecond;iPairStart++)
{
ListNodes::iterator iPairEnd = iPairStart;
iPairEnd++;
Vec3d EdgStart;
Vec3d EdgEnd;
Vec3d EdgDir;
Vec3d EdgCenter;
VectorOfLinks::iterator vli;
for (vli=(*iPairStart)->link.begin();vli!=(*iPairStart)->link.end();vli++)
{
if ((*vli).pLink == (*iPairEnd))
{
if ((m_iFirst == m_lstNodeStack.begin()) || (m_lstPath.empty()))
vRealStart = start;
else
vRealStart = m_lstPath.back();
if ((*m_iSecond) == (*m_lstNodeStack.rbegin()))
vRealEnd = end;
else
vRealEnd = (*m_iSecond)->data.m_pos;
EdgStart = m_pAISystem->m_VertexList.GetVertex((*iPairStart)->vertex[(*vli).nStartIndex]).vPos;
EdgEnd = m_pAISystem->m_VertexList.GetVertex((*iPairStart)->vertex[(*vli).nEndIndex]).vPos;
EdgDir = EdgEnd - EdgStart;
EdgCenter =(*vli).vEdgeCenter;
EdgCenter.z = 0;
vRealStart.z = 0;
vRealEnd.z = 0;
float s=-1,t=-1;
if (m_pAISystem->SegmentsIntersect(vRealStart, vRealEnd-vRealStart,EdgStart,EdgDir,s,t))
{
if ( s>0.f && s<1.f && t>0.f && t<1.f)
{
m_vLastIntersection = EdgStart+t*EdgDir;
float fCenterDist = (m_vLastIntersection - EdgCenter).GetLength();
//if (fCenterDist > ((*vli).fMaxRadius-1.f))
if (fCenterDist > ((*vli).fMaxRadius-m_pRequester->m_fPassRadius))
{
bPassable = false;
}
}
else
bPassable = false;
}
else
{
bPassable = false;
}
break;
}
}
if (!bPassable)
{
float fEdgeLength =EdgDir.Length();
//float dist_from_obstacle = 0.6f;
float dist_from_obstacle = m_pRequester->m_fPassRadius;
// if we have more space, do not get too close to the obstacle
if ((m_pRequester->m_fPassRadius < 1.f) && ((*vli).fMaxRadius > 1.f))
dist_from_obstacle = 1.f;
if ((EdgStart-m_vLastIntersection).Length() < (EdgEnd-m_vLastIntersection).Length() )
{
// closer to start edge
float fFullDistToCenter = (EdgStart - EdgCenter).Length();
float minT = fFullDistToCenter - (*vli).fMaxRadius + dist_from_obstacle;
minT/=fEdgeLength;
m_vBeautifierStart = EdgStart+minT*EdgDir;
}
else
{
// closer to end edge
float fFullDistToCenter = (EdgEnd - EdgCenter).Length();
float minT = fFullDistToCenter - (*vli).fMaxRadius + dist_from_obstacle;
minT/=fEdgeLength;
m_vBeautifierStart = EdgEnd-minT*EdgDir;
}
/* float minT = ((fEdgeLength/2.f)-((*vli).fMaxRadius-dist_from_obstacle)) / fEdgeLength;
// which edge is closer to the intersection point
if ((EdgStart-m_vLastIntersection).Length() < (EdgEnd-m_vLastIntersection).Length() )
m_vBeautifierStart = EdgStart+minT*EdgDir;
else
m_vBeautifierStart = EdgEnd-minT*EdgDir;*/
break;
}
}
if (bPassable)
{
m_iThird = m_iSecond;
if (bSkipEval)
{
iNextFirst = m_lstNodeStack.end();
bSkipEval = false;
}
}
else
{
if (!bSkipEval)
{
iNextFirst = iPairStart;
bSkipEval = true;
}
}
m_iSecond++;
}
if (!bSkipEval)
{
// if the third is not valid, then add midpoint od first and second, and then second
if (m_iThird == m_lstNodeStack.end())
{
m_iSecond = m_iFirst;
m_iSecond++;
if (m_iSecond!=m_lstNodeStack.end() && ((*m_iSecond)->nBuildingID <0))
{
///m_lstPath.push_back(vBestPass);
VectorOfLinks::iterator vli;
for (vli=(*m_iSecond)->link.begin();vli!=(*m_iSecond)->link.end();vli++)
{
if ((*vli).pLink == (*m_iFirst))
m_lstPath.push_back((*vli).vEdgeCenter);
}
}
}
else
{
m_iSecond = m_iFirst;
m_iSecond++;
if ((m_iSecond!=m_iThird) && (m_iSecond != m_lstNodeStack.end()))
{
// there are some nodes to delete, so delete them
for (ListNodes::iterator li=m_iSecond; (li!=m_iThird) && (li!=m_lstNodeStack.end());li=m_lstNodeStack.erase(li));
}
//if (m_iThird == --m_lstNodeStack.end())
if ((*m_iThird) == (*m_lstNodeStack.rbegin()))
{
m_lstPath.push_back(end);
}
else
{
m_lstPath.push_back( (*m_iThird)->data.m_pos);
//vBestPass.z = m_pAISystem->m_pSystem->GetI3DEngine()->GetTerrainElevation(vBestPass.x, vBestPass.y);
//m_lstPath.push_back( vBestPass);
}
}
}
else
{
if (iNextFirst != m_lstNodeStack.end())
{
I3DEngine *pEngine = m_pAISystem->m_pSystem->GetI3DEngine();
if (pEngine)
m_vBeautifierStart.z = pEngine->GetTerrainElevation(m_vBeautifierStart.x,m_vBeautifierStart.y);
m_lstPath.push_back(m_vBeautifierStart);
m_iFirst = iNextFirst;
}
}
}
else
{
m_lstPath.push_back( (*m_iFirst)->data.m_pos);
}
m_iThird = m_lstNodeStack.end();
m_iFirst++;
}
if (m_iFirst==m_lstNodeStack.end())
m_lstPath.push_back(end);
if (m_iFirst == m_lstNodeStack.end())
{
m_bBeautifying = true;
m_lstNodeStack.clear();
if (m_lstPath.size() > 2)
{
int todel = 1;
ListPositions::iterator li = m_lstPath.begin();
Vec3d firstpos = (*li++)-start;
Vec3d secondpos= (*li)-start;
while ((*li) == m_lstPath.front())
{
secondpos= (*++li)-start;;
todel++;
}
if (firstpos.Dot(secondpos) < 0)
{
while (todel--)
{
if (!m_lstPath.empty())
m_lstPath.pop_front();
}
}
}
return PATHFINDER_PATHFOUND;
}
//CryLogAlways("----Ran out of iterations(%.3f,%.3f,%.3f)",(*m_iFirst)->data.m_pos.x,(*m_iFirst)->data.m_pos.y,(*m_iFirst)->data.m_pos.z);
return PATHFINDER_BEAUTIFYINGPATH;
}
void CGraph::ResolveLinkData(GraphNode* pOne, GraphNode* pTwo)
{
if (pOne->nBuildingID>=0 || pTwo->nBuildingID>=0)
return;
if (pOne->vertex.empty() || pTwo->vertex.empty())
return;
int iOneEdge1=-1,iOneEdge2=-1;
int iTwoEdge1=-1,iTwoEdge2=-1;
for (unsigned int i=0;i<pOne->vertex.size();i++)
for (unsigned int j=0;j<pTwo->vertex.size();j++)
{
/* if ( (fabs(pOne->vertex[i].vPos.x - pTwo->vertex[j].vPos.x) < 0.0001f) &&
(fabs(pOne->vertex[i].vPos.y - pTwo->vertex[j].vPos.y) < 0.0001f) &&
(fabs(pOne->vertex[i].vPos.z - pTwo->vertex[j].vPos.z) < 0.0001f) )
*/
// if (IsEquivalent(pOne->vertex[i].vPos,pTwo->vertex[j].vPos,0.001f))
//if (pOne->vertex[i].vPos==pTwo->vertex[j].vPos )
if (pOne->vertex[i]==pTwo->vertex[j])
{
if (iOneEdge1<0)
iOneEdge1 = i;
else
iOneEdge2 = i;
if (iTwoEdge1<0)
iTwoEdge1 = j;
else
iTwoEdge2 = j;
}
}
// find triangle normal for one
Vec3d OneNormal = (m_pAISystem->m_VertexList.GetVertex(pOne->vertex[0]).vPos - m_pAISystem->m_VertexList.GetVertex(pOne->vertex[1]).vPos).Cross(m_pAISystem->m_VertexList.GetVertex(pOne->vertex[2]).vPos - m_pAISystem->m_VertexList.GetVertex(pOne->vertex[1]).vPos);
Vec3d TwoNormal = (m_pAISystem->m_VertexList.GetVertex(pTwo->vertex[0]).vPos - m_pAISystem->m_VertexList.GetVertex(pTwo->vertex[1]).vPos).Cross(m_pAISystem->m_VertexList.GetVertex(pTwo->vertex[2]).vPos - m_pAISystem->m_VertexList.GetVertex(pTwo->vertex[1]).vPos);
if ( iOneEdge1 * iOneEdge2 < 0)
int a=5;
if ( iTwoEdge1 * iTwoEdge2 < 0)
int a=5;
int iNoOne = 3 - (iOneEdge1 + iOneEdge2);
int iNoTwo = 3 - (iTwoEdge1 + iTwoEdge2);
// find which link from one goes to two
VectorOfLinks::iterator vi;
for (vi=pOne->link.begin();vi!=pOne->link.end();vi++)
{
if ((*vi).pLink == pTwo)
{
(*vi).nStartIndex = iOneEdge1;
(*vi).nEndIndex = iOneEdge2;
Vec3d edge = m_pAISystem->m_VertexList.GetVertex(pOne->vertex[iOneEdge1]).vPos - m_pAISystem->m_VertexList.GetVertex(pOne->vertex[iOneEdge2]).vPos;
/// if (pOne->vertex[iOneEdge1].bOccupied && pOne->vertex[iOneEdge2].bOccupied)
// (*vi).fMaxRadius = -1;
Vec3d vCross(edge.Cross(TwoNormal));
float fLen = vCross.GetLength();
Vec3d normal = fLen>0 ? vCross/fLen : Vec3d(0,0,0);
float fdot = normal.Dot(m_pAISystem->m_VertexList.GetVertex(pTwo->vertex[iNoTwo]).vPos - (m_pAISystem->m_VertexList.GetVertex(pTwo->vertex[iTwoEdge1]).vPos + edge/2.f));
(*vi).vWayOut = normal;
if (fdot > 0 )
(*vi).vWayOut *=-1.f;
}
}
for (vi=pTwo->link.begin();vi!=pTwo->link.end();vi++)
{
if ((*vi).pLink == pOne)
{
(*vi).nStartIndex = iTwoEdge1;
(*vi).nEndIndex = iTwoEdge2;
Vec3d edge = m_pAISystem->m_VertexList.GetVertex(pTwo->vertex[iTwoEdge1]).vPos - m_pAISystem->m_VertexList.GetVertex(pTwo->vertex[iTwoEdge2]).vPos;
//if (pTwo->vertex[iTwoEdge1].bOccupied && pTwo->vertex[iTwoEdge2].bOccupied)
// (*vi).fMaxRadius = -1;
Vec3d vCross(edge.Cross(TwoNormal));
float fLen = vCross.GetLength();
Vec3d normal = fLen>0 ? vCross/fLen : Vec3d(0,0,0);
float fdot = normal.Dot(m_pAISystem->m_VertexList.GetVertex(pTwo->vertex[iNoTwo]).vPos - (m_pAISystem->m_VertexList.GetVertex(pTwo->vertex[iTwoEdge1]).vPos + edge/2.f));
(*vi).vWayOut = normal;
if (fdot > 0 )
(*vi).vWayOut *=-1.f;
}
}
}
void CGraph::ConnectNodes(ListNodes & lstNodes)
{
// clear degenerate triangles
ListNodes::iterator it;
for (it=lstNodes.begin();it!=lstNodes.end();)
{
GraphNode *pCurrent = (*it);
ObstacleIndexVector::iterator obi,obi2;
bool bRemoved = false;
for (obi=pCurrent->vertex.begin();obi!=pCurrent->vertex.end();obi++)
{
for (obi2=pCurrent->vertex.begin();obi2!=pCurrent->vertex.end();obi2++)
{
if (obi==obi2)
continue;
if ( (*obi) == (*obi2) )
{
it=lstNodes.erase(it);
bRemoved = true;
break;
}
}
if (bRemoved)
break;
}
if (!bRemoved)
it++;
}
// reconnect triangles in nodes list
ListNodes::iterator it1,it2;
for (it1=lstNodes.begin();it1!=lstNodes.end();it1++)
{
GraphNode *pCurrent = (*it1);
for (it2=lstNodes.begin();it2!=lstNodes.end();it2++)
{
GraphNode *pCandidate = (*it2);
ObstacleIndexVector::iterator obCur,obNext;
for (obCur=pCurrent->vertex.begin();obCur!=pCurrent->vertex.end();obCur++)
{
obNext = obCur;
obNext++;
if (obNext==pCurrent->vertex.end())
obNext = pCurrent->vertex.begin();
ObstacleIndexVector::iterator obCan,obNextCan;
for (obCan=pCandidate->vertex.begin();obCan!=pCandidate->vertex.end();obCan++)
{
obNextCan = obCan;
obNextCan++;
if (obNextCan == pCandidate->vertex.end())
obNextCan = pCandidate->vertex.begin();
/*
if ( (fabs((*obCur).vPos.x - (*obCan).vPos.x) < 0.0001f) &&
(fabs((*obCur).vPos.y - (*obCan).vPos.y) < 0.0001f) &&
(fabs((*obCur).vPos.z - (*obCan).vPos.z) < 0.0001f) &&
(fabs((*obNext).vPos.x - (*obNextCan).vPos.x) < 0.0001f) &&
(fabs((*obNext).vPos.y - (*obNextCan).vPos.y) < 0.0001f) &&
(fabs((*obNext).vPos.z - (*obNextCan).vPos.z) < 0.0001f) )
*/
if ( ((*obCur)==(*obCan)) && ((*obNext)==(*obNextCan)) )
{
if (pCandidate!=pCurrent)
{
Connect(pCurrent,pCandidate);
ResolveLinkData(pCurrent,pCandidate);
}
}
/*
if ( (fabs((*obCur).vPos.x - (*obNextCan).vPos.x) < 0.0001f) &&
(fabs((*obCur).vPos.y - (*obNextCan).vPos.y) < 0.0001f) &&
(fabs((*obCur).vPos.z - (*obNextCan).vPos.z) < 0.0001f) &&
(fabs((*obNext).vPos.x - (*obCan).vPos.x) < 0.0001f) &&
(fabs((*obNext).vPos.y - (*obCan).vPos.y) < 0.0001f) &&
(fabs((*obNext).vPos.z - (*obCan).vPos.z) < 0.0001f) )
*/
if ( ((*obCur)==(*obNextCan)) && ((*obNext)==(*obCan)) )
{
if (pCandidate!=pCurrent)
{
Connect(pCurrent,pCandidate);
ResolveLinkData(pCurrent,pCandidate);
}
}
}
}
}
}
for (it1=lstNodes.begin();it1!=lstNodes.end();it1++)
{
GraphNode *pCurrent = (*it1);
VectorOfLinks::iterator vi;
for (vi=pCurrent->link.begin();vi!=pCurrent->link.end();vi++)
ResolveLinkData(pCurrent,(*vi).pLink);
}
}
void CGraph::FillGraphNodeData(GraphNode* pNode)
{
I3DEngine *pEngine = m_pAISystem->m_pSystem->GetI3DEngine();
//CheckClockness(pNode);
ObstacleIndexVector::iterator oi;
for (oi=pNode->vertex.begin();oi!=pNode->vertex.end();oi++)
{
pNode->data.m_pos.x += m_pAISystem->m_VertexList.GetVertex((*oi)).vPos.x;
pNode->data.m_pos.y += m_pAISystem->m_VertexList.GetVertex((*oi)).vPos.y;
pNode->data.m_pos.z += m_pAISystem->m_VertexList.GetVertex((*oi)).vPos.z;
}
pNode->data.m_pos.x/=3.f;
pNode->data.m_pos.y/=3.f;
pNode->data.m_pos.z/=3.f;
// pNode->data.m_pos.x = (v1->x + v2->x + v3->x) / 3.f;
// pNode->data.m_pos.y = (v1->y + v2->y + v3->y) / 3.f;
float x = pNode->data.m_pos.x ;
float y = pNode->data.m_pos.y ;
// put it on the terrain
float z = pEngine->GetTerrainElevation(x,y);
pNode->data.m_pos.z = z+0.2f; // elevate it a little bit
// calculate slope index
float he,ve;
he = ( pEngine->GetTerrainElevation(x+1,y) - pEngine->GetTerrainElevation(x-1,y) ) / 2.f; // tg(alpha)
he= (float) fabs(he);
if ( he>1.f ) he = 1.0f; //for now, no slopes beyond 45 degrees
ve = ( pEngine->GetTerrainElevation(x,y+1) - pEngine->GetTerrainElevation(x,y-1) ) / 2.f; // tg(alpha)
ve= (float) fabs(ve);
if ( ve>1.f ) ve = 1.0f; //for now, no slopes beyond 45 degrees
if (he>ve) pNode->data.fSlope = he;
else pNode->data.fSlope = ve;
//check if point in water
if (pEngine->IsPointInWater(Vec3d(pNode->data.m_pos)))
pNode->data.bWater = true;
// check if point in shadow
// if (pEngine->IsPointInShadow(Vec3d(pNode->data.m_pos)))
// pNode->data.bShadow = true;
}
void CGraph::SetCurrentHeuristic(unsigned int heuristic_type)
{
if (m_pHeuristic)
{
delete m_pHeuristic;
m_pHeuristic = 0;
}
switch (heuristic_type)
{
case AIHEURISTIC_DEFAULT:
m_pHeuristic = new CHeuristic;
break;
case AIHEURISTIC_STANDARD:
m_pHeuristic = new CStandardHeuristic;
break;
case AIHEURISTIC_VEHICLE:
m_pHeuristic = new CVehicleHeuristic;
break;
}
}
void CGraph::ResolveTotalLinkData(void)
{
m_lstNodeStack.clear();
m_lstNodeStack.push_back(m_pSafeFirst);
while (!m_lstNodeStack.empty())
{
GraphNode *pCurrent = m_lstNodeStack.front();
m_lstNodeStack.pop_front();
MarkNode(pCurrent);
for (VectorOfLinks::iterator vi=pCurrent->link.begin();vi!=pCurrent->link.end();vi++)
{
ResolveLinkData(pCurrent,(*vi).pLink);
if (!vi->pLink->mark)
{
if (std::find(m_lstNodeStack.begin(),m_lstNodeStack.end(),vi->pLink) == m_lstNodeStack.end())
m_lstNodeStack.push_back(vi->pLink);
}
}
}
ClearMarks();
}
bool CGraph::CanReuseLastPath(GraphNode * pBegin)
{
ListNodes::iterator i = m_lstLastPath.begin();
while (i!=m_lstLastPath.end())
{
if ( (*i) == pBegin )
{
m_lstNodeStack.clear();
m_lstNodeStack.insert(m_lstNodeStack.begin(),i,m_lstLastPath.end());
m_lstPath.clear();
for (ListNodes::iterator fi = i;fi!=m_lstLastPath.end();fi++)
m_lstPath.push_back((*fi)->data.m_pos);
return true;
}
++i;
}
return false;
}
GraphNode * CGraph::GetThirdNode(const Vec3d & vFirst, const Vec3d & vSecond, const Vec3d &vThird)
{
GraphNode *pFirst = GetEnclosing(vFirst);
GraphNode *pSecond = GetEnclosing(vSecond);
GraphNode *pThird = GetEnclosing(vThird);
VectorOfLinks::iterator vli;
if (pFirst->link.size()>2)
{
for (vli=pFirst->link.begin();vli!=pFirst->link.end();vli++)
if (((*vli).pLink != pSecond) && ((*vli).pLink!=pThird))
return (*vli).pLink;
}
else
{
for (vli=pFirst->link.begin();vli!=pFirst->link.end();vli++)
if ( (*vli).pLink != pSecond )
return (*vli).pLink;
}
return NULL;
}
void CGraph::Reset(void)
{
m_pWalkBackCurrent = 0;
m_pPathfinderCurrent = 0;
m_lstCurrentHistory.clear();
m_lstLastPath.clear();
m_lstNodeStack.clear();
m_bBeautifying = true;
}
GraphNode * CGraph::GetEntrance(int nBuildingID,const Vec3d &pos)
{
GraphNode *pEntrance=0;
float mindist = 1000000;
EntranceMap::iterator ei = m_mapEntrances.find(nBuildingID);
if (ei != m_mapEntrances.end())
{
pEntrance=ei->second;
if (m_mapEntrances.count(nBuildingID) > 1)
{
mindist = GetLengthSquared((pEntrance->data.m_pos-pos));
for (;ei!=m_mapEntrances.end();ei++)
{
if (ei->first != nBuildingID)
break;
float curr_dist = GetLengthSquared(((ei->second)->data.m_pos-pos));
if (curr_dist<=mindist)
{
pEntrance = ei->second;
mindist = curr_dist;
}
}
}
}
ei = m_mapExits.find(nBuildingID);
if (ei != m_mapExits.end())
{
pEntrance=ei->second;
if (m_mapExits.count(nBuildingID) > 1)
{
mindist = GetLengthSquared((pEntrance->data.m_pos-pos));
for (;ei!=m_mapExits.end();ei++)
{
if (ei->first != nBuildingID)
break;
float curr_dist = GetLengthSquared(((ei->second)->data.m_pos-pos));
if (curr_dist<=mindist)
{
pEntrance = ei->second;
mindist = curr_dist;
}
}
}
}
return pEntrance;
}
void CGraph::RemoveDegenerateTriangle(GraphNode * pDegenerate, bool bRecurse)
{
VectorOfLinks::iterator li, liend = pDegenerate->link.end();
for (li=pDegenerate->link.begin();li!=liend;++li)
{
Vec3d vStart = m_pAISystem->m_VertexList.GetVertex(pDegenerate->vertex[(*li).nStartIndex]).vPos;
Vec3d vEnd = m_pAISystem->m_VertexList.GetVertex(pDegenerate->vertex[(*li).nEndIndex]).vPos;
if (IsEquivalent(vStart,vEnd,0.1f))
break;
}
if (li==liend)
return;
int noStartIndex = (*li).nStartIndex;
int noEndIndex = (*li).nEndIndex;
int noVertex = 3 - (noStartIndex+noEndIndex);
GraphNode *pLink1=0,*pLink2=0;
VectorOfLinks::iterator vol,ivolend = pDegenerate->link.end();
for (vol=pDegenerate->link.begin();vol!=ivolend;++vol)
{
if ( ( (*vol).nStartIndex == noVertex) ||
( (*vol).nEndIndex == noVertex ))
{
// we have one of the other two links - supposedly correct
if ( (*vol).nStartIndex == noStartIndex || (*vol).nEndIndex == noStartIndex)
pLink1 = (*vol).pLink;
else
pLink2 = (*vol).pLink;
}
}
if (!pLink1 || !pLink2)
AIError("!Bad triangle found while trying to eliminate a degenerate triangle.");
if (bRecurse)
RemoveDegenerateTriangle((*li).pLink,false);
Disconnect(pDegenerate);
Connect(pLink1,pLink2);
if (pLink1->mark)
pLink1->mark = false;
if (pLink2->mark)
pLink2->mark = false;
}
void CGraph::FindTrapNodes(GraphNode *pNode, int recCount)
{
if (recCount<0)
return;
MarkNode(pNode);
if (pNode->nBuildingID>=0) // no inside
return;
if (!pNode->link.empty())
{
bool bTrapped = true;
bool bCanHoldPlayer = false;
VectorOfLinks::iterator vli, vliend = pNode->link.end();
for (vli=pNode->link.begin();vli!=vliend;++vli)
{
GraphLink glink = (*vli);
if (glink.fMaxRadius>0.6f)
bTrapped = false;
Vec3d start = m_pAISystem->m_VertexList.GetVertex((*vli).nStartIndex).vPos;
Vec3d end = m_pAISystem->m_VertexList.GetVertex((*vli).nEndIndex).vPos;
if ( GetLength(start-end) > 0.6)
bCanHoldPlayer = true;
}
if (bTrapped && bCanHoldPlayer)
{
m_lstTrapNodes.push_back(pNode);
}
for (vli=pNode->link.begin();vli!=vliend;++vli)
{
GraphLink glink = (*vli);
FindTrapNodes(glink.pLink,recCount-1);
}
}
}
void CGraph::FixDegenerateTriangles(void)
{
m_lstNodeStack.clear();
m_lstNodeStack.push_back(m_pSafeFirst);
// traverse all the graph
while (!m_lstNodeStack.empty())
{
GraphNode *pCurrent = m_lstNodeStack.front();
m_lstNodeStack.pop_front();
MarkNode(pCurrent);
// add neighbors to list
for (VectorOfLinks::iterator vi=pCurrent->link.begin();vi!=pCurrent->link.end();vi++)
{
if (!vi->pLink->mark)
{
if (std::find(m_lstNodeStack.begin(),m_lstNodeStack.end(),vi->pLink) == m_lstNodeStack.end())
m_lstNodeStack.push_back(vi->pLink);
}
}
if (pCurrent == m_pSafeFirst)
continue;
// check for degeneracy
float fDegen = pCurrent->GetDegeneracyValue();
if (fDegen<0.01f)
{
GraphNode *pPair = 0;
float fTSize = (float) GetAISystem()->m_pSystem->GetI3DEngine()->GetTerrainSize();
VectorOfLinks::iterator vli = pCurrent->link.begin(),vliend = pCurrent->link.end();
for (;vli!=vliend;++vli)
{
if ( (*vli).fMaxRadius>0 ) // make sure you do not break forbidden areas
{
GraphNode *pCandidate = (*vli).pLink;
if (pCandidate==m_pSafeFirst)
continue;
// find the link in the candidate that leads to the current node
VectorOfLinks::iterator vback = pCandidate->link.begin(),vbackend = pCandidate->link.end();
for (;vback!=vbackend;++vback)
{
if ( (*vback).pLink == pCurrent )
break;
}
// create the two new nodes
GraphNode *p1 = CreateNewNode();
GraphNode *p2 = CreateNewNode();
p1->vertex.push_back( pCurrent->vertex[(*vli).nStartIndex] );
p1->vertex.push_back( pCurrent->vertex[3 - ((*vli).nEndIndex+ (*vli).nStartIndex)] );
p1->vertex.push_back( pCandidate->vertex[3 - ((*vback).nEndIndex+ (*vback).nStartIndex)] );
FillGraphNodeData(p1);
p2->vertex.push_back( pCurrent->vertex[(*vli).nEndIndex] );
p2->vertex.push_back( pCurrent->vertex[3 - ((*vli).nEndIndex+ (*vli).nStartIndex)] );
p2->vertex.push_back( pCandidate->vertex[3 - ((*vback).nEndIndex+ (*vback).nStartIndex)] );
FillGraphNodeData(p2);
float p1Deg = p1->GetDegeneracyValue();
float p2Deg = p2->GetDegeneracyValue();
if ( (p1Deg>fDegen) && (p2Deg>fDegen) )
{
fDegen = (p1Deg<p2Deg)? p2Deg : p1Deg;
pPair=pCandidate;
}
Disconnect(p1);
Disconnect(p2);
}
}
if (pPair)
{
// find the link in the candidate that leads to the current node
vli = pCurrent->link.begin(),vliend = pCurrent->link.end();
for (;vli!=vliend;++vli)
{
if ( (*vli).pLink == pPair )
break;
}
VectorOfLinks::iterator vback = pPair->link.begin(),vbackend = pPair->link.end();
for (;vback!=vbackend;++vback)
{
if ( (*vback).pLink == pCurrent )
break;
}
// create the two new nodes
GraphNode *p1 = CreateNewNode();
GraphNode *p2 = CreateNewNode();
ListNodes lstNewNodes;
p1->vertex.push_back( pCurrent->vertex[(*vli).nStartIndex] );
p1->vertex.push_back( pCurrent->vertex[3 - ((*vli).nEndIndex+ (*vli).nStartIndex)] );
p1->vertex.push_back( pPair->vertex[3 - ((*vback).nEndIndex+ (*vback).nStartIndex)] );
FillGraphNodeData(p1);
p2->vertex.push_back( pCurrent->vertex[(*vli).nEndIndex] );
p2->vertex.push_back( pCurrent->vertex[3 - ((*vli).nEndIndex+ (*vli).nStartIndex)] );
p2->vertex.push_back( pPair->vertex[3 - ((*vback).nEndIndex+ (*vback).nStartIndex)] );
FillGraphNodeData(p2);
lstNewNodes.push_back(p1);
lstNewNodes.push_back(p2);
// push all neighbors of existing 2 triangles
vback = pPair->link.begin(),vbackend = pPair->link.end();
for (;vback!=vbackend;++vback)
{
if ( std::find(lstNewNodes.begin(),lstNewNodes.end(),(*vback).pLink)==lstNewNodes.end() )
{
if (( (*vback).pLink!=m_pSafeFirst) && ((*vback).pLink!=pCurrent))
lstNewNodes.push_back((*vback).pLink);
}
}
vback = pCurrent->link.begin(),vbackend = pCurrent->link.end();
for (;vback!=vbackend;++vback)
{
if ( std::find(lstNewNodes.begin(),lstNewNodes.end(),(*vback).pLink)==lstNewNodes.end())
{
if (( (*vback).pLink!=m_pSafeFirst) && ((*vback).pLink!=pPair) )
lstNewNodes.push_back((*vback).pLink);
}
}
Disconnect(pPair);
Disconnect(pCurrent);
ListNodes::iterator lif = std::find(m_lstNodeStack.begin(),m_lstNodeStack.end(),pPair);
while (lif != m_lstNodeStack.end())
{
m_lstNodeStack.erase(lif);
lif = std::find(m_lstNodeStack.begin(),m_lstNodeStack.end(),pPair);
}
lif = std::find(m_lstNodeStack.begin(),m_lstNodeStack.end(),pCurrent);
while (lif != m_lstNodeStack.end())
{
m_lstNodeStack.erase(lif);
lif = std::find(m_lstNodeStack.begin(),m_lstNodeStack.end(),pCurrent);
}
ConnectNodes(lstNewNodes);
}
}
// find optimal rearrangement
// rearrange
}
ClearMarks();
}
void CGraph::DisableInSphere(const Vec3 &pos,float fRadius)
{
GetNodesInSphere(pos,fRadius);
ListNodes::iterator li = m_lstNodesInsideSphere.begin(),liend = m_lstNodesInsideSphere.end();
for (;li!=liend;++li)
{
GraphNode *pNode = (*li);
if (pNode->nBuildingID<0)
continue;
VectorOfLinks::iterator vli=pNode->link.begin(),vliend=pNode->link.end();
for (;vli!=vliend;++vli)
{
(*vli).fMaxRadius = -1; // forbid this link
GraphNode *pLink = (*vli).pLink;
VectorOfLinks::iterator bi=pLink->link.begin(),biend=pLink->link.end();
for (;bi!=biend;++bi)
{
if ((*bi).pLink == pNode)
{
(*bi).fMaxRadius = -1;
break;
}
}
}
}
}
void CGraph::EnableInSphere(const Vec3 &pos,float fRadius)
{
GetNodesInSphere(pos,fRadius);
ListNodes::iterator li = m_lstNodesInsideSphere.begin(),liend = m_lstNodesInsideSphere.end();
for (;li!=liend;++li)
{
GraphNode *pNode = (*li);
if (pNode->nBuildingID<0)
continue;
VectorOfLinks::iterator vli=pNode->link.begin(),vliend=pNode->link.end();
for (;vli!=vliend;++vli)
{
(*vli).fMaxRadius = 100; // allow this link
GraphNode *pLink = (*vli).pLink;
VectorOfLinks::iterator bi=pLink->link.begin(),biend=pLink->link.end();
for (;bi!=biend;++bi)
{
if ((*bi).pLink == pNode)
{
(*bi).fMaxRadius = 100;
break;
}
}
}
}
}
int CGraph::GetNodesInSphere(const Vec3 &pos, float fRadius)
{
ClearTagsNow();
m_lstNodesInsideSphere.clear();
GraphNode *pNode = GetEnclosing(pos);
if (pNode->nBuildingID<0)
return 0;
m_lstNodesInsideSphere.push_front(pNode);
while(m_lstNodesInsideSphere.front()->tag == false)
{
ListNodes::iterator li = m_lstNodesInsideSphere.begin(),liend = m_lstNodesInsideSphere.end();
for (;li!=liend;)
{
ListNodes::iterator linext = li;
++linext;
if (linext!=liend)
{
if ((*linext)->tag)
break;
}
++li;
}
if (li!=liend)
pNode = (*li);
TagNode(pNode);
VectorOfLinks::iterator vli=pNode->link.begin(),vliend=pNode->link.end();
for (;vli!=vliend;++vli)
{
GraphNode *pLink = (*vli).pLink;
if (pLink->tag)
continue;
if ( GetLength(pLink->data.m_pos-pos) < fRadius )
{
m_lstNodesInsideSphere.push_front(pLink);
}
}
}
ClearTagsNow();
return m_lstNodesInsideSphere.size();
}
Reference in New Issue View Git Blame Copy Permalink