static inline void SwapOpenSetNodesAtIndexes(VisitedNodes nodes, size_t index1, size_t index2)
{
if (index1 != index2) {
NodeRecord *record1 = NodeGetRecord(NodeMake(nodes, nodes->openNodes[index1]));
NodeRecord *record2 = NodeGetRecord(NodeMake(nodes, nodes->openNodes[index2]));
const size_t tempOpenIndex = record1->openIndex;
const size_t tempNodeIndex = nodes->openNodes[index1];
record1->openIndex = record2->openIndex;
record2->openIndex = tempOpenIndex;
nodes->openNodes[index1] = nodes->openNodes[index2];
nodes->openNodes[index2] = tempNodeIndex;
}
}
static inline void AddNodeToOpenSet(Node n, float cost, Node parent)
{
NodeRecord *record = NodeGetRecord(n);
const size_t openIndex = n.nodes->openNodesCount;
if (!NodeIsNull(parent)) {
record->hasParent = 1;
record->parentIndex = parent.index;
} else {
record->hasParent = 0;
}
if (n.nodes->openNodesCount == n.nodes->openNodesCapacity) {
n.nodes->openNodesCapacity = 1 + (n.nodes->openNodesCapacity * 2);
CREALLOC(n.nodes->openNodes, n.nodes->openNodesCapacity * sizeof(size_t));
}
n.nodes->openNodes[openIndex] = n.index;
n.nodes->openNodesCount++;
record->openIndex = openIndex;
record->isOpen = 1;
record->cost = cost;
DidInsertIntoOpenSetAtIndex(n.nodes, openIndex);
}
static inline Node GetParentNode(Node n)
{
NodeRecord *record = NodeGetRecord(n);
if (record->hasParent) {
return NodeMake(n.nodes, record->parentIndex);
} else {
return NodeNull;
}
}
static inline void RemoveNodeFromOpenSet(Node n)
{
NodeRecord *record = NodeGetRecord(n);
if (record->isOpen) {
record->isOpen = 0;
n.nodes->openNodesCount--;
const size_t index = record->openIndex;
SwapOpenSetNodesAtIndexes(n.nodes, index, n.nodes->openNodesCount);
DidRemoveFromOpenSetAtIndex(n.nodes, index);
}
}
static Node GetNode(VisitedNodes nodes, void *nodeKey)
{
size_t first;
Node node;
NodeRecord *record;
if (!nodeKey) {
return NodeNull;
}
// looks it up in the index, if it's not found it inserts a new record in the sorted index and the nodeRecords array and returns a reference to it
first = 0;
if (nodes->nodeRecordsCount > 0) {
size_t last = nodes->nodeRecordsCount-1;
while (first <= last) {
const size_t mid = (first + last) / 2;
const int comp = NodeKeyCompare(NodeMake(nodes, nodes->nodeRecordsIndex[mid]), nodeKey);
if (comp < 0) {
first = mid + 1;
} else if (comp > 0 && mid > 0) {
last = mid - 1;
} else if (comp > 0) {
break;
} else {
return NodeMake(nodes, nodes->nodeRecordsIndex[mid]);
}
}
}
if (nodes->nodeRecordsCount == nodes->nodeRecordsCapacity) {
nodes->nodeRecordsCapacity = 1 + (nodes->nodeRecordsCapacity * 2);
CREALLOC(nodes->nodeRecords, nodes->nodeRecordsCapacity * (sizeof(NodeRecord) + nodes->source->nodeSize));
CREALLOC(nodes->nodeRecordsIndex, nodes->nodeRecordsCapacity * sizeof(size_t));
}
node = NodeMake(nodes, nodes->nodeRecordsCount);
nodes->nodeRecordsCount++;
memmove(&nodes->nodeRecordsIndex[first+1], &nodes->nodeRecordsIndex[first], (nodes->nodeRecordsCapacity - first - 1) * sizeof(size_t));
nodes->nodeRecordsIndex[first] = node.index;
record = NodeGetRecord(node);
memset(record, 0, sizeof(NodeRecord));
memcpy(record->nodeKey, nodeKey, nodes->source->nodeSize);
return node;
}
static inline int NodeIsGoal(Node n)
{
return !NodeIsNull(n) && NodeGetRecord(n)->isGoal;
}
static inline void SetNodeIsGoal(Node n)
{
if (!NodeIsNull(n)) {
NodeGetRecord(n)->isGoal = 1;
}
}
static inline int NodeHasEstimatedCost(Node n)
{
return NodeGetRecord(n)->hasEstimatedCost;
}
static inline void SetNodeEstimatedCost(Node n, float estimatedCost)
{
NodeRecord *record = NodeGetRecord(n);
record->estimatedCost = estimatedCost;
record->hasEstimatedCost = 1;
}
static inline float GetNodeRank(Node n)
{
NodeRecord *record = NodeGetRecord(n);
return record->estimatedCost + record->cost;
}
static inline void AddNodeToClosedSet(Node n)
{
NodeGetRecord(n)->isClosed = 1;
}
static inline void RemoveNodeFromClosedSet(Node n)
{
NodeGetRecord(n)->isClosed = 0;
}
static inline int NodeIsInClosedSet(Node n)
{
return NodeGetRecord(n)->isClosed;
}
static inline int NodeIsInOpenSet(Node n)
{
return NodeGetRecord(n)->isOpen;
}
static inline void *GetNodeKey(Node node)
{
return NodeGetRecord(node)->nodeKey;
}
static inline float GetNodeCost(Node n)
{
return NodeGetRecord(n)->cost;
}
static inline float GetNodeEstimatedCost(Node n)
{
return NodeGetRecord(n)->estimatedCost;
}