diff options
| -rwxr-xr-x | astar.rs | 282 |
1 files changed, 207 insertions, 75 deletions
@@ -25,6 +25,7 @@ pub mod astar { PROGRESS, SUCCESS, FAIL, + OUT_OF_MEMORY, } #[derive(Debug, Clone, Default)] @@ -43,9 +44,10 @@ pub mod astar { pub count : usize, } + #[derive(Clone)] pub struct State<Node_Id, Cost> { - pub open : Vec<Node<Node_Id, Cost>>, - pub closed : Vec<Node<Node_Id, Cost>>, + pub num_open : usize, + pub num_closed : usize, pub source : Node_Id, pub destination : Node_Id, pub closest_index : usize, @@ -53,6 +55,7 @@ pub mod astar { } pub fn init<Node_Id, Cost>( + open : &mut [Node<Node_Id, Cost>], source : Node_Id, destination : Node_Id, max_cost : Cost @@ -61,23 +64,19 @@ pub mod astar { Node_Id : Clone, Cost : Clone + Default { - let source_node = Node::<Node_Id, Cost> { - id : source.clone(), - previous : None, - exact_distance : Cost::default(), - estimate : max_cost.clone(), - count : 1, - }; - - let mut open : Vec<Node<Node_Id, Cost>> = vec![source_node]; - let mut closed : Vec<Node<Node_Id, Cost>> = vec![]; - - open.reserve(128); - closed.reserve(128); + if open.len() != 0 { + open[0] = Node::<Node_Id, Cost> { + id : source.clone(), + previous : None, + exact_distance : Cost::default(), + estimate : max_cost.clone(), + count : 1, + }; + } return State { - open, - closed, + num_open : if open.len() != 0 { 1 } else { 0 }, + num_closed : 0, source, destination, closest_index : 0, @@ -86,90 +85,107 @@ pub mod astar { } pub fn path<Node_Id, Cost>( - state : &mut State<Node_Id, Cost> - ) -> Option<Vec<Node_Id>> + closed : &[Node<Node_Id, Cost>], + state : &State<Node_Id, Cost>, + node_ids : &mut [Node_Id] + ) -> usize where Node_Id : Clone + PartialEq { - if state.closed.is_empty() || state.closest_index >= state.closed.len() { - return None; + if closed.is_empty() || + state.closest_index >= state.num_closed || + node_ids.len() < state.num_closed { + return 0; } let mut current = state.closest_index; - let mut backward : Vec<Node_Id> = vec![state.closed[current].id.clone()]; - backward.reserve_exact(state.closed[current].count); + let mut num_nodes : usize = 1; + node_ids[0] = closed[current].id.clone(); loop { - if backward[backward.len() - 1] == state.source { + if node_ids[num_nodes - 1] == state.source { break; } - if backward.len() > state.closed.len() { - return None; + if num_nodes >= state.num_closed { + return 0; } let mut index = usize::MAX; - for i in 0..state.closed.len() { - if Some(state.closed[i].id.clone()) == state.closed[current].previous { + for i in 0..state.num_closed { + if Some(closed[i].id.clone()) == closed[current].previous { index = i; break; } } if index == usize::MAX { - return None; + return 0; } - backward.push(state.closed[index].id.clone()); + node_ids[num_nodes] = closed[index].id.clone(); + num_nodes += 1; current = index; } - let mut forward = Vec::<Node_Id>::new(); - forward.reserve_exact(backward.len()); - - for i in 1..=backward.len() { - forward.push(backward[backward.len() - i].clone()); + for i in 0..num_nodes/2 { + let x = node_ids[i].clone(); + node_ids[i] = node_ids[num_nodes - 1 - i].clone(); + node_ids[num_nodes - 1 - i] = x; } - return Some(forward); + return num_nodes; } pub fn iteration<Node_Id, Cost, Neighbor, Error_Type>( - state : &mut State<Node_Id, Cost>, - mut neighbor : Neighbor, + open : &mut [Node<Node_Id, Cost>], + closed : &mut [Node<Node_Id, Cost>], + state : &mut State<Node_Id, Cost>, + mut neighbor : Neighbor, ) -> Result<Status, Error_Type> where Node_Id : Debug + Clone + Default + PartialEq, Cost : Debug + Clone + Default + PartialOrd + Add<Output = Cost>, Neighbor : FnMut(Node_Id, usize) -> Result<Option<Link<Node_Id, Cost>>, Error_Type> { - if state.open.is_empty() { + 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.open.len() { - if state.open[index].estimate < state.open[index_in_open].estimate { + for index in 1..state.num_open { + if open[index].estimate < open[index_in_open].estimate { index_in_open = index; } } - let nearest_node = state.open[index_in_open].clone(); - if index_in_open != state.open.len() - 1 { - state.open[index_in_open] = state.open[state.open.len() - 1].clone(); + let prev_num_open = state.num_open; + let prev_id = 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.open.resize(state.open.len() - 1, Node::default()); + state.num_open -= 1; // Check if we reached the destination // if nearest_node.id == state.destination { - state.closed.push(nearest_node); - state.closest_index = state.closed.len() - 1; - state.closest_estimate = Cost::default(); + 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); @@ -186,6 +202,12 @@ pub mod astar { 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_id; + return Ok(Status::OUT_OF_MEMORY); + } + // Calculate distance estimations // @@ -203,10 +225,11 @@ pub mod astar { // Check if we reached the destination // if neighbor_node.id == state.destination { - state.closed.push(nearest_node); - state.closed.push(neighbor_node.clone()); - state.closest_index = state.closed.len() - 1; - state.closest_estimate = Cost::default(); + 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); @@ -216,8 +239,8 @@ pub mod astar { // let mut index_in_closed = usize::MAX; - for i in 0..state.closed.len() { - if state.closed[i].id == neighbor_node.id { + for i in 0..state.num_closed { + if closed[i].id == neighbor_node.id { index_in_closed = i; break; } @@ -225,14 +248,14 @@ pub mod astar { if index_in_closed != usize::MAX { // Check if this node has a better distance - if neighbor_node.exact_distance < state.closed[index_in_closed].exact_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 - state.closed[index_in_closed] = neighbor_node; + closed[index_in_closed] = neighbor_node; } // Skip this node @@ -244,8 +267,8 @@ pub mod astar { // let mut index_in_open : usize = usize::MAX; - for i in 0..state.open.len() { - if state.open[i].id == neighbor_node.id { + for i in 0..state.num_open { + if open[i].id == neighbor_node.id { index_in_open = i; break; } @@ -253,9 +276,9 @@ pub mod astar { if index_in_open != usize::MAX { // Check if this node has a better distance - if neighbor_node.exact_distance < state.open[index_in_open].exact_distance { + if neighbor_node.exact_distance < open[index_in_open].exact_distance { // Replace the node - state.open[index_in_open] = neighbor_node; + open[index_in_open] = neighbor_node; } // Skip this node @@ -263,18 +286,20 @@ pub mod astar { continue; } - state.open.push(neighbor_node); + 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.closed.len(); + state.closest_index = state.num_closed; state.closest_estimate = nearest_node.clone().estimate; } - state.closed.push(nearest_node); + closed[state.num_closed] = nearest_node; + state.num_closed += 1; return Ok(Status::PROGRESS); } @@ -321,19 +346,29 @@ mod tests { return Ok(None); }; - let mut state = init(0i64, 5i64, i64::MAX); + let mut open : Vec<Node<i64, i64>> = vec![]; + let mut closed : Vec<Node<i64, i64>> = vec![]; + + open .resize(1024, Node::default()); + closed.resize(1024, Node::default()); + + let mut state = init(&mut open, 0i64, 5i64, i64::MAX); let mut steps = 0; loop { steps += 1; - let status = iteration(&mut state, neighbor).unwrap(); + let status = iteration(&mut open, &mut closed, &mut state, neighbor).unwrap(); + if status != Status::PROGRESS { assert_eq!(status, Status::SUCCESS); break; } } - let v = path(&mut state).unwrap(); + let mut v : Vec<i64> = vec![]; + v.resize(state.num_closed, 0); + let n = path(&closed, &state, &mut v); + v.resize(n, 0); assert_eq!(steps, 5); assert_eq!(v.len(), 6); @@ -346,6 +381,74 @@ mod tests { } #[test] + fn out_of_memory() { + let graph : Vec<((i64, i64), i64)> = vec![ + ((0, 1), 5), + ((0, 2), 3), + ((1, 3), 4), + ((2, 4), 1), + ((3, 5), 10), + ((4, 6), 1), + ((6, 7), 1), + ((7, 5), 1), + ]; + + let neighbor = |id : i64, index : usize| -> Result<Option<Link<i64, i64>>, ()> { + let mut k : usize = 0; + for ((src, dst), cost) in graph.clone() { + if src == id { + if k == index { + return Ok(Some(Link::<i64, i64> { + neighbor : dst, + exact_distance : cost, + estimate : (8 - dst).abs(), + })); + } else { + k += 1; + } + } + } + return Ok(None); + }; + + let mut open : Vec<Node<i64, i64>> = vec![Node::default()]; + let mut closed : Vec<Node<i64, i64>> = vec![]; + + let mut state = init(&mut open, 0i64, 5i64, i64::MAX); + + let mut steps = 0; + 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; + } + } + + let mut v : Vec<i64> = vec![]; + v.resize(state.num_closed, 0); + let n = path(&closed, &state, &mut v); + v.resize(n, 0); + + assert_eq!(steps, 6); + assert_eq!(v.len(), 6); + assert_eq!(v[0], 0); + assert_eq!(v[1], 2); + assert_eq!(v[2], 4); + assert_eq!(v[3], 6); + assert_eq!(v[4], 7); + assert_eq!(v[5], 5); + } + + #[test] fn path_does_not_exist() { let graph : Vec<((i64, i64), i64)> = vec![ ((0, 1), 5), @@ -376,19 +479,29 @@ mod tests { return Ok(None); }; - let mut state = init(0i64, 15i64, i64::MAX); + let mut open : Vec<Node<i64, i64>> = vec![]; + let mut closed : Vec<Node<i64, i64>> = vec![]; + + open .resize(1024, Node::default()); + closed.resize(1024, Node::default()); + + let mut state = init(&mut open, 0i64, 15i64, i64::MAX); let mut steps = 0; loop { steps += 1; - let status = iteration(&mut state, neighbor).unwrap(); + let status = iteration(&mut open, &mut closed, &mut state, neighbor).unwrap(); + if status != Status::PROGRESS { assert_eq!(status, Status::FAIL); break; } } - let v = path(&mut state).unwrap(); + let mut v : Vec<i64> = vec![]; + v.resize(state.num_closed, 0); + let n = path(&closed, &state, &mut v); + v.resize(n, 0); assert_eq!(steps, 9); assert_eq!(v.len(), 5); @@ -430,19 +543,28 @@ mod tests { return Ok(None); }; - let mut state = init(2i64, 2i64, i64::MAX); + let mut open : Vec<Node<i64, i64>> = vec![]; + let mut closed : Vec<Node<i64, i64>> = vec![]; + + open .resize(1024, Node::default()); + closed.resize(1024, Node::default()); + + let mut state = init(&mut open, 2i64, 2i64, i64::MAX); let mut steps = 0; loop { steps += 1; - let status = iteration(&mut state, neighbor).unwrap(); + let status = iteration(&mut open, &mut closed, &mut state, neighbor).unwrap(); if status != Status::PROGRESS { assert_eq!(status, Status::SUCCESS); break; } } - let v = path(&mut state).unwrap(); + let mut v : Vec<i64> = vec![]; + v.resize(state.num_closed, 0); + let n = path(&closed, &state, &mut v); + v.resize(n, 0); assert_eq!(steps, 1); assert_eq!(v.len(), 1); @@ -483,19 +605,29 @@ mod tests { return Ok(None); }; - let mut state = init(0i64, 5i64, i64::MAX); + let mut open : Vec<Node<i64, i64>> = vec![]; + let mut closed : Vec<Node<i64, i64>> = vec![]; + + open .resize(1024, Node::default()); + closed.resize(1024, Node::default()); + + let mut state = init(&mut open, 0i64, 5i64, i64::MAX); let mut steps = 0; loop { steps += 1; - let status = iteration(&mut state, neighbor).unwrap(); + let status = iteration(&mut open, &mut closed, &mut state, neighbor).unwrap(); + if status != Status::PROGRESS { assert_eq!(status, Status::SUCCESS); break; } } - let v = path(&mut state).unwrap(); + let mut v : Vec<i64> = vec![]; + v.resize(state.num_closed, 0); + let n = path(&closed, &state, &mut v); + v.resize(n, 0); assert_eq!(steps, 5); assert_eq!(v.len(), 6); |