perf: avoid square root computation when possible

Signed-off-by: winstxnhdw <[email protected]>

mapf-viz/src/visibility_visual.rs

@@ -150,12 +150,13 @@ impl<Message, G: Grid> VisibilityVisual<Message, G> {
     fn find_closest(&self, p: iced::Point) -> Option<Cell> {
         let mut closest: Option<(Cell, f64)> = None;
         let r = self.visibility.agent_radius();
+        let r_squared = r * r;
         let p = Point::new(p.x as f64, p.y as f64);
 
         for (cell, _) in self.visibility.iter_points() {
             let p_cell = cell.center_point(self.grid().cell_size());
-            let dist = (p_cell - p).norm();
-            if dist <= r {
+            let dist = (p_cell - p).norm_squared();
+            if dist <= r_squared {
                 if let Some((_, old_dist)) = closest {
                     if dist < old_dist {
                         closest = Some((*cell, dist));

mapf/src/motion/conflict.rs

@@ -148,7 +148,7 @@ where
         }
 
         // The final state should be almost exactly the same as the last move
-        assert!((to_state.point() - last_p).norm() < 1e-3);
+        assert!((to_state.point() - last_p).norm_squared() < 1e-6);
         Arclength {
             translational,
             rotational,

mapf/src/motion/waypoint.rs

@@ -99,7 +99,7 @@ where
         }
 
         // The final state should be almost exactly the same as the last move
-        assert!((to_state.point() - last_p).norm() < 1e-3);
+        assert!((to_state.point() - last_p).norm_squared() < 1e-6);
         Arclength {
             translational,
             rotational,

mapf/src/negotiation/mod.rs

@@ -68,15 +68,17 @@ pub fn negotiate(
     let cs = scenario.cell_size;
     let mut conflicts = HashMap::new();
     triangular_for(scenario.agents.iter(), |(n_a, a), (n_b, b)| {
+        let min_dist = a.radius + b.radius;
+        let min_dist_squared = min_dist * min_dist;
+
         for (cell_a, cell_b) in [
             (a.start_cell(), b.start_cell()),
             (a.goal_cell(), b.goal_cell()),
         ] {
             let pa = cell_a.center_point(cs);
             let pb = cell_b.center_point(cs);
-            let dist = (pa - pb).norm();
-            let min_dist = a.radius + b.radius;
-            if dist < min_dist {
+            let dist = (pa - pb).norm_squared();
+            if dist < min_dist_squared {
                 conflicts.insert(
                     (**n_a).clone().min((*n_b).clone()),
                     (**n_a).clone().max((*n_b).clone()),