From 2f0047585a9bb196e024a80e8000e71e8f833e9c Mon Sep 17 00:00:00 2001 From: bal7hazar Date: Mon, 9 Sep 2024 15:13:31 -0400 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20pathfinding?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crates/pathfinding/src/helpers/astar.cairo | 192 ++++++++++++++ crates/pathfinding/src/helpers/bitmap.cairo | 187 ++++++++++++++ crates/pathfinding/src/helpers/heap.cairo | 246 ++++++++++++------ crates/pathfinding/src/helpers/power.cairo | 271 ++++++++++++++++++++ crates/pathfinding/src/lib.cairo | 2 + crates/pathfinding/src/types/node.cairo | 40 +-- 6 files changed, 838 insertions(+), 100 deletions(-) create mode 100644 crates/pathfinding/src/helpers/bitmap.cairo create mode 100644 crates/pathfinding/src/helpers/power.cairo diff --git a/crates/pathfinding/src/helpers/astar.cairo b/crates/pathfinding/src/helpers/astar.cairo index 8b137891..5d9b2cb8 100644 --- a/crates/pathfinding/src/helpers/astar.cairo +++ b/crates/pathfinding/src/helpers/astar.cairo @@ -1 +1,193 @@ +// Core imports +use core::dict::{Felt252Dict, Felt252DictTrait}; + +// Internal imports + +use origami_pathfinding::helpers::heap::{Heap, HeapTrait}; +use origami_pathfinding::helpers::bitmap::Bitmap; +use origami_pathfinding::types::node::{Node, NodeTrait}; + +#[generate_trait] +pub impl Astar of AstarTrait { + #[inline] + fn search(grid: felt252, width: u8, height: u8, from: u8, to: u8) -> Span { + let mut start = NodeTrait::new(from, 0, 0, 0); + let target = NodeTrait::new(to, 0, 0, 0); + let mut heap: Heap = HeapTrait::new(); + let mut visited: Felt252Dict = Default::default(); + heap.add(start); + + // [Compute] Evaluate the path until the target is reached + while !heap.is_empty() { + // [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 { + break; + } + // [Compute] Evaluate the neighbors + if Self::check(grid, width, height, current.position, 0, 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, 1, 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, 2, 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, 3, ref visited) { + let neighbor_position = current.position - 1; + Self::assess(width, neighbor_position, current, target, ref heap); + } + }; + + // [Compute] Reconstruct the path from the target to the start + let mut path: Array = array![]; + let mut current = heap.at(target.position); + loop { + if current.position == start.position { + break; + } + path.append(current.position); + current = heap.at(current.source); + }; + + // [Return] The path from the start to the target + path.span() + } + + #[inline] + fn check( + grid: felt252, + width: u8, + height: u8, + position: u8, + direction: u8, + ref visisted: Felt252Dict + ) -> bool { + let (x, y) = (position % width, position / width); + match direction { + 0 => (y < height - 1) + && (Bitmap::get(grid, position + width) == 1) + && !visisted.get((position + width).into()), + 1 => (x < width - 1) + && (Bitmap::get(grid, position + 1) == 1) + && !visisted.get((position + 1).into()), + 2 => (y > 0) + && (Bitmap::get(grid, position - width) == 1) + && !visisted.get((position - width).into()), + _ => (x > 0) + && (Bitmap::get(grid, position - 1) == 1) + && !visisted.get((position - 1).into()), + } + } + + #[inline] + fn assess( + width: u8, neighbor_position: u8, current: Node, target: Node, ref heap: Heap, + ) { + let distance = Self::heuristic(current.position, neighbor_position, width); + let neighbor_gcost = current.gcost + distance; + 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, neighbor_gcost, neighbor_hcost + ), + }; + if neighbor_gcost < neighbor.gcost || !heap.contains(neighbor.position) { + neighbor.gcost = neighbor_gcost; + neighbor.source = current.position; + if !heap.contains(neighbor.position) { + heap.add(neighbor); + } else { + heap.update(neighbor); + } + } + } + + #[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() + } +} + +#[cfg(test)] +mod test { + // Local imports + + use super::{Astar, Node, NodeTrait}; + + #[test] + fn test_astar_search_small() { + // x 1 1 + // 1 0 1 + // 0 1 s + let grid: felt252 = 0x1EB; + let width = 3; + let height = 3; + let from = 0; + let to = 8; + let mut path = Astar::search(grid, width, height, from, to); + assert_eq!(path.len(), 4); + assert_eq!(path, array![8, 7, 6, 3].span()); + } + + #[test] + fn test_astar_search_medium() { + // 1 x 0 0 + // 1 0 1 1 + // 1 1 1 1 + // 1 1 1 s + let grid: felt252 = 0xCBFF; + let width = 4; + let height = 4; + let from = 0; + let to = 14; + let mut path = Astar::search(grid, width, height, from, to); + assert_eq!(path.len(), 7); + assert_eq!(path, array![14, 15, 11, 7, 6, 5, 4].span()); + } + + #[test] + fn test_astar_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 ┌─x 0 0 0 0 0 0 0 0 + // 0 0 1 1 1 1 1 1 │ 0 0 0 0 0 0 0 0 0 + // 0 0 1 1 1 1 1 0 │ 1 0 0 0 0 0 0 0 0 + // 0 0 0 1 1 1 1 ┌─┘ 1 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 └────────────── ──┐ 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 │ 0 + // 0 0 0 0 0 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 x 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 = 19; + let to = 224; + let mut path = Astar::search(grid, width, height, from, to); + assert_eq!(path.len(), 22); + } +} diff --git a/crates/pathfinding/src/helpers/bitmap.cairo b/crates/pathfinding/src/helpers/bitmap.cairo new file mode 100644 index 00000000..27c3bdb9 --- /dev/null +++ b/crates/pathfinding/src/helpers/bitmap.cairo @@ -0,0 +1,187 @@ +// Internal imports + +use origami_pathfinding::helpers::power::{TwoPower, TwoPowerTrait}; + +#[generate_trait] +pub impl Bitmap of BitmapTrait { + /// Count the number of bits set to 1 in the number + /// # Arguments + /// * `x` - The value for which to count the number of bits set to 1 + /// # Returns + /// * The number of bits set to 1 + #[inline] + fn popcount(x: felt252) -> u8 { + let mut x: u256 = x.into(); + let mut count: u8 = 0; + while (x > 0) { + count += PrivateTrait::_popcount((x % 0x100000000).try_into().unwrap()); + x /= 0x100000000; + }; + count + } + + /// Get the bit at the specified index + /// # Arguments + /// * `x` - The bitmap + /// * `index` - The index of the bit to get + /// # Returns + /// * The value of the bit at the specified index + #[inline] + fn get(x: felt252, index: u8) -> u8 { + let x: u256 = x.into(); + let offset: u256 = TwoPower::pow(index); + (x / offset % 2).try_into().unwrap() + } + + /// Set the bit at the specified index + /// # Arguments + /// * `x` - The bitmap + /// * `index` - The index of the bit to set + /// # Returns + /// * The bitmap with the bit at the specified index set to 1 + #[inline] + fn set(x: felt252, index: u8) -> felt252 { + let x: u256 = x.into(); + let offset: u256 = TwoPower::pow(index); + let bit = x / offset % 2; + let offset: u256 = offset * (1 - bit); + (x + offset).try_into().unwrap() + } + + /// Unset the bit at the specified index + /// # Arguments + /// * `x` - The bitmap + /// * `index` - The index of the bit to unset + /// # Returns + /// * The bitmap with the bit at the specified index set to 0 + #[inline] + fn unset(x: felt252, index: u8) -> felt252 { + let x: u256 = x.into(); + let offset: u256 = TwoPower::pow(index); + let bit = x / offset % 2; + let offset: u256 = offset * bit; + (x - offset).try_into().unwrap() + } + + /// The index of the least significant bit of the number, + /// where the least significant bit is at index 0 and the most significant bit is at index 255 + /// # Arguments + /// * `x` - The value for which to compute the least significant bit, must be greater than 0. + /// # Returns + /// * The index of the least significant bit, if 0 returns the index 0 + #[inline] + fn least_significant_bit(x: felt252) -> u8 { + let mut x: u256 = x.into(); + if x == 0 { + return 0; + } + let mut r: u8 = 255; + + if (x & 0xFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF) > 0 { + r -= 128; + } else { + x /= 0x100000000000000000000000000000000; + } + if (x & 0xFFFFFFFFFFFFFFFF) > 0 { + r -= 64; + } else { + x /= 0x10000000000000000; + } + if (x & 0xFFFFFFFF) > 0 { + r -= 32; + } else { + x /= 0x100000000; + } + if (x & 0xFFFF) > 0 { + r -= 16; + } else { + x /= 0x10000; + } + if (x & 0xFF) > 0 { + r -= 8; + } else { + x /= 0x100; + } + if (x & 0xF) > 0 { + r -= 4; + } else { + x /= 0x10; + } + if (x & 0x3) > 0 { + r -= 2; + } else { + x /= 0x4; + } + if (x & 0x1) > 0 { + r -= 1; + } + r + } +} + +#[generate_trait] +impl Private of PrivateTrait { + /// Count the number of bits set to 1 in the number for a u32 + /// # Arguments + /// * `x` - The value for which to count the number of bits set to 1 + /// # Returns + /// * The number of bits set to 1 + #[inline] + fn _popcount(mut x: u32) -> u8 { + x -= ((x / 2) & 0x55555555); + x = (x & 0x33333333) + ((x / 4) & 0x33333333); + x = (x + (x / 16)) & 0x0f0f0f0f; + x += (x / 256); + x += (x / 65536); + return (x % 64).try_into().unwrap(); + } +} + +#[cfg(test)] +mod tests { + // Local imports + + use super::Bitmap; + + #[test] + fn test_bitmap_popcount_large() { + let count = Bitmap::popcount(0x4003FBB391C53CCB8E99752EB665586B695BB2D026BEC9071FF30002); + assert_eq!(count, 109); + } + + #[test] + fn test_bitmap_popcount_small() { + let count = Bitmap::popcount(0b101); + assert_eq!(count, 2); + } + + #[test] + fn test_bitmap_get() { + let bit = Bitmap::get(0b1001011, 0); + assert_eq!(bit, 1); + } + + #[test] + fn test_bitmap_set() { + let bit = Bitmap::set(0b1001010, 0); + assert_eq!(bit, 0b1001011); + } + + #[test] + fn test_bitmap_set_unchanged() { + let bit = Bitmap::set(0b1001011, 0); + assert_eq!(bit, 0b1001011); + } + + #[test] + fn test_bitmap_unset() { + let bit = Bitmap::unset(0b1001011, 0); + assert_eq!(bit, 0b1001010); + } + + #[test] + fn test_bitmap_unset_unchanged() { + let bit = Bitmap::unset(0b1001010, 0); + assert_eq!(bit, 0b1001010); + } +} diff --git a/crates/pathfinding/src/helpers/heap.cairo b/crates/pathfinding/src/helpers/heap.cairo index 0ffe1c47..d8ab5c43 100644 --- a/crates/pathfinding/src/helpers/heap.cairo +++ b/crates/pathfinding/src/helpers/heap.cairo @@ -10,31 +10,38 @@ use origami_pathfinding::types::node::Node; pub trait HeapTrait { fn new() -> Heap; fn is_empty(self: @Heap) -> bool; - fn contains(ref self: Heap, item: T) -> bool; + fn get(ref self: Heap, key: u8) -> Option; + fn at(ref self: Heap, key: u8) -> T; + fn contains(ref self: Heap, key: u8) -> bool; fn add(ref self: Heap, item: T); fn update(ref self: Heap, item: T); fn pop_front(ref self: Heap) -> Option; - fn sort_up(ref self: Heap, item: T); - fn sort_down(ref self: Heap, item: T); - fn swap(ref self: Heap, ref lhs: T, ref rhs: T); + fn sort_up(ref self: Heap, item_key: u8); + fn sort_down(ref self: Heap, item_key: u8); + fn swap(ref self: Heap, lhs: u8, rhs: u8); } pub trait ItemTrait { - fn get_index(self: T) -> u8; - fn set_index(ref self: T, index: u8); + fn key(self: T) -> u8; } /// Types. pub struct Heap { pub len: u8, + pub keys: Felt252Dict, + pub indexes: Felt252Dict, pub data: Felt252Dict>, } /// Implementations. -pub impl HeapImpl, +PartialOrd, +Copy, +Drop> of HeapTrait { +pub impl HeapImpl< + T, +ItemTrait, +PartialOrd, +PartialEq, +Copy, +Drop +> of HeapTrait { #[inline] fn new() -> Heap { - Heap { len: 0, data: Default::default(), } + Heap { + len: 0, keys: Default::default(), indexes: Default::default(), data: Default::default(), + } } #[inline] @@ -43,25 +50,48 @@ pub impl HeapImpl, +PartialOrd, +Copy, +Drop> of HeapT } #[inline] - fn contains(ref self: Heap, item: T) -> bool { - if item.get_index() >= self.len { - return false; + fn get(ref self: Heap, key: u8) -> Option { + let nullable: Nullable = self.data.get(key.into()); + if nullable.is_null() { + Option::None + } else { + Option::Some(nullable.deref()) } - let node: T = self.data.get(item.get_index().into()).deref(); - node.get_index() == item.get_index() } #[inline] - fn add(ref self: Heap, mut item: T) { - item.set_index(self.len); - self.data.insert(item.get_index().into(), NullableTrait::new(item)); - self.sort_up(item); + fn at(ref self: Heap, key: u8) -> T { + self.data.get(key.into()).deref() + } + + #[inline] + fn contains(ref self: Heap, key: u8) -> bool { + let index = self.indexes.get(key.into()); + let item_key = self.keys.get(index.into()); + index < self.len && item_key == key + } + + #[inline] + fn add(ref self: Heap, item: T) { + // [Effect] Update heap length + let key = item.key(); + let index = self.len; self.len += 1; + // [Effect] Insert item at the end + self.data.insert(key.into(), NullableTrait::new(item)); + self.keys.insert(index.into(), key); + self.indexes.insert(key.into(), index); + // [Effect] Sort up + self.sort_up(key); } #[inline] fn update(ref self: Heap, item: T) { - self.sort_up(item); + // [Effect] Update item + let key = item.key(); + self.data.insert(key.into(), NullableTrait::new(item)); + // [Effect] Sort up (since it cannot be updated with a lower value) + self.sort_up(key); } #[inline] @@ -70,24 +100,28 @@ pub impl HeapImpl, +PartialOrd, +Copy, +Drop> of HeapT return Option::None; } self.len -= 1; - let first: T = self.data.get(0).deref(); - let mut last: T = self.data.get(self.len.into()).deref(); - last.set_index(0); - self.data.insert(0, NullableTrait::new(last)); - self.sort_down(last); + let first_key: u8 = self.keys.get(0); + let mut first: T = self.data.get(first_key.into()).deref(); + if self.len != 0 { + let last_key: u8 = self.keys.get(self.len.into()); + self.swap(first_key, last_key); + self.sort_down(last_key); + } Option::Some(first) } #[inline] - fn sort_up(ref self: Heap, mut item: T) { - loop { - if item.get_index() == 0 { - break; - } - let index = (item.get_index() - 1) / 2; - let mut parent: T = self.data.get(index.into()).deref(); + fn sort_up(ref self: Heap, item_key: u8) { + // [Compute] Item + let item: T = self.data.get(item_key.into()).deref(); + let mut index = self.indexes.get(item_key.into()); + // [Compute] Peform swaps until the item is in the right place + while index != 0 { + index = (index - 1) / 2; + let parent_key = self.keys.get(index.into()); + let mut parent: T = self.data.get(parent_key.into()).deref(); if parent > item { - self.swap(ref parent, ref item); + self.swap(parent_key, item_key); } else { break; } @@ -95,40 +129,48 @@ pub impl HeapImpl, +PartialOrd, +Copy, +Drop> of HeapT } #[inline] - fn sort_down(ref self: Heap, mut item: T) { - loop { - // [Check] Stop criteria - let lhs_index = item.get_index() * 2 + 1; - if lhs_index >= self.len { - break; - } + fn sort_down(ref self: Heap, item_key: u8) { + // [Compute] Item + let item: T = self.data.get(item_key.into()).deref(); + let mut index: u8 = self.indexes.get(item_key.into()); + // [Compute] Peform swaps until the item is in the right place + let mut lhs_index = index * 2 + 1; + while lhs_index < self.len { // [Compute] Child to swap - let rhs_index = item.get_index() * 2 + 2; - let lhs: T = self.data.get(lhs_index.into()).deref(); - let rhs: T = self.data.get(rhs_index.into()).deref(); - let mut child: T = if rhs_index < self.len && rhs < lhs { - rhs - } else { - lhs - }; + index = lhs_index; + 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; + 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(); + if rhs < child { + index = rhs_index; + child_key = rhs_key; + child = rhs; + }; + } // [Effect] Swap if necessary if item > child { - self.swap(ref item, ref child); + self.swap(item_key, child_key); } else { break; } + // [Check] Stop criteria, assess left child side + lhs_index = index * 2 + 1; } } #[inline] - fn swap(ref self: Heap, ref lhs: T, ref rhs: T) { - // [Effect] Swap indexes - let (lhs_index, rhs_index) = (lhs.get_index(), rhs.get_index()); - lhs.set_index(rhs_index); - rhs.set_index(lhs_index); - // [Effect] Swap nodes - self.data.insert(lhs_index.into(), NullableTrait::new(rhs)); - self.data.insert(rhs_index.into(), NullableTrait::new(lhs)); + fn swap(ref self: Heap, lhs: u8, rhs: u8) { + // [Effect] Swap keys + let lhs_index = self.indexes.get(lhs.into()); + let rhs_index = self.indexes.get(rhs.into()); + self.indexes.insert(lhs.into(), rhs_index); + self.indexes.insert(rhs.into(), lhs_index); + self.keys.insert(lhs_index.into(), rhs); + self.keys.insert(rhs_index.into(), lhs); } } @@ -142,7 +184,7 @@ impl DestructHeap> of Destruct> { mod tests { // Local imports - use super::{Node, Heap, HeapTrait}; + use super::{Node, Heap, HeapTrait, ItemTrait}; #[test] fn test_heap_new() { @@ -153,7 +195,7 @@ mod tests { #[test] fn test_heap_add() { let mut heap: Heap = HeapTrait::new(); - let node: Node = Node { index: 0, fcost: 0, hcost: 0, }; + let node: Node = Node { position: 1, source: 1, gcost: 1, hcost: 1, }; heap.add(node); assert!(!heap.is_empty()); } @@ -161,62 +203,98 @@ mod tests { #[test] fn test_heap_contains() { let mut heap: Heap = HeapTrait::new(); - let node: Node = Node { index: 0, fcost: 0, hcost: 0, }; + let node: Node = Node { position: 1, source: 1, gcost: 1, hcost: 1, }; heap.add(node); - assert!(heap.contains(node)); + assert!(heap.contains(node.position)); } #[test] fn test_heap_not_contains() { let mut heap: Heap = HeapTrait::new(); - let node: Node = Node { index: 0, fcost: 0, hcost: 0, }; - assert!(!heap.contains(node)); + let node: Node = Node { position: 1, source: 1, gcost: 1, hcost: 1, }; + assert!(!heap.contains(node.position)); } #[test] fn test_heap_pop_front_sorted() { let mut heap: Heap = HeapTrait::new(); - let first: Node = Node { index: 0, fcost: 1, hcost: 0, }; - let second: Node = Node { index: 0, fcost: 1, hcost: 1, }; - let third: Node = Node { index: 0, fcost: 2, hcost: 0, }; + let first: Node = Node { position: 1, source: 1, gcost: 1, hcost: 1, }; + let second: Node = Node { position: 2, source: 2, gcost: 2, hcost: 2, }; + let third: Node = Node { position: 3, source: 3, gcost: 3, hcost: 3, }; heap.add(first); heap.add(second); heap.add(third); let popped: Node = heap.pop_front().unwrap(); - assert_eq!(popped.index, 0); - assert_eq!(popped.fcost, 1); - assert_eq!(popped.hcost, 0); + assert_eq!(popped.gcost, 1); + assert_eq!(popped.hcost, 1); } #[test] fn test_heap_pop_front_reversed() { let mut heap: Heap = HeapTrait::new(); - let first: Node = Node { index: 0, fcost: 2, hcost: 0, }; - let second: Node = Node { index: 0, fcost: 1, hcost: 1, }; - let third: Node = Node { index: 0, fcost: 1, hcost: 0, }; - heap.add(first); - heap.add(second); + let first: Node = Node { position: 1, source: 1, gcost: 1, hcost: 1, }; + let second: Node = Node { position: 2, source: 2, gcost: 2, hcost: 2, }; + let third: Node = Node { position: 3, source: 3, gcost: 3, hcost: 3, }; heap.add(third); + heap.add(second); + heap.add(first); let popped: Node = heap.pop_front().unwrap(); - assert_eq!(popped.index, 0); - assert_eq!(popped.fcost, 1); - assert_eq!(popped.hcost, 0); + assert_eq!(popped.gcost, 1); + assert_eq!(popped.hcost, 1); } #[test] fn test_heap_swap() { let mut heap: Heap = HeapTrait::new(); - let mut first: Node = Node { index: 0, fcost: 1, hcost: 1, }; - let mut second: Node = Node { index: 1, fcost: 2, hcost: 0, }; + let first: Node = Node { position: 1, source: 1, gcost: 1, hcost: 1, }; + let second: Node = Node { position: 2, source: 2, gcost: 2, hcost: 2, }; heap.add(first); heap.add(second); - heap.swap(ref first, ref second); - assert_eq!(first.index, 1); - assert_eq!(first.fcost, 1); - assert_eq!(first.hcost, 1); + heap.swap(first.key(), second.key()); + assert_eq!(first.position, 1); + assert_eq!(first.gcost, 1); let popped: Node = heap.pop_front().unwrap(); - assert_eq!(popped.index, 0); - assert_eq!(popped.fcost, 2); - assert_eq!(popped.hcost, 0); + assert_eq!(popped.position, 2); + assert_eq!(popped.gcost, 2); + } + + #[test] + fn test_heap_get() { + let mut heap: Heap = HeapTrait::new(); + let first: Node = Node { position: 1, source: 1, gcost: 1, hcost: 1, }; + let second: Node = Node { position: 2, source: 2, gcost: 2, hcost: 2, }; + heap.add(first); + heap.add(second); + assert_eq!(heap.get(first.position).unwrap().position, 1); + assert_eq!(heap.get(second.position).unwrap().position, 2); + heap.swap(first.key(), second.key()); + assert_eq!(heap.get(first.position).unwrap().position, 1); + assert_eq!(heap.get(second.position).unwrap().position, 2); + } + + #[test] + fn test_heap_at() { + let mut heap: Heap = HeapTrait::new(); + let first: Node = Node { position: 1, source: 1, gcost: 1, hcost: 1, }; + let second: Node = Node { position: 2, source: 2, gcost: 2, hcost: 2, }; + heap.add(first); + heap.add(second); + assert_eq!(heap.at(first.position).position, 1); + assert_eq!(heap.at(second.position).position, 2); + heap.swap(first.key(), second.key()); + assert_eq!(heap.at(first.position).position, 1); + assert_eq!(heap.at(second.position).position, 2); + } + + #[test] + fn test_heap_add_pop_add() { + let mut heap: Heap = HeapTrait::new(); + let first: Node = Node { position: 1, source: 1, gcost: 1, hcost: 1, }; + let second: Node = Node { position: 2, source: 2, gcost: 2, hcost: 2, }; + heap.add(first); + heap.add(second); + heap.pop_front().unwrap(); + assert_eq!(heap.at(1).position, 1); + assert_eq!(heap.at(2).position, 2); } } diff --git a/crates/pathfinding/src/helpers/power.cairo b/crates/pathfinding/src/helpers/power.cairo new file mode 100644 index 00000000..dfa7d59b --- /dev/null +++ b/crates/pathfinding/src/helpers/power.cairo @@ -0,0 +1,271 @@ +// Powers of two + +const TWO_POWER: [ + u256 + ; 252] = [ + 0x1, + 0x2, + 0x4, + 0x8, + 0x10, + 0x20, + 0x40, + 0x80, + 0x100, + 0x200, + 0x400, + 0x800, + 0x1000, + 0x2000, + 0x4000, + 0x8000, + 0x10000, + 0x20000, + 0x40000, + 0x80000, + 0x100000, + 0x200000, + 0x400000, + 0x800000, + 0x1000000, + 0x2000000, + 0x4000000, + 0x8000000, + 0x10000000, + 0x20000000, + 0x40000000, + 0x80000000, + 0x100000000, + 0x200000000, + 0x400000000, + 0x800000000, + 0x1000000000, + 0x2000000000, + 0x4000000000, + 0x8000000000, + 0x10000000000, + 0x20000000000, + 0x40000000000, + 0x80000000000, + 0x100000000000, + 0x200000000000, + 0x400000000000, + 0x800000000000, + 0x1000000000000, + 0x2000000000000, + 0x4000000000000, + 0x8000000000000, + 0x10000000000000, + 0x20000000000000, + 0x40000000000000, + 0x80000000000000, + 0x100000000000000, + 0x200000000000000, + 0x400000000000000, + 0x800000000000000, + 0x1000000000000000, + 0x2000000000000000, + 0x4000000000000000, + 0x8000000000000000, + 0x10000000000000000, + 0x20000000000000000, + 0x40000000000000000, + 0x80000000000000000, + 0x100000000000000000, + 0x200000000000000000, + 0x400000000000000000, + 0x800000000000000000, + 0x1000000000000000000, + 0x2000000000000000000, + 0x4000000000000000000, + 0x8000000000000000000, + 0x10000000000000000000, + 0x20000000000000000000, + 0x40000000000000000000, + 0x80000000000000000000, + 0x100000000000000000000, + 0x200000000000000000000, + 0x400000000000000000000, + 0x800000000000000000000, + 0x1000000000000000000000, + 0x2000000000000000000000, + 0x4000000000000000000000, + 0x8000000000000000000000, + 0x10000000000000000000000, + 0x20000000000000000000000, + 0x40000000000000000000000, + 0x80000000000000000000000, + 0x100000000000000000000000, + 0x200000000000000000000000, + 0x400000000000000000000000, + 0x800000000000000000000000, + 0x1000000000000000000000000, + 0x2000000000000000000000000, + 0x4000000000000000000000000, + 0x8000000000000000000000000, + 0x10000000000000000000000000, + 0x20000000000000000000000000, + 0x40000000000000000000000000, + 0x80000000000000000000000000, + 0x100000000000000000000000000, + 0x200000000000000000000000000, + 0x400000000000000000000000000, + 0x800000000000000000000000000, + 0x1000000000000000000000000000, + 0x2000000000000000000000000000, + 0x4000000000000000000000000000, + 0x8000000000000000000000000000, + 0x10000000000000000000000000000, + 0x20000000000000000000000000000, + 0x40000000000000000000000000000, + 0x80000000000000000000000000000, + 0x100000000000000000000000000000, + 0x200000000000000000000000000000, + 0x400000000000000000000000000000, + 0x800000000000000000000000000000, + 0x1000000000000000000000000000000, + 0x2000000000000000000000000000000, + 0x4000000000000000000000000000000, + 0x8000000000000000000000000000000, + 0x10000000000000000000000000000000, + 0x20000000000000000000000000000000, + 0x40000000000000000000000000000000, + 0x80000000000000000000000000000000, + 0x100000000000000000000000000000000, + 0x200000000000000000000000000000000, + 0x400000000000000000000000000000000, + 0x800000000000000000000000000000000, + 0x1000000000000000000000000000000000, + 0x2000000000000000000000000000000000, + 0x4000000000000000000000000000000000, + 0x8000000000000000000000000000000000, + 0x10000000000000000000000000000000000, + 0x20000000000000000000000000000000000, + 0x40000000000000000000000000000000000, + 0x80000000000000000000000000000000000, + 0x100000000000000000000000000000000000, + 0x200000000000000000000000000000000000, + 0x400000000000000000000000000000000000, + 0x800000000000000000000000000000000000, + 0x1000000000000000000000000000000000000, + 0x2000000000000000000000000000000000000, + 0x4000000000000000000000000000000000000, + 0x8000000000000000000000000000000000000, + 0x10000000000000000000000000000000000000, + 0x20000000000000000000000000000000000000, + 0x40000000000000000000000000000000000000, + 0x80000000000000000000000000000000000000, + 0x100000000000000000000000000000000000000, + 0x200000000000000000000000000000000000000, + 0x400000000000000000000000000000000000000, + 0x800000000000000000000000000000000000000, + 0x1000000000000000000000000000000000000000, + 0x2000000000000000000000000000000000000000, + 0x4000000000000000000000000000000000000000, + 0x8000000000000000000000000000000000000000, + 0x10000000000000000000000000000000000000000, + 0x20000000000000000000000000000000000000000, + 0x40000000000000000000000000000000000000000, + 0x80000000000000000000000000000000000000000, + 0x100000000000000000000000000000000000000000, + 0x200000000000000000000000000000000000000000, + 0x400000000000000000000000000000000000000000, + 0x800000000000000000000000000000000000000000, + 0x1000000000000000000000000000000000000000000, + 0x2000000000000000000000000000000000000000000, + 0x4000000000000000000000000000000000000000000, + 0x8000000000000000000000000000000000000000000, + 0x10000000000000000000000000000000000000000000, + 0x20000000000000000000000000000000000000000000, + 0x40000000000000000000000000000000000000000000, + 0x80000000000000000000000000000000000000000000, + 0x100000000000000000000000000000000000000000000, + 0x200000000000000000000000000000000000000000000, + 0x400000000000000000000000000000000000000000000, + 0x800000000000000000000000000000000000000000000, + 0x1000000000000000000000000000000000000000000000, + 0x2000000000000000000000000000000000000000000000, + 0x4000000000000000000000000000000000000000000000, + 0x8000000000000000000000000000000000000000000000, + 0x10000000000000000000000000000000000000000000000, + 0x20000000000000000000000000000000000000000000000, + 0x40000000000000000000000000000000000000000000000, + 0x80000000000000000000000000000000000000000000000, + 0x100000000000000000000000000000000000000000000000, + 0x200000000000000000000000000000000000000000000000, + 0x400000000000000000000000000000000000000000000000, + 0x800000000000000000000000000000000000000000000000, + 0x1000000000000000000000000000000000000000000000000, + 0x2000000000000000000000000000000000000000000000000, + 0x4000000000000000000000000000000000000000000000000, + 0x8000000000000000000000000000000000000000000000000, + 0x10000000000000000000000000000000000000000000000000, + 0x20000000000000000000000000000000000000000000000000, + 0x40000000000000000000000000000000000000000000000000, + 0x80000000000000000000000000000000000000000000000000, + 0x100000000000000000000000000000000000000000000000000, + 0x200000000000000000000000000000000000000000000000000, + 0x400000000000000000000000000000000000000000000000000, + 0x800000000000000000000000000000000000000000000000000, + 0x1000000000000000000000000000000000000000000000000000, + 0x2000000000000000000000000000000000000000000000000000, + 0x4000000000000000000000000000000000000000000000000000, + 0x8000000000000000000000000000000000000000000000000000, + 0x10000000000000000000000000000000000000000000000000000, + 0x20000000000000000000000000000000000000000000000000000, + 0x40000000000000000000000000000000000000000000000000000, + 0x80000000000000000000000000000000000000000000000000000, + 0x100000000000000000000000000000000000000000000000000000, + 0x200000000000000000000000000000000000000000000000000000, + 0x400000000000000000000000000000000000000000000000000000, + 0x800000000000000000000000000000000000000000000000000000, + 0x1000000000000000000000000000000000000000000000000000000, + 0x2000000000000000000000000000000000000000000000000000000, + 0x4000000000000000000000000000000000000000000000000000000, + 0x8000000000000000000000000000000000000000000000000000000, + 0x10000000000000000000000000000000000000000000000000000000, + 0x20000000000000000000000000000000000000000000000000000000, + 0x40000000000000000000000000000000000000000000000000000000, + 0x80000000000000000000000000000000000000000000000000000000, + 0x100000000000000000000000000000000000000000000000000000000, + 0x200000000000000000000000000000000000000000000000000000000, + 0x400000000000000000000000000000000000000000000000000000000, + 0x800000000000000000000000000000000000000000000000000000000, + 0x1000000000000000000000000000000000000000000000000000000000, + 0x2000000000000000000000000000000000000000000000000000000000, + 0x4000000000000000000000000000000000000000000000000000000000, + 0x8000000000000000000000000000000000000000000000000000000000, + 0x10000000000000000000000000000000000000000000000000000000000, + 0x20000000000000000000000000000000000000000000000000000000000, + 0x40000000000000000000000000000000000000000000000000000000000, + 0x80000000000000000000000000000000000000000000000000000000000, + 0x100000000000000000000000000000000000000000000000000000000000, + 0x200000000000000000000000000000000000000000000000000000000000, + 0x400000000000000000000000000000000000000000000000000000000000, + 0x800000000000000000000000000000000000000000000000000000000000, + 0x1000000000000000000000000000000000000000000000000000000000000, + 0x2000000000000000000000000000000000000000000000000000000000000, + 0x4000000000000000000000000000000000000000000000000000000000000, + 0x8000000000000000000000000000000000000000000000000000000000000, + 0x10000000000000000000000000000000000000000000000000000000000000, + 0x20000000000000000000000000000000000000000000000000000000000000, + 0x40000000000000000000000000000000000000000000000000000000000000, + 0x80000000000000000000000000000000000000000000000000000000000000, + 0x100000000000000000000000000000000000000000000000000000000000000, + 0x200000000000000000000000000000000000000000000000000000000000000, + 0x400000000000000000000000000000000000000000000000000000000000000, + 0x800000000000000000000000000000000000000000000000000000000000000, +]; + +#[generate_trait] +pub impl TwoPower of TwoPowerTrait { + /// Get the power of two at the specified exponent + /// # Arguments + /// * `exp` - The exponent of the power of two + /// # Returns + /// * The power of two at the specified exponent + #[inline] + fn pow(exp: u8) -> u256 { + *TWO_POWER.span().at(exp.into()) + } +} diff --git a/crates/pathfinding/src/lib.cairo b/crates/pathfinding/src/lib.cairo index f2154416..30b55a7f 100644 --- a/crates/pathfinding/src/lib.cairo +++ b/crates/pathfinding/src/lib.cairo @@ -6,5 +6,7 @@ pub mod types { pub mod helpers { pub mod astar; + pub mod bitmap; + pub mod power; pub mod heap; } diff --git a/crates/pathfinding/src/types/node.cairo b/crates/pathfinding/src/types/node.cairo index 102acc89..83453736 100644 --- a/crates/pathfinding/src/types/node.cairo +++ b/crates/pathfinding/src/types/node.cairo @@ -5,8 +5,9 @@ use origami_pathfinding::helpers::heap::ItemTrait; // Types. #[derive(Copy, Drop)] pub struct Node { - pub index: u8, - pub fcost: u16, + pub position: u8, + pub source: u8, + pub gcost: u16, pub hcost: u16, } @@ -14,53 +15,60 @@ pub struct Node { #[generate_trait] pub impl NodeImpl of NodeTrait { #[inline] - fn new(fcost: u16, hcost: u16) -> Node { - Node { index: 0, fcost: fcost, hcost: hcost } + fn new(position: u8, source: u8, gcost: u16, hcost: u16) -> Node { + Node { position, source, gcost, hcost } } } pub impl ItemImpl of ItemTrait { #[inline] - fn get_index(self: Node) -> u8 { - self.index + fn key(self: Node) -> u8 { + self.position + } +} + +pub impl NodePartialEq of PartialEq { + #[inline] + fn eq(lhs: @Node, rhs: @Node) -> bool { + lhs.position == rhs.position } #[inline] - fn set_index(ref self: Node, index: u8) { - self.index = index; + fn ne(lhs: @Node, rhs: @Node) -> bool { + lhs.position != rhs.position } } pub impl NodePartialOrd of PartialOrd { #[inline] fn lt(lhs: Node, rhs: Node) -> bool { - if lhs.fcost == rhs.fcost { + if lhs.gcost + lhs.hcost == rhs.gcost + rhs.hcost { return lhs.hcost < rhs.hcost; } - lhs.fcost < rhs.fcost + lhs.gcost + lhs.hcost < rhs.gcost + rhs.hcost } #[inline] fn le(lhs: Node, rhs: Node) -> bool { - if lhs.fcost == rhs.fcost { + if lhs.gcost + lhs.hcost == rhs.gcost + rhs.hcost { return lhs.hcost <= rhs.hcost; } - lhs.fcost <= rhs.fcost + lhs.gcost + lhs.hcost <= rhs.gcost + rhs.hcost } #[inline] fn gt(lhs: Node, rhs: Node) -> bool { - if lhs.fcost == rhs.fcost { + if lhs.gcost + lhs.hcost == rhs.gcost + rhs.hcost { return lhs.hcost > rhs.hcost; } - lhs.fcost > rhs.fcost + lhs.gcost + lhs.hcost > rhs.gcost + rhs.hcost } #[inline] fn ge(lhs: Node, rhs: Node) -> bool { - if lhs.fcost == rhs.fcost { + if lhs.gcost + lhs.hcost == rhs.gcost + rhs.hcost { return lhs.hcost >= rhs.hcost; } - lhs.fcost >= rhs.fcost + lhs.gcost + lhs.hcost >= rhs.gcost + rhs.hcost } }