From ba2f4fdf3d91187b54e93841842d377789c356ea Mon Sep 17 00:00:00 2001
From: Yohan Simard <ysimard@etud.insa-toulouse.fr>
Date: Fri, 15 May 2020 17:01:05 +0200
Subject: [PATCH] A* working and tested

---
 .../shortestpath/AStarAlgorithm.java          | 49 ++++++++++
 .../shortestpath/DijkstraAlgorithm.java       | 37 ++++---
 .../graphs/algorithm/shortestpath/Label.java  | 27 ++---
 .../algorithm/shortestpath/LabelStar.java     | 28 ++++++
 .../shortestpath/AStarAlgorithmTest.java      | 11 +++
 .../shortestpath/DijkstraAlgorithmTest.java   | 89 +----------------
 .../shortestpath/ShortestPathTest.java        | 98 +++++++++++++++++++
 .../main/java/org/insa/graphs/model/Path.java |  2 +-
 8 files changed, 226 insertions(+), 115 deletions(-)
 create mode 100644 be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/LabelStar.java
 create mode 100644 be-graphes-algos/src/test/java/org/insa/graphs/algorithm/shortestpath/AStarAlgorithmTest.java
 create mode 100644 be-graphes-algos/src/test/java/org/insa/graphs/algorithm/shortestpath/ShortestPathTest.java

diff --git a/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/AStarAlgorithm.java b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/AStarAlgorithm.java
index fd172f0..27c8612 100644
--- a/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/AStarAlgorithm.java
+++ b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/AStarAlgorithm.java
@@ -1,9 +1,58 @@
 package org.insa.graphs.algorithm.shortestpath;
 
+import org.insa.graphs.algorithm.AbstractInputData;
+import org.insa.graphs.model.*;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import static org.insa.graphs.model.GraphStatistics.NO_MAXIMUM_SPEED;
+
 public class AStarAlgorithm extends DijkstraAlgorithm {
 
     public AStarAlgorithm(ShortestPathData data) {
         super(data);
     }
 
+
+    /**
+     * Generates a list of labelStar associated with a graph
+     *
+     * @param data the data to use to construct the labels
+     * @return A list of labels in the same order as data.getGraph().getNodes()
+     */
+    @Override
+    protected List<Label> generateLabels(ShortestPathData data) {
+        List<Node> nodes = data.getGraph().getNodes();
+        List<Label> labels = new ArrayList<>();
+        int maxSpeed = getMaxSpeed(data);
+        for (Node node : nodes) {
+            double estimatedCost;
+            // if the cost is the distance, take straight line distance as heuristic
+            if (data.getMode() == AbstractInputData.Mode.LENGTH) {
+                estimatedCost = Point.distance(node.getPoint(), data.getDestination().getPoint());
+
+            } else {    // if the cost is the time, multiply straight line distance with the maximum speed
+                estimatedCost = Point.distance(node.getPoint(), data.getDestination().getPoint()) / maxSpeed;
+            }
+            labels.add(new LabelStar(node, estimatedCost));
+        }
+        return labels;
+    }
+
+    /**
+     * @return the maximum speed of this input data in m/s
+     */
+    private int getMaxSpeed(ShortestPathData data) {
+        int dataMaxSpeed = data.getMaximumSpeed();
+        int graphMaxSpeed = data.getGraph().getGraphInformation().getMaximumSpeed();
+        if (dataMaxSpeed == NO_MAXIMUM_SPEED && graphMaxSpeed == NO_MAXIMUM_SPEED)
+            return 1000;
+        if (dataMaxSpeed != NO_MAXIMUM_SPEED && graphMaxSpeed != NO_MAXIMUM_SPEED)
+            return (int) (Math.min(dataMaxSpeed, graphMaxSpeed)/3.6);
+        if (dataMaxSpeed != NO_MAXIMUM_SPEED)
+            return (int) (dataMaxSpeed/3.6);
+        return (int) (graphMaxSpeed/3.6);
+
+    }
 }
diff --git a/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/DijkstraAlgorithm.java b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/DijkstraAlgorithm.java
index fb1e027..4bf28ca 100644
--- a/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/DijkstraAlgorithm.java
+++ b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/DijkstraAlgorithm.java
@@ -14,42 +14,56 @@ import java.util.Collections;
 import java.util.List;
 
 public class DijkstraAlgorithm extends ShortestPathAlgorithm {
+    protected List<Label> labels;
 
     public DijkstraAlgorithm(ShortestPathData data) {
         super(data);
+        // Generate the labels associated with all the nodes of the graph
+        labels = generateLabels(data);
+    }
+
+    /**
+     * Generates a list of labels associated with a graph
+     *
+     * @param data the data to use to construct the labels
+     * @return A list of labels in the same order as data.getGraph().getNodes()
+     */
+    protected List<Label> generateLabels(ShortestPathData data) {
+        List<Node> nodes = data.getGraph().getNodes();
+        List<Label> labels = new ArrayList<>();
+        for (Node node : nodes) {
+            labels.add(new Label(node));
+        }
+        return labels;
+
     }
 
     @Override
     protected ShortestPathSolution doRun() {
         // Retreive the graph
         final ShortestPathData data = getInputData();
-        Graph graph = data.getGraph();
-
-        final int nbNodes = graph.size();
 
         // Construct a binary heap for Label (comparison is based on their cost)
         PriorityQueue<Label> heap = new BinaryHeap<>();
 
-        // Generate the labels associated with all the nodes of the graph
-        List<Label> labels = Label.generateLabels(graph);
-
-
         // Initialize labels and heap with the origin node, then notify observers
         Node origin = data.getOrigin();
         int originId = origin.getId();
         labels.get(originId).mark();
         labels.get(originId).updateCost(0, null);
         heap.insert(labels.get(originId));
-        notifyOriginProcessed(data.getOrigin());
+        notifyOriginProcessed(origin);
 
 //        int iterationCounter = 0;
+        Node destination = data.getDestination();
+        Node currentNode = null;
 
-        while (!heap.isEmpty()) {
+        while (!heap.isEmpty() && (currentNode == null || !currentNode.equals(destination))) {
 //            iterationCounter++;
             // Get the node with the minimum cost
             Label currentLabel = heap.deleteMin();
             currentLabel.mark();
-            Node currentNode = currentLabel.getNode();
+            currentNode = currentLabel.getNode();
 
 //            System.out.printf("Label marked : cost = %f; %d succesors; heap valid = %b \n",
 //                    currentLabel.getCost(),
@@ -79,7 +93,6 @@ public class DijkstraAlgorithm extends ShortestPathAlgorithm {
             }
         }
 
-        Node destination = data.getDestination();
         Label destinationLabel = labels.get(destination.getId());
 
         // If path not found
@@ -102,7 +115,7 @@ public class DijkstraAlgorithm extends ShortestPathAlgorithm {
 
         Collections.reverse(path);
 
-        return new ShortestPathSolution(data, AbstractSolution.Status.OPTIMAL, new Path(graph, path));
+        return new ShortestPathSolution(data, AbstractSolution.Status.OPTIMAL, new Path(data.getGraph(), path));
     }
 
 }
diff --git a/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/Label.java b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/Label.java
index c1b2919..9f0ac14 100644
--- a/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/Label.java
+++ b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/Label.java
@@ -8,7 +8,7 @@ import java.util.ArrayList;
 import java.util.List;
 
 public class Label implements Comparable<Label> {
-    private Node node;
+    private final Node node;
     private boolean marked;
     private double cost;
     private Arc parent;
@@ -25,21 +25,6 @@ public class Label implements Comparable<Label> {
         parent = null;
     }
 
-    /**
-     * Generates a list of labels associated with a graph
-     *
-     * @param graph the graph used to construct the labels
-     * @return A list of labels in the same order as graph.getNodes()
-     */
-    public static List<Label> generateLabels(Graph graph) {
-        List<Node> nodes = graph.getNodes();
-        List<Label> labels = new ArrayList<>();
-        for (Node node : nodes) {
-            labels.add(new Label(node));
-        }
-        return labels;
-    }
-
     /**
      * Marks a Label
      */
@@ -70,12 +55,20 @@ public class Label implements Comparable<Label> {
         return cost;
     }
 
+    public double getTotalCost() {
+        return cost;
+    }
+
     public Arc getParent() {
         return parent;
     }
 
     @Override
     public int compareTo(Label o) {
-        return Double.compare(cost, o.cost);
+        int comp = Double.compare(getTotalCost(), o.getTotalCost());
+        if (comp == 0 && this.getClass() == LabelStar.class && o.getClass() == LabelStar.class)
+            return Double.compare(((LabelStar)this).getEstimatedCost(), ((LabelStar)o).getEstimatedCost());
+        else
+            return comp;
     }
 }
diff --git a/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/LabelStar.java b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/LabelStar.java
new file mode 100644
index 0000000..7906c66
--- /dev/null
+++ b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/LabelStar.java
@@ -0,0 +1,28 @@
+package org.insa.graphs.algorithm.shortestpath;
+
+import org.insa.graphs.model.Node;
+import org.insa.graphs.model.Point;
+
+public class LabelStar extends Label {
+
+    private final double estimatedCost;
+
+    /**
+     * Initalizes a LabelStar (not yet marked, with infinite cost)
+     *
+     * @param node The node associated with the label
+     */
+    public LabelStar(Node node, double estimatedCost) {
+        super(node);
+        this.estimatedCost = estimatedCost;
+    }
+
+    @Override
+    public double getTotalCost() {
+        return super.getCost() + getEstimatedCost();
+    }
+
+    public double getEstimatedCost() {
+        return estimatedCost;
+    }
+}
diff --git a/be-graphes-algos/src/test/java/org/insa/graphs/algorithm/shortestpath/AStarAlgorithmTest.java b/be-graphes-algos/src/test/java/org/insa/graphs/algorithm/shortestpath/AStarAlgorithmTest.java
new file mode 100644
index 0000000..61d19fb
--- /dev/null
+++ b/be-graphes-algos/src/test/java/org/insa/graphs/algorithm/shortestpath/AStarAlgorithmTest.java
@@ -0,0 +1,11 @@
+package org.insa.graphs.algorithm.shortestpath;
+
+import junit.framework.TestCase;
+
+public class AStarAlgorithmTest extends ShortestPathTest {
+    @Override
+    protected ShortestPathSolution runShortestPathAlgo(ShortestPathData data) {
+        return new AStarAlgorithm(data).doRun();
+    }
+
+}
\ No newline at end of file
diff --git a/be-graphes-algos/src/test/java/org/insa/graphs/algorithm/shortestpath/DijkstraAlgorithmTest.java b/be-graphes-algos/src/test/java/org/insa/graphs/algorithm/shortestpath/DijkstraAlgorithmTest.java
index 0bfb7f2..555dd47 100644
--- a/be-graphes-algos/src/test/java/org/insa/graphs/algorithm/shortestpath/DijkstraAlgorithmTest.java
+++ b/be-graphes-algos/src/test/java/org/insa/graphs/algorithm/shortestpath/DijkstraAlgorithmTest.java
@@ -1,89 +1,8 @@
 package org.insa.graphs.algorithm.shortestpath;
 
-import org.insa.graphs.algorithm.ArcInspector;
-import org.insa.graphs.algorithm.ArcInspectorFactory;
-import org.insa.graphs.model.*;
-import org.insa.graphs.model.io.BinaryGraphReader;
-import org.insa.graphs.model.io.GraphReader;
-import org.junit.Test;
-
-import java.io.*;
-import java.util.List;
-import java.util.Random;
-
-import static org.junit.Assert.*;
-
-
-public class DijkstraAlgorithmTest {
-
-    String[] maps = {
-            "/home/yohan/Documents/etudes/be-graphes/maps/carre.mapgr",
-            "/home/yohan/Documents/etudes/be-graphes/maps/insa.mapgr",
-            "/home/yohan/Documents/etudes/be-graphes/maps/toulouse.mapgr",
-            "/home/yohan/Documents/etudes/be-graphes/maps/guadeloupe.mapgr",
-    };
-
-    int[] origIndexes = {
-            22,
-            553,
-            25643,
-            6187,
-    };
-    int[] destIndexes = {
-            15,
-            111,
-            17402,
-            15025,
-    };
-
-    ArcInspector[] arcInspectors = {
-            ArcInspectorFactory.getAllFilters().get(0),
-            ArcInspectorFactory.getAllFilters().get(2),
-    };
-
-    private ShortestPathSolution runDijkstra(String map, ArcInspector arcInspector, int origIndex, int destIndex) throws IOException {
-        try (GraphReader reader = new BinaryGraphReader(new DataInputStream(new BufferedInputStream(new FileInputStream(map))))) {
-            Graph graph = reader.read();
-            List<Node> nodes = graph.getNodes();
-            Node orig = nodes.get(origIndex);
-            Node dest = nodes.get(destIndex);
-            ShortestPathData data = new ShortestPathData(graph, orig, dest, arcInspector);
-            return new DijkstraAlgorithm(data).doRun();
-        }
-    }
-
-    @Test
-    public void testDoRun() throws IOException {
-        // test feasible paths
-        for (int i = 0; i < 3; i++) {
-            for (ArcInspector arcInspector : arcInspectors) {
-                ShortestPathSolution sol = runDijkstra(maps[i], arcInspector, origIndexes[i], destIndexes[i]);
-                assertTrue(sol.isFeasible());
-                Path path = sol.getPath();
-                assertTrue(path.isValid());
-                if (i == 0)
-                    assertEquals(path, Path.createShortestPathFromNodes(sol.getInputData().getGraph(), path.getNodes()));
-                if (i == 1)
-                    assertEquals(path, Path.createFastestPathFromNodes(sol.getInputData().getGraph(), path.getNodes()));
-            }
-        }
-
-        // test 0-length paths
-        for (int i = 0; i < 3; i++) {
-            for (ArcInspector arcInspector : arcInspectors) {
-                ShortestPathSolution sol = runDijkstra(maps[i], arcInspector, origIndexes[i], origIndexes[i]);
-                assertTrue(sol.isFeasible());
-                assertTrue(sol.getPath().isValid());
-                assertEquals(sol.getPath().getLength(), 0, 1e-5);
-            }
-        }
-
-        // test impossible path
-        for (ArcInspector arcInspector : arcInspectors) {
-            ShortestPathSolution sol = runDijkstra(maps[3], arcInspector, origIndexes[3], destIndexes[3]);
-            assertFalse(sol.isFeasible());
-        }
-
-
+public class DijkstraAlgorithmTest extends ShortestPathTest {
+    @Override
+    protected ShortestPathSolution runShortestPathAlgo(ShortestPathData data) {
+        return new DijkstraAlgorithm(data).doRun();
     }
 }
\ No newline at end of file
diff --git a/be-graphes-algos/src/test/java/org/insa/graphs/algorithm/shortestpath/ShortestPathTest.java b/be-graphes-algos/src/test/java/org/insa/graphs/algorithm/shortestpath/ShortestPathTest.java
new file mode 100644
index 0000000..b7a3776
--- /dev/null
+++ b/be-graphes-algos/src/test/java/org/insa/graphs/algorithm/shortestpath/ShortestPathTest.java
@@ -0,0 +1,98 @@
+package org.insa.graphs.algorithm.shortestpath;
+
+import org.insa.graphs.algorithm.ArcInspector;
+import org.insa.graphs.algorithm.ArcInspectorFactory;
+import org.insa.graphs.model.Graph;
+import org.insa.graphs.model.Node;
+import org.insa.graphs.model.Path;
+import org.insa.graphs.model.io.BinaryGraphReader;
+import org.insa.graphs.model.io.GraphReader;
+import org.junit.Test;
+
+import java.io.BufferedInputStream;
+import java.io.DataInputStream;
+import java.io.FileInputStream;
+import java.io.IOException;
+import java.util.List;
+
+import static org.junit.Assert.*;
+
+public abstract class ShortestPathTest {
+    String[] maps = {
+            "/home/yohan/Documents/etudes/be-graphes/maps/carre.mapgr",
+            "/home/yohan/Documents/etudes/be-graphes/maps/insa.mapgr",
+            "/home/yohan/Documents/etudes/be-graphes/maps/toulouse.mapgr",
+            "/home/yohan/Documents/etudes/be-graphes/maps/guadeloupe.mapgr",
+    };
+
+    int[] origIndexes = {
+            22,
+            553,
+            25643,
+            6187,
+    };
+    int[] destIndexes = {
+            15,
+            111,
+            17402,
+            15025,
+    };
+
+    ArcInspector[] arcInspectors = {
+            ArcInspectorFactory.getAllFilters().get(0),
+            ArcInspectorFactory.getAllFilters().get(2),
+    };
+
+    private ShortestPathData createData(String map, ArcInspector arcInspector, int origIndex, int destIndex) throws IOException {
+        try (GraphReader reader = new BinaryGraphReader(new DataInputStream(new BufferedInputStream(new FileInputStream(map))))) {
+            Graph graph = reader.read();
+            List<Node> nodes = graph.getNodes();
+            Node orig = nodes.get(origIndex);
+            Node dest = nodes.get(destIndex);
+            return new ShortestPathData(graph, orig, dest, arcInspector);
+        }
+    }
+
+    protected abstract ShortestPathSolution runShortestPathAlgo(ShortestPathData data);
+
+    @Test
+    public void testDoRun() throws IOException {
+        // test feasible paths
+        for (int i = 0; i < 3; i++) {
+            for (ArcInspector arcInspector : arcInspectors) {
+                ShortestPathData data = createData(maps[i], arcInspector, origIndexes[i], destIndexes[i]);
+                ShortestPathSolution sol = runShortestPathAlgo(data);
+                ShortestPathSolution oracle = new BellmanFordAlgorithm(data).doRun();
+                assertTrue(sol.isFeasible());
+                Path path = sol.getPath();
+                assertTrue(path.isValid());
+                // check the optimality locally
+                if (i == 0)
+                    assertEquals(path, Path.createShortestPathFromNodes(data.getGraph(), path.getNodes()));
+                if (i == 1)
+                    assertEquals(path, Path.createFastestPathFromNodes(data.getGraph(), path.getNodes()));
+                // Check result against Bellman Ford algorithm (except for the square map or there may be several paths with the same cost)
+                if (i != 0)
+                    assertEquals(sol.getPath(), oracle.getPath());
+            }
+        }
+
+        // test 0-length paths
+        for (int i = 0; i < 3; i++) {
+            for (ArcInspector arcInspector : arcInspectors) {
+                ShortestPathData data = createData(maps[i], arcInspector, origIndexes[i], origIndexes[i]);
+                ShortestPathSolution sol = runShortestPathAlgo(data);
+                assertTrue(sol.isFeasible());
+                assertTrue(sol.getPath().isValid());
+                assertEquals(sol.getPath().getLength(), 0, 1e-5);
+            }
+        }
+
+        // test impossible path
+        for (ArcInspector arcInspector : arcInspectors) {
+            ShortestPathData data = createData(maps[3], arcInspector, origIndexes[3], destIndexes[3]);
+            ShortestPathSolution sol = runShortestPathAlgo(data);
+            assertFalse(sol.isFeasible());
+        }
+    }
+}
diff --git a/be-graphes-model/src/main/java/org/insa/graphs/model/Path.java b/be-graphes-model/src/main/java/org/insa/graphs/model/Path.java
index 346bd32..fa522c7 100644
--- a/be-graphes-model/src/main/java/org/insa/graphs/model/Path.java
+++ b/be-graphes-model/src/main/java/org/insa/graphs/model/Path.java
@@ -299,7 +299,7 @@ public class Path {
     }
 
     /**
-     * Checks if the arcs in this path are the same as arcs in obj
+     * Checks if the arcs in this path are the same as arcs in obj.
      * Warning: shallow comparison for the arcs
      */
     @Override