1 /*
2 * This program is free software; you can redistribute it and/or
3 * modify it under the terms of the GNU General Public License
4 * as published by the Free Software Foundation; either version 2
5 * of the License, or (at your option) any later version.
6 *
7 * This program is distributed in the hope that it will be useful,
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 * GNU General Public License for more details.
11 *
12 * You should have received a copy of the GNU General Public License
13 * along with this program; if not, write to the Free Software Foundation,
14 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15 *
16 * The Original Code is Copyright (C) 2014 Blender Foundation.
17 * All rights reserved.
18 */
19
20 /** \file
21 * \ingroup bli
22 * \brief An implementation of the A* (AStar) algorithm to solve shortest path problem.
23 *
24 * This library implements the simple A* (AStar) algorithm, an optimized version of
25 * classical dijkstra shortest path solver. The difference is that each future possible
26 * path is weighted from its 'shortest' (smallest) possible distance to destination,
27 * in addition to distance already walked. This heuristic allows more efficiency
28 * in finding optimal path.
29 *
30 * Implementation based on Wikipedia A* page:
31 * https://en.wikipedia.org/wiki/A*_search_algorithm
32 *
33 * Note that most memory handling here is done through two different MemArena's.
34 * Those should also be used to allocate
35 * custom data needed to a specific use of A*.
36 * The first one, owned by BLI_AStarGraph,
37 * is for 'static' data that will live as long as the graph.
38 * The second one, owned by BLI_AStarSolution, is for data used during a single path solve.
39 * It will be cleared much more often than graph's one.
40 */
41
42 #include <limits.h>
43
44 #include "MEM_guardedalloc.h"
45
46 #include "BLI_compiler_attrs.h"
47 #include "BLI_sys_types.h"
48
49 #include "BLI_heap_simple.h"
50 #include "BLI_listbase.h"
51 #include "BLI_math.h"
52 #include "BLI_memarena.h"
53
54 #include "BLI_astar.h"
55
56 /**
57 * Init a node in A* graph.
58 *
59 * \param custom_data: an opaque pointer attached to this link,
60 * available e.g. to cost callback function.
61 */
BLI_astar_node_init(BLI_AStarGraph * as_graph,const int node_index,void * custom_data)62 void BLI_astar_node_init(BLI_AStarGraph *as_graph, const int node_index, void *custom_data)
63 {
64 as_graph->nodes[node_index].custom_data = custom_data;
65 }
66
67 /**
68 * Add a link between two nodes of our A* graph.
69 *
70 * \param cost: the 'length' of the link
71 * (actual distance between two vertices or face centers e.g.).
72 * \param custom_data: an opaque pointer attached to this link,
73 * available e.g. to cost callback function.
74 */
BLI_astar_node_link_add(BLI_AStarGraph * as_graph,const int node1_index,const int node2_index,const float cost,void * custom_data)75 void BLI_astar_node_link_add(BLI_AStarGraph *as_graph,
76 const int node1_index,
77 const int node2_index,
78 const float cost,
79 void *custom_data)
80 {
81 MemArena *mem = as_graph->mem;
82 BLI_AStarGNLink *link = BLI_memarena_alloc(mem, sizeof(*link));
83 LinkData *ld = BLI_memarena_alloc(mem, sizeof(*ld) * 2);
84
85 link->nodes[0] = node1_index;
86 link->nodes[1] = node2_index;
87 link->cost = cost;
88 link->custom_data = custom_data;
89
90 ld[0].data = ld[1].data = link;
91
92 BLI_addtail(&(as_graph->nodes[node1_index].neighbor_links), &ld[0]);
93 BLI_addtail(&(as_graph->nodes[node2_index].neighbor_links), &ld[1]);
94 }
95
96 /**
97 * \return The index of the other node of given link.
98 */
BLI_astar_node_link_other_node(BLI_AStarGNLink * lnk,const int idx)99 int BLI_astar_node_link_other_node(BLI_AStarGNLink *lnk, const int idx)
100 {
101 return (lnk->nodes[0] == idx) ? lnk->nodes[1] : lnk->nodes[0];
102 }
103
104 /**
105 * Initialize a solution data for given A* graph. Does not compute anything!
106 *
107 * \param custom_data: an opaque pointer attached to this link, available e.g
108 * . to cost callback function.
109 *
110 * \note BLI_AStarSolution stores nearly all data needed during solution compute.
111 */
BLI_astar_solution_init(BLI_AStarGraph * as_graph,BLI_AStarSolution * as_solution,void * custom_data)112 void BLI_astar_solution_init(BLI_AStarGraph *as_graph,
113 BLI_AStarSolution *as_solution,
114 void *custom_data)
115 {
116 MemArena *mem = as_solution->mem;
117 size_t node_num = (size_t)as_graph->node_num;
118
119 if (mem == NULL) {
120 mem = BLI_memarena_new(BLI_MEMARENA_STD_BUFSIZE, __func__);
121 as_solution->mem = mem;
122 }
123 /* else memarena should be cleared */
124
125 as_solution->steps = 0;
126 as_solution->prev_nodes = BLI_memarena_alloc(mem, sizeof(*as_solution->prev_nodes) * node_num);
127 as_solution->prev_links = BLI_memarena_alloc(mem, sizeof(*as_solution->prev_links) * node_num);
128
129 as_solution->custom_data = custom_data;
130
131 as_solution->done_nodes = BLI_BITMAP_NEW_MEMARENA(mem, node_num);
132 as_solution->g_costs = BLI_memarena_alloc(mem, sizeof(*as_solution->g_costs) * node_num);
133 as_solution->g_steps = BLI_memarena_alloc(mem, sizeof(*as_solution->g_steps) * node_num);
134 }
135
136 /**
137 * Clear given solution's data, but does not release its memory. Avoids having to recreate/allocate
138 * a memarena in loops, e.g.
139 *
140 * \note This *has to be called* between each path solving.
141 */
BLI_astar_solution_clear(BLI_AStarSolution * as_solution)142 void BLI_astar_solution_clear(BLI_AStarSolution *as_solution)
143 {
144 if (as_solution->mem) {
145 BLI_memarena_clear(as_solution->mem);
146 }
147
148 as_solution->steps = 0;
149 as_solution->prev_nodes = NULL;
150 as_solution->prev_links = NULL;
151
152 as_solution->custom_data = NULL;
153
154 as_solution->done_nodes = NULL;
155 as_solution->g_costs = NULL;
156 as_solution->g_steps = NULL;
157 }
158
159 /**
160 * Release the memory allocated for this solution.
161 */
BLI_astar_solution_free(BLI_AStarSolution * as_solution)162 void BLI_astar_solution_free(BLI_AStarSolution *as_solution)
163 {
164 if (as_solution->mem) {
165 BLI_memarena_free(as_solution->mem);
166 as_solution->mem = NULL;
167 }
168 }
169
170 /**
171 * Init an A* graph. Total number of nodes must be known.
172 *
173 * Nodes might be e.g. vertices, faces, ...
174 *
175 * \param custom_data: an opaque pointer attached to this link,
176 * available e.g. to cost callback function.
177 */
BLI_astar_graph_init(BLI_AStarGraph * as_graph,const int node_num,void * custom_data)178 void BLI_astar_graph_init(BLI_AStarGraph *as_graph, const int node_num, void *custom_data)
179 {
180 MemArena *mem = as_graph->mem;
181
182 if (mem == NULL) {
183 mem = BLI_memarena_new(BLI_MEMARENA_STD_BUFSIZE, __func__);
184 as_graph->mem = mem;
185 }
186 /* else memarena should be cleared */
187
188 as_graph->node_num = node_num;
189 as_graph->nodes = BLI_memarena_calloc(mem, sizeof(*as_graph->nodes) * (size_t)node_num);
190
191 as_graph->custom_data = custom_data;
192 }
193
BLI_astar_graph_free(BLI_AStarGraph * as_graph)194 void BLI_astar_graph_free(BLI_AStarGraph *as_graph)
195 {
196 if (as_graph->mem) {
197 BLI_memarena_free(as_graph->mem);
198 as_graph->mem = NULL;
199 }
200 }
201
202 /**
203 * Solve a path in given graph, using given 'cost' callback function.
204 *
205 * \param max_steps: maximum number of nodes the found path may have.
206 * Useful in performance-critical usages.
207 * If no path is found within given steps, returns false too.
208 * \return true if a path was found, false otherwise.
209 */
BLI_astar_graph_solve(BLI_AStarGraph * as_graph,const int node_index_src,const int node_index_dst,astar_f_cost f_cost_cb,BLI_AStarSolution * r_solution,const int max_steps)210 bool BLI_astar_graph_solve(BLI_AStarGraph *as_graph,
211 const int node_index_src,
212 const int node_index_dst,
213 astar_f_cost f_cost_cb,
214 BLI_AStarSolution *r_solution,
215 const int max_steps)
216 {
217 HeapSimple *todo_nodes;
218
219 BLI_bitmap *done_nodes = r_solution->done_nodes;
220 int *prev_nodes = r_solution->prev_nodes;
221 BLI_AStarGNLink **prev_links = r_solution->prev_links;
222 float *g_costs = r_solution->g_costs;
223 int *g_steps = r_solution->g_steps;
224
225 r_solution->steps = 0;
226 prev_nodes[node_index_src] = -1;
227 BLI_bitmap_set_all(done_nodes, false, as_graph->node_num);
228 copy_vn_fl(g_costs, as_graph->node_num, FLT_MAX);
229 g_costs[node_index_src] = 0.0f;
230 g_steps[node_index_src] = 0;
231
232 if (node_index_src == node_index_dst) {
233 return true;
234 }
235
236 todo_nodes = BLI_heapsimple_new();
237 BLI_heapsimple_insert(todo_nodes,
238 f_cost_cb(as_graph, r_solution, NULL, -1, node_index_src, node_index_dst),
239 POINTER_FROM_INT(node_index_src));
240
241 while (!BLI_heapsimple_is_empty(todo_nodes)) {
242 const int node_curr_idx = POINTER_AS_INT(BLI_heapsimple_pop_min(todo_nodes));
243 BLI_AStarGNode *node_curr = &as_graph->nodes[node_curr_idx];
244 LinkData *ld;
245
246 if (BLI_BITMAP_TEST(done_nodes, node_curr_idx)) {
247 /* Might happen, because we always add nodes to heap when evaluating them,
248 * without ever removing them. */
249 continue;
250 }
251
252 /* If we are limited in amount of steps to find a path, skip if we reached limit. */
253 if (max_steps && g_steps[node_curr_idx] > max_steps) {
254 continue;
255 }
256
257 if (node_curr_idx == node_index_dst) {
258 /* Success! Path found... */
259 r_solution->steps = g_steps[node_curr_idx] + 1;
260
261 BLI_heapsimple_free(todo_nodes, NULL);
262 return true;
263 }
264
265 BLI_BITMAP_ENABLE(done_nodes, node_curr_idx);
266
267 for (ld = node_curr->neighbor_links.first; ld; ld = ld->next) {
268 BLI_AStarGNLink *link = ld->data;
269 const int node_next_idx = BLI_astar_node_link_other_node(link, node_curr_idx);
270
271 if (!BLI_BITMAP_TEST(done_nodes, node_next_idx)) {
272 float g_cst = g_costs[node_curr_idx] + link->cost;
273
274 if (g_cst < g_costs[node_next_idx]) {
275 prev_nodes[node_next_idx] = node_curr_idx;
276 prev_links[node_next_idx] = link;
277 g_costs[node_next_idx] = g_cst;
278 g_steps[node_next_idx] = g_steps[node_curr_idx] + 1;
279 /* We might have this node already in heap, but since this 'instance'
280 * will be evaluated first, no problem. */
281 BLI_heapsimple_insert(
282 todo_nodes,
283 f_cost_cb(as_graph, r_solution, link, node_curr_idx, node_next_idx, node_index_dst),
284 POINTER_FROM_INT(node_next_idx));
285 }
286 }
287 }
288 }
289
290 BLI_heapsimple_free(todo_nodes, NULL);
291 return false;
292 }
293