path_agent_replan_selection.h

#pragma once
 
#include <tess/core/shape.h>
#include <tess/sim/path_agent.h>
#include <tess/sim/path_agent_tick.h>
 
#include <span>
#include <utility>
 
namespace tess::experimental {

template <typename CostIncreasedFn>
[[nodiscard]] auto request_replans_for_route_crossings(
    std::span<const PathAgentState> agents, const PathAgentRoutes& routes,
    CostIncreasedFn&& cost_increased, PathAgentReplanQueue& queue)
    -> std::size_t {
  std::size_t newly_queued = 0;
  for (std::size_t i = 0; i < agents.size(); ++i) {
    const auto& agent = agents[i];
    if (!agent.has_goal || agent.phase == PathAgentPhase::Unreachable ||
        i >= routes.routes.size()) {
      continue;
    }
    // An agent already waiting in the queue cannot be queued again --
    // `request` refuses it -- so scanning its route can only reach a
    // conclusion that is then discarded. Under a bounded planning
    // budget the backlog persists across repricings, so this is the
    // difference between rescanning the whole backlog every time and
    // scanning only what is new.
    if (queue.contains(i)) {
      continue;
    }
    const auto& route = routes.routes[i];
    if (!detail::has_next_step(agent.path_index, route.size())) {
      continue;
    }
    for (auto step = agent.path_index + 1; step < route.size(); ++step) {
      if (cost_increased(route[step])) {
        if (queue.request(i, agent)) {
          ++newly_queued;
        }
        break;
      }
    }
  }
  return newly_queued;
}
 
}  // namespace tess::experimental