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