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