router: calculate lengths and estimate costs for A* search

src/band.rs

@@ -1,10 +1,11 @@
 use crate::{
     connectivity::{BandIndex, BandWeight, ConnectivityWeight, GetNet},
-    geometry::{DotIndex, FixedDotIndex},
+    geometry::{DotIndex, FixedDotIndex, GeometryIndex, MakePrimitive},
     graph::GetNodeIndex,
     layout::Layout,
     loose::{GetNextLoose, LooseIndex},
-    primitive::{GetEnds, GetOtherEnd},
+    primitive::{GetEnds, GetOtherEnd, MakeShape},
+    shape::ShapeTrait,
 };
 
 pub struct Band<'a> {
@@ -60,6 +61,25 @@ impl<'a> Band<'a> {
             _ => unreachable!(),
         }
     }
+
+    pub fn length(&self) -> f64 {
+        let mut maybe_loose = self.layout.primitive(self.from()).first_loose(self.index);
+        let mut prev = None;
+        let mut length = 0.0;
+
+        while let Some(loose) = maybe_loose {
+            length += GeometryIndex::from(loose)
+                .primitive(self.layout)
+                .shape()
+                .length();
+
+            let prev_prev = prev;
+            prev = maybe_loose;
+            maybe_loose = self.layout.loose(loose).next_loose(prev_prev);
+        }
+
+        length
+    }
 }
 
 impl<'a> GetNet for Band<'a> {

src/main.rs

@@ -496,8 +496,8 @@ fn main() -> Result<(), anyhow::Error> {
     let _ = router.route_band(
         dot_start,
         dot_end,
-        &mut EmptyRouterObserver,
-        //&mut DebugRouterObserver::new(&mut event_pump, &window, &mut renderer, &font_context),
+        //&mut EmptyRouterObserver,
+        &mut DebugRouterObserver::new(&mut event_pump, &window, &mut renderer, &font_context),
     )?;
 
     render_times(
@@ -528,8 +528,8 @@ fn main() -> Result<(), anyhow::Error> {
     let band2 = router.route_band(
         dot_start2,
         dot_end2,
-        &mut EmptyRouterObserver,
-        //&mut DebugRouterObserver::new(&mut event_pump, &window, &mut renderer, &font_context),
+        //&mut EmptyRouterObserver,
+        &mut DebugRouterObserver::new(&mut event_pump, &window, &mut renderer, &font_context),
     )?;
 
     render_times(
@@ -549,8 +549,8 @@ fn main() -> Result<(), anyhow::Error> {
     let band3 = router.route_band(
         dot_start3,
         dot_end3,
-        &mut EmptyRouterObserver,
-        //&mut DebugRouterObserver::new(&mut event_pump, &window, &mut renderer, &font_context),
+        //&mut EmptyRouterObserver,
+        &mut DebugRouterObserver::new(&mut event_pump, &window, &mut renderer, &font_context),
     )?;
 
     render_times(

src/mesh.rs

@@ -33,6 +33,16 @@ pub enum TriangulationVertexIndex {
     FixedBend(FixedBendIndex),
 }
 
+impl From<VertexIndex> for GeometryIndex {
+    fn from(vertex: VertexIndex) -> Self {
+        match vertex {
+            VertexIndex::FixedDot(dot) => GeometryIndex::FixedDot(dot),
+            VertexIndex::FixedBend(bend) => GeometryIndex::FixedBend(bend),
+            VertexIndex::LooseBend(bend) => GeometryIndex::LooseBend(bend),
+        }
+    }
+}
+
 impl From<TriangulationVertexIndex> for VertexIndex {
     fn from(vertex: TriangulationVertexIndex) -> Self {
         match vertex {

src/router.rs

@@ -1,4 +1,5 @@
 use geo::geometry::Point;
+use geo::EuclideanDistance;
 use petgraph::visit::EdgeRef;
 use spade::InsertionError;
 use thiserror::Error;
@@ -6,14 +7,16 @@ use thiserror::Error;
 use crate::astar::{astar, AstarStrategy, PathTracker};
 use crate::connectivity::{BandIndex, GetNet};
 use crate::draw::DrawException;
-use crate::geometry::{FixedDotIndex, FixedDotWeight};
+use crate::geometry::{FixedDotIndex, FixedDotWeight, GeometryIndex, MakePrimitive};
 use crate::guide::HeadTrait;
 use crate::layout::Layout;
 
 use crate::math::Circle;
 use crate::mesh::{Mesh, MeshEdgeReference, VertexIndex};
 
+use crate::primitive::MakeShape;
 use crate::rules::Rules;
+use crate::shape::ShapeTrait;
 use crate::tracer::{Trace, Tracer};
 
 #[derive(Error, Debug, Clone, Copy)]
@@ -70,7 +73,7 @@ impl<'a, RO: RouterObserver> RouterAstarStrategy<'a, RO> {
     }
 }
 
-impl<'a, RO: RouterObserver> AstarStrategy<&Mesh, u64> for RouterAstarStrategy<'a, RO> {
+impl<'a, RO: RouterObserver> AstarStrategy<&Mesh, f64> for RouterAstarStrategy<'a, RO> {
     fn is_goal(&mut self, vertex: VertexIndex, tracker: &PathTracker<&Mesh>) -> bool {
         let new_path = tracker.reconstruct_path_to(vertex);
 
@@ -82,27 +85,36 @@ impl<'a, RO: RouterObserver> AstarStrategy<&Mesh, u64> for RouterAstarStrategy<'
         self.tracer.finish(&mut self.trace, self.to, 5.0).is_ok()
     }
 
-    fn edge_cost(&mut self, edge: MeshEdgeReference) -> Option<u64> {
+    fn edge_cost(&mut self, edge: MeshEdgeReference) -> Option<f64> {
         self.observer.before_probe(&self.tracer, &self.trace, edge);
         if edge.target() == self.to.into() {
             return None;
         }
 
+        let before_probe_length = self.tracer.layout.band(self.trace.head.band()).length();
+
         let result = self.tracer.step(&mut self.trace, edge.target(), 5.0);
         self.observer
             .on_probe(&self.tracer, &self.trace, edge, result);
 
+        let probe_length = self.tracer.layout.band(self.trace.head.band()).length();
+
         if result.is_ok() {
             self.tracer.undo_step(&mut self.trace);
-            Some(1)
+            Some(probe_length - before_probe_length)
         } else {
             None
         }
     }
 
-    fn estimate_cost(&mut self, vertex: VertexIndex) -> u64 {
+    fn estimate_cost(&mut self, vertex: VertexIndex) -> f64 {
         self.observer.on_estimate(&self.tracer, vertex);
-        0
+        let start_point = GeometryIndex::from(vertex)
+            .primitive(self.tracer.layout)
+            .shape()
+            .center();
+        let end_point = self.tracer.layout.primitive(self.to).shape().center();
+        end_point.euclidean_distance(&start_point)
     }
 }
 

src/shape.rs

@@ -11,6 +11,7 @@ pub trait ShapeTrait {
     fn intersects(&self, other: &Shape) -> bool;
     fn envelope(&self) -> AABB<[f64; 2]>;
     fn width(&self) -> f64;
+    fn length(&self) -> f64;
 }
 
 #[enum_dispatch(ShapeTrait)]
@@ -72,6 +73,10 @@ impl ShapeTrait for DotShape {
     fn width(&self) -> f64 {
         self.c.r * 2.0
     }
+
+    fn length(&self) -> f64 {
+        0.0
+    }
 }
 
 #[derive(Debug, Clone, Copy, PartialEq)]
@@ -151,6 +156,10 @@ impl ShapeTrait for SegShape {
     fn width(&self) -> f64 {
         self.width
     }
+
+    fn length(&self) -> f64 {
+        self.to.euclidean_distance(&self.from)
+    }
 }
 
 #[derive(Debug, Clone, Copy, PartialEq)]
@@ -247,6 +256,17 @@ impl ShapeTrait for BendShape {
     fn width(&self) -> f64 {
         self.width
     }
+
+    fn length(&self) -> f64 {
+        // We obtain the angle from the law of cosines and multiply with radius to get the length.
+        let d = self.to.euclidean_distance(&self.from);
+
+        if d > 0.0 {
+            (1.0 - d * d / (2.0 * d * d)).acos()
+        } else {
+            0.0
+        }
+    }
 }
 
 impl RTreeObject for Shape {