mirror of
https://github.com/mat-1/azalea.git
synced 2025-08-02 14:26:04 +00:00
delete azalea-pathfinder crate
already in azalea::pathfinder module
This commit is contained in:
parent
4669f73395
commit
04c8d8db9f
7 changed files with 0 additions and 792 deletions
18
Cargo.lock
generated
18
Cargo.lock
generated
|
|
@ -270,24 +270,6 @@ dependencies = [
|
|||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "azalea-pathfinder"
|
||||
version = "0.3.0"
|
||||
dependencies = [
|
||||
"anyhow",
|
||||
"async-trait",
|
||||
"azalea",
|
||||
"azalea-block",
|
||||
"azalea-client",
|
||||
"azalea-core",
|
||||
"azalea-physics",
|
||||
"azalea-world",
|
||||
"num-traits",
|
||||
"parking_lot",
|
||||
"priority-queue",
|
||||
"tokio",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "azalea-physics"
|
||||
version = "0.3.0"
|
||||
|
|
|
|||
|
|
@ -16,7 +16,6 @@ members = [
|
|||
"azalea-buf",
|
||||
"azalea-physics",
|
||||
"azalea-registry",
|
||||
"azalea-pathfinder",
|
||||
]
|
||||
|
||||
[profile.release]
|
||||
|
|
|
|||
|
|
@ -1,22 +0,0 @@
|
|||
[package]
|
||||
edition = "2021"
|
||||
name = "azalea-pathfinder"
|
||||
version = "0.3.0"
|
||||
|
||||
# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
|
||||
|
||||
[dependencies]
|
||||
anyhow = "1.0.65"
|
||||
async-trait = "0.1.57"
|
||||
azalea = { version = "0.3.0", path = "../azalea" }
|
||||
azalea-block = { version = "0.3.0", path = "../azalea-block" }
|
||||
azalea-client = { version = "0.3.0", path = "../azalea-client" }
|
||||
azalea-core = { version = "0.3.0", path = "../azalea-core" }
|
||||
azalea-physics = { version = "0.3.0", path = "../azalea-physics" }
|
||||
azalea-world = { version = "0.3.0", path = "../azalea-world" }
|
||||
num-traits = "0.2.15"
|
||||
parking_lot = {version = "0.12.1", features = ["deadlock_detection"]}
|
||||
priority-queue = "1.2.3"
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = "1.21.2"
|
||||
|
|
@ -1,9 +0,0 @@
|
|||
# Azalea Pathfinder
|
||||
|
||||
Depended on by `azalea` for pathfinding.
|
||||
|
||||
## How it works
|
||||
|
||||
The pathfinder uses the [Moving Target D* Lite](http://idm-lab.org/bib/abstracts/papers/aamas10a.pdf) pathfinding algorithm to determine a path from a starting point to a goal area. MT-D* Lite has the advantage of being able to replan the path when anything changes very efficiently, which is very useful for following a moving target.
|
||||
|
||||
|
||||
|
|
@ -1,158 +0,0 @@
|
|||
mod moves;
|
||||
mod mtdstarlite;
|
||||
|
||||
use async_trait::async_trait;
|
||||
use azalea::{prelude::*, WalkDirection};
|
||||
use azalea::{Client, Event};
|
||||
use azalea_core::BlockPos;
|
||||
use azalea_world::entity::EntityData;
|
||||
use mtdstarlite::Edge;
|
||||
pub use mtdstarlite::MTDStarLite;
|
||||
use parking_lot::Mutex;
|
||||
use std::collections::VecDeque;
|
||||
use std::sync::Arc;
|
||||
|
||||
#[derive(Default, Clone)]
|
||||
pub struct Plugin {
|
||||
pub state: State,
|
||||
}
|
||||
|
||||
#[derive(Default, Clone)]
|
||||
pub struct State {
|
||||
// pathfinder: Option<MTDStarLite<Node, f32>>,
|
||||
pub path: Arc<Mutex<VecDeque<Node>>>,
|
||||
}
|
||||
|
||||
#[async_trait]
|
||||
impl azalea::Plugin for Plugin {
|
||||
async fn handle(self: Box<Self>, event: Event, mut bot: Client) {
|
||||
let mut path = self.state.path.lock();
|
||||
|
||||
if !path.is_empty() {
|
||||
tick_execute_path(&mut bot, &mut path);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub trait Trait {
|
||||
fn goto(&self, goal: impl Goal);
|
||||
}
|
||||
|
||||
impl Trait for azalea_client::Client {
|
||||
fn goto(&self, goal: impl Goal) {
|
||||
let start = Node {
|
||||
pos: BlockPos::from(self.entity().pos()),
|
||||
};
|
||||
let end = goal.goal_node();
|
||||
println!("start: {:?}, end: {:?}", start, end);
|
||||
|
||||
let successors = |node: &Node| {
|
||||
println!("successors for {:?}", node);
|
||||
let mut edges = Vec::new();
|
||||
let possible_moves: Vec<&dyn moves::Move> = vec![
|
||||
&moves::NorthMove,
|
||||
&moves::SouthMove,
|
||||
&moves::EastMove,
|
||||
&moves::WestMove,
|
||||
];
|
||||
let dimension = self.dimension.read();
|
||||
for possible_move in possible_moves.iter() {
|
||||
let can_execute = possible_move.can_execute(&dimension, &node.pos);
|
||||
edges.push(Edge {
|
||||
target: Node {
|
||||
pos: node.pos + possible_move.offset(),
|
||||
},
|
||||
cost: if can_execute { 1.0 } else { f32::INFINITY },
|
||||
});
|
||||
println!("can execute for {:?}: {}", node, can_execute);
|
||||
}
|
||||
println!("edges: {}", edges.len());
|
||||
edges
|
||||
};
|
||||
|
||||
let mut pf = MTDStarLite::new(
|
||||
start,
|
||||
end,
|
||||
|n| goal.heuristic(n),
|
||||
successors,
|
||||
successors,
|
||||
|n| goal.success(n),
|
||||
);
|
||||
let p = pf.find_path();
|
||||
|
||||
let state = self.plugins.get::<Plugin>().unwrap().state.clone();
|
||||
// convert the Option<Vec<Node>> to a VecDeque<Node>
|
||||
*state.path.lock() = p.expect("no path").into_iter().collect();
|
||||
}
|
||||
}
|
||||
|
||||
fn tick_execute_path(bot: &mut Client, path: &mut VecDeque<Node>) {
|
||||
let target = if let Some(target) = path.front() {
|
||||
target
|
||||
} else {
|
||||
return;
|
||||
};
|
||||
let center = target.pos.center();
|
||||
println!("going to {center:?} (at {pos:?})", pos = bot.entity().pos());
|
||||
bot.look_at(¢er);
|
||||
bot.walk(WalkDirection::Forward);
|
||||
|
||||
if target.is_reached(&bot.entity()) {
|
||||
println!("ok target reached");
|
||||
path.pop_front();
|
||||
if path.is_empty() {
|
||||
bot.walk(WalkDirection::None);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Eq, PartialEq, Hash, Clone, Copy, Debug)]
|
||||
pub struct Node {
|
||||
pub pos: BlockPos,
|
||||
}
|
||||
|
||||
pub trait Goal {
|
||||
fn heuristic(&self, n: &Node) -> f32;
|
||||
fn success(&self, n: &Node) -> bool;
|
||||
// TODO: this should be removed and mtdstarlite should stop depending on
|
||||
// being given a goal node
|
||||
fn goal_node(&self) -> Node;
|
||||
}
|
||||
|
||||
impl Node {
|
||||
/// Returns whether the entity is at the node and should start going to the
|
||||
/// next node.
|
||||
pub fn is_reached(&self, entity: &EntityData) -> bool {
|
||||
// println!(
|
||||
// "entity.yya: {} {:?}=={:?}",
|
||||
// entity.yya,
|
||||
// BlockPos::from(entity.pos()),
|
||||
// self.pos
|
||||
// );
|
||||
entity.yya == 0. && BlockPos::from(entity.pos()) == self.pos
|
||||
}
|
||||
}
|
||||
|
||||
pub struct BlockPosGoal {
|
||||
pub pos: BlockPos,
|
||||
}
|
||||
impl Goal for BlockPosGoal {
|
||||
fn heuristic(&self, n: &Node) -> f32 {
|
||||
let dx = (self.pos.x - n.pos.x) as f32;
|
||||
let dy = (self.pos.y - n.pos.y) as f32;
|
||||
let dz = (self.pos.z - n.pos.z) as f32;
|
||||
dx * dx + dy * dy + dz * dz
|
||||
}
|
||||
fn success(&self, n: &Node) -> bool {
|
||||
n.pos == self.pos
|
||||
}
|
||||
fn goal_node(&self) -> Node {
|
||||
Node { pos: self.pos }
|
||||
}
|
||||
}
|
||||
|
||||
impl From<BlockPos> for BlockPosGoal {
|
||||
fn from(pos: BlockPos) -> Self {
|
||||
Self { pos }
|
||||
}
|
||||
}
|
||||
|
|
@ -1,131 +0,0 @@
|
|||
use azalea_block::Block;
|
||||
use azalea_core::BlockPos;
|
||||
use azalea_physics::collision::{self, BlockWithShape};
|
||||
use azalea_world::Dimension;
|
||||
|
||||
/// whether this block is passable
|
||||
fn is_passable(pos: &BlockPos, dim: &Dimension) -> bool {
|
||||
if let Some(block) = dim.get_block_state(pos) {
|
||||
println!(
|
||||
"is passable {pos:?} {} = {}",
|
||||
Box::<dyn Block>::from(block).id(),
|
||||
block.shape() == &collision::empty_shape()
|
||||
);
|
||||
block.shape() == &collision::empty_shape()
|
||||
} else {
|
||||
false
|
||||
}
|
||||
}
|
||||
|
||||
/// whether this block has a solid hitbox (i.e. we can stand on it)
|
||||
fn is_solid(pos: &BlockPos, dim: &Dimension) -> bool {
|
||||
if let Some(block) = dim.get_block_state(pos) {
|
||||
println!(
|
||||
"is solid {pos:?} {} = {}",
|
||||
Box::<dyn Block>::from(block).id(),
|
||||
block.shape() == &collision::block_shape()
|
||||
);
|
||||
block.shape() == &collision::block_shape()
|
||||
} else {
|
||||
false
|
||||
}
|
||||
}
|
||||
|
||||
/// Whether we can stand in this position. Checks if the block below is solid,
|
||||
/// and that the two blocks above that are passable.
|
||||
fn is_standable(pos: &BlockPos, dim: &Dimension) -> bool {
|
||||
is_solid(&pos.down(1), dim) && is_passable(&pos, dim) && is_passable(&pos.up(1), dim)
|
||||
}
|
||||
|
||||
pub trait Move {
|
||||
fn can_execute(&self, dim: &Dimension, pos: &BlockPos) -> bool;
|
||||
/// Returns by how much the entity's position should be changed when this move is executed.
|
||||
fn offset(&self) -> BlockPos;
|
||||
}
|
||||
|
||||
pub struct NorthMove;
|
||||
impl Move for NorthMove {
|
||||
fn can_execute(&self, dim: &Dimension, pos: &BlockPos) -> bool {
|
||||
is_standable(&(pos + &self.offset()), dim)
|
||||
}
|
||||
fn offset(&self) -> BlockPos {
|
||||
BlockPos::new(0, 0, -1)
|
||||
}
|
||||
}
|
||||
|
||||
pub struct SouthMove;
|
||||
impl Move for SouthMove {
|
||||
fn can_execute(&self, dim: &Dimension, pos: &BlockPos) -> bool {
|
||||
is_standable(&(pos + &self.offset()), dim)
|
||||
}
|
||||
fn offset(&self) -> BlockPos {
|
||||
BlockPos::new(0, 0, 1)
|
||||
}
|
||||
}
|
||||
|
||||
pub struct EastMove;
|
||||
impl Move for EastMove {
|
||||
fn can_execute(&self, dim: &Dimension, pos: &BlockPos) -> bool {
|
||||
is_standable(&(pos + &self.offset()), dim)
|
||||
}
|
||||
fn offset(&self) -> BlockPos {
|
||||
BlockPos::new(1, 0, 0)
|
||||
}
|
||||
}
|
||||
|
||||
pub struct WestMove;
|
||||
impl Move for WestMove {
|
||||
fn can_execute(&self, dim: &Dimension, pos: &BlockPos) -> bool {
|
||||
is_standable(&(pos + &self.offset()), dim)
|
||||
}
|
||||
fn offset(&self) -> BlockPos {
|
||||
BlockPos::new(-1, 0, 0)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use azalea_block::BlockState;
|
||||
use azalea_core::ChunkPos;
|
||||
use azalea_world::Chunk;
|
||||
|
||||
#[test]
|
||||
fn test_is_passable() {
|
||||
let mut dim = Dimension::default();
|
||||
dim.set_chunk(&ChunkPos { x: 0, z: 0 }, Some(Chunk::default()))
|
||||
.unwrap();
|
||||
dim.set_block_state(&BlockPos::new(0, 0, 0), BlockState::Stone);
|
||||
dim.set_block_state(&BlockPos::new(0, 1, 0), BlockState::Air);
|
||||
|
||||
assert_eq!(is_passable(&BlockPos::new(0, 0, 0), &dim), false);
|
||||
assert_eq!(is_passable(&BlockPos::new(0, 1, 0), &dim), true);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_is_solid() {
|
||||
let mut dim = Dimension::default();
|
||||
dim.set_chunk(&ChunkPos { x: 0, z: 0 }, Some(Chunk::default()))
|
||||
.unwrap();
|
||||
dim.set_block_state(&BlockPos::new(0, 0, 0), BlockState::Stone);
|
||||
dim.set_block_state(&BlockPos::new(0, 1, 0), BlockState::Air);
|
||||
|
||||
assert_eq!(is_solid(&BlockPos::new(0, 0, 0), &dim), true);
|
||||
assert_eq!(is_solid(&BlockPos::new(0, 1, 0), &dim), false);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_is_standable() {
|
||||
let mut dim = Dimension::default();
|
||||
dim.set_chunk(&ChunkPos { x: 0, z: 0 }, Some(Chunk::default()))
|
||||
.unwrap();
|
||||
dim.set_block_state(&BlockPos::new(0, 0, 0), BlockState::Stone);
|
||||
dim.set_block_state(&BlockPos::new(0, 1, 0), BlockState::Air);
|
||||
dim.set_block_state(&BlockPos::new(0, 2, 0), BlockState::Air);
|
||||
dim.set_block_state(&BlockPos::new(0, 3, 0), BlockState::Air);
|
||||
|
||||
assert!(is_standable(&BlockPos::new(0, 1, 0), &dim));
|
||||
assert!(!is_standable(&BlockPos::new(0, 0, 0), &dim));
|
||||
assert!(!is_standable(&BlockPos::new(0, 2, 0), &dim));
|
||||
}
|
||||
}
|
||||
|
|
@ -1,453 +0,0 @@
|
|||
//! An implementation of Moving Target D* Lite as described in
|
||||
//! <http://idm-lab.org/bib/abstracts/papers/aamas10a.pdf>
|
||||
//!
|
||||
//! Future optimization attempt ideas:
|
||||
//! - Use a different priority queue (e.g. fibonacci heap)
|
||||
//! - Use FxHash instead of the default hasher
|
||||
//! - Have `par` be a raw pointer
|
||||
//! - Try borrowing vs copying the Node in several places (like state_mut)
|
||||
//! - Store edge costs in their own map
|
||||
|
||||
use priority_queue::DoublePriorityQueue;
|
||||
use std::{collections::HashMap, fmt::Debug, hash::Hash, ops::Add};
|
||||
|
||||
/// Nodes are coordinates.
|
||||
pub struct MTDStarLite<
|
||||
N: Eq + Hash + Copy + Debug,
|
||||
W: PartialOrd + Default + Copy + num_traits::Bounded + Debug,
|
||||
HeuristicFn: Fn(&N) -> W,
|
||||
SuccessorsFn: Fn(&N) -> Vec<Edge<N, W>>,
|
||||
PredecessorsFn: Fn(&N) -> Vec<Edge<N, W>>,
|
||||
SuccessFn: Fn(&N) -> bool,
|
||||
> {
|
||||
/// Returns a rough estimate of how close we are to the goal. Lower = closer.
|
||||
pub heuristic: HeuristicFn,
|
||||
/// Returns the nodes that can be reached from the given node.
|
||||
pub successors: SuccessorsFn,
|
||||
/// Returns the nodes that would direct us to the given node. If the graph
|
||||
/// isn't directed (i.e. you can always return to the previous node), this
|
||||
/// can be the same as `successors`.
|
||||
pub predecessors: PredecessorsFn,
|
||||
/// Returns true if the given node is at the goal.
|
||||
/// A simple implementation is to check if the given node is equal to the goal.
|
||||
pub success: SuccessFn,
|
||||
|
||||
start: N,
|
||||
goal: N,
|
||||
|
||||
old_start: N,
|
||||
old_goal: N,
|
||||
|
||||
k_m: W,
|
||||
open: DoublePriorityQueue<N, Priority<W>>,
|
||||
node_states: HashMap<N, NodeState<N, W>>,
|
||||
updated_edge_costs: Vec<ChangedEdge<N, W>>,
|
||||
|
||||
/// This only exists so it can be referenced by `state()` when there's no state.
|
||||
default_state: NodeState<N, W>,
|
||||
}
|
||||
|
||||
impl<
|
||||
N: Eq + Hash + Copy + Debug,
|
||||
W: PartialOrd + Add<Output = W> + Default + Copy + num_traits::Bounded + Debug,
|
||||
HeuristicFn: Fn(&N) -> W,
|
||||
SuccessorsFn: Fn(&N) -> Vec<Edge<N, W>>,
|
||||
PredecessorsFn: Fn(&N) -> Vec<Edge<N, W>>,
|
||||
SuccessFn: Fn(&N) -> bool,
|
||||
> MTDStarLite<N, W, HeuristicFn, SuccessorsFn, PredecessorsFn, SuccessFn>
|
||||
{
|
||||
fn calculate_key(&self, n: &N) -> Priority<W> {
|
||||
let s = self.state(n);
|
||||
let min_score = if s.g < s.rhs { s.g } else { s.rhs };
|
||||
Priority(
|
||||
if min_score == W::max_value() {
|
||||
min_score
|
||||
} else {
|
||||
min_score + (self.heuristic)(n) + self.k_m
|
||||
},
|
||||
min_score,
|
||||
)
|
||||
}
|
||||
|
||||
pub fn new(
|
||||
start: N,
|
||||
goal: N,
|
||||
heuristic: HeuristicFn,
|
||||
successors: SuccessorsFn,
|
||||
predecessors: PredecessorsFn,
|
||||
success: SuccessFn,
|
||||
) -> Self {
|
||||
let open = DoublePriorityQueue::default();
|
||||
let k_m = W::default();
|
||||
|
||||
let known_nodes = vec![start, goal];
|
||||
|
||||
let mut pf = MTDStarLite {
|
||||
heuristic,
|
||||
successors,
|
||||
predecessors,
|
||||
success,
|
||||
|
||||
start,
|
||||
goal,
|
||||
|
||||
old_start: start,
|
||||
old_goal: goal,
|
||||
|
||||
k_m,
|
||||
open,
|
||||
node_states: HashMap::new(),
|
||||
updated_edge_costs: Vec::new(),
|
||||
|
||||
default_state: NodeState::default(),
|
||||
};
|
||||
|
||||
for n in &known_nodes {
|
||||
*pf.state_mut(n) = NodeState::default();
|
||||
}
|
||||
(*pf.state_mut(&start)).rhs = W::default();
|
||||
pf.open.push(start, pf.calculate_key(&start));
|
||||
|
||||
pf
|
||||
}
|
||||
|
||||
fn update_state(&mut self, n: &N) {
|
||||
let u = self.state_mut(n);
|
||||
if u.g != u.rhs {
|
||||
if self.open.get(n).is_some() {
|
||||
self.open.change_priority(n, self.calculate_key(n));
|
||||
} else {
|
||||
self.open.push(*n, self.calculate_key(n));
|
||||
}
|
||||
} else if self.open.get(n).is_some() {
|
||||
self.open.remove(n);
|
||||
}
|
||||
}
|
||||
|
||||
fn compute_cost_minimal_path(&mut self) {
|
||||
while {
|
||||
if let Some((_, top_key)) = self.open.peek_min() {
|
||||
(top_key < &self.calculate_key(&self.goal)) || {
|
||||
let goal_state = self.state(&self.goal);
|
||||
goal_state.rhs > goal_state.g
|
||||
}
|
||||
} else {
|
||||
false
|
||||
}
|
||||
} {
|
||||
let (u_node, k_old) = self.open.pop_min().unwrap();
|
||||
let k_new = self.calculate_key(&u_node);
|
||||
if k_old < k_new {
|
||||
self.open.change_priority(&u_node, k_new);
|
||||
continue;
|
||||
}
|
||||
let u = self.state_mut(&u_node);
|
||||
if u.g > u.rhs {
|
||||
u.g = u.rhs;
|
||||
self.open.remove(&u_node);
|
||||
for edge in (self.successors)(&u_node) {
|
||||
let s_node = edge.target;
|
||||
let s = self.state(&s_node);
|
||||
let u = self.state(&u_node);
|
||||
if s_node != self.start && (s.rhs > u.g + edge.cost) {
|
||||
let s_rhs = u.g + edge.cost;
|
||||
let s = self.state_mut(&s_node);
|
||||
s.par = Some(u_node);
|
||||
s.rhs = s_rhs;
|
||||
self.update_state(&s_node);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
u.g = W::max_value();
|
||||
let u_edge = Edge {
|
||||
target: u_node,
|
||||
cost: W::default(),
|
||||
};
|
||||
for edge in (self.successors)(&u_node)
|
||||
.iter()
|
||||
.chain([&u_edge].into_iter())
|
||||
{
|
||||
let s_node = edge.target;
|
||||
let s = self.state(&s_node);
|
||||
if s_node != self.start && s.par == Some(u_node) {
|
||||
let mut min_pred = u_node;
|
||||
let mut min_score = W::max_value();
|
||||
|
||||
for edge in (self.predecessors)(&s_node) {
|
||||
let s = self.state(&edge.target);
|
||||
let score = s.g + edge.cost;
|
||||
if score < min_score {
|
||||
min_score = score;
|
||||
min_pred = edge.target;
|
||||
}
|
||||
}
|
||||
|
||||
let s = self.state_mut(&s_node);
|
||||
s.rhs = min_score;
|
||||
if s.rhs == W::max_value() {
|
||||
s.par = None;
|
||||
} else {
|
||||
s.par = Some(min_pred);
|
||||
}
|
||||
}
|
||||
self.update_state(&s_node);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn find_path(&mut self) -> Option<Vec<N>> {
|
||||
if (self.success)(&self.start) {
|
||||
return None;
|
||||
}
|
||||
|
||||
//
|
||||
self.k_m = self.k_m + (self.heuristic)(&self.old_goal);
|
||||
|
||||
if self.old_start != self.start {
|
||||
self.optimized_deletion();
|
||||
}
|
||||
|
||||
while let Some(edge) = self.updated_edge_costs.pop() {
|
||||
let (u_node, v_node) = (edge.predecessor, edge.successor);
|
||||
// update the edge cost c(u, v);
|
||||
if edge.old_cost > edge.cost {
|
||||
let u_g = self.state(&u_node).g;
|
||||
if v_node != self.start && self.state(&v_node).rhs > u_g + edge.cost {
|
||||
let v = self.state_mut(&v_node);
|
||||
v.par = Some(u_node);
|
||||
v.rhs = u_g + edge.cost;
|
||||
}
|
||||
} else if v_node != self.start && self.state(&v_node).par == Some(u_node) {
|
||||
let mut min_pred = u_node;
|
||||
let mut min_score = W::max_value();
|
||||
|
||||
for edge in (self.predecessors)(&v_node) {
|
||||
let s = self.state(&edge.target);
|
||||
let score = s.g + edge.cost;
|
||||
if score < min_score {
|
||||
min_score = score;
|
||||
min_pred = edge.target;
|
||||
}
|
||||
}
|
||||
|
||||
let v = self.state_mut(&v_node);
|
||||
v.rhs = min_score;
|
||||
if v.rhs == W::max_value() {
|
||||
v.par = None;
|
||||
} else {
|
||||
v.par = Some(min_pred);
|
||||
}
|
||||
self.update_state(&v_node);
|
||||
}
|
||||
}
|
||||
//
|
||||
|
||||
self.old_start = self.start;
|
||||
self.old_goal = self.goal;
|
||||
|
||||
self.compute_cost_minimal_path();
|
||||
if self.state(&self.goal).rhs == W::max_value() {
|
||||
// no path exists
|
||||
return None;
|
||||
}
|
||||
|
||||
let mut reverse_path = vec![self.goal];
|
||||
|
||||
// identify a path from sstart to sgoal using the parent pointers
|
||||
let mut target = self.state(&self.goal).par;
|
||||
while !(Some(self.start) == target) {
|
||||
let this_target = if let Some(this_target) = target {
|
||||
this_target
|
||||
} else {
|
||||
break;
|
||||
};
|
||||
// hunter follows path from start to goal;
|
||||
reverse_path.push(this_target);
|
||||
target = self.state(&this_target).par;
|
||||
}
|
||||
|
||||
// if hunter caught target {
|
||||
// return None;
|
||||
// }
|
||||
|
||||
let path: Vec<N> = reverse_path.into_iter().rev().collect();
|
||||
|
||||
Some(path)
|
||||
}
|
||||
|
||||
fn optimized_deletion(&mut self) {
|
||||
let start = self.start;
|
||||
self.state_mut(&start).par = None;
|
||||
|
||||
let mut min_pred = self.old_start;
|
||||
let mut min_score = W::max_value();
|
||||
|
||||
for edge in (self.predecessors)(&self.old_start) {
|
||||
let s = self.state(&edge.target);
|
||||
let score = s.g + edge.cost;
|
||||
if score < min_score {
|
||||
min_score = score;
|
||||
min_pred = edge.target;
|
||||
}
|
||||
}
|
||||
|
||||
let old_start = self.old_start;
|
||||
let s = self.state_mut(&old_start);
|
||||
s.rhs = min_score;
|
||||
if s.rhs == W::max_value() {
|
||||
s.par = None;
|
||||
} else {
|
||||
s.par = Some(min_pred);
|
||||
}
|
||||
self.update_state(&old_start);
|
||||
}
|
||||
|
||||
fn state(&self, n: &N) -> &NodeState<N, W> {
|
||||
self.node_states.get(n).unwrap_or(&self.default_state)
|
||||
}
|
||||
|
||||
fn state_mut(&mut self, n: &N) -> &mut NodeState<N, W> {
|
||||
self.node_states.entry(*n).or_default()
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(PartialEq, Debug)]
|
||||
pub struct Priority<W>(W, W)
|
||||
where
|
||||
W: PartialOrd + Debug;
|
||||
|
||||
impl<W: PartialOrd + Debug> PartialOrd for Priority<W> {
|
||||
fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
|
||||
if self.0 < other.0 {
|
||||
Some(std::cmp::Ordering::Less)
|
||||
} else if self.0 > other.0 {
|
||||
Some(std::cmp::Ordering::Greater)
|
||||
} else if self.1 < other.1 {
|
||||
Some(std::cmp::Ordering::Less)
|
||||
} else if self.1 > other.1 {
|
||||
Some(std::cmp::Ordering::Greater)
|
||||
} else {
|
||||
Some(std::cmp::Ordering::Equal)
|
||||
}
|
||||
}
|
||||
}
|
||||
impl<W: PartialOrd + Debug> Ord for Priority<W> {
|
||||
fn cmp(&self, other: &Self) -> std::cmp::Ordering {
|
||||
self.partial_cmp(other)
|
||||
.expect("Partial compare should not fail for Priority")
|
||||
}
|
||||
}
|
||||
impl<W: PartialOrd + Debug> Eq for Priority<W> {}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct NodeState<N: Eq + Hash + Copy + Debug, W: Default + num_traits::Bounded + Debug> {
|
||||
pub g: W,
|
||||
pub rhs: W,
|
||||
// future possible optimization: try making this a pointer
|
||||
pub par: Option<N>,
|
||||
}
|
||||
|
||||
impl<N: Eq + Hash + Copy + Debug, W: Default + num_traits::Bounded + Debug> Default
|
||||
for NodeState<N, W>
|
||||
{
|
||||
fn default() -> Self {
|
||||
NodeState {
|
||||
g: W::max_value(),
|
||||
rhs: W::max_value(),
|
||||
par: None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub struct Edge<N: Eq + Hash + Copy, W: PartialOrd + Copy> {
|
||||
pub target: N,
|
||||
pub cost: W,
|
||||
}
|
||||
|
||||
pub struct ChangedEdge<N: Eq + Hash + Clone, W: PartialOrd + Copy> {
|
||||
pub predecessor: N,
|
||||
pub successor: N,
|
||||
pub old_cost: W,
|
||||
pub cost: W,
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_mtdstarlite() {
|
||||
let maze = [
|
||||
[0, 1, 0, 0, 0],
|
||||
[0, 1, 0, 1, 0],
|
||||
[0, 0, 0, 1, 0],
|
||||
[0, 1, 0, 1, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
];
|
||||
let width = maze[0].len();
|
||||
let height = maze.len();
|
||||
|
||||
let goal = (4, 4);
|
||||
|
||||
let heuristic = |n: &(usize, usize)| -> usize {
|
||||
((n.0 as isize - goal.0 as isize).abs() + (n.1 as isize - goal.1 as isize).abs())
|
||||
as usize
|
||||
};
|
||||
let successors = |n: &(usize, usize)| -> Vec<Edge<(usize, usize), usize>> {
|
||||
let mut successors = Vec::with_capacity(4);
|
||||
let (x, y) = *n;
|
||||
|
||||
if x > 0 && maze[y][x - 1] == 0 {
|
||||
successors.push(Edge {
|
||||
target: ((x - 1, y)),
|
||||
cost: 1,
|
||||
});
|
||||
}
|
||||
if x < width - 1 && maze[y][x + 1] == 0 {
|
||||
successors.push(Edge {
|
||||
target: ((x + 1, y)),
|
||||
cost: 1,
|
||||
});
|
||||
}
|
||||
if y > 0 && maze[y - 1][x] == 0 {
|
||||
successors.push(Edge {
|
||||
target: ((x, y - 1)),
|
||||
cost: 1,
|
||||
});
|
||||
}
|
||||
if y < height - 1 && maze[y + 1][x] == 0 {
|
||||
successors.push(Edge {
|
||||
target: ((x, y + 1)),
|
||||
cost: 1,
|
||||
});
|
||||
}
|
||||
|
||||
successors
|
||||
};
|
||||
let predecessors =
|
||||
|n: &(usize, usize)| -> Vec<Edge<(usize, usize), usize>> { successors(n) };
|
||||
|
||||
let mut pf = MTDStarLite::new((0, 0), goal, heuristic, successors, predecessors, |n| {
|
||||
n == &goal
|
||||
});
|
||||
let path = pf.find_path().unwrap();
|
||||
assert_eq!(
|
||||
path,
|
||||
vec![
|
||||
(0, 1),
|
||||
(0, 2),
|
||||
(1, 2),
|
||||
(2, 2),
|
||||
(2, 1),
|
||||
(2, 0),
|
||||
(3, 0),
|
||||
(4, 0),
|
||||
(4, 1),
|
||||
(4, 2),
|
||||
(4, 3),
|
||||
(4, 4),
|
||||
]
|
||||
);
|
||||
}
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue