From 8bd6422067848b7c029a67ef64ba310601ce6033 Mon Sep 17 00:00:00 2001 From: Mitya Selivanov Date: Mon, 29 Jul 2024 20:36:05 +0200 Subject: Decouple neighbor proc --- astar.rs | 505 +++++++++++++++++++++++++++++++++++---------------------------- 1 file changed, 281 insertions(+), 224 deletions(-) diff --git a/astar.rs b/astar.rs index 3bbe1a5..1742dbf 100755 --- a/astar.rs +++ b/astar.rs @@ -8,12 +8,6 @@ #/ #/ ---------------------------------------------------------------- #/ -#/ To-Do list -#/ -#/ - Decouple `neighbor` proc -#/ -#/ ---------------------------------------------------------------- -#/ #/ (C) 2024 Mitya Selivanov , MIT License #/ #/ ================================================================ @@ -29,12 +23,19 @@ exit $? # */ mod astar_internal { use std::{ops::Add, fmt::Debug}; + #[derive(Debug, Clone, PartialEq, Default)] + pub struct Neighbor_Request { + pub node : Node_Id, + pub index : usize, + } + #[derive(Debug, Clone, PartialEq)] - pub enum Status { + pub enum Status { PROGRESS, SUCCESS, FAIL, OUT_OF_MEMORY, + NEIGHBOR(Neighbor_Request), } #[derive(Debug, Clone, Default)] @@ -53,14 +54,23 @@ mod astar_internal { pub count : usize, } + #[derive(Debug, Clone, PartialEq)] + pub enum Stage { + SEARCH_NEAREST, + ENUM_NEIGHBORS, + } + #[derive(Clone)] pub struct State { + pub stage : Stage, pub num_open : usize, pub num_closed : usize, pub source : Node_Id, pub destination : Node_Id, pub closest_index : usize, pub closest_estimate : Cost, + pub node : Option>, + pub neighbor_index : usize, } pub fn init( @@ -84,12 +94,15 @@ mod astar_internal { } return State { - num_open : if open.len() != 0 { 1 } else { 0 }, + stage : Stage::SEARCH_NEAREST, + num_open : if open.len() != 0 { 1 } else { 0 }, num_closed : 0, source, destination, closest_index : 0, closest_estimate : max_cost, + node : None, + neighbor_index : 0, }; } @@ -147,173 +160,208 @@ mod astar_internal { return num_nodes; } - pub fn iteration( - open : &mut [Node], - closed : &mut [Node], - state : &mut State, - mut neighbor : Neighbor, - ) -> Result + pub fn iteration( + open : &mut [Node], + closed : &mut [Node], + state : &mut State, + neighbor : Option>, + ) -> Status where - Node_Id : Debug + Clone + Default + PartialEq, - Cost : Debug + Clone + Default + PartialOrd + Add, - Neighbor : FnMut(Node_Id, usize) -> Result>, Error_Type> + Node_Id : Debug + Clone + Default + PartialEq, + Cost : Debug + Clone + Default + PartialOrd + Add, { - if state.num_open == 0 { - return Ok(Status::FAIL); - } - - // Check if we need more memory - // - if state.num_open + 1 >= open .len() || - state.num_closed + 2 >= closed.len() { - return Ok(Status::OUT_OF_MEMORY); - } - - // Find the nearest node to the destination in the open set - // - - let mut index_in_open : usize = 0; - for index in 1..state.num_open { - let node = open[index].clone(); - let nearest = open[index_in_open].clone(); - if node .exact_distance + node .estimate < - nearest.exact_distance + nearest.estimate { - index_in_open = index; - } - } - - let prev_num_open = state.num_open; - let prev_node = open[prev_num_open - 1].clone(); - - let nearest_node = open[index_in_open].clone(); - if index_in_open != state.num_open - 1 { - open[index_in_open] = open[state.num_open - 1].clone(); - } - state.num_open -= 1; - - // Check if we reached the destination - // - if nearest_node.id == state.destination { - state.closest_index = state.num_closed; - state.closest_estimate = Cost::default(); - closed[state.num_closed] = nearest_node; - state.num_closed += 1; - - // Finish the search - return Ok(Status::SUCCESS); - } - - // Enumerate all neighbors - // - let mut neighbor_index : usize = 0; - loop { - // Get a link to the neighbor node - // - let link = match neighbor(nearest_node.clone().id, neighbor_index)? { - Some(x) => x, - None => break, // There is no more neighbors, so end the loop. - }; - - if state.num_open + 1 >= open.len() { - state.num_open = prev_num_open; - open[prev_num_open - 1] = prev_node; - return Ok(Status::OUT_OF_MEMORY); - } - - // Calculate distance estimations - // - - let exact_distance = nearest_node.clone().exact_distance + link.clone().exact_distance; - let estimate = link.clone().estimate; - - let neighbor_node = Node { - id : link.neighbor, - previous : Some(nearest_node.clone().id), - exact_distance, - estimate, - count : nearest_node.count + 1, - }; - - // Check if we reached the destination - // - if neighbor_node.id == state.destination { - state.closest_index = state.num_closed + 1; - state.closest_estimate = Cost::default(); - closed[state.num_closed] = nearest_node; - closed[state.num_closed + 1] = neighbor_node.clone(); - state.num_closed += 2; - - // Finish the search - return Ok(Status::SUCCESS); - } - - // Check if this node is already in the closed set - // - - let mut index_in_closed = usize::MAX; - for i in 0..state.num_closed { - if closed[i].id == neighbor_node.id { - index_in_closed = i; - break; + match state.stage { + Stage::SEARCH_NEAREST => { + if state.num_open == 0 { + return Status::FAIL; } - } - if index_in_closed != usize::MAX { - // Check if this node has a better distance - if neighbor_node.exact_distance < closed[index_in_closed].exact_distance { - if neighbor_node.estimate < state.closest_estimate { - state.closest_index = index_in_closed; - state.closest_estimate = neighbor_node.clone().estimate; - } - - // Replace the node - closed[index_in_closed] = neighbor_node; + // Check if we need more memory + // + if state.num_open + 1 >= open .len() || + state.num_closed + 2 >= closed.len() { + return Status::OUT_OF_MEMORY; } - // Skip this node - neighbor_index += 1; - continue; - } + // Find the nearest node to the destination in the open set + // - // Check if this node is already in the open set - // - - let mut index_in_open : usize = usize::MAX; - for i in 0..state.num_open { - if open[i].id == neighbor_node.id { - index_in_open = i; - break; + let mut index_in_open : usize = 0; + for index in 1..state.num_open { + let node = open[index].clone(); + let nearest = open[index_in_open].clone(); + if node .exact_distance + node .estimate < + nearest.exact_distance + nearest.estimate { + index_in_open = index; + } } - } - if index_in_open != usize::MAX { - // Check if this node has a better distance - if neighbor_node.exact_distance < open[index_in_open].exact_distance { - // Replace the node - open[index_in_open] = neighbor_node; + let nearest_node = open[index_in_open].clone(); + state.node = Some(nearest_node.clone()); + if index_in_open != state.num_open - 1 { + open[index_in_open] = open[state.num_open - 1].clone(); + } + state.num_open -= 1; + + // Check if we reached the destination + // + if nearest_node.id == state.destination { + state.closest_index = state.num_closed; + state.closest_estimate = Cost::default(); + closed[state.num_closed] = nearest_node; + state.num_closed += 1; + + // Finish the search + return Status::SUCCESS; } - // Skip this node - neighbor_index += 1; - continue; - } - - open[state.num_open] = neighbor_node; - state.num_open += 1; - - // Proceed to the next neighbor node - neighbor_index += 1; - } - - if nearest_node.estimate < state.closest_estimate { - state.closest_index = state.num_closed; - state.closest_estimate = nearest_node.clone().estimate; - } - - closed[state.num_closed] = nearest_node; - state.num_closed += 1; - - return Ok(Status::PROGRESS); + // Proceed to the neighbors enumeration stage + // + + state.stage = Stage::ENUM_NEIGHBORS; + state.neighbor_index = 0; + + return Status::NEIGHBOR(Neighbor_Request { + node : nearest_node.id, + index : state.neighbor_index, + }); + }, + + Stage::ENUM_NEIGHBORS => match state.node.clone() { + None => panic!(), + Some(nearest_node) => match neighbor { + Some(link) => { + // Check if we need more memory + // + if state.num_open + 1 >= open.len() { + return Status::OUT_OF_MEMORY; + } + + // Calculate distance estimations + // + + let exact_distance = nearest_node.clone().exact_distance + link.clone().exact_distance; + let estimate = link.clone().estimate; + + let neighbor_node = Node { + id : link.neighbor, + previous : Some(nearest_node.clone().id), + exact_distance, + estimate, + count : nearest_node.count + 1, + }; + + // Check if we reached the destination + // + if neighbor_node.id == state.destination { + state.closest_index = state.num_closed + 1; + state.closest_estimate = Cost::default(); + closed[state.num_closed] = nearest_node; + closed[state.num_closed + 1] = neighbor_node.clone(); + state.node = None; + state.num_closed += 2; + + // Finish the search + return Status::SUCCESS; + } + + // Check if this node is already in the closed set + // + + let mut index_in_closed = usize::MAX; + for i in 0..state.num_closed { + if closed[i].id == neighbor_node.id { + index_in_closed = i; + break; + } + } + + if index_in_closed != usize::MAX { + // Check if this node has a better distance + if neighbor_node.exact_distance < closed[index_in_closed].exact_distance { + if neighbor_node.estimate < state.closest_estimate { + state.closest_index = index_in_closed; + state.closest_estimate = neighbor_node.clone().estimate; + } + + // Replace the node + closed[index_in_closed] = neighbor_node; + } + + // Skip this node and proceed to the next neighbor node + // + + state.neighbor_index += 1; + + return Status::NEIGHBOR(Neighbor_Request { + node : nearest_node.id, + index : state.neighbor_index, + }); + } + + // Check if this node is already in the open set + // + + let mut index_in_open : usize = usize::MAX; + for i in 0..state.num_open { + if open[i].id == neighbor_node.id { + index_in_open = i; + break; + } + } + + if index_in_open != usize::MAX { + // Check if this node has a better distance + if neighbor_node.exact_distance < open[index_in_open].exact_distance { + // Replace the node + open[index_in_open] = neighbor_node; + } + + // Skip this node and proceed to the next neighbor node + // + + state.neighbor_index += 1; + + return Status::NEIGHBOR(Neighbor_Request { + node : nearest_node.id, + index : state.neighbor_index, + }); + } + + open[state.num_open] = neighbor_node; + state.num_open += 1; + + // Proceed to the next neighbor node + // + + state.neighbor_index += 1; + + return Status::NEIGHBOR(Neighbor_Request { + node : nearest_node.id, + index : state.neighbor_index, + }); + }, + + None => { + if nearest_node.estimate < state.closest_estimate { + state.closest_index = state.num_closed; + state.closest_estimate = nearest_node.clone().estimate; + } + + closed[state.num_closed] = nearest_node; + state.node = None; + state.num_closed += 1; + + // Proceed to the nearest node search + // + + state.stage = Stage::SEARCH_NEAREST; + + return Status::PROGRESS; + }, + }, + }, + }; } } @@ -342,22 +390,22 @@ mod tests { ((7, 5), 1), ]; - let neighbor = |id : i64, index : usize| -> Result>, ()> { + let get_neighbor = |id : i64, index : usize| -> Option> { let mut k : usize = 0; for ((src, dst), cost) in graph.clone() { if src == id { if k == index { - return Ok(Some(Link:: { + return Some(Link:: { neighbor : dst, exact_distance : cost, estimate : (8 - dst).abs(), - })); + }); } else { k += 1; } } } - return Ok(None); + return None; }; let mut open : Vec> = vec![]; @@ -368,15 +416,17 @@ mod tests { let mut state = init(&mut open, 0i64, 5i64, i64::MAX); - let mut steps = 0; + let mut steps = 0; + let mut neighbor = None; loop { steps += 1; - let status = iteration(&mut open, &mut closed, &mut state, neighbor).unwrap(); - if status != Status::PROGRESS { - assert_eq!(status, Status::SUCCESS); - break; - } + match iteration(&mut open, &mut closed, &mut state, neighbor.clone()) { + Status::NEIGHBOR(request) => neighbor = get_neighbor(request.node, request.index), + Status::SUCCESS => break, + Status::PROGRESS => {}, + _ => assert!(false), + }; } let mut v : Vec = vec![]; @@ -384,7 +434,7 @@ mod tests { let n = path(&closed, &state, &mut v); v.resize(n, 0); - assert_eq!(steps, 5); + assert_eq!(steps, 15); assert_eq!(v.len(), 6); assert_eq!(v[0], 0); assert_eq!(v[1], 2); @@ -407,22 +457,22 @@ mod tests { ((7, 5), 1), ]; - let neighbor = |id : i64, index : usize| -> Result>, ()> { + let get_neighbor = |id : i64, index : usize| -> Option> { let mut k : usize = 0; for ((src, dst), cost) in graph.clone() { if src == id { if k == index { - return Ok(Some(Link:: { + return Some(Link:: { neighbor : dst, exact_distance : cost, estimate : (8 - dst).abs(), - })); + }); } else { k += 1; } } } - return Ok(None); + return None; }; let mut open : Vec> = vec![Node::default()]; @@ -430,21 +480,21 @@ mod tests { let mut state = init(&mut open, 0i64, 5i64, i64::MAX); - let mut steps = 0; + let mut steps = 0; + let mut neighbor = None; loop { steps += 1; - let status = iteration(&mut open, &mut closed, &mut state, neighbor).unwrap(); - if status == Status::OUT_OF_MEMORY { - open .resize(open .len() + 1024, Node::default()); - closed.resize(closed.len() + 1024, Node::default()); - continue; - } - - if status != Status::PROGRESS { - assert_eq!(status, Status::SUCCESS); - break; - } + match iteration(&mut open, &mut closed, &mut state, neighbor.clone()) { + Status::NEIGHBOR(request) => neighbor = get_neighbor(request.node, request.index), + Status::SUCCESS => break, + Status::PROGRESS => {}, + Status::OUT_OF_MEMORY => { + open .resize(open .len() + 1024, Node::default()); + closed.resize(closed.len() + 1024, Node::default()); + }, + _ => assert!(false), + }; } let mut v : Vec = vec![]; @@ -452,7 +502,7 @@ mod tests { let n = path(&closed, &state, &mut v); v.resize(n, 0); - assert_eq!(steps, 6); + assert_eq!(steps, 16); assert_eq!(v.len(), 6); assert_eq!(v[0], 0); assert_eq!(v[1], 2); @@ -475,22 +525,22 @@ mod tests { ((7, 5), 1), ]; - let neighbor = |id : i64, index : usize| -> Result>, ()> { + let get_neighbor = |id : i64, index : usize| -> Option> { let mut k : usize = 0; for ((src, dst), cost) in graph.clone() { if src == id { if k == index { - return Ok(Some(Link:: { + return Some(Link:: { neighbor : dst, exact_distance : cost, estimate : (15 - dst).abs(), - })); + }); } else { k += 1; } } } - return Ok(None); + return None; }; let mut open : Vec> = vec![]; @@ -501,15 +551,17 @@ mod tests { let mut state = init(&mut open, 0i64, 15i64, i64::MAX); - let mut steps = 0; + let mut steps = 0; + let mut neighbor = None; loop { steps += 1; - let status = iteration(&mut open, &mut closed, &mut state, neighbor).unwrap(); - if status != Status::PROGRESS { - assert_eq!(status, Status::FAIL); - break; - } + match iteration(&mut open, &mut closed, &mut state, neighbor.clone()) { + Status::NEIGHBOR(request) => neighbor = get_neighbor(request.node, request.index), + Status::FAIL => break, + Status::PROGRESS => {}, + _ => assert!(false), + }; } let mut v : Vec = vec![]; @@ -517,7 +569,7 @@ mod tests { let n = path(&closed, &state, &mut v); v.resize(n, 0); - assert_eq!(steps, 9); + assert_eq!(steps, 25); assert_eq!(v.len(), 5); assert_eq!(v[0], 0); assert_eq!(v[1], 2); @@ -539,22 +591,22 @@ mod tests { ((7, 5), 1), ]; - let neighbor = |id : i64, index : usize| -> Result>, ()> { + let get_neighbor = |id : i64, index : usize| -> Option> { let mut k : usize = 0; for ((src, dst), cost) in graph.clone() { if src == id { if k == index { - return Ok(Some(Link:: { + return Some(Link:: { neighbor : dst, exact_distance : cost, estimate : (2 - dst).abs(), - })); + }); } else { k += 1; } } } - return Ok(None); + return None; }; let mut open : Vec> = vec![]; @@ -565,14 +617,17 @@ mod tests { let mut state = init(&mut open, 2i64, 2i64, i64::MAX); - let mut steps = 0; + let mut steps = 0; + let mut neighbor = None; loop { steps += 1; - let status = iteration(&mut open, &mut closed, &mut state, neighbor).unwrap(); - if status != Status::PROGRESS { - assert_eq!(status, Status::SUCCESS); - break; - } + + match iteration(&mut open, &mut closed, &mut state, neighbor.clone()) { + Status::NEIGHBOR(request) => neighbor = get_neighbor(request.node, request.index), + Status::SUCCESS => break, + Status::PROGRESS => {}, + _ => assert!(false), + }; } let mut v : Vec = vec![]; @@ -601,22 +656,22 @@ mod tests { ((6, 2), 5), ]; - let neighbor = |id : i64, index : usize| -> Result>, ()> { + let get_neighbor = |id : i64, index : usize| -> Option> { let mut k : usize = 0; for ((src, dst), cost) in graph.clone() { if src == id { if k == index { - return Ok(Some(Link:: { + return Some(Link:: { neighbor : dst, exact_distance : cost, estimate : (5 - dst).abs(), - })); + }); } else { k += 1; } } } - return Ok(None); + return None; }; let mut open : Vec> = vec![]; @@ -627,15 +682,17 @@ mod tests { let mut state = init(&mut open, 0i64, 5i64, i64::MAX); - let mut steps = 0; + let mut steps = 0; + let mut neighbor = None; loop { steps += 1; - let status = iteration(&mut open, &mut closed, &mut state, neighbor).unwrap(); - if status != Status::PROGRESS { - assert_eq!(status, Status::SUCCESS); - break; - } + match iteration(&mut open, &mut closed, &mut state, neighbor.clone()) { + Status::NEIGHBOR(request) => neighbor = get_neighbor(request.node, request.index), + Status::SUCCESS => break, + Status::PROGRESS => {}, + _ => assert!(false), + }; } let mut v : Vec = vec![]; @@ -643,7 +700,7 @@ mod tests { let n = path(&closed, &state, &mut v); v.resize(n, 0); - assert_eq!(steps, 6); + assert_eq!(steps, 19); assert_eq!(v.len(), 6); assert_eq!(v[0], 0); assert_eq!(v[1], 2); -- cgit v1.2.3