Hi there everyone, I'm trying to program a game in Rust. I'm a roughly intermediate level Rust programmer but not super good at data structures or algorithms. I'm using what is roughly a greedy BFS pathfinding algorithm, with a few modifications of my own. It should (ostensibly) be much faster than almost any other pathfinding algorithm, but it can't take having more even five AI's trying to path-find without tanking the framerate of my application. I've tried to multithread this several times but I've never been able to successfully get multithreading to work, so I'd rather not. Here's the code, can anyone help me try to improve the performance?

```
pub fn find_path<'a>(map: &WorldMap,
start: Point3D,
goal: Point3D,
can_move: Box<Fn(Point3D) -> bool + 'a>)
-> Option<Vec<Point3D>> {
let mut frontier = BinaryHeap::new();
frontier.push(State {
cost: 0,
pos: start,
});
let mut came_from = HashMap::new();
came_from.insert(start, None);
while frontier.len() != 0 {
let current = frontier.pop();
if current.unwrap().pos == goal {
break;
}
for next in strict_3d_adjacent(current.unwrap().pos, map) {
if !came_from.contains_key(&next) && can_move(next) {
frontier.push(State {
pos: next,
cost: distance3_d(goal, next) as
isize,
});
came_from.insert(next, current.map(|a| a.pos));
}
}
}
let mut current = goal;
let mut path = vec![current];
while current != start {
if let Some(c) = came_from.get(¤t) {
if let Some(c) = *c {
current = c;
path.push(current);
} else {
return None;
}
} else {
return None;
}
}
path.push(start);
Some(path)
}
```

Just some notes, Point3D is a type alias for a 3-tuple, and strict_3d_adjacent just gives all the points adjacent to the point on a 2D plane, with their highest 3d point (I have a 2D plane containing stacks of 3D tiles).

Please help, thanks!