Blender  V3.3
astar.c
Go to the documentation of this file.
1 /* SPDX-License-Identifier: GPL-2.0-or-later
2  * Copyright 2014 Blender Foundation. All rights reserved. */
3 
26 #include <limits.h>
27 
28 #include "MEM_guardedalloc.h"
29 
30 #include "BLI_compiler_attrs.h"
31 #include "BLI_sys_types.h"
32 
33 #include "BLI_heap_simple.h"
34 #include "BLI_listbase.h"
35 #include "BLI_math.h"
36 #include "BLI_memarena.h"
37 
38 #include "BLI_astar.h"
39 
40 void 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;
52  BLI_AStarGNLink *link = BLI_memarena_alloc(mem, sizeof(*link));
53  LinkData *ld = BLI_memarena_alloc(mem, sizeof(*ld) * 2);
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 == NULL) {
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(mem, sizeof(*as_solution->prev_nodes) * node_num);
86  as_solution->prev_links = BLI_memarena_alloc(mem, sizeof(*as_solution->prev_links) * 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(mem, sizeof(*as_solution->g_costs) * node_num);
92  as_solution->g_steps = BLI_memarena_alloc(mem, sizeof(*as_solution->g_steps) * 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 = NULL;
103  as_solution->prev_links = NULL;
104 
105  as_solution->custom_data = NULL;
106 
107  as_solution->done_nodes = NULL;
108  as_solution->g_costs = NULL;
109  as_solution->g_steps = NULL;
110 }
111 
113 {
114  if (as_solution->mem) {
115  BLI_memarena_free(as_solution->mem);
116  as_solution->mem = NULL;
117  }
118 }
119 
120 void 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 == NULL) {
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(mem, sizeof(*as_graph->nodes) * (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 = NULL;
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  copy_vn_fl(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();
171  BLI_heapsimple_insert(todo_nodes,
172  f_cost_cb(as_graph, r_solution, NULL, -1, node_index_src, node_index_dst),
173  POINTER_FROM_INT(node_index_src));
174 
175  while (!BLI_heapsimple_is_empty(todo_nodes)) {
176  const int node_curr_idx = POINTER_AS_INT(BLI_heapsimple_pop_min(todo_nodes));
177  BLI_AStarGNode *node_curr = &as_graph->nodes[node_curr_idx];
178  LinkData *ld;
179 
180  if (BLI_BITMAP_TEST(done_nodes, node_curr_idx)) {
181  /* Might happen, because we always add nodes to heap when evaluating them,
182  * without ever removing them. */
183  continue;
184  }
185 
186  /* If we are limited in amount of steps to find a path, skip if we reached limit. */
187  if (max_steps && g_steps[node_curr_idx] > max_steps) {
188  continue;
189  }
190 
191  if (node_curr_idx == node_index_dst) {
192  /* Success! Path found... */
193  r_solution->steps = g_steps[node_curr_idx] + 1;
194 
195  BLI_heapsimple_free(todo_nodes, NULL);
196  return true;
197  }
198 
199  BLI_BITMAP_ENABLE(done_nodes, node_curr_idx);
200 
201  for (ld = node_curr->neighbor_links.first; ld; ld = ld->next) {
202  BLI_AStarGNLink *link = ld->data;
203  const int node_next_idx = BLI_astar_node_link_other_node(link, node_curr_idx);
204 
205  if (!BLI_BITMAP_TEST(done_nodes, node_next_idx)) {
206  float g_cst = g_costs[node_curr_idx] + link->cost;
207 
208  if (g_cst < g_costs[node_next_idx]) {
209  prev_nodes[node_next_idx] = node_curr_idx;
210  prev_links[node_next_idx] = link;
211  g_costs[node_next_idx] = g_cst;
212  g_steps[node_next_idx] = g_steps[node_curr_idx] + 1;
213  /* We might have this node already in heap, but since this 'instance'
214  * will be evaluated first, no problem. */
216  todo_nodes,
217  f_cost_cb(as_graph, r_solution, link, node_curr_idx, node_next_idx, node_index_dst),
218  POINTER_FROM_INT(node_next_idx));
219  }
220  }
221  }
222  }
223 
224  BLI_heapsimple_free(todo_nodes, NULL);
225  return false;
226 }
An implementation of the A* (AStar) algorithm to solve shortest path problem.
float(* astar_f_cost)(BLI_AStarGraph *as_graph, BLI_AStarSolution *as_solution, BLI_AStarGNLink *link, int node_idx_curr, int node_idx_next, int node_idx_dst)
Definition: BLI_astar.h:119
#define BLI_BITMAP_TEST(_bitmap, _index)
Definition: BLI_bitmap.h:64
#define BLI_BITMAP_ENABLE(_bitmap, _index)
Definition: BLI_bitmap.h:81
#define BLI_BITMAP_NEW_MEMARENA(_mem, _num)
Definition: BLI_bitmap.h:52
void BLI_bitmap_set_all(BLI_bitmap *bitmap, bool set, size_t bits)
Definition: bitmap.c:17
unsigned int BLI_bitmap
Definition: BLI_bitmap.h:16
A min-heap / priority queue ADT.
void BLI_heapsimple_free(HeapSimple *heap, HeapSimpleFreeFP ptrfreefp) ATTR_NONNULL(1)
HeapSimple * BLI_heapsimple_new(void) ATTR_WARN_UNUSED_RESULT
void * BLI_heapsimple_pop_min(HeapSimple *heap) ATTR_NONNULL(1)
bool BLI_heapsimple_is_empty(const HeapSimple *heap) ATTR_NONNULL(1)
void BLI_heapsimple_insert(HeapSimple *heap, float value, void *ptr) ATTR_NONNULL(1)
void BLI_addtail(struct ListBase *listbase, void *vlink) ATTR_NONNULL(1)
Definition: listbase.c:80
void copy_vn_fl(float *array_tar, int size, float val)
Definition: math_vector.c:1259
void BLI_memarena_free(struct MemArena *ma) ATTR_NONNULL(1)
Definition: BLI_memarena.c:94
struct MemArena * BLI_memarena_new(size_t bufsize, const char *name) ATTR_WARN_UNUSED_RESULT ATTR_RETURNS_NONNULL ATTR_NONNULL(2) ATTR_MALLOC
Definition: BLI_memarena.c:64
#define BLI_MEMARENA_STD_BUFSIZE
Definition: BLI_memarena.h:20
void * BLI_memarena_alloc(struct MemArena *ma, size_t size) ATTR_WARN_UNUSED_RESULT ATTR_NONNULL(1) ATTR_MALLOC ATTR_ALLOC_SIZE(2)
Definition: BLI_memarena.c:116
void void BLI_memarena_clear(MemArena *ma) ATTR_NONNULL(1)
Definition: BLI_memarena.c:208
void * BLI_memarena_calloc(struct MemArena *ma, size_t size) ATTR_WARN_UNUSED_RESULT ATTR_NONNULL(1) ATTR_MALLOC ATTR_ALLOC_SIZE(2)
Definition: BLI_memarena.c:153
#define POINTER_FROM_INT(i)
#define POINTER_AS_INT(i)
Read Guarded memory(de)allocation.
void BLI_astar_graph_init(BLI_AStarGraph *as_graph, const int node_num, void *custom_data)
Definition: astar.c:120
void BLI_astar_solution_clear(BLI_AStarSolution *as_solution)
Definition: astar.c:95
void BLI_astar_node_init(BLI_AStarGraph *as_graph, const int node_index, void *custom_data)
Definition: astar.c:40
void BLI_astar_solution_init(BLI_AStarGraph *as_graph, BLI_AStarSolution *as_solution, void *custom_data)
Definition: astar.c:71
void BLI_astar_node_link_add(BLI_AStarGraph *as_graph, const int node1_index, const int node2_index, const float cost, void *custom_data)
Definition: astar.c:45
bool 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)
Definition: astar.c:144
void BLI_astar_solution_free(BLI_AStarSolution *as_solution)
Definition: astar.c:112
void BLI_astar_graph_free(BLI_AStarGraph *as_graph)
Definition: astar.c:136
int BLI_astar_node_link_other_node(BLI_AStarGNLink *lnk, const int idx)
Definition: astar.c:66
void * custom_data
Definition: BLI_astar.h:31
struct ListBase neighbor_links
Definition: BLI_astar.h:29
BLI_AStarGNode * nodes
Definition: BLI_astar.h:56
void * custom_data
Definition: BLI_astar.h:58
struct MemArena * mem
Definition: BLI_astar.h:60
BLI_AStarGNLink ** prev_links
Definition: BLI_astar.h:42
struct MemArena * mem
Definition: BLI_astar.h:51
float * g_costs
Definition: BLI_astar.h:48
void * custom_data
Definition: BLI_astar.h:44
BLI_bitmap * done_nodes
Definition: BLI_astar.h:47
void * data
Definition: DNA_listBase.h:26
struct LinkData * next
Definition: DNA_listBase.h:25
void * first
Definition: DNA_listBase.h:31