mirror of
https://github.com/ptitSeb/Serious-Engine
synced 2024-11-22 18:30:27 +01:00
276 lines
7.9 KiB
C++
276 lines
7.9 KiB
C++
#include "../StdH/StdH.h"
|
|
#include "Entities/Common/PathFinding.h"
|
|
#include "Entities/NavigationMarker.h"
|
|
|
|
#define PRINTOUT(_dummy)
|
|
//#define PRINTOUT(something) something
|
|
|
|
// open and closed lists of nodes
|
|
static CListHead _lhOpen;
|
|
static CListHead _lhClosed;
|
|
|
|
FLOAT NodeDistance(CPathNode *ppn0, CPathNode *ppn1)
|
|
{
|
|
return (
|
|
ppn0->pn_pnmMarker->GetPlacement().pl_PositionVector -
|
|
ppn1->pn_pnmMarker->GetPlacement().pl_PositionVector).Length();
|
|
}
|
|
|
|
CPathNode::CPathNode(class CNavigationMarker *penMarker): pn_pnmMarker(penMarker), pn_ppnParent(NULL), pn_fG(0.0f), pn_fH(0.0f), pn_fF(0.0f){}
|
|
|
|
CPathNode::~CPathNode(void)
|
|
{
|
|
// detach from marker when deleting
|
|
ASSERT(pn_pnmMarker!=NULL);
|
|
pn_pnmMarker->m_ppnNode = NULL;
|
|
}
|
|
|
|
// get name of this node
|
|
const CTString &CPathNode::GetName(void)
|
|
{
|
|
ASSERT(this!=NULL);
|
|
static CTString strNone="<none>";
|
|
if (pn_pnmMarker==NULL) {
|
|
return strNone;
|
|
} else {
|
|
return pn_pnmMarker->GetName();
|
|
}
|
|
}
|
|
|
|
// get link with given index or null if no more (for iteration along the graph)
|
|
CPathNode *CPathNode::GetLink(INDEX i)
|
|
{
|
|
ASSERT(this!=NULL);
|
|
if (pn_pnmMarker==NULL) {
|
|
ASSERT(FALSE);
|
|
return NULL;
|
|
}
|
|
CNavigationMarker *pnm = pn_pnmMarker->GetLink(i);
|
|
if (pnm==NULL) {
|
|
return NULL;
|
|
}
|
|
return pnm->GetPathNode();
|
|
}
|
|
|
|
// add given node to open list, sorting best first
|
|
static void SortIntoOpenList(CPathNode *ppnLink)
|
|
{
|
|
// start at head of the open list
|
|
LISTITER(CPathNode, pn_lnInOpen) itpn(_lhOpen);
|
|
// while the given node is further than the one in list
|
|
while(ppnLink->pn_fF>itpn->pn_fF && !itpn.IsPastEnd()) {
|
|
// move to next node
|
|
itpn.MoveToNext();
|
|
}
|
|
|
|
// if past the end of list
|
|
if (itpn.IsPastEnd()) {
|
|
// add to the end of list
|
|
_lhOpen.AddTail(ppnLink->pn_lnInOpen);
|
|
// if not past end of list
|
|
} else {
|
|
// add before current node
|
|
itpn.InsertBeforeCurrent(ppnLink->pn_lnInOpen);
|
|
}
|
|
}
|
|
|
|
// find shortest path from one marker to another
|
|
static BOOL FindPath(CNavigationMarker *pnmSrc, CNavigationMarker *pnmDst)
|
|
{
|
|
|
|
ASSERT(pnmSrc!=pnmDst);
|
|
CPathNode *ppnSrc = pnmSrc->GetPathNode();
|
|
CPathNode *ppnDst = pnmDst->GetPathNode();
|
|
|
|
PRINTOUT(CPrintF("--------------------\n"));
|
|
PRINTOUT(CPrintF("FindPath(%s, %s)\n", ppnSrc->GetName(), ppnDst->GetName()));
|
|
|
|
// start with empty open and closed lists
|
|
ASSERT(_lhOpen.IsEmpty());
|
|
ASSERT(_lhClosed.IsEmpty());
|
|
|
|
// add the start node to open list
|
|
ppnSrc->pn_fG = 0.0f;
|
|
ppnSrc->pn_fH = NodeDistance(ppnSrc, ppnDst);
|
|
ppnSrc->pn_fF = ppnSrc->pn_fG +ppnSrc->pn_fH;
|
|
_lhOpen.AddTail(ppnSrc->pn_lnInOpen);
|
|
PRINTOUT(CPrintF("StartState: %s\n", ppnSrc->GetName()));
|
|
|
|
// while the open list is not empty
|
|
while (!_lhOpen.IsEmpty()) {
|
|
// get the first node from open list (that is, the one with lowest F)
|
|
CPathNode *ppnNode = LIST_HEAD(_lhOpen, CPathNode, pn_lnInOpen);
|
|
ppnNode->pn_lnInOpen.Remove();
|
|
_lhClosed.AddTail(ppnNode->pn_lnInClosed);
|
|
PRINTOUT(CPrintF("Node: %s - moved from OPEN to CLOSED\n", ppnNode->GetName()));
|
|
|
|
// if this is the goal
|
|
if (ppnNode==ppnDst) {
|
|
PRINTOUT(CPrintF("PATH FOUND!\n"));
|
|
// the path is found
|
|
return TRUE;
|
|
}
|
|
|
|
// for each link of current node
|
|
CPathNode *ppnLink = NULL;
|
|
for(INDEX i=0; (ppnLink=ppnNode->GetLink(i))!=NULL; i++) {
|
|
PRINTOUT(CPrintF(" Link %d: %s\n", i, ppnLink->GetName()));
|
|
// get cost to get to this node if coming from current node
|
|
FLOAT fNewG = ppnLink->pn_fG+NodeDistance(ppnNode, ppnLink);
|
|
// if a shorter path already exists
|
|
if ((ppnLink->pn_lnInOpen.IsLinked() || ppnLink->pn_lnInClosed.IsLinked()) && fNewG>=ppnLink->pn_fG) {
|
|
PRINTOUT(CPrintF(" shorter path exists through: %s\n", ppnLink->pn_ppnParent->GetName()));
|
|
// skip this link
|
|
continue;
|
|
}
|
|
// remember this path
|
|
ppnLink->pn_ppnParent = ppnNode;
|
|
ppnLink->pn_fG = fNewG;
|
|
ppnLink->pn_fH = NodeDistance(ppnLink, ppnDst);
|
|
ppnLink->pn_fF = ppnLink->pn_fG + ppnLink->pn_fH;
|
|
// remove from closed list, if in it
|
|
if (ppnLink->pn_lnInClosed.IsLinked()) {
|
|
ppnLink->pn_lnInClosed.Remove();
|
|
PRINTOUT(CPrintF(" %s removed from CLOSED\n", ppnLink->GetName()));
|
|
}
|
|
// add to open if not in it
|
|
if (!ppnLink->pn_lnInOpen.IsLinked()) {
|
|
SortIntoOpenList(ppnLink);
|
|
PRINTOUT(CPrintF(" %s added to OPEN\n", ppnLink->GetName()));
|
|
}
|
|
}
|
|
}
|
|
|
|
// if we get here, there is no path
|
|
PRINTOUT(CPrintF("PATH NOT FOUND!\n"));
|
|
return FALSE;
|
|
}
|
|
|
|
// clear all temporary structures used for path finding
|
|
static void ClearPath(CEntity *penThis)
|
|
{
|
|
{FORDELETELIST(CPathNode, pn_lnInOpen, _lhOpen, itpn) {
|
|
delete &itpn.Current();
|
|
}}
|
|
{FORDELETELIST(CPathNode, pn_lnInClosed, _lhClosed, itpn) {
|
|
delete &itpn.Current();
|
|
}}
|
|
|
|
#ifndef NDEBUG
|
|
// for each navigation marker in the world
|
|
{FOREACHINDYNAMICCONTAINER(penThis->en_pwoWorld->wo_cenEntities, CEntity, iten) {
|
|
if (!IsOfClass(iten, "NavigationMarker")) {
|
|
continue;
|
|
}
|
|
CNavigationMarker &nm = (CNavigationMarker&)*iten;
|
|
ASSERT(nm.m_ppnNode==NULL);
|
|
}}
|
|
#endif
|
|
}
|
|
|
|
// find marker closest to a given position
|
|
static void FindClosestMarker(
|
|
CEntity *penThis, const FLOAT3D &vSrc, CEntity *&penMarker, FLOAT3D &vPath)
|
|
{
|
|
CNavigationMarker *pnmMin = NULL;
|
|
FLOAT fMinDist = UpperLimit(0.0f);
|
|
// for each sector this entity is in
|
|
{FOREACHSRCOFDST(penThis->en_rdSectors, CBrushSector, bsc_rsEntities, pbsc)
|
|
// for each navigation marker in that sector
|
|
{FOREACHDSTOFSRC(pbsc->bsc_rsEntities, CEntity, en_rdSectors, pen)
|
|
if (!IsOfClass(pen, "NavigationMarker")) {
|
|
continue;
|
|
}
|
|
CNavigationMarker &nm = (CNavigationMarker&)*pen;
|
|
|
|
// get distance from source
|
|
FLOAT fDist = (vSrc-nm.GetPlacement().pl_PositionVector).Length();
|
|
// if closer than best found
|
|
if(fDist<fMinDist) {
|
|
// remember it
|
|
fMinDist = fDist;
|
|
pnmMin = &nm;
|
|
}
|
|
ENDFOR}
|
|
ENDFOR}
|
|
|
|
// if none found
|
|
if (pnmMin==NULL) {
|
|
// fail
|
|
vPath = vSrc;
|
|
penMarker = NULL;
|
|
return;
|
|
}
|
|
|
|
// return position
|
|
vPath = pnmMin->GetPlacement().pl_PositionVector;
|
|
penMarker = pnmMin;
|
|
}
|
|
|
|
// find first marker for path navigation
|
|
void PATH_FindFirstMarker(CEntity *penThis, const FLOAT3D &vSrc, const FLOAT3D &vDst, CEntity *&penMarker, FLOAT3D &vPath)
|
|
{
|
|
// find closest markers to source and destination positions
|
|
CNavigationMarker *pnmSrc;
|
|
FLOAT3D vSrcPath;
|
|
FindClosestMarker(penThis, vSrc, (CEntity*&)pnmSrc, vSrcPath);
|
|
CNavigationMarker *pnmDst;
|
|
FLOAT3D vDstPath;
|
|
FindClosestMarker(penThis, vDst, (CEntity*&)pnmDst, vDstPath);
|
|
|
|
// if at least one is not found, or if they are same
|
|
if (pnmSrc==NULL || pnmDst==NULL || pnmSrc==pnmDst) {
|
|
// fail
|
|
penMarker = NULL;
|
|
vPath = vSrc;
|
|
return;
|
|
}
|
|
|
|
// go to the source marker position
|
|
vPath = vSrcPath;
|
|
penMarker = pnmSrc;
|
|
}
|
|
|
|
// find next marker for path navigation
|
|
void PATH_FindNextMarker(CEntity *penThis, const FLOAT3D &vSrc, const FLOAT3D &vDst, CEntity *&penMarker, FLOAT3D &vPath)
|
|
{
|
|
// find closest marker to destination position
|
|
CNavigationMarker *pnmDst;
|
|
FLOAT3D vDstPath;
|
|
FindClosestMarker(penThis, vDst, (CEntity*&)pnmDst, vDstPath);
|
|
|
|
// if at not found, or if same as current
|
|
if (pnmDst==NULL || penMarker==pnmDst) {
|
|
// fail
|
|
penMarker = NULL;
|
|
vPath = vSrc;
|
|
return;
|
|
}
|
|
|
|
// try to find shortest path to the destination
|
|
BOOL bFound = FindPath((CNavigationMarker*)penMarker, pnmDst);
|
|
|
|
// if not found
|
|
if (!bFound) {
|
|
// just clean up and fail
|
|
delete pnmDst->GetPathNode();
|
|
ClearPath(penThis);
|
|
penMarker = NULL;
|
|
vPath = vSrc;
|
|
return;
|
|
}
|
|
|
|
// find the first marker position after current
|
|
CPathNode *ppn = pnmDst->GetPathNode();
|
|
while (ppn->pn_ppnParent!=NULL && ppn->pn_ppnParent->pn_pnmMarker!=penMarker) {
|
|
ppn = ppn->pn_ppnParent;
|
|
}
|
|
penMarker = ppn->pn_pnmMarker;
|
|
|
|
// go there
|
|
vPath = penMarker->GetPlacement().pl_PositionVector;
|
|
|
|
// clean up
|
|
ClearPath(penThis);
|
|
}
|