Skip to content

Commit

Permalink
Example implementation of A* (eugenp#8159)
Browse files Browse the repository at this point in the history
  • Loading branch information
sazzer authored and pivovarit committed Nov 11, 2019
1 parent cbbc407 commit cdb31fd
Show file tree
Hide file tree
Showing 8 changed files with 877 additions and 0 deletions.
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
package com.baeldung.algorithms.astar;

import java.util.Map;
import java.util.Set;
import java.util.stream.Collectors;


public class Graph<T extends GraphNode> {
private final Set<T> nodes;

private final Map<String, Set<String>> connections;

public Graph(Set<T> nodes, Map<String, Set<String>> connections) {
this.nodes = nodes;
this.connections = connections;
}

public T getNode(String id) {
return nodes.stream()
.filter(node -> node.getId().equals(id))
.findFirst()
.orElseThrow(() -> new IllegalArgumentException("No node found with ID"));
}

public Set<T> getConnections(T node) {
return connections.get(node.getId()).stream()
.map(this::getNode)
.collect(Collectors.toSet());
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
package com.baeldung.algorithms.astar;

public interface GraphNode {
String getId();
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
package com.baeldung.algorithms.astar;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.PriorityQueue;
import java.util.Queue;
import java.util.stream.Collectors;

public class RouteFinder<T extends GraphNode> {
private final Graph<T> graph;
private final Scorer<T> nextNodeScorer;
private final Scorer<T> targetScorer;

public RouteFinder(Graph<T> graph, Scorer<T> nextNodeScorer, Scorer<T> targetScorer) {
this.graph = graph;
this.nextNodeScorer = nextNodeScorer;
this.targetScorer = targetScorer;
}

public List<T> findRoute(T from, T to) {
Map<T, RouteNode<T>> allNodes = new HashMap<>();
Queue<RouteNode> openSet = new PriorityQueue<>();

RouteNode<T> start = new RouteNode<>(from, null, 0d, targetScorer.computeCost(from, to));
allNodes.put(from, start);
openSet.add(start);

while (!openSet.isEmpty()) {
System.out.println("Open Set contains: " + openSet.stream().map(RouteNode::getCurrent).collect(Collectors.toSet()));
RouteNode<T> next = openSet.poll();
System.out.println("Looking at node: " + next);
if (next.getCurrent().equals(to)) {
System.out.println("Found our destination!");

List<T> route = new ArrayList<>();
RouteNode<T> current = next;
do {
route.add(0, current.getCurrent());
current = allNodes.get(current.getPrevious());
} while (current != null);

System.out.println("Route: " + route);
return route;
}

graph.getConnections(next.getCurrent()).forEach(connection -> {
double newScore = next.getRouteScore() + nextNodeScorer.computeCost(next.getCurrent(), connection);
RouteNode<T> nextNode = allNodes.getOrDefault(connection, new RouteNode<>(connection));
allNodes.put(connection, nextNode);

if (nextNode.getRouteScore() > newScore) {
nextNode.setPrevious(next.getCurrent());
nextNode.setRouteScore(newScore);
nextNode.setEstimatedScore(newScore + targetScorer.computeCost(connection, to));
openSet.add(nextNode);
System.out.println("Found a better route to node: " + nextNode);
}
});
}

throw new IllegalStateException("No route found");
}

}
Original file line number Diff line number Diff line change
@@ -0,0 +1,67 @@
package com.baeldung.algorithms.astar;

import java.util.StringJoiner;

class RouteNode<T extends GraphNode> implements Comparable<RouteNode> {
private final T current;
private T previous;
private double routeScore;
private double estimatedScore;

RouteNode(T current) {
this(current, null, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
}

RouteNode(T current, T previous, double routeScore, double estimatedScore) {
this.current = current;
this.previous = previous;
this.routeScore = routeScore;
this.estimatedScore = estimatedScore;
}

T getCurrent() {
return current;
}

T getPrevious() {
return previous;
}

double getRouteScore() {
return routeScore;
}

double getEstimatedScore() {
return estimatedScore;
}

void setPrevious(T previous) {
this.previous = previous;
}

void setRouteScore(double routeScore) {
this.routeScore = routeScore;
}

void setEstimatedScore(double estimatedScore) {
this.estimatedScore = estimatedScore;
}

@Override
public int compareTo(RouteNode other) {
if (this.estimatedScore > other.estimatedScore) {
return 1;
} else if (this.estimatedScore < other.estimatedScore) {
return -1;
} else {
return 0;
}
}

@Override
public String toString() {
return new StringJoiner(", ", RouteNode.class.getSimpleName() + "[", "]").add("current=" + current)
.add("previous=" + previous).add("routeScore=" + routeScore).add("estimatedScore=" + estimatedScore)
.toString();
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
package com.baeldung.algorithms.astar;

public interface Scorer<T extends GraphNode> {
double computeCost(T from, T to);
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
package com.baeldung.algorithms.astar.underground;

import com.baeldung.algorithms.astar.Scorer;

public class HaversineScorer implements Scorer<Station> {
@Override
public double computeCost(Station from, Station to) {
double R = 6372.8; // In kilometers

double dLat = Math.toRadians(to.getLatitude() - from.getLatitude());
double dLon = Math.toRadians(to.getLongitude() - from.getLongitude());
double lat1 = Math.toRadians(from.getLatitude());
double lat2 = Math.toRadians(to.getLatitude());

double a = Math.pow(Math.sin(dLat / 2),2) + Math.pow(Math.sin(dLon / 2),2) * Math.cos(lat1) * Math.cos(lat2);
double c = 2 * Math.asin(Math.sqrt(a));
return R * c;
}
}
Loading

0 comments on commit cdb31fd

Please sign in to comment.