use std::cmp::Ordering;
use std::collections::{BinaryHeap, HashMap, HashSet};
#[derive(Debug, Clone)]
pub struct CfgGraphNode {
pub id: u64,
pub x: f32,
pub y: f32,
pub z: f32,
pub successors: Vec<u64>,
}
#[derive(Debug, Clone)]
pub struct CfgPath {
pub node_ids: Vec<u64>,
pub total_cost: f32,
pub heuristic_cost: f32,
pub actual_cost: f32,
}
#[derive(Debug, Clone, Copy, PartialEq)]
struct AStarNode {
node_id: u64,
f_score: f32,
g_score: f32,
}
impl Eq for AStarNode {}
impl Ord for AStarNode {
fn cmp(&self, other: &Self) -> Ordering {
other
.f_score
.partial_cmp(&self.f_score)
.unwrap_or(Ordering::Equal)
}
}
impl PartialOrd for AStarNode {
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(other))
}
}
#[inline]
fn euclidean_distance(x1: f32, y1: f32, z1: f32, x2: f32, y2: f32, z2: f32) -> f32 {
let dx = x2 - x1;
let dy = y2 - y1;
let dz = z2 - z1;
(dx * dx + dy * dy + dz * dz).sqrt()
}
pub fn astar_find_path(nodes: &[CfgGraphNode], start_id: u64, goal_id: u64) -> Option<CfgPath> {
eprintln!(
"DEBUG astar_find_path: start_id={}, goal_id={}, nodes.len()={}",
start_id,
goal_id,
nodes.len()
);
let node_map: HashMap<u64, &CfgGraphNode> = nodes.iter().map(|n| (n.id, n)).collect();
let start_node = node_map.get(&start_id)?;
let actual_goal_id = if goal_id == u64::MAX {
let mut farthest_id = start_id;
let mut max_dist = 0.0f32;
for (&id, node) in &node_map {
if id == start_id {
continue;
}
let dist = euclidean_distance(
start_node.x,
start_node.y,
start_node.z,
node.x,
node.y,
node.z,
);
if dist > max_dist {
max_dist = dist;
farthest_id = id;
}
}
eprintln!(
"DEBUG: start_id={}, goal_id=MAX, farthest_id={}, max_dist={}",
start_id, farthest_id, max_dist
);
farthest_id
} else {
goal_id
};
let goal_node = node_map.get(&actual_goal_id)?;
let mut open_set = BinaryHeap::new();
let mut in_open_set: HashSet<u64> = HashSet::new();
let mut closed_set: HashSet<u64> = HashSet::new();
let mut g_score: HashMap<u64, f32> = HashMap::new();
let mut came_from: HashMap<u64, u64> = HashMap::new();
let initial_h = euclidean_distance(
start_node.x,
start_node.y,
start_node.z,
goal_node.x,
goal_node.y,
goal_node.z,
);
g_score.insert(start_id, 0.0);
open_set.push(AStarNode {
node_id: start_id,
f_score: initial_h,
g_score: 0.0,
});
in_open_set.insert(start_id);
while let Some(current) = open_set.pop() {
let current_id = current.node_id;
if closed_set.contains(¤t_id) {
continue;
}
if current_id == actual_goal_id {
let mut path = vec![actual_goal_id];
let mut cur = actual_goal_id;
while let Some(&prev) = came_from.get(&cur) {
path.push(prev);
cur = prev;
}
path.reverse();
return Some(CfgPath {
node_ids: path,
total_cost: current.f_score,
heuristic_cost: initial_h,
actual_cost: current.g_score,
});
}
in_open_set.remove(¤t_id);
closed_set.insert(current_id);
let current_node = node_map.get(¤t_id)?;
eprintln!(
"DEBUG: Processing node_id={}, successors={:?}",
current_id, current_node.successors
);
for &succ_id in ¤t_node.successors {
if closed_set.contains(&succ_id) {
continue;
}
let succ_node = match node_map.get(&succ_id) {
Some(n) => n,
None => continue,
};
let tentative_g = current.g_score + 1.0;
let existing_g = g_score.get(&succ_id).copied().unwrap_or(f32::INFINITY);
if tentative_g < existing_g {
came_from.insert(succ_id, current_id);
g_score.insert(succ_id, tentative_g);
let h = euclidean_distance(
succ_node.x,
succ_node.y,
succ_node.z,
goal_node.x,
goal_node.y,
goal_node.z,
);
if !in_open_set.contains(&succ_id) {
open_set.push(AStarNode {
node_id: succ_id,
f_score: tentative_g + h,
g_score: tentative_g,
});
in_open_set.insert(succ_id);
}
}
}
}
None
}
pub fn astar_find_k_paths(
nodes: &[CfgGraphNode],
start_id: u64,
goal_id: u64,
k: usize,
) -> Vec<CfgPath> {
let mut paths: Vec<CfgPath> = Vec::new();
if let Some(first) = astar_find_path(nodes, start_id, goal_id) {
paths.push(first);
} else {
return paths;
}
let mut candidates: Vec<CfgPath> = Vec::new();
for i in 0..(k - 1).min(paths.len()) {
let base = &paths[i];
for j in 0..(base.node_ids.len() - 1) {
let spur = base.node_ids[j];
let modified: Vec<CfgGraphNode> = nodes
.iter()
.map(|n| {
let mut m = n.clone();
if j > 0 && n.id == base.node_ids[j - 1] {
m.successors.retain(|&s| s != spur);
}
m
})
.collect();
if let Some(spur_path) = astar_find_path(&modified, spur, goal_id) {
let mut combined: Vec<u64> = base.node_ids[..=j].to_vec();
combined.extend_from_slice(&spur_path.node_ids[1..]);
let cp = CfgPath {
node_ids: combined,
total_cost: spur_path.total_cost + j as f32,
heuristic_cost: spur_path.heuristic_cost,
actual_cost: spur_path.actual_cost + j as f32,
};
if !candidates.iter().any(|p| p.node_ids == cp.node_ids) {
candidates.push(cp);
}
}
}
candidates.sort_by(|a, b| {
a.total_cost
.partial_cmp(&b.total_cost)
.unwrap_or(Ordering::Equal)
});
if let Some(best) = candidates.pop() {
paths.push(best);
}
}
paths
}
#[derive(Debug, Clone)]
pub struct PathComplexity {
pub path_length: usize,
pub max_loop_depth: u32,
pub branch_count: usize,
pub total_spatial_distance: f32,
pub heuristic_efficiency: f32,
}
pub fn analyze_path_complexity(path: &CfgPath, nodes: &[CfgGraphNode]) -> PathComplexity {
let node_map: HashMap<u64, &CfgGraphNode> = nodes.iter().map(|n| (n.id, n)).collect();
let mut max_ld = 0.0f32;
let mut bc = 0;
let mut tsd = 0.0f32;
for i in 0..path.node_ids.len() {
if let Some(&n) = node_map.get(&path.node_ids[i]) {
max_ld = max_ld.max(n.y);
if n.z > 0.0 {
bc += 1;
}
if i > 0 {
if let Some(&p) = node_map.get(&path.node_ids[i - 1]) {
tsd += euclidean_distance(p.x, p.y, p.z, n.x, n.y, n.z);
}
}
}
}
PathComplexity {
path_length: path.node_ids.len(),
max_loop_depth: max_ld as u32,
branch_count: bc,
total_spatial_distance: tsd,
heuristic_efficiency: if path.heuristic_cost > 0.0 {
path.actual_cost / path.heuristic_cost
} else {
1.0
},
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_astar_simple() {
let n = vec![
CfgGraphNode {
id: 0,
x: 0.0,
y: 0.0,
z: 0.0,
successors: vec![1],
},
CfgGraphNode {
id: 1,
x: 1.0,
y: 0.0,
z: 0.0,
successors: vec![2],
},
CfgGraphNode {
id: 2,
x: 2.0,
y: 0.0,
z: 0.0,
successors: vec![3],
},
CfgGraphNode {
id: 3,
x: 3.0,
y: 0.0,
z: 0.0,
successors: vec![],
},
];
assert_eq!(
astar_find_path(&n, 0, 3).unwrap().node_ids,
vec![0, 1, 2, 3]
);
}
#[test]
fn test_astar_branch() {
let n = vec![
CfgGraphNode {
id: 0,
x: 0.0,
y: 0.0,
z: 0.0,
successors: vec![1, 2],
},
CfgGraphNode {
id: 1,
x: 1.0,
y: 0.0,
z: 1.0,
successors: vec![3],
},
CfgGraphNode {
id: 2,
x: 1.0,
y: 0.0,
z: 0.0,
successors: vec![3],
},
CfgGraphNode {
id: 3,
x: 2.0,
y: 0.0,
z: 0.0,
successors: vec![],
},
];
assert!(astar_find_path(&n, 0, 3).is_some());
}
#[test]
fn test_astar_nopath() {
let n = vec![
CfgGraphNode {
id: 0,
x: 0.0,
y: 0.0,
z: 0.0,
successors: vec![1],
},
CfgGraphNode {
id: 1,
x: 1.0,
y: 0.0,
z: 0.0,
successors: vec![],
},
];
assert!(astar_find_path(&n, 0, 2).is_none());
}
#[test]
fn test_astar_kpaths() {
let n = vec![
CfgGraphNode {
id: 0,
x: 0.0,
y: 0.0,
z: 0.0,
successors: vec![1, 2, 3],
},
CfgGraphNode {
id: 1,
x: 1.0,
y: 0.0,
z: 0.0,
successors: vec![4],
},
CfgGraphNode {
id: 2,
x: 1.0,
y: 0.0,
z: 0.0,
successors: vec![4],
},
CfgGraphNode {
id: 3,
x: 1.0,
y: 0.0,
z: 0.0,
successors: vec![4],
},
CfgGraphNode {
id: 4,
x: 2.0,
y: 0.0,
z: 0.0,
successors: vec![],
},
];
let paths = astar_find_k_paths(&n, 0, 4, 3);
assert!(paths.len() >= 2 && paths.len() <= 3);
}
}