Skip to content

Commit

Permalink
Add greedy pathfinding algorithm
Browse files Browse the repository at this point in the history
  • Loading branch information
Jonatan Chaverri committed Sep 18, 2024
1 parent 1d4eba4 commit 8a89446
Show file tree
Hide file tree
Showing 2 changed files with 250 additions and 0 deletions.
249 changes: 249 additions & 0 deletions crates/map/src/helpers/greedy.cairo
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,249 @@
//! Greedy algorithm implementation for pathfinding.

// Core imports
use core::dict::{Felt252Dict, Felt252DictTrait};

// Internal imports
use origami_map::helpers::heap::{Heap, HeapTrait};
use origami_map::helpers::bitmap::Bitmap;
use origami_map::types::node::{Node, NodeTrait};
use origami_map::types::direction::Direction;

#[generate_trait]
pub impl Greedy of GreedyTrait {
/// Search for the shortest path from a start to a target position using Greedy Search.
/// # Arguments
/// * `grid` - The grid to search (1 is walkable and 0 is not)
/// * `width` - The width of the grid
/// * `height` - The height of the grid
/// * `from` - The starting position
/// * `to` - The target position
/// # Returns
/// * The path from the target (incl.) to the start (excl.)
#[inline]
fn search(grid: felt252, width: u8, height: u8, from: u8, to: u8) -> Span<u8> {
// [Check] The start and target are walkable
if Bitmap::get(grid, from) == 0 || Bitmap::get(grid, to) == 0 {
return array![].span();
}
// [Effect] Initialize the start and target nodes
let mut start = NodeTrait::new(from, 0, 0, 0);
let target = NodeTrait::new(to, 0, 0, 0);
// [Effect] Initialize the heap and the visited nodes
let mut heap: Heap<Node> = HeapTrait::new();
let mut visited: Felt252Dict<bool> = Default::default();
heap.add(start);
// [Compute] Evaluate the path until the target is reached
while !heap.is_empty() {
// [Compute] Get the node with the smallest heuristic cost (to the target)
let current: Node = heap.pop_front().unwrap();
visited.insert(current.position.into(), true);
// [Check] Stop if we reached the target
if current.position == target.position {
break;
}
// [Compute] Evaluate the neighbors for all 4 directions
if Self::check(grid, width, height, current.position, Direction::North, ref visited) {
let neighbor_position = current.position + width;
Self::assess(width, neighbor_position, current, target, ref heap);
}
if Self::check(grid, width, height, current.position, Direction::East, ref visited) {
let neighbor_position = current.position + 1;
Self::assess(width, neighbor_position, current, target, ref heap);
}
if Self::check(grid, width, height, current.position, Direction::South, ref visited) {
let neighbor_position = current.position - width;
Self::assess(width, neighbor_position, current, target, ref heap);
}
if Self::check(grid, width, height, current.position, Direction::West, ref visited) {
let neighbor_position = current.position - 1;
Self::assess(width, neighbor_position, current, target, ref heap);
}
};

// [Return] The path from the start to the target
Self::path(ref heap, start, target)
}

/// Check if the position can be visited in the specified direction.
/// # Arguments
/// * `grid` - The grid to search (1 is walkable and 0 is not)
/// * `width` - The width of the grid
/// * `height` - The height of the grid
/// * `position` - The current position
/// * `direction` - The direction to check
/// * `visited` - The visited nodes
/// # Returns
/// * Whether the position can be visited in the specified direction
#[inline]
fn check(
grid: felt252,
width: u8,
height: u8,
position: u8,
direction: Direction,
ref visited: Felt252Dict<bool>
) -> bool {
let (x, y) = (position % width, position / width);
match direction {
Direction::North => (y < height - 1)
&& (Bitmap::get(grid, position + width) == 1)
&& !visited.get((position + width).into()),
Direction::East => (x < width - 1)
&& (Bitmap::get(grid, position + 1) == 1)
&& !visited.get((position + 1).into()),
Direction::South => (y > 0)
&& (Bitmap::get(grid, position - width) == 1)
&& !visited.get((position - width).into()),
Direction::West => (x > 0)
&& (Bitmap::get(grid, position - 1) == 1)
&& !visited.get((position - 1).into()),
_ => false,
}
}

/// Assess the neighbor node and update the heap.
/// # Arguments
/// * `width` - The width of the grid
/// * `neighbor_position` - The position of the neighbor
/// * `current` - The current node
/// * `target` - The target node
/// * `heap` - The heap of nodes
/// # Effects
/// * Update the heap with the neighbor node
#[inline]
fn assess(
width: u8, neighbor_position: u8, current: Node, target: Node, ref heap: Heap<Node>,
) {
let neighbor_hcost = Self::heuristic(neighbor_position, target.position, width);
let mut neighbor = match heap.get(neighbor_position.into()) {
Option::Some(node) => node,
Option::None => NodeTrait::new(neighbor_position, current.position, 0, neighbor_hcost),
};
if !heap.contains(neighbor.position) {
neighbor.source = current.position;
return heap.add(neighbor);
}
return heap.update(neighbor);
}

/// Compute the heuristic cost between two positions.
/// # Arguments
/// * `position` - The current position
/// * `target` - The target position
/// * `width` - The width of the grid
/// # Returns
/// * The heuristic cost between the two positions
#[inline]
fn heuristic(position: u8, target: u8, width: u8) -> u16 {
let (x1, y1) = (position % width, position / width);
let (x2, y2) = (target % width, target / width);
let dx = if x1 > x2 {
x1 - x2
} else {
x2 - x1
};
let dy = if y1 > y2 {
y1 - y2
} else {
y2 - y1
};
(dx + dy).into()
}

/// Reconstruct the path from the target to the start.
#[inline]
fn path(ref heap: Heap<Node>, start: Node, target: Node) -> Span<u8> {
let mut path: Array<u8> = array![];
match heap.get(target.position) {
Option::None => { path.span() },
Option::Some(mut current) => {
loop {
if current.position == start.position {
break;
}
path.append(current.position);
current = heap.at(current.source);
};
path.span()
},
}
}
}

#[cfg(test)]
mod test {
use super::{Greedy, Node, NodeTrait};

#[test]
fn test_greedy_search_small() {
let grid: felt252 = 0x1EB;
let width = 3;
let height = 3;
let from = 0;
let to = 8;
let mut path = Greedy::search(grid, width, height, from, to);
assert_eq!(path, array![8, 7, 6, 3].span());
}

#[test]
fn test_greedy_search_impossible() {
let grid: felt252 = 0x1AB;
let width = 3;
let height = 3;
let from = 0;
let to = 8;
let mut path = Greedy::search(grid, width, height, from, to);
assert_eq!(path, array![].span());
}

#[test]
fn test_greedy_search_large() {
// 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
// 0 0 0 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0
// 0 0 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0
// 0 0 1 1 1 1 1 0 1 1 0 0 0 0 0 0 0 0
// 0 0 0 1 1 1 1 ┌───x 0 0 0 0 0 0 0 0
// 0 0 0 0 1 1 1 │ 0 0 0 1 0 0 1 0 0 0
// 0 0 0 1 1 1 1 │ 0 0 0 ┌───────┐ 0 0
// 0 0 1 1 1 1 1 └───────┘ 1 1 1 └─┐ 0
// 0 0 0 1 1 1 1 0 1 1 1 0 1 1 1 1 │ 0
// 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 │ 0
// 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 s 0
// 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 0
// 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0
// 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
let grid: felt252 = 0x7F003F800FB001FC003C481F1F0FFFE1EEF83FFE1FFF81FFE03FF80000;
let width = 18;
let height = 14;
let from = 55;
let to = 170;
let mut path = Greedy::search(grid, width, height, from, to);

assert_eq!(
path,
array![
170,
171,
172,
154,
136,
118,
117,
116,
115,
114,
132,
131,
130,
129,
128,
110,
109,
91,
73
]
.span()
);
}
}
1 change: 1 addition & 0 deletions crates/map/src/lib.cairo
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ pub mod helpers {
pub mod spreader;
pub mod astar;
pub mod heap;
pub mod greedy;

#[cfg(target: "test")]
pub mod printer;
Expand Down

0 comments on commit 8a89446

Please sign in to comment.