Blender V5.3
astar.cc
Go to the documentation of this file.
1/* SPDX-FileCopyrightText: 2014 Blender Authors
2 *
3 * SPDX-License-Identifier: GPL-2.0-or-later */
4
26
27#include <algorithm>
28
29#include "BLI_sys_types.hh"
30
31#include "BLI_heap_simple.hh"
32#include "BLI_listbase.hh"
33#include "BLI_math_vector_c.hh"
34#include "BLI_memarena.hh"
35
36#include "BLI_astar.hh"
37
38namespace blender {
39
40void BLI_astar_node_init(BLI_AStarGraph *as_graph, const int node_index, void *custom_data)
41{
42 as_graph->nodes[node_index].custom_data = custom_data;
43}
44
46 const int node1_index,
47 const int node2_index,
48 const float cost,
49 void *custom_data)
50{
51 MemArena *mem = as_graph->mem;
54
55 link->nodes[0] = node1_index;
56 link->nodes[1] = node2_index;
57 link->cost = cost;
58 link->custom_data = custom_data;
59
60 ld[0].data = ld[1].data = link;
61
62 BLI_addtail(&(as_graph->nodes[node1_index].neighbor_links), &ld[0]);
63 BLI_addtail(&(as_graph->nodes[node2_index].neighbor_links), &ld[1]);
64}
65
67{
68 return (lnk->nodes[0] == idx) ? lnk->nodes[1] : lnk->nodes[0];
69}
70
72 BLI_AStarSolution *as_solution,
73 void *custom_data)
74{
75 MemArena *mem = as_solution->mem;
76 size_t node_num = size_t(as_graph->node_num);
77
78 if (mem == nullptr) {
80 as_solution->mem = mem;
81 }
82 /* else memarena should be cleared */
83
84 as_solution->steps = 0;
85 as_solution->prev_nodes = BLI_memarena_alloc<int>(mem, node_num);
86 as_solution->prev_links = BLI_memarena_alloc<BLI_AStarGNLink *>(mem, node_num);
87
88 as_solution->custom_data = custom_data;
89
90 as_solution->done_nodes = BLI_BITMAP_NEW_MEMARENA(mem, node_num);
91 as_solution->g_costs = BLI_memarena_alloc<float>(mem, node_num);
92 as_solution->g_steps = BLI_memarena_alloc<int>(mem, node_num);
93}
94
96{
97 if (as_solution->mem) {
98 BLI_memarena_clear(as_solution->mem);
99 }
100
101 as_solution->steps = 0;
102 as_solution->prev_nodes = nullptr;
103 as_solution->prev_links = nullptr;
104
105 as_solution->custom_data = nullptr;
106
107 as_solution->done_nodes = nullptr;
108 as_solution->g_costs = nullptr;
109 as_solution->g_steps = nullptr;
110}
111
113{
114 if (as_solution->mem) {
115 BLI_memarena_free(as_solution->mem);
116 as_solution->mem = nullptr;
117 }
118}
119
120void BLI_astar_graph_init(BLI_AStarGraph *as_graph, const int node_num, void *custom_data)
121{
122 MemArena *mem = as_graph->mem;
123
124 if (mem == nullptr) {
126 as_graph->mem = mem;
127 }
128 /* else memarena should be cleared */
129
130 as_graph->node_num = node_num;
131 as_graph->nodes = BLI_memarena_calloc<BLI_AStarGNode>(mem, size_t(node_num));
132
133 as_graph->custom_data = custom_data;
134}
135
137{
138 if (as_graph->mem) {
139 BLI_memarena_free(as_graph->mem);
140 as_graph->mem = nullptr;
141 }
142}
143
145 const int node_index_src,
146 const int node_index_dst,
147 astar_f_cost f_cost_cb,
148 BLI_AStarSolution *r_solution,
149 const int max_steps)
150{
151 HeapSimple *todo_nodes;
152
153 BLI_bitmap *done_nodes = r_solution->done_nodes;
154 int *prev_nodes = r_solution->prev_nodes;
155 BLI_AStarGNLink **prev_links = r_solution->prev_links;
156 float *g_costs = r_solution->g_costs;
157 int *g_steps = r_solution->g_steps;
158
159 r_solution->steps = 0;
160 prev_nodes[node_index_src] = -1;
161 BLI_bitmap_set_all(done_nodes, false, as_graph->node_num);
162 std::fill_n(g_costs, as_graph->node_num, FLT_MAX);
163 g_costs[node_index_src] = 0.0f;
164 g_steps[node_index_src] = 0;
165
166 if (node_index_src == node_index_dst) {
167 return true;
168 }
169
170 todo_nodes = BLI_heapsimple_new();
172 todo_nodes,
173 f_cost_cb(as_graph, r_solution, nullptr, -1, node_index_src, node_index_dst),
174 POINTER_FROM_INT(node_index_src));
175
176 while (!BLI_heapsimple_is_empty(todo_nodes)) {
177 const int node_curr_idx = POINTER_AS_INT(BLI_heapsimple_pop_min(todo_nodes));
178 BLI_AStarGNode *node_curr = &as_graph->nodes[node_curr_idx];
179 LinkData *ld;
180
181 if (BLI_BITMAP_TEST(done_nodes, node_curr_idx)) {
182 /* Might happen, because we always add nodes to heap when evaluating them,
183 * without ever removing them. */
184 continue;
185 }
186
187 /* If we are limited in amount of steps to find a path, skip if we reached limit. */
188 if (max_steps && g_steps[node_curr_idx] > max_steps) {
189 continue;
190 }
191
192 if (node_curr_idx == node_index_dst) {
193 /* Success! Path found... */
194 r_solution->steps = g_steps[node_curr_idx] + 1;
195
196 BLI_heapsimple_free(todo_nodes, nullptr);
197 return true;
198 }
199
200 BLI_BITMAP_ENABLE(done_nodes, node_curr_idx);
201
202 for (ld = node_curr->neighbor_links.first(); ld; ld = ld->next) {
203 BLI_AStarGNLink *link = static_cast<BLI_AStarGNLink *>(ld->data);
204 const int node_next_idx = BLI_astar_node_link_other_node(link, node_curr_idx);
205
206 if (!BLI_BITMAP_TEST(done_nodes, node_next_idx)) {
207 float g_cst = g_costs[node_curr_idx] + link->cost;
208
209 if (g_cst < g_costs[node_next_idx]) {
210 prev_nodes[node_next_idx] = node_curr_idx;
211 prev_links[node_next_idx] = link;
212 g_costs[node_next_idx] = g_cst;
213 g_steps[node_next_idx] = g_steps[node_curr_idx] + 1;
214 /* We might have this node already in heap, but since this 'instance'
215 * will be evaluated first, no problem. */
217 todo_nodes,
218 f_cost_cb(as_graph, r_solution, link, node_curr_idx, node_next_idx, node_index_dst),
219 POINTER_FROM_INT(node_next_idx));
220 }
221 }
222 }
223 }
224
225 BLI_heapsimple_free(todo_nodes, nullptr);
226 return false;
227}
228
229} // namespace blender
An implementation of the A* (AStar) algorithm to solve shortest path problem.
#define BLI_BITMAP_TEST(_bitmap, _index)
Definition BLI_bitmap.hh:63
#define BLI_BITMAP_ENABLE(_bitmap, _index)
Definition BLI_bitmap.hh:80
#define BLI_BITMAP_NEW_MEMARENA(_mem, _num)
Definition BLI_bitmap.hh:51
A min-heap / priority queue ADT.
#define BLI_MEMARENA_STD_BUFSIZE
#define POINTER_FROM_INT(i)
#define POINTER_AS_INT(i)
#define FLT_MAX
BMesh Iterators.
void BLI_astar_node_init(BLI_AStarGraph *as_graph, int node_index, void *custom_data)
Definition astar.cc:40
MemArena * BLI_memarena_new(size_t bufsize, const char *name) ATTR_WARN_UNUSED_RESULT ATTR_RETURNS_NONNULL ATTR_NONNULL(2) ATTR_MALLOC
int BLI_astar_node_link_other_node(BLI_AStarGNLink *lnk, int idx)
Definition astar.cc:66
void * BLI_heapsimple_pop_min(HeapSimple *heap) ATTR_NONNULL(1)
bool BLI_astar_graph_solve(BLI_AStarGraph *as_graph, int node_index_src, int node_index_dst, astar_f_cost f_cost_cb, BLI_AStarSolution *r_solution, int max_steps)
Definition astar.cc:144
float(*)(BLI_AStarGraph *as_graph, BLI_AStarSolution *as_solution, BLI_AStarGNLink *link, int node_idx_curr, int node_idx_next, int node_idx_dst) astar_f_cost
Definition BLI_astar.hh:120
void BLI_astar_node_link_add(BLI_AStarGraph *as_graph, int node1_index, int node2_index, float cost, void *custom_data)
Definition astar.cc:45
void BLI_memarena_clear(MemArena *ma) ATTR_NONNULL(1)
void BLI_astar_solution_free(BLI_AStarSolution *as_solution)
Definition astar.cc:112
void BLI_heapsimple_free(HeapSimple *heap, HeapSimpleFreeFP ptrfreefp) ATTR_NONNULL(1)
HeapSimple * BLI_heapsimple_new() ATTR_WARN_UNUSED_RESULT
void * BLI_memarena_alloc(MemArena *ma, size_t size) ATTR_WARN_UNUSED_RESULT ATTR_NONNULL(1) ATTR_MALLOC ATTR_ALLOC_SIZE(2)
void BLI_heapsimple_insert(HeapSimple *heap, float value, void *ptr) ATTR_NONNULL(1)
void BLI_addtail(ListBase *listbase, void *vlink) ATTR_NONNULL(1)
Definition listbase.cc:118
unsigned int BLI_bitmap
Definition BLI_bitmap.hh:15
bool BLI_heapsimple_is_empty(const HeapSimple *heap) ATTR_NONNULL(1)
void BLI_astar_solution_init(BLI_AStarGraph *as_graph, BLI_AStarSolution *as_solution, void *custom_data)
Definition astar.cc:71
void BLI_bitmap_set_all(BLI_bitmap *bitmap, bool set, size_t bits)
Definition bitmap.cc:19
void BLI_astar_graph_init(BLI_AStarGraph *as_graph, int node_num, void *custom_data)
Definition astar.cc:120
void BLI_astar_solution_clear(BLI_AStarSolution *as_solution)
Definition astar.cc:95
void * BLI_memarena_calloc(MemArena *ma, size_t size) ATTR_WARN_UNUSED_RESULT ATTR_NONNULL(1) ATTR_MALLOC ATTR_ALLOC_SIZE(2)
void BLI_memarena_free(MemArena *ma) ATTR_NONNULL(1)
void BLI_astar_graph_free(BLI_AStarGraph *as_graph)
Definition astar.cc:136
ListBaseT< LinkData > neighbor_links
Definition BLI_astar.hh:28
BLI_AStarGNode * nodes
Definition BLI_astar.hh:57
struct MemArena * mem
Definition BLI_astar.hh:61
struct MemArena * mem
Definition BLI_astar.hh:52
BLI_AStarGNLink ** prev_links
Definition BLI_astar.hh:43
struct LinkData * next