* @brief insert `front` node into the frontier (keeping the frontier sorted by total distance) */
| 72 | * @brief insert `front` node into the frontier (keeping the frontier sorted by total distance) |
| 73 | */ |
| 74 | void NextNode(uint16_t front) |
| 75 | { |
| 76 | if (Path2Nodes->nextNodeIndex == PathNode::InvalidIndex) { |
| 77 | Path2Nodes->nextNodeIndex = front; |
| 78 | return; |
| 79 | } |
| 80 | |
| 81 | PathNode *current = Path2Nodes; |
| 82 | uint16_t nextIndex = Path2Nodes->nextNodeIndex; |
| 83 | const uint8_t maxF = PathNodes[front].f; |
| 84 | while (nextIndex != PathNode::InvalidIndex && PathNodes[nextIndex].f < maxF) { |
| 85 | current = &PathNodes[nextIndex]; |
| 86 | nextIndex = current->nextNodeIndex; |
| 87 | } |
| 88 | PathNodes[front].nextNodeIndex = nextIndex; |
| 89 | current->nextNodeIndex = front; |
| 90 | } |
| 91 | |
| 92 | /** A linked list of all visited nodes */ |
| 93 | PathNode *VisitedNodes; |