thirdpartymirrors/RingRacers
1
0
Fork 0
mirror of https://github.com/KartKrewDev/RingRacers.git synced 2025-10-30 08:01:28 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

Split pathfinding itself into its own module.

Browse source
This commit is contained in:
Sryder 2019-06-12 23:43:55 +01:00
parent 4c26589d5c
commit e937e35a78
5 changed files with 739 additions and 431 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
src/Makefile
View file

@ -490,6 +490,7 @@ OBJS:=$(i_main_o) \
$(OBJDIR)/st_stuff.o \
$(OBJDIR)/k_kart.o \
$(OBJDIR)/k_waypoint.o\
$(OBJDIR)/k_pathfind.o\
$(OBJDIR)/k_bheap.o \
$(OBJDIR)/m_aatree.o \
$(OBJDIR)/m_anigif.o \

521
src/k_pathfind.c Normal file
View file

@ -0,0 +1,521 @@
#include "k_pathfind.h"
#include "doomdef.h"
#include "z_zone.h"
#include "k_bheap.h"
static const size_t DEFAULT_NODEARRAY_CAPACITY = 8U;
static const size_t DEFAULT_OPENSET_CAPACITY = 8U;
static const size_t DEFAULT_CLOSEDSET_CAPACITY = 8U;
/*--------------------------------------------------
static UINT32 K_NodeGetFScore(const pathfindnode_t *const node)
Gets the FScore of a node. The FScore is the GScore plus the HScore.
Input Arguments:-
node - The node to get the FScore of
Return:-
The FScore of the node.
--------------------------------------------------*/
static UINT32 K_NodeGetFScore(const pathfindnode_t *const node)
{
UINT32 fscore = UINT32_MAX;
if (node == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL node in K_PathfindNodeGetFScore.");
}
else
{
fscore = node->gscore + node->hscore;
}
return fscore;
}
/*--------------------------------------------------
static void K_NodeUpdateHeapIndex(void *const node, const size_t newheapindex)
A callback for the Openset Binary Heap to be able to update the heapindex of the pathfindnodes when they are
moved.
Input Arguments:-
node - The node that has been updated, should be a pointer to a pathfindnode_t
newheapindex - The new heapindex of the node.
Return:-
None
--------------------------------------------------*/
static void K_NodeUpdateHeapIndex(void *const node, const size_t newheapindex)
{
if (node == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL node in K_PathfindNodeUpdateHeapIndex.\n");
}
else
{
pathfindnode_t *truenode = (pathfindnode_t*)node;
truenode->heapindex = newheapindex;
}
}
/*--------------------------------------------------
static pathfindnode_t *K_NodesArrayContainsNodeData(
pathfindnode_t *nodesarray,
void* nodedata,
size_t nodesarraycount)
Checks whether the Nodes Array contains a node with a waypoint. Searches from the end to the start for speed
reasons.
Input Arguments:-
nodesarray - The nodes array within the A* algorithm
waypoint - The waypoint to check is within the nodes array
nodesarraycount - The current size of the nodes array
Return:-
The pathfind node that has the waypoint if there is one. NULL if the waypoint is not in the nodes array.
--------------------------------------------------*/
static pathfindnode_t *K_NodesArrayContainsNodeData(
pathfindnode_t *nodesarray,
void* nodedata,
size_t nodesarraycount)
{
pathfindnode_t *foundnode = NULL;
if (nodesarray == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL nodesarray in K_NodesArrayContainsWaypoint.\n");
}
else if (nodedata == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL nodedata in K_NodesArrayContainsWaypoint.\n");
}
else
{
size_t i;
// It is more likely that we'll find the node we are looking for from the end of the array
// Yes, the for loop looks weird, remember that size_t is unsigned and we want to check 0, after it hits 0 it
// will loop back up to SIZE_MAX
for (i = nodesarraycount - 1U; i < nodesarraycount; i--)
{
if (nodesarray[i].nodedata == nodedata)
{
foundnode = &nodesarray[i];
break;
}
}
}
return foundnode;
}
/*--------------------------------------------------
static boolean K_ClosedsetContainsNode(pathfindnode_t **closedset, pathfindnode_t *node, size_t closedsetcount)
Checks whether the Closedset contains a node. Searches from the end to the start for speed reasons.
Input Arguments:-
closedset - The closed set within the A* algorithm
node - The node to check is within the closed set
closedsetcount - The current size of the closedset
Return:-
True if the node is in the closed set, false if it isn't
--------------------------------------------------*/
static boolean K_ClosedsetContainsNode(pathfindnode_t **closedset, pathfindnode_t *node, size_t closedsetcount)
{
boolean nodeisinclosedset = false;
if (closedset == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL closedset in K_PathfindClosedsetContainsNode.\n");
}
else if (node == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL node in K_PathfindClosedsetContainsNode.\n");
}
else
{
size_t i;
// It is more likely that we'll find the node we are looking for from the end of the array
// Yes, the for loop looks weird, remember that size_t is unsigned and we want to check 0, after it hits 0 it
// will loop back up to SIZE_MAX
for (i = closedsetcount - 1U; i < closedsetcount; i--)
{
if (closedset[i] == node)
{
nodeisinclosedset = true;
break;
}
}
}
return nodeisinclosedset;
}
/*--------------------------------------------------
static boolean K_PathfindSetupValid(const pathfindsetup_t *const pathfindsetup)
Checks that the setup given for pathfinding is valid and can be used.
Input Arguments:-
pathfindsetup - The setup for the pathfinding given
Return:-
True if pathfinding setup is valid, false if it isn't.
--------------------------------------------------*/
static boolean K_PathfindSetupValid(const pathfindsetup_t *const pathfindsetup)
{
boolean pathfindsetupvalid = false;
size_t sourcenodenumconnectednodes = 0U;
size_t endnodenumconnectednodes = 0U;
if (pathfindsetup == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL pathfindsetup in K_PathfindSetupValid.\n");
}
else if (pathfindsetup->startnodedata == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "Pathfindsetup has NULL startnodedata.\n");
}
else if (pathfindsetup->endnodedata == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "Pathfindsetup has NULL endnodedata.\n");
}
else if (pathfindsetup->getconnectednodes == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "Pathfindsetup has NULL getconnectednodes function.\n");
}
else if (pathfindsetup->getconnectioncosts == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "Pathfindsetup has NULL getconnectioncosts function.\n");
}
else if (pathfindsetup->getheuristic == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "Pathfindsetup has NULL getheuristic function.\n");
}
else if (pathfindsetup->gettraversable == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "Pathfindsetup has NULL gettraversable function.\n");
}
else if (pathfindsetup->getconnectednodes(pathfindsetup->startnodedata, &sourcenodenumconnectednodes) == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "K_PathfindSetupValid: Source node returned NULL connecting nodes.\n");
}
else if (sourcenodenumconnectednodes == 0U)
{
CONS_Debug(DBG_GAMELOGIC, "K_PathfindSetupValid: Source node has 0 connecting nodes.\n");
}
else if (pathfindsetup->getconnectednodes(pathfindsetup->endnodedata, &endnodenumconnectednodes) == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "K_PathfindSetupValid: End node returned NULL connecting nodes.\n");
}
else if (endnodenumconnectednodes == 0U)
{
CONS_Debug(DBG_GAMELOGIC, "K_PathfindSetupValid: End node has 0 connecting nodes.\n");
}
else
{
pathfindsetupvalid = true;
}
return pathfindsetupvalid;
}
static boolean K_ReconstructPath(path_t *const path, pathfindnode_t *const destinationnode)
{
boolean reconstructsuccess = false;
if (path == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL path in K_ReconstructPath.\n");
}
else if (destinationnode == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL destinationnode in K_ReconstructPath.\n");
}
else
{
size_t numnodes = 0U;
pathfindnode_t *thisnode = destinationnode;
// If the path we're placing our new path into already has data, free it
if (path->array != NULL)
{
Z_Free(path->array);
path->numnodes = 0U;
path->totaldist = 0U;
}
// Do a fast check of how many nodes there are so we know how much space to allocate
for (thisnode = destinationnode; thisnode; thisnode = thisnode->camefrom)
{
numnodes++;
}
if (numnodes > 0U)
{
// Allocate memory for the path
path->numnodes = numnodes;
path->array = Z_Calloc(numnodes * sizeof(pathfindnode_t), PU_STATIC, NULL);
path->totaldist = destinationnode->gscore;
if (path->array == NULL)
{
I_Error("K_ReconstructPath: Out of memory.");
}
// Put the nodes into the return array
for (thisnode = destinationnode; thisnode; thisnode = thisnode->camefrom)
{
path->array[numnodes - 1U] = *thisnode;
// Correct the camefrom element to point to the previous element in the array instead
if ((path->array[numnodes - 1U].camefrom != NULL) && (numnodes > 1U))
{
path->array[numnodes - 1U].camefrom = &path->array[numnodes - 2U];
}
else
{
path->array[numnodes - 1U].camefrom = NULL;
}
numnodes--;
}
reconstructsuccess = true;
}
}
return reconstructsuccess;
}
/*--------------------------------------------------
boolean K_PathfindAStar(path_t *const path, pathfindsetup_t *const pathfindsetup)
See header file for description.
--------------------------------------------------*/
boolean K_PathfindAStar(path_t *const path, pathfindsetup_t *const pathfindsetup)
{
boolean pathfindsuccess = false;
if (path == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL path in K_PathfindAStar.\n");
}
else if (pathfindsetup == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL pathfindsetup in K_PathfindAStar.\n");
}
else if (!K_PathfindSetupValid(pathfindsetup))
{
CONS_Debug(DBG_GAMELOGIC, "K_PathfindAStar: Pathfinding setup is not valid.\n");
}
else if (pathfindsetup->startnodedata == pathfindsetup->endnodedata)
{
// At the destination, return a simple 1 node path
pathfindnode_t singlenode = {};
singlenode.camefrom = NULL;
singlenode.nodedata = pathfindsetup->endnodedata;
singlenode.heapindex = SIZE_MAX;
singlenode.hscore = 0U;
singlenode.gscore = 0U;
K_ReconstructPath(path, &singlenode);
pathfindsuccess = true;
}
else
{
bheap_t openset = {};
bheapitem_t poppedbheapitem = {};
pathfindnode_t *nodesarray = NULL;
pathfindnode_t **closedset = NULL;
pathfindnode_t *newnode = NULL;
pathfindnode_t *currentnode = NULL;
pathfindnode_t *connectingnode = NULL;
void **connectingnodesdata = NULL;
void *checknodedata = NULL;
UINT32 *connectingnodecosts = NULL;
size_t numconnectingnodes = 0U;
size_t connectingnodeheapindex = 0U;
size_t nodesarraycount = 0U;
size_t closedsetcount = 0U;
size_t i = 0U;
UINT32 tentativegscore = 0U;
// Set the dynamic structure capacites to defaults if they are 0
if (pathfindsetup->nodesarraycapacity == 0U)
{
pathfindsetup->nodesarraycapacity = DEFAULT_NODEARRAY_CAPACITY;
}
if (pathfindsetup->opensetcapacity == 0U)
{
pathfindsetup->opensetcapacity = DEFAULT_OPENSET_CAPACITY;
}
if (pathfindsetup->closedsetcapacity == 0U)
{
pathfindsetup->closedsetcapacity = DEFAULT_CLOSEDSET_CAPACITY;
}
// Allocate the necessary memory
nodesarray = Z_Calloc(pathfindsetup->nodesarraycapacity * sizeof(pathfindnode_t), PU_STATIC, NULL);
if (nodesarray == NULL)
{
I_Error("K_PathfindAStar: Out of memory allocating nodes array.");
}
closedset = Z_Calloc(pathfindsetup->closedsetcapacity * sizeof(pathfindnode_t*), PU_STATIC, NULL);
if (closedset == NULL)
{
I_Error("K_PathfindAStar: Out of memory allocating closed set.");
}
K_BHeapInit(&openset, pathfindsetup->opensetcapacity);
// Create the first node and add it to the open set
newnode = &nodesarray[nodesarraycount];
newnode->heapindex = SIZE_MAX;
newnode->nodedata = pathfindsetup->startnodedata;
newnode->camefrom = NULL;
newnode->gscore = 0U;
newnode->hscore = pathfindsetup->getheuristic(newnode->nodedata, pathfindsetup->endnodedata);
nodesarraycount++;
K_BHeapPush(&openset, newnode, K_NodeGetFScore(newnode), K_NodeUpdateHeapIndex);
// update openset capacity if it changed
if (openset.capacity != pathfindsetup->opensetcapacity)
{
pathfindsetup->opensetcapacity = openset.capacity;
}
// Go through each node in the openset, adding new ones from each node to it
// this continues until a path is found or there are no more nodes to check
while (openset.count > 0U)
{
// pop the best node off of the openset
K_BHeapPop(&openset, &poppedbheapitem);
currentnode = (pathfindnode_t*)poppedbheapitem.data;
if (currentnode->nodedata == pathfindsetup->endnodedata)
{
pathfindsuccess = K_ReconstructPath(path, currentnode);
break;
}
// Place the node we just popped into the closed set, as we are now evaluating it
if (closedsetcount >= pathfindsetup->closedsetcapacity)
{
// Need to reallocate closedset to fit another node
pathfindsetup->closedsetcapacity = pathfindsetup->closedsetcapacity * 2;
closedset =
Z_Realloc(closedset, pathfindsetup->closedsetcapacity * sizeof(pathfindnode_t*), PU_STATIC, NULL);
if (closedset == NULL)
{
I_Error("K_PathfindAStar: Out of memory reallocating closed set.");
}
}
closedset[closedsetcount] = currentnode;
closedsetcount++;
// Get the needed data for the next nodes from the current node
connectingnodesdata = pathfindsetup->getconnectednodes(currentnode->nodedata, &numconnectingnodes);
connectingnodecosts = pathfindsetup->getconnectioncosts(currentnode->nodedata);
if (connectingnodesdata == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "K_PathfindAStar: A Node returned NULL connecting node data.\n");
}
else if (connectingnodecosts == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "K_PathfindAStar: A Node returned NULL connecting node costs.\n");
}
else
{
// For each connecting node add it to the openset if it's unevaluated and not there,
// skip it if it's in the closedset or not traversable
for (i = 0; i < numconnectingnodes; i++)
{
checknodedata = connectingnodesdata[i];
if (checknodedata == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "K_PathfindAStar: A Node has a NULL connecting node.\n");
}
else
{
// skip this node if it isn't traversable
if (pathfindsetup->gettraversable(checknodedata) == false)
{
continue;
}
// Figure out what the gscore of this route for the connecting node is
tentativegscore = currentnode->gscore + connectingnodecosts[i];
// find this data in the nodes array if it's been generated before
connectingnode = K_NodesArrayContainsNodeData(nodesarray, checknodedata, nodesarraycount);
if (connectingnode != NULL)
{
// The connecting node has been seen before, so it must be in either the closedset (skip it)
// or the openset (re-evaluate it's gscore)
if (K_ClosedsetContainsNode(closedset, connectingnode, closedsetcount) == true)
{
continue;
}
else if (tentativegscore < connectingnode->gscore)
{
// The node is not in the closedset, update it's gscore if this path to it is faster
connectingnode->gscore = tentativegscore;
connectingnode->camefrom = currentnode;
connectingnodeheapindex =
K_BHeapContains(&openset, connectingnode, connectingnode->heapindex);
if (connectingnodeheapindex != SIZE_MAX)
{
K_UpdateBHeapItemValue(
&openset.array[connectingnodeheapindex], K_NodeGetFScore(connectingnode));
}
else
{
// SOMEHOW the node is not in either the closed set OR the open set
CONS_Debug(DBG_GAMELOGIC, "K_PathfindAStar: A Node is not in either set.\n");
}
}
}
else
{
// Node is not created yet, so it hasn't been seen so far
// Reallocate nodesarray if it's full
if (nodesarraycount >= pathfindsetup->nodesarraycapacity)
{
pathfindsetup->nodesarraycapacity = pathfindsetup->nodesarraycapacity * 2;
nodesarray = Z_Realloc(nodesarray, pathfindsetup->nodesarraycapacity, PU_STATIC, NULL);
if (nodesarray == NULL)
{
I_Error("K_PathfindAStar: Out of memory reallocating nodes array.");
}
}
// Create the new node and add it to the nodes array and open set
newnode = &nodesarray[nodesarraycount];
newnode->heapindex = SIZE_MAX;
newnode->nodedata = checknodedata;
newnode->camefrom = currentnode;
newnode->gscore = tentativegscore;
newnode->hscore = pathfindsetup->getheuristic(newnode->nodedata, pathfindsetup->endnodedata);
nodesarraycount++;
K_BHeapPush(&openset, newnode, K_NodeGetFScore(newnode), K_NodeUpdateHeapIndex);
}
}
}
}
}
// Clean up the memory
K_BHeapFree(&openset);
Z_Free(closedset);
Z_Free(nodesarray);
}
return pathfindsuccess;
}

70
src/k_pathfind.h Normal file
View file

@ -0,0 +1,70 @@
#ifndef __K_PATHFIND__
#define __K_PATHFIND__
#include "doomtype.h"
// function pointer for returning a node's connected node data
// should return a pointer to an array of pointers to the data, as arguments takes a node's data and a pointer that the
// number of connected nodes should be placed into
typedef void**(*getconnectednodesfunc)(void*, size_t*);
// function pointer for getting the list of connected node costs/distances
typedef UINT32*(*getnodeconnectioncostsfunc)(void*);
// function pointer for getting a heuristic between 2 nodes from their base data
typedef UINT32(*getnodeheuristicfunc)(void*, void*);
// function pointer for getting if a node is traversable from its base data
typedef boolean(*getnodetraversablefunc)(void*);
// A pathfindnode contains information about a node from the pathfinding
// heapindex is only used within the pathfinding algorithm itself, and is always 0 after it is completed
typedef struct pathfindnode_s {
size_t heapindex; // The index in the openset binary heap. Only valid while the node is in the openset.
void *nodedata;
struct pathfindnode_s *camefrom; // should eventually be the most efficient predecessor node
UINT32 gscore; // The accumulated distance from the start to this node
UINT32 hscore; // The heuristic from this node to the goal
} pathfindnode_t;
// Contains the final created path after pathfinding is completed
typedef struct path_s {
size_t numnodes;
struct pathfindnode_s *array;
UINT32 totaldist;
} path_t;
// Contains info about the pathfinding used to setup the algorithm
// (e.g. the base capacities of the dynamically allocated arrays)
// should be setup by the caller before starting pathfinding
// base capacities will be 8 if they aren't setup, missing callback functions will cause an error.
// Can be accessed after the pathfinding is complete to get the final capacities of them
typedef struct pathfindsetup_s {
size_t opensetcapacity;
size_t closedsetcapacity;
size_t nodesarraycapacity;
void *startnodedata;
void *endnodedata;
getconnectednodesfunc getconnectednodes;
getnodeconnectioncostsfunc getconnectioncosts;
getnodeheuristicfunc getheuristic;
getnodetraversablefunc gettraversable;
} pathfindsetup_t;
/*--------------------------------------------------
boolean K_PathfindAStar(path_t *const path, pathfindsetup_t *const pathfindsetup);
From a source waypoint and destination waypoint, find the best path between them using the A* algorithm.
Input Arguments:-
path - The return location of the found path
pathfindsetup - The information regarding pathfinding setup, see pathfindsetup_t
Return:-
True if a path was found between source and destination, false otherwise.
--------------------------------------------------*/
boolean K_PathfindAStar(path_t *const path, pathfindsetup_t *const pathfindsetup);
#endif

545
src/k_waypoint.c
View file

@ -4,16 +4,12 @@
#include "p_local.h"
#include "p_tick.h"
#include "z_zone.h"
#include "k_bheap.h"
// The number of sparkles per waypoint connection in the waypoint visualisation
static const UINT32 SPARKLES_PER_CONNECTION = 16U;
// Some defaults for the size of the dynamically allocated sets for pathfinding. When the sets reach their max capacity
// they are reallocated to contain their old capacity plus these defines. Openset is smaller because most of the time
// the nodes will quickly be moved to closedset, closedset could contain an entire maps worth of waypoints.
// Additonally, in order to keep later calls to pathfinding quick and avoid reallocation, the highest size of the
// allocation is saved into a variable.
// Some defaults for the size of the dynamically allocated sets for pathfinding. These are kept for the purpose of
// allocating a size that is less likely to need reallocating again during the pathfinding.
static const size_t OPENSET_BASE_SIZE = 16U;
static const size_t CLOSEDSET_BASE_SIZE = 256U;
static const size_t NODESARRAY_BASE_SIZE = 256U;
@ -21,6 +17,7 @@ static const size_t NODESARRAY_BASE_SIZE = 256U;
static waypoint_t **waypointheap = NULL;
static waypoint_t *firstwaypoint = NULL;
static waypoint_t *finishline = NULL;
static size_t numwaypoints = 0U;
static size_t numwaypointmobjs = 0U;
static size_t baseopensetsize = OPENSET_BASE_SIZE;
@ -455,468 +452,139 @@ static UINT32 K_DistanceBetweenWaypoints(waypoint_t *const waypoint1, waypoint_t
return finaldist;
}
/*--------------------------------------------------
static UINT32 K_GetNodeFScore(pathfindnode_t *node)
Gets the FScore of a node. The FScore is the GScore plus the HScore.
Input Arguments:-
node - The node to get the FScore of
Return:-
The FScore of the node.
--------------------------------------------------*/
static UINT32 K_GetNodeFScore(pathfindnode_t *node)
static void **K_WaypointPathfindGetNext(void *data, size_t *numconnections)
{
UINT32 nodefscore = UINT32_MAX;
waypoint_t **connectingwaypoints = NULL;
if (node == NULL)
if (data == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL node in K_GetNodeFScore.\n");
CONS_Debug(DBG_GAMELOGIC, "K_WaypointPathfindGetNext received NULL data.\n");
}
else if (numconnections == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "K_WaypointPathfindGetNext received NULL numconnections.\n");
}
else
{
nodefscore = node->gscore + node->hscore;
waypoint_t *waypoint = (waypoint_t *)data;
connectingwaypoints = waypoint->nextwaypoints;
*numconnections = waypoint->numnextwaypoints;
}
return nodefscore;
return (void**)connectingwaypoints;
}
/*--------------------------------------------------
static boolean K_ClosedsetContainsNode(pathfindnode_t **closedset, pathfindnode_t *node, size_t closedsetcount)
Checks whether the Closedset contains a node. Searches from the end to the start for speed reasons.
Input Arguments:-
closedset - The closed set within the A* algorithm
node - The node to check is within the closed set
closedsetcount - The current size of the closedset
Return:-
True if the node is in the closed set, false if it isn't
--------------------------------------------------*/
static boolean K_ClosedsetContainsNode(pathfindnode_t **closedset, pathfindnode_t *node, size_t closedsetcount)
static void **K_WaypointPathfindGetPrev(void *data, size_t *numconnections)
{
boolean nodeisinclosedset = false;
waypoint_t **connectingwaypoints = NULL;
if (closedset == NULL)
if (data == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL closedset in K_SetContainsWaypoint.\n");
CONS_Debug(DBG_GAMELOGIC, "K_WaypointPathfindGetPrev received NULL data.\n");
}
else if (node == NULL)
else if (numconnections == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL node in K_SetContainsWaypoint.\n");
CONS_Debug(DBG_GAMELOGIC, "K_WaypointPathfindGetPrev received NULL numconnections.\n");
}
else
{
size_t i;
// It is more likely that we'll find the node we are looking for from the end of the array
// Yes, the for loop looks weird, remember that size_t is unsigned and we want to check 0, after it hits 0 it
// will loop back up to SIZE_MAX
for (i = closedsetcount - 1U; i < closedsetcount; i--)
{
if (closedset[i] == node)
{
nodeisinclosedset = true;
break;
waypoint_t *waypoint = (waypoint_t *)data;
connectingwaypoints = waypoint->prevwaypoints;
*numconnections = waypoint->numprevwaypoints;
}
}
}
return nodeisinclosedset;
return (void**)connectingwaypoints;
}
/*--------------------------------------------------
static pathfindnode_t *K_NodesArrayContainsWaypoint(
pathfindnode_t *nodesarray,
waypoint_t* waypoint,
size_t nodesarraycount)
Checks whether the Nodes Array contains a node with a waypoint. Searches from the end to the start for speed
reasons.
Input Arguments:-
nodesarray - The nodes array within the A* algorithm
waypoint - The waypoint to check is within the nodes array
nodesarraycount - The current size of the nodes array
Return:-
The pathfind node that has the waypoint if there is one. NULL if the waypoint is not in the nodes array.
--------------------------------------------------*/
static pathfindnode_t *K_NodesArrayContainsWaypoint(
pathfindnode_t *nodesarray,
waypoint_t* waypoint,
size_t nodesarraycount)
static UINT32 *K_WaypointPathfindGetNextCosts(void* data)
{
pathfindnode_t *foundnode = NULL;
UINT32 *connectingnodecosts = NULL;
if (nodesarray == NULL)
if (data == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL nodesarray in K_NodesArrayContainsWaypoint.\n");
}
else if (waypoint == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL waypoint in K_NodesArrayContainsWaypoint.\n");
CONS_Debug(DBG_GAMELOGIC, "K_WaypointPathfindGetNextCosts received NULL data.\n");
}
else
{
size_t i;
// It is more likely that we'll find the node we are looking for from the end of the array
// Yes, the for loop looks weird, remember that size_t is unsigned and we want to check 0, after it hits 0 it
// will loop back up to SIZE_MAX
for (i = nodesarraycount - 1U; i < nodesarraycount; i--)
{
if (nodesarray[i].waypoint == waypoint)
{
foundnode = &nodesarray[i];
break;
waypoint_t *waypoint = (waypoint_t *)data;
connectingnodecosts = waypoint->nextwaypointdistances;
}
}
}
return foundnode;
return connectingnodecosts;
}
/*--------------------------------------------------
static void K_NodeUpdateHeapIndex(void *const node, const size_t newheapindex)
A callback for the Openset Binary Heap to be able to update the heapindex of the pathfindnodes when they are
moved.
Input Arguments:-
node - The node that has been updated, should be a pointer to a pathfindnode_t
newheapindex - The new heapindex of the node.
Return:-
None
--------------------------------------------------*/
static void K_NodeUpdateHeapIndex(void *const node, const size_t newheapindex)
static UINT32 *K_WaypointPathfindGetPrevCosts(void* data)
{
if (node == NULL)
UINT32 *connectingnodecosts = NULL;
if (data == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL node in K_NodeUpdateHeapIndex.\n");
CONS_Debug(DBG_GAMELOGIC, "K_WaypointPathfindGetPrevCosts received NULL data.\n");
}
else
{
pathfindnode_t *truenode = (pathfindnode_t*)node;
truenode->heapindex = newheapindex;
waypoint_t *waypoint = (waypoint_t *)data;
connectingnodecosts = waypoint->prevwaypointdistances;
}
return connectingnodecosts;
}
/*--------------------------------------------------
static boolean K_ReconstructPath(path_t *const returnpath, pathfindnode_t *const destinationnode)
From a pathfindnode that should be the destination, reconstruct a path from start to finish.
Input Arguments:-
returnpath - The location of the path that is being created
destinationnode - The node that is the destination from the pathfinding
Return:-
True if the path reconstruction was successful, false if it wasn't.
--------------------------------------------------*/
static boolean K_ReconstructPath(path_t *const returnpath, pathfindnode_t *const destinationnode)
static UINT32 K_WaypointPathfindGetHeuristic(void *data1, void *data2)
{
boolean reconstructsuccess = false;
UINT32 nodeheuristic = UINT32_MAX;
if (returnpath == NULL)
if (data1 == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL returnpath in K_ReconstructPath.\n");
CONS_Debug(DBG_GAMELOGIC, "K_WaypointPathfindGetHeuristic received NULL data1.\n");
}
else if (destinationnode == NULL)
else if (data2 == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL destinationnode in K_ReconstructPath.\n");
CONS_Debug(DBG_GAMELOGIC, "K_WaypointPathfindGetHeuristic received NULL data2.\n");
}
else
{
size_t numnodes = 0U;
pathfindnode_t *thisnode = destinationnode;
waypoint_t *waypoint1 = (waypoint_t *)data1;
waypoint_t *waypoint2 = (waypoint_t *)data2;
// If the path we're placing our new path into already has data, free it
if (returnpath->array != NULL)
{
Z_Free(returnpath->array);
returnpath->numnodes = 0U;
returnpath->totaldist = 0U;
nodeheuristic = K_DistanceBetweenWaypoints(waypoint1, waypoint2);
}
// Do a fast check of how many nodes there are so we know how much space to allocate
for (thisnode = destinationnode; thisnode; thisnode = thisnode->camefrom)
{
numnodes++;
}
if (numnodes > 0U)
{
// Allocate memory for the path
returnpath->numnodes = numnodes;
returnpath->array = Z_Calloc(numnodes * sizeof(pathfindnode_t), PU_STATIC, NULL);
returnpath->totaldist = destinationnode->gscore;
if (returnpath->array == NULL)
{
I_Error("K_ReconstructPath: Out of memory.");
}
// Put the nodes into the return array
for (thisnode = destinationnode; thisnode; thisnode = thisnode->camefrom)
{
returnpath->array[numnodes - 1U] = *thisnode;
// Correct the camefrom element to point to the previous element in the array instead
if ((returnpath->array[numnodes - 1U].camefrom != NULL) && (numnodes > 1U))
{
returnpath->array[numnodes - 1U].camefrom = &returnpath->array[numnodes - 2U];
}
else
{
returnpath->array[numnodes - 1U].camefrom = NULL;
}
numnodes--;
}
reconstructsuccess = true;
}
}
return reconstructsuccess;
return nodeheuristic;
}
/*--------------------------------------------------
static boolean K_WaypointAStar(
waypoint_t *const sourcewaypoint,
waypoint_t *const destinationwaypoint,
path_t *const returnpath,
const boolean useshortcuts,
const boolean huntbackwards)
From a source waypoint and destination waypoint, find the best path between them using the A* algorithm.
Input Arguments:-
sourcewaypoint - The source waypoint to pathfind from
destinationwaypoint - The destination waypoint to pathfind to
returnpath - The path to return to if the pathfinding was successful.
useshortcuts - Whether the pathfinding can use shortcut waypoints.
huntbackwards - Whether the pathfinding should hunt through previous or next waypoints
Return:-
True if a path was found between source and destination, false otherwise.
--------------------------------------------------*/
static boolean K_WaypointAStar(
waypoint_t *const sourcewaypoint,
waypoint_t *const destinationwaypoint,
path_t *const returnpath,
const boolean useshortcuts,
const boolean huntbackwards)
static boolean K_WaypointPathfindTraversableAllEnabled(void *data)
{
boolean pathfindsuccess = false;
boolean traversable = false;
if (sourcewaypoint == NULL)
if (data == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL sourcewaypoint in K_WaypointAStar.\n");
}
else if (destinationwaypoint == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL destinationwaypoint in K_WaypointAStar.\n");
}
else if (returnpath == NULL)
{
CONS_Debug(DBG_GAMELOGIC, "NULL returnpath in K_WaypointAStar.\n");
}
else if (sourcewaypoint == destinationwaypoint)
{
// Source and destination waypoint are the same, we're already there
// Just for simplicity's sake, create a single node on the destination and reconstruct path
pathfindnode_t singlenode;
singlenode.camefrom = NULL;
singlenode.waypoint = destinationwaypoint;
singlenode.heapindex = SIZE_MAX;
singlenode.hscore = 0U;
singlenode.gscore = 0U;
pathfindsuccess = K_ReconstructPath(returnpath, &singlenode);
}
else if (((huntbackwards == false) && (sourcewaypoint->numnextwaypoints == 0))
|| ((huntbackwards == true) && (sourcewaypoint->numprevwaypoints == 0)))
{
CONS_Debug(DBG_GAMELOGIC,
"K_WaypointAStar: sourcewaypoint with ID %d has no next waypoint\n",
K_GetWaypointID(sourcewaypoint));
}
else if (((huntbackwards == false) && (destinationwaypoint->numprevwaypoints == 0))
|| ((huntbackwards == true) && (destinationwaypoint->numnextwaypoints == 0)))
{
CONS_Debug(DBG_GAMELOGIC,
"K_WaypointAStar: destinationwaypoint with ID %d has no previous waypoint\n",
K_GetWaypointID(destinationwaypoint));
}
else if ((K_GetWaypointIsEnabled(destinationwaypoint) == false) ||
(!useshortcuts &&(K_GetWaypointIsShortcut(destinationwaypoint) == true)))
{
// No path to the destination is possible
CONS_Debug(DBG_GAMELOGIC, "K_WaypointPathfindTraversableAllEnabled received NULL data.\n");
}
else
{
size_t opensetcapacity = K_GetOpensetBaseSize();
size_t nodesarraycapacity = K_GetNodesArrayBaseSize();
size_t closedsetcapacity = K_GetClosedsetBaseSize();
bheapitem_t poppeditem = {};
pathfindnode_t *currentnode = NULL;
pathfindnode_t *neighbournode = NULL;
bheap_t openset = {};
pathfindnode_t **closedset = Z_Calloc(closedsetcapacity * sizeof(pathfindnode_t*), PU_STATIC, NULL);
pathfindnode_t *nodesarray = Z_Calloc(nodesarraycapacity * sizeof(pathfindnode_t), PU_STATIC, NULL);
pathfindnode_t *newnode = NULL;
size_t closedsetcount = 0U;
size_t nodesarraycount = 0U;
size_t numcheckwaypoints = 0U;
waypoint_t **checkwaypoints = NULL;
UINT32 *checkwaypointsdists = NULL;
UINT32 tentativegscore = UINT32_MAX;
size_t i = 0U;
size_t findopensetindex = 0U;
if (closedset == NULL || nodesarray == NULL)
{
I_Error("K_WaypointAStar: Out of memory.");
waypoint_t *waypoint = (waypoint_t *)data;
traversable = (K_GetWaypointIsEnabled(waypoint) == true);
}
K_BHeapInit(&openset, opensetcapacity);
return traversable;
}
newnode = &nodesarray[0];
newnode->waypoint = sourcewaypoint;
newnode->hscore = K_DistanceBetweenWaypoints(sourcewaypoint, destinationwaypoint);
newnode->gscore = 0U;
newnode->camefrom = NULL;
nodesarraycount++;
static boolean K_WaypointPathfindTraversableNoShortcuts(void *data)
{
boolean traversable = false;
K_BHeapPush(&openset, &nodesarray[0], K_GetNodeFScore(&nodesarray[0]), K_NodeUpdateHeapIndex);
if (opensetcapacity != openset.capacity)
if (data == NULL)
{
opensetcapacity = openset.capacity;
K_UpdateOpensetBaseSize(opensetcapacity);
}
while (openset.count > 0)
{
K_BHeapPop(&openset, &poppeditem);
currentnode = ((pathfindnode_t*)poppeditem.data);
if (currentnode->waypoint == destinationwaypoint)
{
pathfindsuccess = K_ReconstructPath(returnpath, currentnode);
break;
}
// The node is now placed into the closed set because it is evaluated
if (closedsetcount >= closedsetcapacity)
{
K_UpdateClosedsetBaseSize(closedsetcapacity * 2);
closedsetcapacity = K_GetClosedsetBaseSize();
closedset = Z_Realloc(closedset, closedsetcapacity * sizeof (pathfindnode_t*), PU_STATIC, NULL);
if (closedset == NULL)
{
I_Error("K_WaypointAStar: Out of memory");
}
}
closedset[closedsetcount] = currentnode;
closedsetcount++;
if (huntbackwards)
{
numcheckwaypoints = currentnode->waypoint->numprevwaypoints;
checkwaypoints = currentnode->waypoint->prevwaypoints;
checkwaypointsdists = currentnode->waypoint->prevwaypointdistances;
CONS_Debug(DBG_GAMELOGIC, "K_WaypointPathfindTraversableNoShortcuts received NULL data.\n");
}
else
{
numcheckwaypoints = currentnode->waypoint->numnextwaypoints;
checkwaypoints = currentnode->waypoint->nextwaypoints;
checkwaypointsdists = currentnode->waypoint->nextwaypointdistances;
waypoint_t *waypoint = (waypoint_t *)data;
traversable = ((K_GetWaypointIsShortcut(waypoint) == false) && (K_GetWaypointIsEnabled(waypoint) == true));
}
for (i = 0; i < numcheckwaypoints; i++)
{
tentativegscore = currentnode->gscore + checkwaypointsdists[i];
// Can this double search be sped up at all? I feel like allocating and deallocating memory for nodes
// constantly would be slower
// Find if the neighbournode is already created first, if it is then check if it's in the closedset
// If it's in the closedset, then skip as we don't need to check it again, if it isn't then see if the
// new route from currentnode is faster to it and update accordingly
neighbournode = K_NodesArrayContainsWaypoint(nodesarray, checkwaypoints[i], nodesarraycount);
if (neighbournode != NULL)
{
// If the closedset contains the node, then it is already evaluated and doesn't need to be checked
if (K_ClosedsetContainsNode(closedset, neighbournode, closedsetcount) != false)
{
continue;
}
if (tentativegscore < neighbournode->gscore)
{
neighbournode->camefrom = currentnode;
neighbournode->gscore = tentativegscore;
findopensetindex = K_BHeapContains(&openset, neighbournode, neighbournode->heapindex);
if (findopensetindex != SIZE_MAX)
{
K_UpdateBHeapItemValue(&openset.array[findopensetindex], K_GetNodeFScore(neighbournode));
}
else
{
// What??? How is this node NOT in the openset???
// A node should always be in either the openset or closedset
CONS_Debug(DBG_GAMELOGIC, "Node unexpectedly not in openset in K_WaypointAStar.\n");
K_BHeapPush(&openset, neighbournode, K_GetNodeFScore(neighbournode), K_NodeUpdateHeapIndex);
}
}
}
else
{
// Don't process this waypoint if it's not traversable
if ((K_GetWaypointIsEnabled(checkwaypoints[i]) == false)
|| (!useshortcuts && K_GetWaypointIsShortcut(checkwaypoints[i]) == true))
{
continue;
}
// reallocate the nodesarray if needed
if (nodesarraycount >= nodesarraycapacity)
{
K_UpdateNodesArrayBaseSize(nodesarraycapacity * 2);
nodesarraycapacity = K_GetNodesArrayBaseSize();
nodesarray = Z_Realloc(nodesarray, nodesarraycapacity * sizeof(pathfindnode_t), PU_STATIC, NULL);
if (nodesarray == NULL)
{
I_Error("K_WaypointAStar: Out of memory");
}
}
// There is currently no node created for this waypoint, so make one
newnode = &nodesarray[nodesarraycount];
newnode->camefrom = currentnode;
newnode->heapindex = SIZE_MAX;
newnode->gscore = tentativegscore;
newnode->hscore = K_DistanceBetweenWaypoints(checkwaypoints[i], destinationwaypoint);
newnode->waypoint = checkwaypoints[i];
nodesarraycount++;
// because there was no node for the waypoint, it's also not in the openset, add it
K_BHeapPush(&openset, newnode, K_GetNodeFScore(newnode), K_NodeUpdateHeapIndex);
}
}
}
// Clean up the memory
K_BHeapFree(&openset);
Z_Free(closedset);
Z_Free(nodesarray);
}
return pathfindsuccess;
return traversable;
}
/*--------------------------------------------------
@ -962,7 +630,38 @@ boolean K_PathfindToWaypoint(
}
else
{
pathfound = K_WaypointAStar(sourcewaypoint, destinationwaypoint, returnpath, useshortcuts, huntbackwards);
pathfindsetup_t pathfindsetup = {};
getconnectednodesfunc nextnodesfunc = K_WaypointPathfindGetNext;
getnodeconnectioncostsfunc nodecostsfunc = K_WaypointPathfindGetNextCosts;
getnodeheuristicfunc heuristicfunc = K_WaypointPathfindGetHeuristic;
getnodetraversablefunc traversablefunc = K_WaypointPathfindTraversableNoShortcuts;
if (huntbackwards)
{
nextnodesfunc = K_WaypointPathfindGetPrev;
nodecostsfunc = K_WaypointPathfindGetPrevCosts;
}
if (useshortcuts)
{
traversablefunc = K_WaypointPathfindTraversableAllEnabled;
}
pathfindsetup.opensetcapacity = K_GetOpensetBaseSize();
pathfindsetup.closedsetcapacity = K_GetClosedsetBaseSize();
pathfindsetup.nodesarraycapacity = K_GetNodesArrayBaseSize();
pathfindsetup.startnodedata = sourcewaypoint;
pathfindsetup.endnodedata = destinationwaypoint;
pathfindsetup.getconnectednodes = nextnodesfunc;
pathfindsetup.getconnectioncosts = nodecostsfunc;
pathfindsetup.getheuristic = heuristicfunc;
pathfindsetup.gettraversable = traversablefunc;
pathfound = K_PathfindAStar(returnpath, &pathfindsetup);
K_UpdateOpensetBaseSize(pathfindsetup.opensetcapacity);
K_UpdateClosedsetBaseSize(pathfindsetup.closedsetcapacity);
K_UpdateNodesArrayBaseSize(pathfindsetup.nodesarraycapacity);
}
return pathfound;
@ -1025,22 +724,53 @@ waypoint_t *K_GetNextWaypointToDestination(
}
else
{
path_t pathtowaypoint;
boolean pathfindsuccess =
K_WaypointAStar(sourcewaypoint, destinationwaypoint, &pathtowaypoint, useshortcuts, huntbackwards);
path_t pathtowaypoint = {};
pathfindsetup_t pathfindsetup = {};
boolean pathfindsuccess = false;
getconnectednodesfunc nextnodesfunc = K_WaypointPathfindGetNext;
getnodeconnectioncostsfunc nodecostsfunc = K_WaypointPathfindGetNextCosts;
getnodeheuristicfunc heuristicfunc = K_WaypointPathfindGetHeuristic;
getnodetraversablefunc traversablefunc = K_WaypointPathfindTraversableNoShortcuts;
if (huntbackwards)
{
nextnodesfunc = K_WaypointPathfindGetPrev;
nodecostsfunc = K_WaypointPathfindGetPrevCosts;
}
if (useshortcuts)
{
traversablefunc = K_WaypointPathfindTraversableAllEnabled;
}
pathfindsetup.opensetcapacity = K_GetOpensetBaseSize();
pathfindsetup.closedsetcapacity = K_GetClosedsetBaseSize();
pathfindsetup.nodesarraycapacity = K_GetNodesArrayBaseSize();
pathfindsetup.startnodedata = sourcewaypoint;
pathfindsetup.endnodedata = destinationwaypoint;
pathfindsetup.getconnectednodes = nextnodesfunc;
pathfindsetup.getconnectioncosts = nodecostsfunc;
pathfindsetup.getheuristic = heuristicfunc;
pathfindsetup.gettraversable = traversablefunc;
pathfindsuccess = K_PathfindAStar(&pathtowaypoint, &pathfindsetup);
K_UpdateOpensetBaseSize(pathfindsetup.opensetcapacity);
K_UpdateClosedsetBaseSize(pathfindsetup.closedsetcapacity);
K_UpdateNodesArrayBaseSize(pathfindsetup.nodesarraycapacity);
if (pathfindsuccess)
{
// A direct path to the destination has been found.
if (pathtowaypoint.numnodes > 1)
{
nextwaypoint = pathtowaypoint.array[1].waypoint;
nextwaypoint = (waypoint_t*)pathtowaypoint.array[1].nodedata;
}
else
{
// Shouldn't happen, as this is the source waypoint.
CONS_Debug(DBG_GAMELOGIC, "Only one waypoint pathfound in K_GetNextWaypointToDestination.\n");
nextwaypoint = pathtowaypoint.array[0].waypoint;
nextwaypoint = (waypoint_t*)pathtowaypoint.array[0].nodedata;
}
Z_Free(pathtowaypoint.array);
@ -1257,7 +987,6 @@ searchwaypointstart:
{
// No next waypoints, this function will be returned from
}
}
}
}

17
src/k_waypoint.h
View file

@ -1,8 +1,9 @@
#ifndef __K_WAYPOINT__
#define __K_WAYPOINT__
#include "doomdef.h"
#include "doomtype.h"
#include "p_mobj.h"
#include "k_pathfind.h"
typedef struct waypoint_s
{
@ -16,20 +17,6 @@ typedef struct waypoint_s
size_t numprevwaypoints;
} waypoint_t;
typedef struct pathfindnode_s {
size_t heapindex; // The index in the openset binary heap. Only valid while the node is in the openset.
waypoint_t *waypoint;
struct pathfindnode_s *camefrom; // should eventually be the most efficient predecessor node
UINT32 gscore; // The accumulated distance from the start to this node
UINT32 hscore; // The heuristic from this node to the goal, saved to avoid expensive recalculation
} pathfindnode_t;
typedef struct path_s {
size_t numnodes;
struct pathfindnode_s *array;
UINT32 totaldist;
} path_t;
// AVAILABLE FOR LUA