🐛 Fix pathfinding

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<u8> {
+        let mut start = NodeTrait::new(from, 0, 0, 0);
+        let target = NodeTrait::new(to, 0, 0, 0);
+        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 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<u8> = 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>
+    ) -> 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<Node>,
+    ) {
+        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);
+    }
+}

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);
+    }
+}