astar.cc

Go to the documentation of this file.

/* SPDX-FileCopyrightText: 2014 Blender Authors
 *
 * SPDX-License-Identifier: GPL-2.0-or-later */

 
#include "BLI_sys_types.h"
 
#include "BLI_heap_simple.h"
#include "BLI_listbase.h"
#include "BLI_math_vector.h"
#include "BLI_memarena.h"
 
#include "BLI_astar.h"
 
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;
}
 
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<BLI_AStarGNLink>(mem);
  LinkData *ld = BLI_memarena_alloc<LinkData>(mem, 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]);
}
 
int BLI_astar_node_link_other_node(BLI_AStarGNLink *lnk, const int idx)
{
  return (lnk->nodes[0] == idx) ? lnk->nodes[1] : lnk->nodes[0];
}
 
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 == nullptr) {
    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<int>(mem, node_num);
  as_solution->prev_links = BLI_memarena_alloc<BLI_AStarGNLink *>(mem, 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<float>(mem, node_num);
  as_solution->g_steps = BLI_memarena_alloc<int>(mem, node_num);
}
 
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 = nullptr;
  as_solution->prev_links = nullptr;
 
  as_solution->custom_data = nullptr;
 
  as_solution->done_nodes = nullptr;
  as_solution->g_costs = nullptr;
  as_solution->g_steps = nullptr;
}
 
void BLI_astar_solution_free(BLI_AStarSolution *as_solution)
{
  if (as_solution->mem) {
    BLI_memarena_free(as_solution->mem);
    as_solution->mem = nullptr;
  }
}
 
void BLI_astar_graph_init(BLI_AStarGraph *as_graph, const int node_num, void *custom_data)
{
  MemArena *mem = as_graph->mem;
 
  if (mem == nullptr) {
    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<BLI_AStarGNode>(mem, 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 = nullptr;
  }
}
 
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)
{
  HeapSimple *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);
  copy_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_heapsimple_new();
  BLI_heapsimple_insert(
      todo_nodes,
      f_cost_cb(as_graph, r_solution, nullptr, -1, node_index_src, node_index_dst),
      POINTER_FROM_INT(node_index_src));
 
  while (!BLI_heapsimple_is_empty(todo_nodes)) {
    const int node_curr_idx = POINTER_AS_INT(BLI_heapsimple_pop_min(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_heapsimple_free(todo_nodes, nullptr);
      return true;
    }
 
    BLI_BITMAP_ENABLE(done_nodes, node_curr_idx);
 
    for (ld = static_cast<LinkData *>(node_curr->neighbor_links.first); ld; ld = ld->next) {
      BLI_AStarGNLink *link = static_cast<BLI_AStarGNLink *>(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_heapsimple_insert(
              todo_nodes,
              f_cost_cb(as_graph, r_solution, link, node_curr_idx, node_next_idx, node_index_dst),
              POINTER_FROM_INT(node_next_idx));
        }
      }
    }
  }
 
  BLI_heapsimple_free(todo_nodes, nullptr);
  return false;
}