15803813298

Committed 22 Jun 2025 06:32AM UTC coverage: 95.944% (+0.06%) from 95.886%

Build # 15803813298

Build Type

push

github

Committed by

web-flow

Commit Message

Add A* algo (#15)

Coverage Stats

276 of 282 new or added lines in 1 file covered. (97.87%)

9272 of 9664 relevant lines covered (95.94%)

39.22 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

97.87

/src/algorithms/astar.rs

// SPDX-License-Identifier: Apache-2.0
//! A* pathfinding algorithm
//!
//! A* is an extension of Dijkstra's algorithm that uses a heuristic function
//! to guide the search toward the goal node, making it more efficient for
//! single-target pathfinding problems.

use super::ContainerResultExt;

use crate::containers::maps::MapTrait;
use crate::graph::{GraphWithEdgeValues, NodeIndex};

use super::AlgorithmError;

/// Configuration structure for A* algorithm to avoid too many function parameters
pub struct AStarConfig<OS, CF, GS> {
    /// Map to track nodes in the open set with their f-scores
    pub open_set: OS,
    /// Map to track the path (parent of each node)
    pub came_from: CF,
    /// Map to store actual cost from start to each node
    pub g_score: GS,
}

/// A* pathfinding algorithm to find shortest path between two specific nodes
///
/// Finds the shortest path from a start node to a goal node using a heuristic
/// function to guide the search. The heuristic must be admissible (never overestimate
/// the actual cost) for A* to guarantee optimal results.
///
/// # Arguments
/// * `graph` - Graph implementing GraphWithEdgeValues
/// * `start` - Starting node
/// * `goal` - Goal node to find path to
/// * `heuristic` - Heuristic function estimating cost from any node to goal
/// * `open_set` - Map to track nodes in the open set with their f-scores
/// * `came_from` - Map to track the path (parent of each node)
/// * `g_score` - Map to store actual cost from start to each node
/// * `path_buffer` - Buffer to store the reconstructed path
///
/// # Returns
/// * `Ok(Some(path_slice))` if path found, containing the path from start to goal
/// * `Ok(None)` if no path exists
/// * `Err(AlgorithmError)` if the operation fails
///
/// # Heuristic Function
/// The heuristic function should estimate the cost from a node to the goal.
/// For optimal results, it must be admissible (never overestimate) and
/// consistent (satisfy triangle inequality).
///
/// # Example
/// ```
/// use heapless_graphs::algorithms::astar;
/// use heapless_graphs::containers::maps::{staticdict::Dictionary, MapTrait};
/// use heapless_graphs::edgelist::edge_list::EdgeList;
/// use heapless_graphs::edges::EdgeValueStruct;
///
/// // Create a simple graph: 0 --(2)--> 1 --(3)--> 2
/// let edge_data = EdgeValueStruct([(0usize, 1usize, 2i32), (1, 2, 3)]);
/// let graph = EdgeList::<8, _, _>::new(edge_data);
///
/// // Simple heuristic that returns 0 (converts A* to Dijkstra)
/// let heuristic = |_node: usize, _goal: usize| 0i32;
///
/// let open_set = Dictionary::<usize, i32, 16>::new();
/// let came_from = Dictionary::<usize, usize, 16>::new();
/// let g_score = Dictionary::<usize, Option<i32>, 16>::new();
/// let mut path_buffer = [0usize; 16];
///
/// let result = astar(&graph, 0, 2, heuristic, open_set, came_from, g_score, &mut path_buffer).unwrap();
///
/// assert!(result.is_some());
/// let path = result.unwrap();
/// assert_eq!(path, &[0, 1, 2]);
/// ```
#[allow(clippy::too_many_arguments)]
pub fn astar<'a, G, NI, V, OS, CF, GS, H>(
    graph: &G,
    start: NI,
    goal: NI,
    heuristic: H,
    mut open_set: OS,
    mut came_from: CF,
    mut g_score: GS,
    path_buffer: &'a mut [NI],
) -> Result<Option<&'a [NI]>, AlgorithmError<NI>>
where
    G: GraphWithEdgeValues<NI, V>,
    NI: NodeIndex,
    OS: MapTrait<NI, V>,         // Maps node to f-score (g + h)
    CF: MapTrait<NI, NI>,        // Maps node to its parent in the path
    GS: MapTrait<NI, Option<V>>, // Maps node to g-score (actual cost from start)
    H: Fn(NI, NI) -> V,          // Heuristic function (node, goal) -> estimated cost
    V: PartialOrd + Copy + core::ops::Add<Output = V> + Default,
    AlgorithmError<NI>: From<G::Error>,
{
    // Initialize g-score for all nodes (None means infinite)
    for node in graph.iter_nodes()? {
        g_score.insert(node, None).capacity_error()?;
    }

    // Initialize start node
    let start_g_score = V::default(); // 0
    let start_f_score = start_g_score + heuristic(start, goal);

    g_score
        .insert(start, Some(start_g_score))
        .capacity_error()?;
    open_set.insert(start, start_f_score).capacity_error()?;

    while !open_set.is_empty() {
        // Find node in open_set with lowest f-score
        let mut current = None;
        let mut lowest_f_score = None;

        for (&node, &f_score) in open_set.iter() {
            if let Some(current_lowest) = lowest_f_score {
                if f_score < current_lowest {
                    lowest_f_score = Some(f_score);
                    current = Some(node);
                }
            } else {
                lowest_f_score = Some(f_score);
                current = Some(node);
            }
        }

        // If no current node found, something is wrong with the open_set state
        let current = match current {
            Some(node) => node,
            None => return Err(AlgorithmError::InvalidState),
        };

        // If we reached the goal, reconstruct path
        if current == goal {
            return Ok(Some(reconstruct_path(&came_from, current, path_buffer)?));
        }

        // Remove current from open set
        open_set.remove(&current);

        // Check all neighbors
        for (neighbor, edge_weight_opt) in graph.outgoing_edge_values(current)? {
            if let Some(edge_weight) = edge_weight_opt {
                // Calculate tentative g-score
                let current_g_score = match g_score.get(&current).and_then(|opt| *opt) {
                    Some(score) => score,
                    None => return Err(AlgorithmError::InvalidState), // current should have g_score
                };

                let tentative_g_score = current_g_score + *edge_weight;

                // Check if this path to neighbor is better
                let neighbor_g_score = g_score.get(&neighbor).and_then(|opt| *opt);

                if neighbor_g_score.is_none_or(|existing| tentative_g_score < existing) {
                    // This path is better, record it
                    came_from.insert(neighbor, current).capacity_error()?;
                    g_score
                        .insert(neighbor, Some(tentative_g_score))
                        .capacity_error()?;

                    let f_score = tentative_g_score + heuristic(neighbor, goal);

                    // Add neighbor to open set if not already there with better score
                    if let Some(&existing_f) = open_set.get(&neighbor) {
                        if f_score < existing_f {
                            open_set.insert(neighbor, f_score).capacity_error()?;
                        }
                    } else {
                        open_set.insert(neighbor, f_score).capacity_error()?;
                    }
                }
            }
        }
    }

    // No path found
    Ok(None)
}

/// Reconstructs the path from start to goal using the came_from map
fn reconstruct_path<'a, NI, CF>(
    came_from: &CF,
    mut current: NI,
    path_buffer: &'a mut [NI],
) -> Result<&'a [NI], AlgorithmError<NI>>
where
    NI: NodeIndex,
    CF: MapTrait<NI, NI>,
{
    // First pass: count the path length
    let mut path_len = 1; // Start with 1 for the goal node
    let mut temp_current = current;

    while let Some(&parent) = came_from.get(&temp_current) {
        path_len += 1;
        temp_current = parent;
    }

    // Check if path fits in buffer
    if path_len > path_buffer.len() {
        return Err(AlgorithmError::ResultCapacityExceeded);
    }

    // Second pass: fill the buffer backwards
    let mut index = path_len - 1;
    path_buffer[index] = current;

    while let Some(&parent) = came_from.get(&current) {
        index -= 1;
        path_buffer[index] = parent;
        current = parent;
    }

    Ok(&path_buffer[..path_len])
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::containers::maps::staticdict::Dictionary;
    use crate::edgelist::edge_list::EdgeList;
    use crate::edges::EdgeValueStruct;

    #[test]
    fn test_astar_simple_path() {
        // Create a linear graph: 0 --(2)--> 1 --(3)--> 2
        let edge_data = EdgeValueStruct([(0usize, 1usize, 2i32), (1, 2, 3)]);
        let graph = EdgeList::<8, _, _>::new(edge_data);

        // Zero heuristic (makes A* behave like Dijkstra)
        let heuristic = |_node: usize, _goal: usize| 0i32;

        let open_set = Dictionary::<usize, i32, 16>::new();
        let came_from = Dictionary::<usize, usize, 16>::new();
        let g_score = Dictionary::<usize, Option<i32>, 16>::new();
        let mut path_buffer = [0usize; 16];

        let result = astar(
            &graph,
            0,
            2,
            heuristic,
            open_set,
            came_from,
            g_score,
            &mut path_buffer,
        )
        .unwrap();

        assert!(result.is_some());
        let path = result.unwrap();
        assert_eq!(path, &[0, 1, 2]);
    }

    #[test]
    fn test_astar_with_heuristic() {
        // Create a diamond graph:
        //     1
        //   /   \
        //  0     3
        //   \   /
        //     2
        let edge_data = EdgeValueStruct([
            (0usize, 1usize, 1i32), // 0 -> 1 (cost 1)
            (0, 2, 4),              // 0 -> 2 (cost 4) - longer path
            (1, 3, 1),              // 1 -> 3 (cost 1)
            (2, 3, 1),              // 2 -> 3 (cost 1)
        ]);
        let graph = EdgeList::<8, _, _>::new(edge_data);

        // Heuristic that guides toward node 3 (goal)
        let heuristic = |node: usize, goal: usize| {
            if node == goal {
                0
            } else {
                1
            }
        };

        let open_set = Dictionary::<usize, i32, 16>::new();
        let came_from = Dictionary::<usize, usize, 16>::new();
        let g_score = Dictionary::<usize, Option<i32>, 16>::new();
        let mut path_buffer = [0usize; 16];

        let result = astar(
            &graph,
            0,
            3,
            heuristic,
            open_set,
            came_from,
            g_score,
            &mut path_buffer,
        )
        .unwrap();

        assert!(result.is_some());
        let path = result.unwrap();
        // Should take the shorter path: 0 -> 1 -> 3 (cost 2) instead of 0 -> 2 -> 3 (cost 5)
        assert_eq!(path, &[0, 1, 3]);
    }

    #[test]
    fn test_astar_no_path() {
        // Create disconnected graph: 0 -> 1, 2 isolated
        let edge_data = EdgeValueStruct([(0usize, 1usize, 1i32)]);
        let graph = EdgeList::<8, _, _>::new(edge_data);

        let heuristic = |_node: usize, _goal: usize| 0i32;

        let open_set = Dictionary::<usize, i32, 16>::new();
        let came_from = Dictionary::<usize, usize, 16>::new();
        let g_score = Dictionary::<usize, Option<i32>, 16>::new();
        let mut path_buffer = [0usize; 16];

        let result = astar(
            &graph,
            0,
            2,
            heuristic,
            open_set,
            came_from,
            g_score,
            &mut path_buffer,
        )
        .unwrap();

        assert!(result.is_none()); // No path from 0 to 2
    }

    #[test]
    fn test_astar_same_start_goal() {
        // Test when start == goal
        let edge_data = EdgeValueStruct([(0usize, 1usize, 1i32)]);
        let graph = EdgeList::<8, _, _>::new(edge_data);

        let heuristic = |_node: usize, _goal: usize| 0i32;

        let open_set = Dictionary::<usize, i32, 16>::new();
        let came_from = Dictionary::<usize, usize, 16>::new();
        let g_score = Dictionary::<usize, Option<i32>, 16>::new();
        let mut path_buffer = [0usize; 16];

        let result = astar(
            &graph,
            0,
            0,
            heuristic,
            open_set,
            came_from,
            g_score,
            &mut path_buffer,
        )
        .unwrap();

        assert!(result.is_some());
        let path = result.unwrap();
        assert_eq!(path, &[0]); // Path should just be the start node
    }

    #[test]
    fn test_astar_complex_graph() {
        // Create a more complex graph to test A* optimization
        //   1 --3-- 4
        //  /|      /|
        // 0 |     / |
        //  \|    /  |
        //   2 --5-- 3 --1-- 6
        let edge_data = EdgeValueStruct([
            (0usize, 1usize, 1i32),
            (0, 2, 1),
            (1, 2, 1),
            (1, 4, 3),
            (2, 3, 5),
            (4, 3, 1),
            (4, 6, 1),
            (3, 6, 1),
        ]);
        let graph = EdgeList::<16, _, _>::new(edge_data);

        // Heuristic based on "distance" to goal (node 6)
        let heuristic = |node: usize, goal: usize| {
            match (node, goal) {
                (6, 6) => 0,
                (3, 6) | (4, 6) => 1,
                (1, 6) | (2, 6) => 2,
                (0, 6) => 3,
                _ => 10, // Unknown nodes get high cost
            }
        };

        let open_set = Dictionary::<usize, i32, 32>::new();
        let came_from = Dictionary::<usize, usize, 32>::new();
        let g_score = Dictionary::<usize, Option<i32>, 32>::new();
        let mut path_buffer = [0usize; 16];

        let result = astar(
            &graph,
            0,
            6,
            heuristic,
            open_set,
            came_from,
            g_score,
            &mut path_buffer,
        )
        .unwrap();

        assert!(result.is_some());
        let path = result.unwrap();

        // Verify we got a valid path from 0 to 6
        assert_eq!(path[0], 0);
        assert_eq!(path[path.len() - 1], 6);

        // The optimal path should be 0 -> 1 -> 4 -> 6 (cost: 1 + 3 + 1 = 5)
        // or 0 -> 1 -> 4 -> 3 -> 6 (cost: 1 + 3 + 1 + 1 = 6)
        assert!(path.len() >= 3 && path.len() <= 5);
    }
}

1	// SPDX-License-Identifier: Apache-2.0
2	//! A* pathfinding algorithm
3	//!
4	//! A* is an extension of Dijkstra's algorithm that uses a heuristic function
5	//! to guide the search toward the goal node, making it more efficient for
6	//! single-target pathfinding problems.
7
8	use super::ContainerResultExt;
9
10	use crate::containers::maps::MapTrait;
11	use crate::graph::{GraphWithEdgeValues, NodeIndex};
12
13	use super::AlgorithmError;
14
15	/// Configuration structure for A* algorithm to avoid too many function parameters
16	pub struct AStarConfig<OS, CF, GS> {
17	/// Map to track nodes in the open set with their f-scores
18	pub open_set: OS,
19	/// Map to track the path (parent of each node)
20	pub came_from: CF,
21	/// Map to store actual cost from start to each node
22	pub g_score: GS,
23	}
24
25	/// A* pathfinding algorithm to find shortest path between two specific nodes
26	///
27	/// Finds the shortest path from a start node to a goal node using a heuristic
28	/// function to guide the search. The heuristic must be admissible (never overestimate
29	/// the actual cost) for A* to guarantee optimal results.
30	///
31	/// # Arguments
32	/// * `graph` - Graph implementing GraphWithEdgeValues
33	/// * `start` - Starting node
34	/// * `goal` - Goal node to find path to
35	/// * `heuristic` - Heuristic function estimating cost from any node to goal
36	/// * `open_set` - Map to track nodes in the open set with their f-scores
37	/// * `came_from` - Map to track the path (parent of each node)
38	/// * `g_score` - Map to store actual cost from start to each node
39	/// * `path_buffer` - Buffer to store the reconstructed path
40	///
41	/// # Returns
42	/// * `Ok(Some(path_slice))` if path found, containing the path from start to goal
43	/// * `Ok(None)` if no path exists
44	/// * `Err(AlgorithmError)` if the operation fails
45	///
46	/// # Heuristic Function
47	/// The heuristic function should estimate the cost from a node to the goal.
48	/// For optimal results, it must be admissible (never overestimate) and
49	/// consistent (satisfy triangle inequality).
50	///
51	/// # Example
52	/// ```
53	/// use heapless_graphs::algorithms::astar;
54	/// use heapless_graphs::containers::maps::{staticdict::Dictionary, MapTrait};
55	/// use heapless_graphs::edgelist::edge_list::EdgeList;
56	/// use heapless_graphs::edges::EdgeValueStruct;
57	///
58	/// // Create a simple graph: 0 --(2)--> 1 --(3)--> 2
59	/// let edge_data = EdgeValueStruct([(0usize, 1usize, 2i32), (1, 2, 3)]);
60	/// let graph = EdgeList::<8, _, _>::new(edge_data);
61	///
62	/// // Simple heuristic that returns 0 (converts A* to Dijkstra)
63	/// let heuristic = \|_node: usize, _goal: usize\| 0i32;
64	///
65	/// let open_set = Dictionary::<usize, i32, 16>::new();
66	/// let came_from = Dictionary::<usize, usize, 16>::new();
67	/// let g_score = Dictionary::<usize, Option<i32>, 16>::new();
68	/// let mut path_buffer = [0usize; 16];
69	///
70	/// let result = astar(&graph, 0, 2, heuristic, open_set, came_from, g_score, &mut path_buffer).unwrap();
71	///
72	/// assert!(result.is_some());
73	/// let path = result.unwrap();
74	/// assert_eq!(path, &[0, 1, 2]);
75	/// ```
76	#[allow(clippy::too_many_arguments)]
77	pub fn astar<'a, G, NI, V, OS, CF, GS, H>(	5✔
78	graph: &G,	5✔
79	start: NI,	5✔
80	goal: NI,	5✔
81	heuristic: H,	5✔
82	mut open_set: OS,	5✔
83	mut came_from: CF,	5✔
84	mut g_score: GS,	5✔
85	path_buffer: &'a mut [NI],	5✔
86	) -> Result<Option<&'a [NI]>, AlgorithmError<NI>>	5✔
87	where	5✔
88	G: GraphWithEdgeValues<NI, V>,	5✔
89	NI: NodeIndex,	5✔
90	OS: MapTrait<NI, V>, // Maps node to f-score (g + h)	5✔
91	CF: MapTrait<NI, NI>, // Maps node to its parent in the path	5✔
92	GS: MapTrait<NI, Option<V>>, // Maps node to g-score (actual cost from start)	5✔
93	H: Fn(NI, NI) -> V, // Heuristic function (node, goal) -> estimated cost	5✔
94	V: PartialOrd + Copy + core::ops::Add<Output = V> + Default,	5✔
95	AlgorithmError<NI>: From<G::Error>,	5✔
96	{	5✔
97	// Initialize g-score for all nodes (None means infinite)
98	for node in graph.iter_nodes()? {	17✔
99	g_score.insert(node, None).capacity_error()?;	17✔
100	}
101
102	// Initialize start node
103	let start_g_score = V::default(); // 0	5✔
104	let start_f_score = start_g_score + heuristic(start, goal);	5✔
105		5✔
106	g_score	5✔
107	.insert(start, Some(start_g_score))	5✔
108	.capacity_error()?;	5✔
109	open_set.insert(start, start_f_score).capacity_error()?;	5✔
110
111	while !open_set.is_empty() {	15✔
112	// Find node in open_set with lowest f-score
113	let mut current = None;	14✔
114	let mut lowest_f_score = None;	14✔
115
116	for (&node, &f_score) in open_set.iter() {	20✔
117	if let Some(current_lowest) = lowest_f_score {	20✔
118	if f_score < current_lowest {	6✔
119	lowest_f_score = Some(f_score);	3✔
120	current = Some(node);	3✔
121	}	3✔
122	} else {	14✔
123	lowest_f_score = Some(f_score);	14✔
124	current = Some(node);	14✔
125	}	14✔
126	}
127
128	// If no current node found, something is wrong with the open_set state
129	let current = match current {	14✔
130	Some(node) => node,	14✔
NEW 131	None => return Err(AlgorithmError::InvalidState),	×
132	};
133
134	// If we reached the goal, reconstruct path
135	if current == goal {	14✔
136	return Ok(Some(reconstruct_path(&came_from, current, path_buffer)?));	4✔
137	}	10✔
138		10✔
139	// Remove current from open set	10✔
140	open_set.remove(&current);	10✔
141
142	// Check all neighbors
143	for (neighbor, edge_weight_opt) in graph.outgoing_edge_values(current)? {	13✔
144	if let Some(edge_weight) = edge_weight_opt {	13✔
145	// Calculate tentative g-score
146	let current_g_score = match g_score.get(&current).and_then(\|opt\| *opt) {	13✔
147	Some(score) => score,	13✔
NEW 148	None => return Err(AlgorithmError::InvalidState), // current should have g_score	×
149	};
150
151	let tentative_g_score = current_g_score + *edge_weight;	13✔
152		13✔
153	// Check if this path to neighbor is better	13✔
154	let neighbor_g_score = g_score.get(&neighbor).and_then(\|opt\| *opt);	13✔
155		13✔
156	if neighbor_g_score.is_none_or(\|existing\| tentative_g_score < existing) {	13✔
157	// This path is better, record it
158	came_from.insert(neighbor, current).capacity_error()?;	12✔
159	g_score	12✔
160	.insert(neighbor, Some(tentative_g_score))	12✔
161	.capacity_error()?;	12✔
162
163	let f_score = tentative_g_score + heuristic(neighbor, goal);	12✔
164
165	// Add neighbor to open set if not already there with better score
166	if let Some(&existing_f) = open_set.get(&neighbor) {	12✔
167	if f_score < existing_f {	1✔
168	open_set.insert(neighbor, f_score).capacity_error()?;	1✔
NEW 169	}	×
170	} else {
171	open_set.insert(neighbor, f_score).capacity_error()?;	11✔
172	}
173	}	1✔
NEW 174	}	×
175	}
176	}
177
178	// No path found
179	Ok(None)	1✔
180	}	5✔
181
182	/// Reconstructs the path from start to goal using the came_from map
183	fn reconstruct_path<'a, NI, CF>(	4✔
184	came_from: &CF,	4✔
185	mut current: NI,	4✔
186	path_buffer: &'a mut [NI],	4✔
187	) -> Result<&'a [NI], AlgorithmError<NI>>	4✔
188	where	4✔
189	NI: NodeIndex,	4✔
190	CF: MapTrait<NI, NI>,	4✔
191	{	4✔
192	// First pass: count the path length	4✔
193	let mut path_len = 1; // Start with 1 for the goal node	4✔
194	let mut temp_current = current;	4✔
195
196	while let Some(&parent) = came_from.get(&temp_current) {	11✔
197	path_len += 1;	7✔
198	temp_current = parent;	7✔
199	}	7✔
200
201	// Check if path fits in buffer
202	if path_len > path_buffer.len() {	4✔
NEW 203	return Err(AlgorithmError::ResultCapacityExceeded);	×
204	}	4✔
205		4✔
206	// Second pass: fill the buffer backwards	4✔
207	let mut index = path_len - 1;	4✔
208	path_buffer[index] = current;	4✔
209
210	while let Some(&parent) = came_from.get(&current) {	11✔
211	index -= 1;	7✔
212	path_buffer[index] = parent;	7✔
213	current = parent;	7✔
214	}	7✔
215
216	Ok(&path_buffer[..path_len])	4✔
217	}	4✔
218
219	#[cfg(test)]
220	mod tests {
221	use super::*;
222	use crate::containers::maps::staticdict::Dictionary;
223	use crate::edgelist::edge_list::EdgeList;
224	use crate::edges::EdgeValueStruct;
225
226	#[test]
227	fn test_astar_simple_path() {	1✔
228	// Create a linear graph: 0 --(2)--> 1 --(3)--> 2	1✔
229	let edge_data = EdgeValueStruct([(0usize, 1usize, 2i32), (1, 2, 3)]);	1✔
230	let graph = EdgeList::<8, _, _>::new(edge_data);	1✔
231		1✔
232	// Zero heuristic (makes A* behave like Dijkstra)	1✔
233	let heuristic = \|_node: usize, _goal: usize\| 0i32;	3✔
234
235	let open_set = Dictionary::<usize, i32, 16>::new();	1✔
236	let came_from = Dictionary::<usize, usize, 16>::new();	1✔
237	let g_score = Dictionary::<usize, Option<i32>, 16>::new();	1✔
238	let mut path_buffer = [0usize; 16];	1✔
239		1✔
240	let result = astar(	1✔
241	&graph,	1✔
242	0,	1✔
243	2,	1✔
244	heuristic,	1✔
245	open_set,	1✔
246	came_from,	1✔
247	g_score,	1✔
248	&mut path_buffer,	1✔
249	)	1✔
250	.unwrap();	1✔
251		1✔
252	assert!(result.is_some());	1✔
253	let path = result.unwrap();	1✔
254	assert_eq!(path, &[0, 1, 2]);	1✔
255	}	1✔
256
257	#[test]
258	fn test_astar_with_heuristic() {	1✔
259	// Create a diamond graph:	1✔
260	// 1	1✔
261	// / \	1✔
262	// 0 3	1✔
263	// \ /	1✔
264	// 2	1✔
265	let edge_data = EdgeValueStruct([	1✔
266	(0usize, 1usize, 1i32), // 0 -> 1 (cost 1)	1✔
267	(0, 2, 4), // 0 -> 2 (cost 4) - longer path	1✔
268	(1, 3, 1), // 1 -> 3 (cost 1)	1✔
269	(2, 3, 1), // 2 -> 3 (cost 1)	1✔
270	]);	1✔
271	let graph = EdgeList::<8, _, _>::new(edge_data);	1✔
272		1✔
273	// Heuristic that guides toward node 3 (goal)	1✔
274	let heuristic = \|node: usize, goal: usize\| {	4✔
275	if node == goal {	4✔
276	0	1✔
277	} else {
278	1	3✔
279	}
280	};	4✔
281
282	let open_set = Dictionary::<usize, i32, 16>::new();	1✔
283	let came_from = Dictionary::<usize, usize, 16>::new();	1✔
284	let g_score = Dictionary::<usize, Option<i32>, 16>::new();	1✔
285	let mut path_buffer = [0usize; 16];	1✔
286		1✔
287	let result = astar(	1✔
288	&graph,	1✔
289	0,	1✔
290	3,	1✔
291	heuristic,	1✔
292	open_set,	1✔
293	came_from,	1✔
294	g_score,	1✔
295	&mut path_buffer,	1✔
296	)	1✔
297	.unwrap();	1✔
298		1✔
299	assert!(result.is_some());	1✔
300	let path = result.unwrap();	1✔
301	// Should take the shorter path: 0 -> 1 -> 3 (cost 2) instead of 0 -> 2 -> 3 (cost 5)	1✔
302	assert_eq!(path, &[0, 1, 3]);	1✔
303	}	1✔
304
305	#[test]
306	fn test_astar_no_path() {	1✔
307	// Create disconnected graph: 0 -> 1, 2 isolated	1✔
308	let edge_data = EdgeValueStruct([(0usize, 1usize, 1i32)]);	1✔
309	let graph = EdgeList::<8, _, _>::new(edge_data);	1✔
310		1✔
311	let heuristic = \|_node: usize, _goal: usize\| 0i32;	2✔
312
313	let open_set = Dictionary::<usize, i32, 16>::new();	1✔
314	let came_from = Dictionary::<usize, usize, 16>::new();	1✔
315	let g_score = Dictionary::<usize, Option<i32>, 16>::new();	1✔
316	let mut path_buffer = [0usize; 16];	1✔
317		1✔
318	let result = astar(	1✔
319	&graph,	1✔
320	0,	1✔
321	2,	1✔
322	heuristic,	1✔
323	open_set,	1✔
324	came_from,	1✔
325	g_score,	1✔
326	&mut path_buffer,	1✔
327	)	1✔
328	.unwrap();	1✔
329		1✔
330	assert!(result.is_none()); // No path from 0 to 2	1✔
331	}	1✔
332
333	#[test]
334	fn test_astar_same_start_goal() {	1✔
335	// Test when start == goal	1✔
336	let edge_data = EdgeValueStruct([(0usize, 1usize, 1i32)]);	1✔
337	let graph = EdgeList::<8, _, _>::new(edge_data);	1✔
338		1✔
339	let heuristic = \|_node: usize, _goal: usize\| 0i32;	1✔
340
341	let open_set = Dictionary::<usize, i32, 16>::new();	1✔
342	let came_from = Dictionary::<usize, usize, 16>::new();	1✔
343	let g_score = Dictionary::<usize, Option<i32>, 16>::new();	1✔
344	let mut path_buffer = [0usize; 16];	1✔
345		1✔
346	let result = astar(	1✔
347	&graph,	1✔
348	0,	1✔
349	0,	1✔
350	heuristic,	1✔
351	open_set,	1✔
352	came_from,	1✔
353	g_score,	1✔
354	&mut path_buffer,	1✔
355	)	1✔
356	.unwrap();	1✔
357		1✔
358	assert!(result.is_some());	1✔
359	let path = result.unwrap();	1✔
360	assert_eq!(path, &[0]); // Path should just be the start node	1✔
361	}	1✔
362
363	#[test]
364	fn test_astar_complex_graph() {	1✔
365	// Create a more complex graph to test A* optimization	1✔
366	// 1 --3-- 4	1✔
367	// /\| /\|	1✔
368	// 0 \| / \|	1✔
369	// \\| / \|	1✔
370	// 2 --5-- 3 --1-- 6	1✔
371	let edge_data = EdgeValueStruct([	1✔
372	(0usize, 1usize, 1i32),	1✔
373	(0, 2, 1),	1✔
374	(1, 2, 1),	1✔
375	(1, 4, 3),	1✔
376	(2, 3, 5),	1✔
377	(4, 3, 1),	1✔
378	(4, 6, 1),	1✔
379	(3, 6, 1),	1✔
380	]);	1✔
381	let graph = EdgeList::<16, _, _>::new(edge_data);	1✔
382		1✔
383	// Heuristic based on "distance" to goal (node 6)	1✔
384	let heuristic = \|node: usize, goal: usize\| {	7✔
385	match (node, goal) {	7✔
386	(6, 6) => 0,	1✔
387	(3, 6) \| (4, 6) => 1,	3✔
388	(1, 6) \| (2, 6) => 2,	2✔
389	(0, 6) => 3,	1✔
NEW 390	_ => 10, // Unknown nodes get high cost	×
391	}
392	};	7✔
393
394	let open_set = Dictionary::<usize, i32, 32>::new();	1✔
395	let came_from = Dictionary::<usize, usize, 32>::new();	1✔
396	let g_score = Dictionary::<usize, Option<i32>, 32>::new();	1✔
397	let mut path_buffer = [0usize; 16];	1✔
398		1✔
399	let result = astar(	1✔
400	&graph,	1✔
401	0,	1✔
402	6,	1✔
403	heuristic,	1✔
404	open_set,	1✔
405	came_from,	1✔
406	g_score,	1✔
407	&mut path_buffer,	1✔
408	)	1✔
409	.unwrap();	1✔
410		1✔
411	assert!(result.is_some());	1✔
412	let path = result.unwrap();	1✔
413		1✔
414	// Verify we got a valid path from 0 to 6	1✔
415	assert_eq!(path[0], 0);	1✔
416	assert_eq!(path[path.len() - 1], 6);	1✔
417
418	// The optimal path should be 0 -> 1 -> 4 -> 6 (cost: 1 + 3 + 1 = 5)
419	// or 0 -> 1 -> 4 -> 3 -> 6 (cost: 1 + 3 + 1 + 1 = 6)
420	assert!(path.len() >= 3 && path.len() <= 5);	1✔
421	}	1✔
422	}

kaidokert / heapless-graphs-rs / 15803813298

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous