79 {})
80{
82 using QNode = QNode<Tracer>;
83 using TracerTraits = TracerTraits_<remove_cvref_t<Tracer>>;
84
85 struct LessPred
86 {
87 NodeMap &m;
88 bool operator()(size_t node_a, size_t node_b) { return m[node_a].f() < m[node_b].f(); }
89 };
90
92
94 size_t source_id = TracerTraits::unique_id(tracer, source);
95 cached_nodes[source_id] = initial;
96 qopen.push(source_id);
97
98 size_t goal_id = TracerTraits::goal_heuristic(tracer, source) < 0.f ? source_id :
Unassigned;
99
100 while (goal_id ==
Unassigned && !qopen.empty()) {
101 size_t q_id = qopen.top();
102 qopen.pop();
103 QNode &q = cached_nodes[q_id];
104
105
106 assert(!std::isinf(q.g));
107
108 TracerTraits::foreach_reachable(tracer, q.node, [&](const Node &succ_nd) {
109 if (goal_id != Unassigned) return true;
110
111 float h = TracerTraits::goal_heuristic(tracer, succ_nd);
112 float dst = TracerTraits::distance(tracer, q.node, succ_nd);
113 size_t succ_id = TracerTraits::unique_id(tracer, succ_nd);
114 QNode qsucc_nd{succ_nd, q_id, q.g + dst, h};
115
116 if (h < 0.f) {
117 goal_id = succ_id;
118 cached_nodes[succ_id] = qsucc_nd;
119 } else {
120
121 QNode &prev_nd = cached_nodes[succ_id];
122
123 if (qsucc_nd.g < prev_nd.g) {
124
125
126
127 size_t queue_id = prev_nd.queue_id;
128
129
130 prev_nd = qsucc_nd;
131
133
134 qopen.push(succ_id);
135 else
136 qopen.update(queue_id);
137 }
138 }
139
141 });
142 }
143
144
146 const QNode *q = &cached_nodes[goal_id];
148 assert(!std::isinf(q->g));
149
150 *out = q->node;
151 ++out;
152 q = &cached_nodes[q->parent];
153 }
154 }
155
157}
constexpr auto InvalidQueueID
Definition MutablePriorityQueue.hpp:10
MutablePriorityQueue< T, IndexSetter, LessPredicate, ResetIndexWhenRemoved > make_mutable_priority_queue(IndexSetter &&index_setter, LessPredicate &&less_predicate)
Definition MutablePriorityQueue.hpp:56
constexpr auto Unassigned
Definition AStar.hpp:48
typename TracerTraits_< remove_cvref_t< T > >::Node TracerNodeT
Definition AStar.hpp:46