diff --git a/src/main/java/fr/u_paris/gla/project/itinerary/Finder.java b/src/main/java/fr/u_paris/gla/project/itinerary/Finder.java index 23c3378..f0d67a0 100644 --- a/src/main/java/fr/u_paris/gla/project/itinerary/Finder.java +++ b/src/main/java/fr/u_paris/gla/project/itinerary/Finder.java @@ -1,21 +1,83 @@ package fr.u_paris.gla.project.itinerary; -import java.util.List; - -public class Finder { - private final Graph graph; +import java.util.*; +public class Finder > { + private Graph graph; public Finder(Graph graph) { this.graph = graph; } - public List findPath(T from, T to) { - // TODO + public List findPath(T startNode, T goalNode) { + PriorityQueue openSet = new PriorityQueue<>(Comparator.comparingDouble(GraphNode::getF)); + HashSet closedSet = new HashSet<>(); + HashMap cameFrom = new HashMap<>(); + HashMap gScore = new HashMap<>(); + HashMap fScore = new HashMap<>(); + + // Initialize scores for all nodes to infinity + for (T node : graph.getNodes()) { + gScore.put(node, Double.POSITIVE_INFINITY); + fScore.put(node, Double.POSITIVE_INFINITY); + } + + // The cost of going from start to start is zero + gScore.put(startNode, 0.0); + // For the first node, fScore = gScore + heuristic + fScore.put(startNode, startNode.getHeuristicCost(goalNode)); + openSet.add(startNode); + + while (!openSet.isEmpty()) { + T current = openSet.poll(); + + if (current.equals(goalNode)) { + return reconstructPath(cameFrom, current); + } + + closedSet.add(current); + + for (T neighbor : current.getNeighbors()) { + if (closedSet.contains(neighbor)) { + continue; // Ignore the neighbor which is already evaluated. + } + + double tentativeGScore = gScore.get(current) + current.getCost(neighbor); + + if (!openSet.contains(neighbor)) { + openSet.add(neighbor); + } else if (tentativeGScore >= gScore.get(neighbor)) { + continue; // This is not a better path. + } + + // This path is the best until now. Record it! + cameFrom.put(neighbor, current); + gScore.put(neighbor, tentativeGScore); + fScore.put(neighbor, tentativeGScore + neighbor.getHeuristicCost(goalNode)); + } + } + + // If we reach here, it means there's no path from start to goal return null; } - public List findPath(double longitude, double latitude) { - // TODO + private List reconstructPath(HashMap cameFrom, T current) { + List totalPath = new ArrayList<>(); + totalPath.add(current); + + while (cameFrom.containsKey(current)) { + current = cameFrom.get(current); + totalPath.add(0, current); // Add to the beginning of the list to maintain order + } + + return totalPath; + } + + //TODO: + public List findPath(double longitude, double latitude){ return null; } + + } + + diff --git a/src/main/java/fr/u_paris/gla/project/itinerary/Graph.java b/src/main/java/fr/u_paris/gla/project/itinerary/Graph.java index 9f263e8..23f47e2 100644 --- a/src/main/java/fr/u_paris/gla/project/itinerary/Graph.java +++ b/src/main/java/fr/u_paris/gla/project/itinerary/Graph.java @@ -12,4 +12,8 @@ public class Graph { this.nodes = nodes; this.connections = connections; } + + public Set getNodes() { + return nodes; + } } diff --git a/src/main/java/fr/u_paris/gla/project/itinerary/GraphNode.java b/src/main/java/fr/u_paris/gla/project/itinerary/GraphNode.java index 2b9e2a6..197bcf4 100644 --- a/src/main/java/fr/u_paris/gla/project/itinerary/GraphNode.java +++ b/src/main/java/fr/u_paris/gla/project/itinerary/GraphNode.java @@ -1,7 +1,13 @@ package fr.u_paris.gla.project.itinerary; import java.util.List; +import java.util.Set; -public interface GraphNode{ +public interface GraphNode{ int getId(); + double getHeuristicCost(T goalNode); + + Set getNeighbors(); + double getCost(T neighbor); + double getF(); } diff --git a/src/main/java/fr/u_paris/gla/project/itinerary/Stop.java b/src/main/java/fr/u_paris/gla/project/itinerary/Stop.java index 2cbd2db..2da2427 100644 --- a/src/main/java/fr/u_paris/gla/project/itinerary/Stop.java +++ b/src/main/java/fr/u_paris/gla/project/itinerary/Stop.java @@ -27,6 +27,27 @@ public class Stop implements GraphNode { return id; } + @Override + public double getHeuristicCost(Object goalNode) { + return 0; + } + + @Override + public Set getNeighbors() { + return lines; + } + + + @Override + public double getCost(Object neighbor) { + return 0; + } + + @Override + public double getF() { + return 0; + } + public String getName(){ return name; }