Serious-Engine/Sources/Entities/Common/PathFinding.cpp
2021-12-27 11:22:24 +02:00

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);
}