BOOL CSTRPath::FindPath(CObject* pObject,VECTOR3 * pSrcPos, VECTOR3 * pDestPos, VECTOR3 * pWayPointBuf, WORD buffCount, BYTE& wWayPointNum, WORD wDepth)
{
m_wCurDepth = 0;
m_wLimiteDepth = wDepth;
PreInit(NULL, (int)(pSrcPos->x/50), (int)(pSrcPos->z/50), (int)(pDestPos->x/50), (int)(pDestPos->z/50));
while(GetBestNode())
{
if(m_pCurBestNode->cellNumber == m_TargetCellNumber)
break;
if(m_wLimiteDepth <= m_wCurDepth)
break;
Traversal(pObject,m_pCurBestNode);
}
if(!m_pCurBestNode)
return FALSE;
CalcWayPoint(pWayPointBuf, buffCount, wWayPointNum);
return TRUE;
}
void cAStarHandler::IterateAlgorithm()
{
int lIterationCount=0;
while(m_setOpenList.empty()==false && (mlMaxIterations <0 || lIterationCount < mlMaxIterations))
{
cAStarNode *pNode = GetBestNode();
cAINode *pAINode = pNode->mpAINode;
//////////////////////
// Check if current node can reach goal
if(IsGoalNode(pAINode))
{
mpGoalNode = pNode;
break;
}
/////////////////////
//Add nodes connected to current
int lEdgeCount = pAINode->GetEdgeNum();
for(int i=0; i< lEdgeCount; ++i)
{
cAINodeEdge *pEdge = pAINode->GetEdge(i);
if(mpCallback == NULL || mpCallback->CanAddNode(pAINode, pEdge->mpNode))
{
AddOpenNode(pEdge->mpNode, pNode, pNode->mfDistance + pEdge->mfDistance);
//AddOpenNode(pEdge->mpNode, pNode, pNode->mfDistance + pEdge->mfSqrDistance);
}
}
++lIterationCount;
}
}
//updated
int GetBestTrailMassByView (edict_t *ent)
{
edict_t *node = NULL;
node = GetBestNode(ent);
if (node)
return node->mass;
else
return -1;
}
/* --| ASTAR FUNCTION |-----------------------------------------------------------------------*\
| Name: FindPath
| Desc: the main function of AStar,this one finds the path,or not,and stores it in the PATH
\* --| ASTAR FUNCTION |-----------------------------------------------------------------------*/
bool AStar::FindPath(POINT StartPos,POINT EndPos)
{
// -- Memory allocation -------------------
if (nPath>0 && nPath<300) free(PATH);
OPEN =(NODE*) malloc(1000*sizeof(NODE));
CLOSED=(NODE*) malloc(1000*sizeof(NODE));
nOpen =1000;
nClosed=1000;
cOpen =1;
cClosed=0;
cID=0;
// -- Search initialization ---------------
NODE n;
n.f=n.Parent=n.ID=-1;n.x=StartPos.x;n.y=StartPos.y;
nStart=n;
n.f=n.Parent=n.ID=0;n.x= EndPos.x;n.y= EndPos.y;
nGoal=n;
OPEN[0]=nStart;
// -- Pathfinding -------------------------
while (cOpen) // Do while OPEN is not empty,if OPEN is empty,it means that the path was not found
{
n=GetBestNode();
if (n.x==nGoal.x && n.y==nGoal.y)
break;
GenerateSuccs(n);
}
// -- Memory deallocation -----------------
if (cOpen !=0) ConstructPath(n);
free(OPEN);
free(CLOSED);
if (cOpen==0)
return false;
else
return true;
}
int CLocalNav::FindPath(Vector &vecStart, Vector &vecDest, float flTargetRadius, BOOL fNoMonsters)
{
int nIndexBest;
node_index_t *node;
Vector vecNodeLoc;
float flDistToDest;
#ifdef _DEBUG
CONSOLE_ECHO("findpath: %f\n", gpGlobals->time);
#endif
nIndexBest = FindDirectPath(vecStart, vecDest, flTargetRadius, fNoMonsters);
if (nIndexBest != -1)
return nIndexBest;
m_vecStartingLoc = vecStart;
m_nindexAvailableNode = 0;
AddPathNodes(-1, fNoMonsters);
vecNodeLoc = vecStart;
nIndexBest = GetBestNode(vecNodeLoc, vecDest);
while (nIndexBest != -1)
{
node = GetNode(nIndexBest);
vecNodeLoc = node->vecLoc;
node->fSearched = TRUE;
flDistToDest = (vecDest - node->vecLoc).Length2D();
if (flDistToDest <= flTargetRadius)
break;
if (flDistToDest <= HOSTAGE_STEPSIZE)
break;
if ((flDistToDest - flTargetRadius) > (MAX_NODES - m_nindexAvailableNode) * HOSTAGE_STEPSIZE || m_nindexAvailableNode == MAX_NODES)
{
nIndexBest = -1;
break;
}
AddPathNodes(nIndexBest, fNoMonsters);
nIndexBest = GetBestNode(vecNodeLoc, vecDest);
}
if (m_nindexAvailableNode <= 10)
nodeval += 2;
else if (m_nindexAvailableNode <= 20)
nodeval += 4;
else if (m_nindexAvailableNode <= 30)
nodeval += 8;
else if (m_nindexAvailableNode <= 40)
nodeval += 13;
else if (m_nindexAvailableNode <= 50)
nodeval += 19;
else if (m_nindexAvailableNode <= 60)
nodeval += 26;
else if (m_nindexAvailableNode <= 70)
nodeval += 34;
else if (m_nindexAvailableNode <= 80)
nodeval += 43;
else if (m_nindexAvailableNode <= 90)
nodeval += 53;
else if (m_nindexAvailableNode <= 100)
nodeval += 64;
else if (m_nindexAvailableNode <= 110)
nodeval += 76;
else if (m_nindexAvailableNode <= 120)
nodeval += 89;
else if (m_nindexAvailableNode <= 130)
nodeval += 103;
else if (m_nindexAvailableNode <= 140)
nodeval += 118;
else if (m_nindexAvailableNode <= 150)
nodeval += 134;
else if (m_nindexAvailableNode <= 160)
nodeval += 151;
else
nodeval += 169;
return nIndexBest;
}
node_index_t CLocalNav::FindPath(Vector &vecStart, Vector &vecDest, float flTargetRadius, int fNoMonsters)
{
node_index_t nIndexBest = FindDirectPath(vecStart, vecDest, flTargetRadius, fNoMonsters);
if (nIndexBest != NODE_INVALID_EMPTY)
{
return nIndexBest;
}
localnode_t *node;
Vector vecNodeLoc;
float_precision flDistToDest;
m_vecStartingLoc = vecStart;
m_nindexAvailableNode = 0;
AddPathNodes(NODE_INVALID_EMPTY, fNoMonsters);
nIndexBest = GetBestNode(vecStart, vecDest);
while (nIndexBest != NODE_INVALID_EMPTY)
{
node = GetNode(nIndexBest);
node->fSearched = TRUE;
vecNodeLoc = node->vecLoc;
flDistToDest = (vecDest - node->vecLoc).Length2D();
if (flDistToDest <= flTargetRadius)
break;
if (flDistToDest <= HOSTAGE_STEPSIZE)
break;
if (((flDistToDest - flTargetRadius) > ((MAX_NODES - m_nindexAvailableNode) * HOSTAGE_STEPSIZE))
|| m_nindexAvailableNode == MAX_NODES)
{
nIndexBest = NODE_INVALID_EMPTY;
break;
}
AddPathNodes(nIndexBest, fNoMonsters);
nIndexBest = GetBestNode(vecNodeLoc, vecDest);
}
if (m_nindexAvailableNode <= 10)
nodeval += 2;
else if (m_nindexAvailableNode <= 20)
nodeval += 4;
else if (m_nindexAvailableNode <= 30)
nodeval += 8;
else if (m_nindexAvailableNode <= 40)
nodeval += 13;
else if (m_nindexAvailableNode <= 50)
nodeval += 19;
else if (m_nindexAvailableNode <= 60)
nodeval += 26;
else if (m_nindexAvailableNode <= 70)
nodeval += 34;
else if (m_nindexAvailableNode <= 80)
nodeval += 43;
else if (m_nindexAvailableNode <= 90)
nodeval += 53;
else if (m_nindexAvailableNode <= 100)
nodeval += 64;
else if (m_nindexAvailableNode <= 110)
nodeval += 76;
else if (m_nindexAvailableNode <= 120)
nodeval += 89;
else if (m_nindexAvailableNode <= 130)
nodeval += 103;
else if (m_nindexAvailableNode <= 140)
nodeval += 118;
else if (m_nindexAvailableNode <= 150)
nodeval += 134;
else if (m_nindexAvailableNode <= 160)
nodeval += 151;
else
nodeval += 169;
return nIndexBest;
}
UBool PathFinder::Flee(const ULong & flags, const ULong & f, const EERIE_3D & danger, const Float & safe_dist, SLong * rstep, UWord ** rlist)
{
MINOSNode * node, *child;
long _from;
//Init open and close lists
Clean();
if (!rlist || !rstep)
return UFALSE;
if (Distance(map_d[f].pos, danger) >= safe_dist)
{
*rlist = (UWord *)malloc(sizeof(UWord));
** rlist = (UWord)f;
*rstep = 1;
return UTRUE;
}
_from = f;
//Create start node and put it on open list
if (!(node = CreateNode(_from, NULL)))
{
Clean();
*rstep = 0;
return UFALSE;
}
node->g_cost = 0;
if (flags & MINOS_STEALTH)
AddEnlightmentCost(node);
node->f_cost = safe_dist - Distance(map_d[_from].pos, danger);
if (node->f_cost < 0.0F)
node->f_cost = 0.0F;
node->f_cost += node->g_cost;
if (open.Append(node))
{
free(node);
Clean();
*rstep = 0;
return UFALSE;
}
//A* main loop
while (node = GetBestNode())
{
//If it's the goal node then we've done
if (Distance(map_d[node->data].pos, danger) >= safe_dist)
{
if (close.Append(node))
{
free(node);
*rstep = 0;
return UFALSE;
}
//BuildPath(rlist, rstep);
if (BuildPath(rlist, rstep))
{
Clean();
*rstep = 0;
return UFALSE;
}
Clean();
return UTRUE;
}
//Otherwise, generate child from current node
long _pipo = node->data;
for (SWord i(0); i < map_d[_pipo].nblinked; i++)
{
child = CreateNode(map_d[_pipo].linked[i], node);
if (!child)
{
free(node);
Clean();
*rstep = 0;
return UFALSE;
}
//Cost to reach this node
if ((map_d[child->data].flags & ANCHOR_FLAG_BLOCKED) || map_d[child->data].height > height || map_d[child->data].radius < radius)
free(child);
else
{
child->g_cost = node->g_cost + Distance(map_d[child->data].pos, map_d[node->data].pos);
if (flags & MINOS_STEALTH)
AddEnlightmentCost(child);
if (Check(child))
{
Float dist;
if (open.Append(child))
{
free(node);
free(child);
*rstep = 0;
return UFALSE;
}
//Get total cost for this node
child->f_cost = child->g_cost;
dist = Distance(map_d[child->data].pos, danger);
if ((dist = safe_dist - dist) > 0.0F)
child->f_cost += fac5 * dist;
}
else free(child);
}
}
//Put node onto close list as we have now examined this node
if (close.Append(node))
{
free(node);
Clean();
*rstep = 0;
return UFALSE;
}
}
Clean();
*rstep = 0;
//No path found!!!
return UFALSE;
}
///////////////////////////////////////////////////////////////////////////////
// //
// Methods //
// //
///////////////////////////////////////////////////////////////////////////////
UBool PathFinder::Move(const ULong & flags, const ULong & f, const ULong & t, SLong * rstep, UWord ** rlist)
{
MINOSNode * node, *child;
long _from, _to;
//Init open and close lists
Clean();
if (!rlist || !rstep) return UFALSE;
if (f == t)
{
*rlist = (UWord *)malloc(sizeof(UWord));
** rlist = (UWord)t;
*rstep = 1;
return UTRUE;
}
_from = f, _to = t;
//Create start node and put it on open list
if (!(node = CreateNode(_from, NULL)))
{
Clean(); // Cyril
*rstep = 0;
return UFALSE;
}
node->g_cost = 0;
node->f_cost = Distance(map_d[_from].pos, map_d[_to].pos);
if (flags & MINOS_STEALTH) AddEnlightmentCost(node);
if (open.Append(node))
{
free(node);
Clean(); // Cyril
*rstep = 0;
return UFALSE;
}
//A* main loop
while (node = GetBestNode())
{
//If it's the goal node then we've done
if (node->data == _to)
{
if (close.Append(node))
{
free(node);
Clean();
*rstep = 0;
return UFALSE;
}
if (BuildPath(rlist, rstep))
{
Clean();
*rstep = 0;
return UFALSE;
}
Clean(); // Cyril
return UTRUE;
}
//Otherwise, generate child from current node
long _pipo(node->data);
for (SWord i(0); i < map_d[_pipo].nblinked; i++)
{
//Create new child
child = CreateNode(map_d[_pipo].linked[i], node);
if (!child)
{
free(node);
Clean(); // Cyril
*rstep = 0;
return UFALSE;
}
//Cost to reach this node
if ((map_d[child->data].flags & ANCHOR_FLAG_BLOCKED) || map_d[child->data].height > height || map_d[child->data].radius < radius)
free(child);
else
{
child->g_cost = node->g_cost + Distance(map_d[child->data].pos, map_d[node->data].pos);
if (flags & MINOS_STEALTH) AddEnlightmentCost(child);
if (Check(child))
{
if (open.Append(child))
{
free(node);
free(child);
*rstep = 0;
return UFALSE;
}
//Get total cost for this node
child->f_cost = heuristic * child->g_cost + (1.0F - heuristic) * Distance(map_d[child->data].pos, map_d[_to].pos);
}
else free(child);
}
}
//Put node onto close list as we have now examined this node
if (close.Append(node))
{
free(node);
Clean(); // Cyril
*rstep = 0;
return UFALSE;
}
}
//No path found!!!
Clean();
*rstep = 0;
return UFALSE;
}
