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