Skip to content

Commit

Permalink
🐛 Fix pathfinding
Browse files Browse the repository at this point in the history
  • Loading branch information
@bal7hazar
bal7hazar committed Sep 16, 2024
1 parent b3f7311 commit 694d35f
Show file tree
Hide file tree
Showing 2 changed files with 96 additions and 87 deletions.
85 changes: 70 additions & 15 deletions crates/map/src/helpers/astar.cairo
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,9 +36,8 @@ pub impl Astar of AstarTrait {
let mut visited: Felt252Dict<bool> = Default::default();
heap.add(start);
// [Compute] Evaluate the path until the target is reached
while !heap.is_empty() {
while let Option::Some(current) = heap.pop_front() {
// [Compute] Get the less expensive node
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 {
Expand All @@ -50,15 +49,15 @@ pub impl Astar of AstarTrait {
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;
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;
let neighbor_position = current.position + 1;
Self::assess(width, neighbor_position, current, target, ref heap);
}
};
Expand Down Expand Up @@ -91,15 +90,15 @@ pub impl Astar of AstarTrait {
Direction::North => (y < height - 1)
&& (Bitmap::get(grid, position + width) == 1)
&& !visisted.get((position + width).into()),
Direction::East => (x < width - 1)
&& (Bitmap::get(grid, position + 1) == 1)
&& !visisted.get((position + 1).into()),
Direction::East => (x > 0)
&& (Bitmap::get(grid, position - 1) == 1)
&& !visisted.get((position - 1).into()),
Direction::South => (y > 0)
&& (Bitmap::get(grid, position - width) == 1)
&& !visisted.get((position - width).into()),
Direction::West => (x > 0)
&& (Bitmap::get(grid, position - 1) == 1)
&& !visisted.get((position - 1).into()),
Direction::West => (x < width - 1)
&& (Bitmap::get(grid, position + 1) == 1)
&& !visisted.get((position + 1).into()),
_ => false,
}
}
Expand Down Expand Up @@ -247,10 +246,10 @@ mod test {
// 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 1 1 1 1 1 0 0
// 0 0 1 1 1 1 1 └───┐ 1 1 1 1 1 1 1 0
// 0 0 0 1 1 1 1 0 1 │ 1 0 1 1 1 1 1 0
// 0 0 0 0 1 1 1 1 1 └─┐ 1 1 1 1 1 1 0
// 0 0 0 1 1 1 1 1 1 1 └───────────s 0
// 0 0 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
Expand All @@ -262,7 +261,63 @@ mod test {
let mut path = Astar::search(grid, width, height, from, to);
assert_eq!(
path,
array![170, 171, 172, 154, 136, 118, 117, 116, 98, 80, 79, 61, 60, 59, 58, 57, 56]
array![
170, 171, 172, 154, 136, 118, 117, 116, 115, 114, 113, 112, 111, 110, 109, 91, 73
]
.span()
);
}

#[test]
fn test_astar_search_issue() {
// 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0
// 0 1 1 1 1 1 1 1 0 1 1 0 0 1 0
// 0 1 1 1 1 1 1 1 0 0 E * 1 1 0 <-- 180 + 4 = 184
// 0 1 1 1 1 1 1 1 1 1 0 * 0 1 0
// 0 1 1 1 1 1 1 1 1 1 0 * 0 1 0
// 0 1 1 1 0 0 1 0 0 0 1 * * 0 0
// 0 1 1 1 1 0 0 1 1 1 1 0 * * 0
// 1 1 1 1 0 1 1 1 0 0 1 1 0 * 1
// 0 1 1 1 1 0 0 0 0 1 0 0 * * 0
// 0 1 1 1 0 0 * * * * * * * 0 0
// 0 1 1 1 0 0 * 1 0 1 1 1 0 0 0
// 0 1 1 * * * * 0 1 0 1 1 1 1 0
// 0 1 1 S 1 1 1 0 1 0 1 1 1 1 0 <-- 30 + 11 = 41
// 0 1 1 1 1 1 1 1 1 1 0 1 1 1 0
// 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0
let grid: felt252 = 0x201fd93f9e7fd4ffa9c8e3cf6f736f099cfe39b87ebcfd79ff70080;
let width = 15;
let height = 15;
let from = 41;
let to = 184;
let mut path = Astar::search(grid, width, height, from, to);
assert_eq!(
path,
array![
184,
183,
168,
153,
138,
137,
122,
121,
106,
91,
92,
77,
78,
79,
80,
81,
82,
83,
68,
53,
54,
55,
56,
]
.span()
);
}
Expand Down
98 changes: 26 additions & 72 deletions crates/map/src/helpers/heap.cairo
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,11 @@ const KEY_OFFSET: felt252 = 252;

/// Traits.
pub trait HeapTrait<T> {
/// Create a new heap.
/// # Returns
/// * The heap
fn new() -> Heap<T>;
/// Create if the heap is empty.
/// Check if the heap is empty.
/// # Arguments
/// * `self` - The heap
/// # Returns
Expand Down Expand Up @@ -87,6 +90,14 @@ pub trait HeapTrait<T> {
/// # Effects
/// * The items are swapped
fn swap(ref self: Heap<T>, lhs: u8, rhs: u8);
/// Print the heap.
/// # Arguments
/// * `self` - The heap to print
/// # Effects
/// * The heap is printed
/// # Note
/// * This function is only for debugging purposes
fn print(ref self: Heap<T>);
}

pub trait ItemTrait<T> {
Expand All @@ -111,30 +122,16 @@ pub struct Heap<T> {

/// Implementations.
pub impl HeapImpl<T, +ItemTrait<T>, +PartialOrd<T>, +Copy<T>, +Drop<T>> of HeapTrait<T> {
/// Create a new heap.
/// # Returns
/// * The heap
#[inline]
fn new() -> Heap<T> {
Heap { len: 0, keys: Default::default(), data: Default::default(), }
}

/// Check if the heap is empty.
/// # Arguments
/// * `self` - The heap
/// # Returns
/// * `true` if the heap is empty, `false` otherwise
#[inline]
fn is_empty(self: @Heap<T>) -> bool {
*self.len == 0
}

/// Get an item from the heap if it exists.
/// # Arguments
/// * `self` - The heap
/// * `key` - The key of the item
/// # Returns
/// * The item if it exists, `None` otherwise
#[inline]
fn get(ref self: Heap<T>, key: u8) -> Option<T> {
let nullable: Nullable<T> = self.data.get(key.into());
Expand All @@ -144,38 +141,18 @@ pub impl HeapImpl<T, +ItemTrait<T>, +PartialOrd<T>, +Copy<T>, +Drop<T>> of HeapT
Option::Some(nullable.deref())
}

/// Get an item from the heap.
/// # Arguments
/// * `self` - The heap
/// * `key` - The key of the item
/// # Returns
/// * The item
/// # Panics
/// * If the item does not exist
#[inline]
fn at(ref self: Heap<T>, key: u8) -> T {
self.data.get(key.into()).deref()
}

/// Check if the heap contains an item.
/// # Arguments
/// * `self` - The heap
/// * `key` - The key of the item
/// # Returns
/// * `true` if the item exists, `false` otherwise
#[inline]
fn contains(ref self: Heap<T>, key: u8) -> bool {
let index = self.keys.get(key.into() + KEY_OFFSET);
let item_key = self.keys.get(index.into());
index < self.len && item_key == key
}

/// Add an item to the heap.
/// # Arguments
/// * `self` - The heap
/// * `item` - The item to add
/// # Effects
/// * The item is added at the end of the heap and the heap is sorted up
#[inline]
fn add(ref self: Heap<T>, item: T) {
// [Effect] Update heap length
Expand All @@ -190,15 +167,6 @@ pub impl HeapImpl<T, +ItemTrait<T>, +PartialOrd<T>, +Copy<T>, +Drop<T>> of HeapT
self.sort_up(key);
}

/// Update an item in the heap.
/// # Arguments
/// * `self` - The heap
/// * `item` - The item to update
/// # Effects
/// * The item is updated and the heap is sorted up
/// # Info
/// * The heap is only sorted up since it cannot be updated with a lower score in case of A*
/// algorithm
#[inline]
fn update(ref self: Heap<T>, item: T) {
// [Effect] Update item
Expand All @@ -208,13 +176,6 @@ pub impl HeapImpl<T, +ItemTrait<T>, +PartialOrd<T>, +Copy<T>, +Drop<T>> of HeapT
self.sort_up(key);
}

/// Pop the first item from the heap.
/// # Arguments
/// * `self` - The heap
/// # Returns
/// * The first item if the heap is not empty, `None` otherwise
/// # Effects
/// * The first item is removed, replaced by the last item and the heap is sorted down
#[inline]
fn pop_front(ref self: Heap<T>) -> Option<T> {
if self.is_empty() {
Expand All @@ -231,12 +192,6 @@ pub impl HeapImpl<T, +ItemTrait<T>, +PartialOrd<T>, +Copy<T>, +Drop<T>> of HeapT
Option::Some(first)
}

/// Sort an item up in the heap.
/// # Arguments
/// * `self` - The heap
/// * `item_key` - The key of the item to sort up
/// # Effects
/// * The items are swapped from bottom to top until the item is in the right place
#[inline]
fn sort_up(ref self: Heap<T>, item_key: u8) {
// [Compute] Item
Expand All @@ -254,12 +209,6 @@ pub impl HeapImpl<T, +ItemTrait<T>, +PartialOrd<T>, +Copy<T>, +Drop<T>> of HeapT
}
}

/// Sort an item down in the heap.
/// # Arguments
/// * `self` - The heap
/// * `item_key` - The key of the item to sort down
/// # Effects
/// * The items are swapped from top to bottom until the item is in the right place
#[inline]
fn sort_down(ref self: Heap<T>, item_key: u8) {
// [Compute] Item
Expand All @@ -273,7 +222,7 @@ pub impl HeapImpl<T, +ItemTrait<T>, +PartialOrd<T>, +Copy<T>, +Drop<T>> of HeapT
let mut child_key: u8 = self.keys.get(index.into());
let mut child: T = self.data.get(child_key.into()).deref();
// [Compute] Assess right child side
let rhs_index = index * 2 + 2;
let rhs_index = lhs_index + 1;
if rhs_index < self.len {
let rhs_key: u8 = self.keys.get(rhs_index.into());
let rhs: T = self.data.get(rhs_key.into()).deref();
Expand All @@ -293,13 +242,6 @@ pub impl HeapImpl<T, +ItemTrait<T>, +PartialOrd<T>, +Copy<T>, +Drop<T>> of HeapT
}
}

/// Swap two items in the heap.
/// # Arguments
/// * `self` - The heap
/// * `lhs` - The key of the first item
/// * `rhs` - The key of the second item
/// # Effects
/// * The items are swapped
#[inline]
fn swap(ref self: Heap<T>, lhs: u8, rhs: u8) {
// [Effect] Swap keys
Expand All @@ -310,9 +252,21 @@ pub impl HeapImpl<T, +ItemTrait<T>, +PartialOrd<T>, +Copy<T>, +Drop<T>> of HeapT
self.keys.insert(lhs_index.into(), rhs);
self.keys.insert(rhs_index.into(), lhs);
}

#[inline]
fn print(ref self: Heap<T>) {
println!("");
let mut index = 0;
while index < self.len {
let key = self.keys.get(index.into());
println!("{} : {}", index, key);
index += 1;
};
println!("");
}
}

impl DestructHeap<T, +Drop<T>> of Destruct<Heap<T>> {
pub impl DestructHeap<T, +Drop<T>> of Destruct<Heap<T>> {
fn destruct(self: Heap<T>) nopanic {
self.keys.squash();
self.data.squash();
Expand Down

0 comments on commit 694d35f

Please sign in to comment.