use std::iter::successors;
use crate::{
IndexPriorityQueue,
collections::{Queue, Stack},
graphs::{
EdgeWeightedGraph, WeightedDiGraph, WeightedDirectedEdge,
weighted::MinFloatTotalOrder,
},
};
pub struct ShortestPath;
#[inline(always)]
fn dijkstra_relax(
g: &WeightedDiGraph,
v: usize,
edge_to: &mut [Option<WeightedDirectedEdge>],
dist_to: &mut [f64],
pq: &mut IndexPriorityQueue<MinFloatTotalOrder>,
) -> Option<()> {
for e in g.adjacent(v) {
if e.weight() < 0. {
return None;
}
let w = e.to();
if dist_to[w] > dist_to[v] + e.weight() {
dist_to[w] = dist_to[v] + e.weight();
edge_to[w] = Some(e);
if pq.contains(w) {
pq.change_key(w, MinFloatTotalOrder(dist_to[w])).unwrap();
} else {
pq.insert(w, MinFloatTotalOrder(dist_to[w])).unwrap();
}
}
}
Some(())
}
#[inline(always)]
fn find_negative_cycle(
g: &WeightedDiGraph,
edge_to: &mut [Option<WeightedDirectedEdge>],
) -> Option<Stack<WeightedDirectedEdge>> {
let mut spt = WeightedDiGraph::new(g.vertices());
for e in edge_to.iter().flatten() {
spt.add_edge(*e);
}
let cycle = spt.get_cycle();
if cycle.is_empty() { None } else { Some(cycle) }
}
#[inline(always)]
fn bellman_ford_relax(
g: &WeightedDiGraph,
v: usize,
edge_to: &mut [Option<WeightedDirectedEdge>],
dist_to: &mut [f64],
on_q: &mut [bool],
queue: &mut Queue<usize>,
cost: &mut usize,
) -> Result<(), Stack<WeightedDirectedEdge>> {
for e in g.adjacent(v) {
let w = e.to();
if dist_to[w] > dist_to[v] + e.weight() {
dist_to[w] = dist_to[v] + e.weight();
edge_to[w] = Some(e);
if !on_q[w] {
queue.enqueue(w);
on_q[w] = true;
}
}
*cost += 1;
if cost.is_multiple_of(g.vertices())
&& let Some(cycle) = find_negative_cycle(g, edge_to)
{
return Err(cycle);
}
}
Ok(())
}
#[derive(Debug, Clone, Copy)]
struct AStarNode(f64);
impl PartialEq for AStarNode {
fn eq(&self, other: &Self) -> bool {
self.0.total_cmp(&other.0).is_eq()
}
}
impl Eq for AStarNode {}
impl PartialOrd for AStarNode {
fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
Some(self.cmp(other))
}
}
impl Ord for AStarNode {
fn cmp(&self, other: &Self) -> std::cmp::Ordering {
self.0.total_cmp(&other.0).reverse() }
}
#[inline(always)]
fn a_star_relax<F>(
g: &WeightedDiGraph,
v: usize,
edge_to: &mut [Option<WeightedDirectedEdge>],
dist_to: &mut [f64],
pq: &mut IndexPriorityQueue<AStarNode>,
heuristic: &F,
) -> Option<()>
where
F: Fn(usize) -> f64,
{
for e in g.adjacent(v) {
if e.weight() < 0. {
return None;
}
let w = e.to();
if dist_to[w] > dist_to[v] + e.weight() {
dist_to[w] = dist_to[v] + e.weight();
edge_to[w] = Some(e);
if pq.contains(w) {
pq.change_key(w, AStarNode(dist_to[w] + heuristic(w)))
.unwrap();
} else {
pq.insert(w, AStarNode(dist_to[w] + heuristic(w))).unwrap();
}
}
}
Some(())
}
#[must_use = "The result from Dijkstra takes compute, and is useless on its own."]
pub struct DijkstraResult(Vec<Option<WeightedDirectedEdge>>, Vec<f64>);
impl DijkstraResult {
pub fn dist_to(&self, v: usize) -> f64 {
self.1[v]
}
pub fn has_path_to(&self, v: usize) -> bool {
self.0[v].is_some()
}
pub fn path_to(
&self,
v: usize,
) -> impl Iterator<Item = WeightedDirectedEdge> {
successors(self.0[v], |e| self.0[e.from()])
}
}
#[must_use = "The result from Bellman Ford takes compute, and is useless on its own."]
pub struct BellmanFordResult(
Vec<Option<WeightedDirectedEdge>>,
Vec<f64>,
Option<Stack<WeightedDirectedEdge>>,
);
impl BellmanFordResult {
pub fn dist_to(&self, v: usize) -> f64 {
self.1[v]
}
pub fn has_path_to(&self, v: usize) -> bool {
self.0[v].is_some()
}
pub fn has_negative_cycle(&self) -> bool {
self.2.is_some()
}
pub fn get_negative_cycle(
&self,
) -> Option<impl Iterator<Item = WeightedDirectedEdge>> {
self.2.as_ref().map(|f| f.iter().copied())
}
pub fn path_to(
&self,
v: usize,
) -> impl Iterator<Item = WeightedDirectedEdge> {
assert!(
!self.has_negative_cycle(),
"This has a negative cycle, invalidating all paths"
);
successors(self.0[v], |e| self.0[e.from()])
.collect::<Vec<_>>()
.into_iter()
.rev()
}
}
#[must_use = "The result from A* takes compute, and is useless on its own."]
pub struct AStarResult(std::vec::IntoIter<WeightedDirectedEdge>);
impl Iterator for AStarResult {
type Item = WeightedDirectedEdge;
fn next(&mut self) -> Option<Self::Item> {
self.0.next()
}
fn size_hint(&self) -> (usize, Option<usize>) {
self.0.size_hint()
}
}
impl ExactSizeIterator for AStarResult {}
impl DoubleEndedIterator for AStarResult {
fn next_back(&mut self) -> Option<Self::Item> {
self.0.next_back()
}
}
impl ShortestPath {
pub fn dijkstra(
graph: &WeightedDiGraph,
start: usize,
) -> Option<DijkstraResult> {
let mut edge_to = vec![None; graph.vertices()];
let mut dist_to = vec![f64::INFINITY; graph.vertices()];
let mut pq = IndexPriorityQueue::new(graph.vertices());
dist_to[start] = 0.;
pq.insert(start, MinFloatTotalOrder(0.)).unwrap();
while let Some(v) = pq.pop() {
dijkstra_relax(graph, v, &mut edge_to, &mut dist_to, &mut pq)?;
}
Some(DijkstraResult(edge_to, dist_to))
}
pub fn bellman_ford(
graph: &WeightedDiGraph,
start: usize,
) -> BellmanFordResult {
let mut edge_to = vec![None; graph.vertices()];
let mut dist_to = vec![f64::INFINITY; graph.vertices()];
let mut on_q = vec![false; graph.vertices()];
let mut queue = Queue::new();
let mut cost = 0;
let mut cycle = None;
dist_to[start] = 0.;
queue.enqueue(start);
on_q[start] = true;
while let Some(v) = queue.dequeue()
&& cycle.is_none()
{
on_q[v] = false;
if let Err(x) = bellman_ford_relax(
graph,
v,
&mut edge_to,
&mut dist_to,
&mut on_q,
&mut queue,
&mut cost,
) {
cycle = Some(x);
}
}
BellmanFordResult(edge_to, dist_to, cycle)
}
pub fn a_star<F>(
graph: &WeightedDiGraph,
start: usize,
end: usize,
heuristic: &F,
) -> Option<AStarResult>
where
F: Fn(usize) -> f64,
{
let mut edge_to = vec![None; graph.vertices()];
let mut dist_to = vec![f64::INFINITY; graph.vertices()];
let mut pq = IndexPriorityQueue::new(graph.vertices());
dist_to[start] = 0.;
pq.insert(start, AStarNode(heuristic(start))).unwrap();
while let Some(v) = pq.pop() {
let e: WeightedDirectedEdge = edge_to[v].unwrap();
debug_assert!(
dist_to[e.from()] + e.weight() >= dist_to[v],
"Heuristic function is NOT admissible."
);
if v == end {
break;
}
a_star_relax(
graph,
v,
&mut edge_to,
&mut dist_to,
&mut pq,
heuristic,
)?;
}
Some(AStarResult(
successors(edge_to[end], |e| edge_to[e.from()])
.collect::<Vec<_>>()
.into_iter(),
))
}
}