static inline void DidRemoveFromOpenSetAtIndex(
VisitedNodes nodes, size_t idx)
{
size_t smallestIndex = idx;
do {
size_t leftIndex;
size_t rightIndex;
if (smallestIndex != idx)
{
SwapOpenSetNodesAtIndexes(nodes, smallestIndex, idx);
idx = smallestIndex;
}
leftIndex = (2 * idx) + 1;
rightIndex = (2 * idx) + 2;
if (leftIndex < nodes->openNodesCount && NodeRankCompare(NodeMake(nodes, nodes->openNodes[leftIndex]), NodeMake(nodes, nodes->openNodes[smallestIndex])) < 0) {
smallestIndex = leftIndex;
}
if (rightIndex < nodes->openNodesCount && NodeRankCompare(NodeMake(nodes, nodes->openNodes[rightIndex]), NodeMake(nodes, nodes->openNodes[smallestIndex])) < 0) {
smallestIndex = rightIndex;
}
} while (smallestIndex != idx);
}
static inline void DidInsertIntoOpenSetAtIndex(VisitedNodes nodes, size_t index)
{
while (index > 0) {
/*const size_t parentIndex = floorf((index-1) / 2);*/
const size_t parentIndex = (int)((index-1) / 2);
if (NodeRankCompare(NodeMake(nodes, nodes->openNodes[parentIndex]), NodeMake(nodes, nodes->openNodes[index])) < 0) {
break;
} else {
SwapOpenSetNodesAtIndexes(nodes, parentIndex, index);
index = parentIndex;
}
}
}
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 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 DidInsertIntoOpenSetAtIndex(VisitedNodes nodes, size_t idx)
{
while (idx > 0)
{
const size_t parentIndex = (size_t)floorf((float)(idx-1) / 2);
if (NodeRankCompare(NodeMake(nodes, nodes->openNodes[parentIndex]), NodeMake(nodes, nodes->openNodes[idx])) < 0)
{
break;
}
else
{
SwapOpenSetNodesAtIndexes(nodes, parentIndex, idx);
idx = parentIndex;
}
}
}
static inline Node GetParentNode(Node n)
{
NodeRecord *record = NodeGetRecord(n);
if (record->hasParent) {
return NodeMake(n.nodes, record->parentIndex);
} else {
return NodeNull;
}
}
static inline Node GetOpenNode(VisitedNodes nodes)
{
return NodeMake(nodes, nodes->openNodes[0]);
}
