Dijkstra search with heuristics (A*)

gistfile1.java

package graph;

import java.util.*;

import GUI.GUIGraph;

public class DijkstraSearchHeuristics {
    static final HashMap<KrakNode, Double> dist = new HashMap<KrakNode, Double>();
    static final HashMap<KrakNode, KrakNode> previous = new HashMap<KrakNode, KrakNode>();
    static final HashSet<KrakNode> hasVisited = new HashSet<KrakNode>();
    static final HashMap<KrakNode, Float> h_score = new HashMap<KrakNode, Float>();
    static final HashMap<KrakNode, Double> f_score = new HashMap<KrakNode, Double>();

    public static List<KrakNode> search(KrakGraph graph, KrakNode source, KrakNode target, GUIGraph guiGraph) {
	dist.clear();
	previous.clear();
	hasVisited.clear();
	h_score.clear();
	f_score.clear();

	Comparator<KrakNode> comparator = new Comparator<KrakNode>() {

	    @Override
	    public int compare(KrakNode node1, KrakNode node2) {

		
		double val1;
		double val2;
		if(f_score.get(node1) == null) {
		    val1 = Double.POSITIVE_INFINITY;
		} else {
		    val1 = f_score.get(node1);
		}

		if(f_score.get(node2) == null) {
		    val2 = Double.POSITIVE_INFINITY;
		} else {
		    val2 = f_score.get(node2);
		}

		
		//System.out.println("comparing " + val1 + " with " + val2);
		if(val1 > val2) {
		    //System.out.println("val1 > val2");
		    return 1;
		} else if(val1 < val2) {
		    //System.out.println("val2 > val1");
		    return -1;
		} else {
		    //System.out.println("val2 == val1");
		    return 0;
		}
	    }

	};

	final PriorityQueue<KrakNode> queue = new PriorityQueue<KrakNode>(5000, comparator);

	System.out.println("Queue" + queue);

	System.out.println("queue size: " + queue.size());
	System.out.println("dist size: " + dist.size());

	/*
	for(KrakNode node : graph.getNodes()) {
	    if(node != null) {
		if(node == source) {
		    
		} else {
		    dist.put(node, Double.POSITIVE_INFINITY);
		}
		//queue.offer(node);
	    } 
	}*/
	dist.put(source, 0.0);
	h_score.put(source, 0.0f);
	f_score.put(source, 0.0);
	queue.offer(source);

	System.out.println("Source:" + source + " dist:" + dist.get(source));
	while(!queue.isEmpty()) {
	    KrakNode node = queue.poll();
	    //System.out.println("First in queue:" + node + " dist:" + dist.get(node));
	    //System.out.println("compare source, node: " + comparator.compare(source, node));
	    if(dist.get(node) == Double.POSITIVE_INFINITY)
		break;
	    //System.out.println("LÅLÅLÅLÅLÅLÅLÅ");
	    if(node == target) {
		System.out.println("Found path");
		return getPathToTarget(target);
	    }
	    
	    

	    hasVisited.add(node);
	    //System.out.println("looking at node: " + node + " with " + node.getNeighbors(graph).size() + " neighbors");

	    for(KrakEdge edge : graph.getOutgoingEdges(node)) {
		KrakNode neighborNode = edge.getOtherEnd(node);
		
		if(hasVisited.contains(neighborNode)) {
		    continue;
		}
		if(!queue.contains(neighborNode)) {
		    queue.offer(neighborNode);
		}

		double alt = dist.get(node) + node.getCost(neighborNode, graph);

		if(!dist.containsKey(neighborNode) || alt < dist.get(neighborNode)) {
		    dist.put(neighborNode, alt);
		    h_score.put(neighborNode, neighborNode.getEstimatedCost(target, graph));
		    f_score.put(neighborNode, alt+h_score.get(neighborNode));
		    queue.remove(neighborNode); queue.offer(neighborNode);
		    //System.out.println("updated distance for neighbor");
		    previous.put(neighborNode, node);
		}
	    }
	}
	return null;
    }

    public static List<KrakNode> getPathToTarget(KrakNode target) {
	LinkedList<KrakNode> path = new LinkedList<KrakNode>();
	while(previous.get(target) != null) {
	    path.addFirst(target);
	    target = previous.get(target);
	}
	return path;
    }
}