From 88ee44a9ac3676db44181c5dac9bab442c2a6e1a Mon Sep 17 00:00:00 2001 From: Bastien Montagne Date: Fri, 9 Jan 2015 10:56:17 +0100 Subject: BLI: add 'A*' (AStar) shortest path solver algorithm. Needed for transfer data. --- source/blender/blenlib/BLI_astar.h | 98 ++++++++++++ source/blender/blenlib/CMakeLists.txt | 2 + source/blender/blenlib/intern/astar.c | 294 ++++++++++++++++++++++++++++++++++ 3 files changed, 394 insertions(+) create mode 100644 source/blender/blenlib/BLI_astar.h create mode 100644 source/blender/blenlib/intern/astar.c (limited to 'source/blender/blenlib') diff --git a/source/blender/blenlib/BLI_astar.h b/source/blender/blenlib/BLI_astar.h new file mode 100644 index 00000000000..b99a2534d33 --- /dev/null +++ b/source/blender/blenlib/BLI_astar.h @@ -0,0 +1,98 @@ +/* + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * + * The Original Code is Copyright (C) 2014 Blender Foundation. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): Bastien Montagne. + * + * ***** END GPL LICENSE BLOCK ***** + */ + +#ifndef __BLI_ASTAR_H__ +#define __BLI_ASTAR_H__ + +/** \file BLI_astar.h + * \ingroup bli + * \brief An implementation of the A* (AStar) algorithm to solve shortest path problem. + */ + +#include "BLI_utildefines.h" + +#include "BLI_bitmap.h" + +/* -------------------------------------------------------------------- */ + +typedef struct BLI_AStarGNLink { + int nodes[2]; + float cost; + + void *custom_data; +} BLI_AStarGNLink; + +typedef struct BLI_AStarGNode { + struct ListBase neighbor_links; + + void *custom_data; +} BLI_AStarGNode; + +typedef struct BLI_AStarSolution { + /* Final 'most useful' data. */ + int steps; /* Number of steps (i.e. walked links) in path (nodes num, including start and end, is steps + 1). */ + int *prev_nodes; /* Store the path, in reversed order (from destination to source node), as indices. */ + BLI_AStarGNLink **prev_links; /* Indices are nodes' ones, as prev_nodes, but they map to relevant link. */ + + void *custom_data; + + /* Mostly runtime data. */ + BLI_bitmap *done_nodes; + float *g_costs; + int *g_steps; + + struct MemArena *mem; /* Memory arena. */ +} BLI_AStarSolution; + +typedef struct BLI_AStarGraph { + int node_num; + BLI_AStarGNode *nodes; + + void *custom_data; + + struct MemArena *mem; /* Memory arena. */ +} BLI_AStarGraph; + +void BLI_astar_node_init(BLI_AStarGraph *as_graph, const int node_index, void *custom_data); +void BLI_astar_node_link_add( + BLI_AStarGraph *as_graph, const int node1_index, const int node2_index, const float cost, void *custom_data); +int BLI_astar_node_link_other_node(BLI_AStarGNLink *lnk, const int idx); + +void BLI_astar_solution_init(BLI_AStarGraph *as_graph, BLI_AStarSolution *as_solution, void *custom_data); +void BLI_astar_solution_clear(BLI_AStarSolution *as_solution); +void BLI_astar_solution_free(BLI_AStarSolution *as_solution); + +typedef float (*astar_f_cost)(BLI_AStarGraph *as_graph, BLI_AStarSolution *as_solution, BLI_AStarGNLink *link, + const int node_idx_curr, const int node_idx_next, const int node_idx_dst); + +void BLI_astar_graph_init(BLI_AStarGraph *as_graph, const int node_num, void *custom_data); +void BLI_astar_graph_free(BLI_AStarGraph *as_graph); +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); + +#endif /* __BLI_ASTAR_H__ */ diff --git a/source/blender/blenlib/CMakeLists.txt b/source/blender/blenlib/CMakeLists.txt index c8f0e1bf26b..fa08856517a 100644 --- a/source/blender/blenlib/CMakeLists.txt +++ b/source/blender/blenlib/CMakeLists.txt @@ -52,6 +52,7 @@ set(SRC intern/BLI_memarena.c intern/BLI_mempool.c intern/DLRB_tree.c + intern/astar.c intern/boxpack2d.c intern/buffer.c intern/callbacks.c @@ -113,6 +114,7 @@ set(SRC BLI_alloca.h BLI_args.h BLI_array.h + BLI_astar.h BLI_bitmap.h BLI_blenlib.h BLI_boxpack2d.h diff --git a/source/blender/blenlib/intern/astar.c b/source/blender/blenlib/intern/astar.c new file mode 100644 index 00000000000..b7b41d2497e --- /dev/null +++ b/source/blender/blenlib/intern/astar.c @@ -0,0 +1,294 @@ +/* + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * + * The Original Code is Copyright (C) 2014 Blender Foundation. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): Bastien Montagne + * + * ***** END GPL LICENSE BLOCK ***** + */ + +/** \file blender/blenlib/intern/astar.c + * \ingroup bli + * \brief An implementation of the A* (AStar) algorithm to solve shortest path problem. + * + * This library implements the simple A* (AStar) algorithm, an optimized version of + * classical dijkstra shortest path solver. The difference is that each future possible + * path is weighted from its 'shortest' (smallest) possible distance to destination, + * in addition to distance already walked. This heuristic allows more efficiency + * in finding optimal path. + * + * Implementation based on Wikipedia A* page [http://en.wikipedia.org/wiki/A*_search_algorithm]. + * + * Note that most memory handling here is done through two different MemArena's. Those should also be used to allocate + * custom data needed to a specific use of A*. + * The first one, owned by BLI_AStarGraph, is for 'static' data that will live as long as the graph. + * The second one, owned by BLI_AStarSolution, is for data used during a single path solve. It will be cleared + * much more often than graph's one. + */ + +#include + +#include "MEM_guardedalloc.h" + +#include "BLI_sys_types.h" +#include "BLI_compiler_attrs.h" + +#include "BLI_alloca.h" +#include "BLI_heap.h" +#include "BLI_listbase.h" +#include "BLI_math.h" +#include "BLI_memarena.h" + +#include "BLI_astar.h" + +/** + * Init a node in A* graph. + * + * \param custom_data an opaque pointer attached to this link, available e.g. to cost callback function. + */ +void BLI_astar_node_init(BLI_AStarGraph *as_graph, const int node_index, void *custom_data) +{ + as_graph->nodes[node_index].custom_data = custom_data; +} + +/** + * Add a link between two nodes of our A* graph. + * + * \param cost the 'length' of the link (actual distance between two vertices or face centers e.g.). + * \param custom_data an opaque pointer attached to this link, available e.g. to cost callback function. + */ +void BLI_astar_node_link_add( + BLI_AStarGraph *as_graph, const int node1_index, const int node2_index, const float cost, void *custom_data) +{ + MemArena *mem = as_graph->mem; + BLI_AStarGNLink *link = BLI_memarena_alloc(mem, sizeof(*link)); + LinkData *ld = BLI_memarena_alloc(mem, sizeof(*ld) * 2); + + link->nodes[0] = node1_index; + link->nodes[1] = node2_index; + link->cost = cost; + link->custom_data = custom_data; + + ld[0].data = ld[1].data = link; + + BLI_addtail(&(as_graph->nodes[node1_index].neighbor_links), &ld[0]); + BLI_addtail(&(as_graph->nodes[node2_index].neighbor_links), &ld[1]); +} + +/** + * \return The index of the other node of given link. + */ +int BLI_astar_node_link_other_node(BLI_AStarGNLink *lnk, const int idx) +{ + return (lnk->nodes[0] == idx) ? lnk->nodes[1] : lnk->nodes[0]; +} + +/** + * Initialize a solution data for given A* graph. Does not compute anything! + * + * \param custom_data an opaque pointer attached to this link, available e.g. to cost callback function. + * + * \note BLI_AStarSolution stores nearly all data needed during solution compute. + */ +void BLI_astar_solution_init(BLI_AStarGraph *as_graph, BLI_AStarSolution *as_solution, void *custom_data) +{ + MemArena *mem = as_solution->mem; + size_t node_num = (size_t)as_graph->node_num; + + if (mem == NULL) { + mem = BLI_memarena_new(BLI_MEMARENA_STD_BUFSIZE, __func__); + as_solution->mem = mem; + } + /* else memarena should be cleared */ + + as_solution->steps = 0; + as_solution->prev_nodes = BLI_memarena_alloc(mem, sizeof(*as_solution->prev_nodes) * node_num); + as_solution->prev_links = BLI_memarena_alloc(mem, sizeof(*as_solution->prev_links) * node_num); + + as_solution->custom_data = custom_data; + + as_solution->done_nodes = BLI_BITMAP_NEW_MEMARENA(mem, node_num); + as_solution->g_costs = BLI_memarena_alloc(mem, sizeof(*as_solution->g_costs) * node_num); + as_solution->g_steps = BLI_memarena_alloc(mem, sizeof(*as_solution->g_steps) * node_num); +} + +/** + * Clear given solution's data, but does not release its memory. Avoids having to recreate/allocate + * a memarena in loops, e.g. + * + * \note This *has to be called* between each path solving. + */ +void BLI_astar_solution_clear(BLI_AStarSolution *as_solution) +{ + if (as_solution->mem) { + BLI_memarena_clear(as_solution->mem); + } + + as_solution->steps = 0; + as_solution->prev_nodes = NULL; + as_solution->prev_links = NULL; + + as_solution->custom_data = NULL; + + as_solution->done_nodes = NULL; + as_solution->g_costs = NULL; + as_solution->g_steps = NULL; +} + +/** + * Release the memory allocated for this solution. + */ +void BLI_astar_solution_free(BLI_AStarSolution *as_solution) +{ + if (as_solution->mem) { + BLI_memarena_free(as_solution->mem); + as_solution->mem = NULL; + } +} + +/** + * Callback computing the current cost (distance) to next node, and the estimated overall cost to destination node + * (A* expects this estimation to always be less or equal than actual shortest path from next node to destination one). + * + * \param link the graph link between current node and next one. + * \param node_idx_curr current node index. + * \param node_idx_next next node index. + * \param node_idx_dst destination node index. + */ +typedef float (*astar_f_cost)(BLI_AStarGraph *as_graph, BLI_AStarSolution *as_solution, BLI_AStarGNLink *link, + const int node_idx_curr, const int node_idx_next, const int node_idx_dst); + +/** + * Init an A* graph. Total number of nodes must be known. + * + * Nodes might be e.g. vertices, faces, ... + * + * \param custom_data an opaque pointer attached to this link, available e.g. to cost callback function. + */ +void BLI_astar_graph_init(BLI_AStarGraph *as_graph, const int node_num, void *custom_data) +{ + MemArena *mem = as_graph->mem; + + if (mem == NULL) { + mem = BLI_memarena_new(BLI_MEMARENA_STD_BUFSIZE, __func__); + as_graph->mem = mem; + } + /* else memarena should be cleared */ + + as_graph->node_num = node_num; + as_graph->nodes = BLI_memarena_calloc(mem, sizeof(*as_graph->nodes) * (size_t)node_num); + + as_graph->custom_data = custom_data; +} + +void BLI_astar_graph_free(BLI_AStarGraph *as_graph) +{ + if (as_graph->mem) { + BLI_memarena_free(as_graph->mem); + as_graph->mem = NULL; + } +} + +/** + * Solve a path in given graph, using given 'cost' callback function. + * + * \param max_steps maximum number of nodes the found path may have. Useful in performance-critical usages. + * If no path is found within given steps, returns false too. + * \return true if a path was found, false otherwise. + */ +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) +{ + Heap *todo_nodes; + + BLI_bitmap *done_nodes = r_solution->done_nodes; + int *prev_nodes = r_solution->prev_nodes; + BLI_AStarGNLink **prev_links = r_solution->prev_links; + float *g_costs = r_solution->g_costs; + int *g_steps = r_solution->g_steps; + + r_solution->steps = 0; + prev_nodes[node_index_src] = -1; + BLI_BITMAP_SET_ALL(done_nodes, false, as_graph->node_num); + fill_vn_fl(g_costs, as_graph->node_num, FLT_MAX); + g_costs[node_index_src] = 0.0f; + g_steps[node_index_src] = 0; + + if (node_index_src == node_index_dst) { + return true; + } + + todo_nodes = BLI_heap_new(); + BLI_heap_insert(todo_nodes, + f_cost_cb(as_graph, r_solution, NULL, -1, node_index_src, node_index_dst), + SET_INT_IN_POINTER(node_index_src)); + + while (!BLI_heap_is_empty(todo_nodes)) { + const int node_curr_idx = GET_INT_FROM_POINTER(BLI_heap_popmin(todo_nodes)); + BLI_AStarGNode *node_curr = &as_graph->nodes[node_curr_idx]; + LinkData *ld; + + if (BLI_BITMAP_TEST(done_nodes, node_curr_idx)) { + /* Might happen, because we always add nodes to heap when evaluating them, without ever removing them. */ + continue; + } + + /* If we are limited in amount of steps to find a path, skip if we reached limit. */ + if (max_steps && g_steps[node_curr_idx] > max_steps) { + continue; + } + + if (node_curr_idx == node_index_dst) { + /* Success! Path found... */ + r_solution->steps = g_steps[node_curr_idx] + 1; + + BLI_heap_free(todo_nodes, NULL); + return true; + } + + BLI_BITMAP_ENABLE(done_nodes, node_curr_idx); + + for (ld = node_curr->neighbor_links.first; ld; ld = ld->next) { + BLI_AStarGNLink *link = ld->data; + const int node_next_idx = BLI_astar_node_link_other_node(link, node_curr_idx); + + if (!BLI_BITMAP_TEST(done_nodes, node_next_idx)) { + float g_cst = g_costs[node_curr_idx] + link->cost; + + if (g_cst < g_costs[node_next_idx]) { + prev_nodes[node_next_idx] = node_curr_idx; + prev_links[node_next_idx] = link; + g_costs[node_next_idx] = g_cst; + g_steps[node_next_idx] = g_steps[node_curr_idx] + 1; + /* We might have this node already in heap, but since this 'instance' will be evaluated first, + * no problem. */ + BLI_heap_insert(todo_nodes, + f_cost_cb(as_graph, r_solution, link, node_curr_idx, node_next_idx, node_index_dst), + SET_INT_IN_POINTER(node_next_idx)); + } + } + } + } + + BLI_heap_free(todo_nodes, NULL); + return false; +} -- cgit v1.2.3