update graph heuristic functions

Author: eendroroy

Diff for: src/algorithm/graph.rs

@@ -292,7 +292,7 @@ pub fn dijkstra<W: GraphWeightTrait, const NODES: usize>(
 /// ]);
 /// ```
 //noinspection DuplicatedCode
-pub fn a_star<W: GraphWeightTrait, const NODES: usize, H: Fn(usize, usize) -> W>(
+pub fn a_star<W: GraphWeightTrait, const NODES: usize, H: Fn(usize, usize, usize) -> W>(
     graph: &Graph<W, NODES>,
     source: usize,
     destination: usize,
@@ -326,7 +326,7 @@ pub fn a_star<W: GraphWeightTrait, const NODES: usize, H: Fn(usize, usize) -> W>
         for adj in 0..NODES {
             if let Some(weight) = graph[(current, adj)] {
                 let new_cost = costs[current].unwrap() + weight;
-                let new_heu = new_cost + heu(adj, destination);
+                let new_heu = new_cost + heu(adj, destination, NODES);
                 match costs[adj] {
                     None => {
                         costs[adj] = Some(new_cost);

Diff for: src/algorithm/graph/heuristics.rs

@@ -1,38 +1,54 @@
 use crate::structure::graph::graph_trait::GraphWeightTrait;
 
 /// Manhattan distance heuristic for A*
-pub fn manhattan_heuristic<W: GraphWeightTrait + From<i32>>(node: usize, goal: usize) -> W {
-    let ((x1, y1), (x2, y2)) = get_points(node, goal);
+pub fn manhattan_heuristic<W: GraphWeightTrait + From<i32>>(
+    node: usize,
+    goal: usize,
+    nodes: usize,
+) -> W {
+    let ((x1, y1), (x2, y2)) = get_points(node, goal, nodes);
     W::from((x1 - x2).abs() + (y1 - y2).abs())
 }
 
 /// Euclidean distance heuristic for A*
-pub fn euclidean_heuristic<W: GraphWeightTrait + From<i32>>(node: usize, goal: usize) -> W {
-    let ((x1, y1), (x2, y2)) = get_points(node, goal);
+pub fn euclidean_heuristic<W: GraphWeightTrait + From<i32>>(
+    node: usize,
+    goal: usize,
+    nodes: usize,
+) -> W {
+    let ((x1, y1), (x2, y2)) = get_points(node, goal, nodes);
     W::from(((x1 as f64 - x2 as f64).powi(2) + (y1 as f64 - y2 as f64).powi(2)).sqrt() as i32)
 }
 
 /// Chebyshev distance heuristic for A*
-pub fn chebyshev_heuristic<W: GraphWeightTrait + From<i32>>(node: usize, goal: usize) -> W {
-    let ((x1, y1), (x2, y2)) = get_points(node, goal);
+pub fn chebyshev_heuristic<W: GraphWeightTrait + From<i32>>(
+    node: usize,
+    goal: usize,
+    nodes: usize,
+) -> W {
+    let ((x1, y1), (x2, y2)) = get_points(node, goal, nodes);
     W::from((x1 - x2).abs().max((y1 - y2).abs()))
 }
 
 /// Octile distance heuristic for A*
-pub fn octile_heuristic<W: GraphWeightTrait + From<i32>>(node: usize, goal: usize) -> W {
-    let ((x1, y1), (x2, y2)) = get_points(node, goal);
+pub fn octile_heuristic<W: GraphWeightTrait + From<i32>>(
+    node: usize,
+    goal: usize,
+    nodes: usize,
+) -> W {
+    let ((x1, y1), (x2, y2)) = get_points(node, goal, nodes);
     let (dx, dy) = ((x1 - x2).abs(), (y1 - y2).abs());
     W::from((dx + dy) + ((2 - 2_i32.isqrt()) * dx.min(dy)))
 }
 
 /// Dijkstra's Heuristic
-pub fn dijkstra_heuristic<W: GraphWeightTrait>(_: usize, _: usize) -> W {
+pub fn dijkstra_heuristic<W: GraphWeightTrait>(_: usize, _: usize, _: usize) -> W {
     W::zero()
 }
 
-fn get_points(node: usize, goal: usize) -> ((i32, i32), (i32, i32)) {
+fn get_points(node: usize, goal: usize, nodes: usize) -> ((i32, i32), (i32, i32)) {
     (
-        (node as i32 % 3, node as i32 / 3),
-        (goal as i32 % 3, goal as i32 / 3),
+        ((node % nodes) as i32, (node / nodes) as i32),
+        ((goal % nodes) as i32, (goal / nodes) as i32),
     )
 }