Add beltfinding timeout and debugging.

src/graph/wheighted_graph/shortest_path.rs

@@ -61,6 +61,10 @@ where
     G::Node: Eq + Hash + Clone + Debug,
     G: WheightedGraph,
 {
+    if start == end {
+        return Some(vec![start]);
+    }
+
     let mut map: HashMap<G::Node, MapObject<G::Node, P::Handle>> = HashMap::new();
 
     let mut q = P::new();