Cleanup: comments (long lines) in blenlib
[blender.git] / source / blender / blenlib / intern / astar.c
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_sys_types.h"
47 #include "BLI_compiler_attrs.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  */
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  */
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  */
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  */
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  */
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  */
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  */
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
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  */
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 }