path: root/source/blender/blenlib/intern/astar.c

/*
 * ***** 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 <limits.h>

#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;
}