103 {})
104{
105 using Node = TracerNodeT<Tracer>;
106 using QNode = QNode<Tracer>;
107 using TracerTraits = TracerTraits_<remove_cvref_t<Tracer>>;
108
109 struct LessPred {
110 NodeMap &m;
111 bool operator ()(size_t node_a, size_t node_b) {
112 return m[node_a].f() < m[node_b].f();
113 }
114 };
115
116 auto qopen = make_mutable_priority_queue<size_t, true>(
117 [&cached_nodes](size_t el, size_t qidx) {
118 cached_nodes[el].queue_id = qidx;
119 },
120 LessPred{cached_nodes});
121
123 size_t source_id = TracerTraits::unique_id(tracer, source);
124 cached_nodes[source_id] = initial;
125 qopen.push(source_id);
126
127 size_t goal_id = TracerTraits::goal_heuristic(tracer, source) < 0.f ?
128 source_id :
130
131 while (goal_id == Unassigned && !qopen.empty()) {
132 size_t q_id = qopen.top();
133 qopen.pop();
134 QNode &q = cached_nodes[q_id];
135
136
137 assert(!std::isinf(q.g));
138
139 TracerTraits::foreach_reachable(tracer, q.node, [&](const Node &succ_nd) {
140 if (goal_id != Unassigned)
141 return true;
142
143 float h = TracerTraits::goal_heuristic(tracer, succ_nd);
144 float dst = TracerTraits::distance(tracer, q.node, succ_nd);
145 size_t succ_id = TracerTraits::unique_id(tracer, succ_nd);
146 QNode qsucc_nd{succ_nd, q_id, q.g + dst, h};
147
148 if (h < 0.f) {
149 goal_id = succ_id;
150 cached_nodes[succ_id] = qsucc_nd;
151 } else {
152
153 QNode &prev_nd = cached_nodes[succ_id];
154
155 if (qsucc_nd.g < prev_nd.g) {
156
157
158
159 size_t queue_id = prev_nd.queue_id;
160
161
162 prev_nd = qsucc_nd;
163
164 if (queue_id == InvalidQueueID)
165
166 qopen.push(succ_id);
167 else
168 qopen.update(queue_id);
169 }
170 }
171
173 });
174 }
175
176
177 if (goal_id != Unassigned) {
178 const QNode *q = &cached_nodes[goal_id];
179 while (q->parent != Unassigned) {
180 assert(!std::isinf(q->g));
181
182 *out = q->node;
183 ++out;
184 q = &cached_nodes[q->parent];
185 }
186 }
187
189}
constexpr auto Unassigned
Definition AStar.hpp:59