I am currently working on an implementation of the A* Algorithm with irregular distances between two nodes. The graph containing the nodes is a directed and weighted graph. Every node is connected to at least one other node, there may also be symmetrical connections with different distances. A node is nothing more than a label and doesn't contain any special information
What I need is a heuristic to determine the shortest path from any node A to another node B as accurate as possible. I tried to use a heuristic that returns the distance to the nearest neighbor of a node, but of course that wasn't as effective as no heuristic at all (= Dijkstra).
My implementation of the A* Algorithm consists mainly of 2 classes, the class for the algorithm itself (AStar) and one for the nodes (Node). The code is heavily based on the Wikipedia pseudocode.
Source code of AStar.java
public class AStar {
private AStar() {}
private static Node[] reconstructPath(Map<Node, Node> paths, Node current) {
List<Node> path = new ArrayList<Node>();
path.add(0, current);
while (paths.containsKey(current)) {
current = paths.get(current);
path.add(0, current);
}
return path.toArray(new Node[0]);
}
public static Node[] calculate(Node start, Node target, IHeuristic heuristic) {
List<Node> closed = new ArrayList<Node>();
PriorityQueue<Node> open = new PriorityQueue<Node>();
Map<Node, Double> g_score = new HashMap<Node, Double>();
Map<Node, Double> f_score = new HashMap<Node, Double>();
Map<Node, Node> paths = new HashMap<Node, Node>();
g_score.put(start, 0d);
f_score.put(start, g_score.get(start) + heuristic.estimateDistance(start, target));
open.set(start, f_score.get(start));
while (!open.isEmpty()) {
Node current = null;
// find the node with lowest f_score value
double min_f_score = Double.POSITIVE_INFINITY;
for (Entry<Node, Double> entry : f_score.entrySet()) {
if (!closed.contains(entry.getKey()) && entry.getValue() < min_f_score) {
min_f_score = entry.getValue();
current = entry.getKey();
}
}
if (current.equals(target)) return reconstructPath(paths, target);
open.remove(current);
closed.add(current);
for (Node neighbor : current.getAdjacentNodes()) {
if (closed.contains(neighbor)) {
continue;
}
double tentative_g_score = g_score.get(current) + current.getDistance(neighbor);
if (!open.contains(neighbor) || tentative_g_score < g_score.get(neighbor)) {
paths.put(neighbor, current);
g_score.put(neighbor, tentative_g_score);
f_score.put(neighbor, g_score.get(neighbor) + heuristic.estimateDistance(neighbor, target));
if (!open.contains(neighbor)) {
open.set(neighbor, f_score.get(neighbor));
}
}
}
}
throw new RuntimeException("no path between " + start + " and " + target);
}
}
Source code of Node.java
public class Node {
private Map<Node, Double> distances = new HashMap<Node, Double>();
public final String name;
public Node(String name) {
this.name = name;
}
public Set<Node> getAdjacentNodes() {
return Collections.unmodifiableSet(distances.keySet());
}
public double getDistance(Node node) {
return distances.get(node);
}
public void setDistance(Node node, double distance) {
distances.put(node, distance);
}
#Override
public String toString() {
return (name == null ? "Node#" + Integer.toHexString(hashCode()) : name);
}
}
Source code of PriorityQueue.java
public class PriorityQueue<T> {
transient ArrayList<PriorityEntry<T>> elements = null;
private static final int DEFAULT_SIZE = 10;
public PriorityQueue() {
elements = new ArrayList<PriorityEntry<T>>(DEFAULT_SIZE);
}
public PriorityQueue(int initialCapacity) {
elements = new ArrayList<PriorityEntry<T>>(initialCapacity);
}
public boolean push(T element, double priority) {
PriorityEntry<T> entry = new PriorityEntry<T>(element, priority);
if (elements.contains(entry)) return false;
elements.add(entry);
elements.sort(null);
return true;
}
public void set(T element, double priority) {
PriorityEntry<T> entry = new PriorityEntry<T>(element, priority);
int index = elements.indexOf(entry);
if (index >= 0) {
elements.get(index).setPriority(priority);
} else {
elements.add(entry);
}
elements.sort(null);
}
public T peek() {
return size() <= 0 ? null : elements.get(0).getValue();
}
public T pop() {
return size() <= 0 ? null : elements.remove(0).getValue();
}
public boolean remove(T element) {
return elements.remove(new PriorityEntry<T>(element, 0));
}
public int size() {
return elements.size();
}
public boolean isEmpty() {
return elements.isEmpty();
}
public boolean contains(T element) {
return elements.contains(new PriorityEntry<T>(element, 0));
}
private class PriorityEntry<E> implements Comparable<PriorityEntry<? extends T>> {
private final E value;
private double priority = Double.MIN_VALUE;
public PriorityEntry(E value, double priority) {
this.value = value;
this.priority = priority;
}
public E getValue() {
return value;
}
public double getPriority() {
return priority;
}
public void setPriority(double priority) {
this.priority = priority;
}
#Override
#SuppressWarnings("unchecked")
public boolean equals(Object o) {
if (!(o instanceof PriorityEntry)) return false;
PriorityEntry<?> entry = (PriorityEntry<?>) o;
return value.equals(entry);
}
#Override
public int compareTo(PriorityEntry<? extends T> entry) {
return (int) (getPriority() - entry.getPriority());
}
}
}
Before trying to define a heuristic function for your problem, consider that in many cases, using a poor (or incorrect) estimation of the cost to the goal is as self-defeating as not using an heuristic at all.
In the case of a graph with weighted arcs, you need to consider if there is some information in the nodes which can lead to obtain the heuristic values (for example, if your nodes are cities, a good estimation can be the lenght of the straight line between them; or if your nodes are arrays, a similarity measurement between them). If your nodes are only labels and there is no information that you can use to obtain your heuristic values, maybe the best solution is not using a heuristic at all. This is not the worst scenario for most problems of this type. It is better to use a Dijkstra search (which is the same of A* but using heuristic=0), letting the algorithm expand the nodes based on the cost from the start, than using a bad heuristic which is not consistent, because in this situation you might be expanding unncecesary nodes that seem to be promising based on a bad estimation of the cost to the goal.
I don't know how big are your graphs, but for most problems there is not a significant difference in computation time between using a heuristic and don't using it at all. Specially in the case of a bad heuristic.
I can see that you have your own implementation of A*. I recommend you to take a look of an heuristic search library like Hipster. This library allows you to define your graph and test different search algorithms to know the best one that fits your problem. Some code examples describe exactly your case: seach in weighted directed graphs. It might be useful for your problem.
I hope my answer helps. Regards,
Without going into other possible problems I would like to point out the main problem - you lack the plane. In case of shortest distance problem between cities, you have
node - city
weight - numerical value describing cost to get from city a to city b
plane - describes environment ex: city position (in your square grid)
From plane you can extrapolate meaningful heuristic. For example you can make assumption from city position like city with lowest arithmetical distance should be looked first.
If you do not have a plane you do not have any means to predict a meaningful heuristic. A* will still work, but it hardly differs from exhaustive search. You can create plane from the weight, but it .
Ex:
Iterate over weights and find the weight quantiles 20/60/20 - now you have a relative plane
- good weight threshold (lowest 20%)
- average weight threshold (middle 60%)
- bad weight threshold (highest 20%)
With priority queue your algorithm would pick good moves, then average and finally bad ones.
If you want you can have more then 3 segments.
Just as a reminder: A* returns good enough result fast. Using exhaustive search you can find the best solution, but it would become exponentially slower if problem size grows.
To add to #kiheru comment above. Your solution will only be as good as the heuristic provided.
If the following line and the heuristic.estimate, has too narrow of a scope. The algorithm will quickly reach a local minimum. Alternatively, if the heuristic isn't admissible the algorithm will result in either no solution or an incorrect random solution.
f_score.put(start, g_score.get(start) + heuristic.estimateDistance(start, target));
Take a close look at your heuristic and confirm it's admissible. If it is admissible, it may need to be improved in order to provide a more accurate estimate.
In the case of your node class it seems to have an X and Y if these represent the node's position in a 2D space maybe you could use a heuristic based on the straight line distance between the nodes calculated from the X and Y values.
Related
I want to make a program where you can make nodes and connect them, I wanna make Graphical interface so you can see the graphs. My problem is I need to store the edges in between nodes(vertices) some people at my university told me to use maps?
I've tried to store the nodes with a map structure and making my own pair class:
import java.util.Stack;
public class Node extends Main {
public String data;
int ID;
Boolean IsConnected = false;
public Node(String data, int ID, int Connection ) {
this.data = data;
this.ID = ID;
}
public void Connect(Node N1, Node N2) {
}
public boolean IsConnected(Node N1, Node N2) {
if (map.containsValue(N1) && map.containsKey(N2)) {
System.out.println("connected");
return true;
}
System.out.println("not connected");
return false;
}
}
This addresses one way to logically represent edges of a graph. It does not address how to represent a graph visually.
I'm going to assume the number of nodes needed will be known. If that is not the case, this answer will need to be changed.
A common way to represent edges of a graph is to use an adjacency matrix. Given n Nodes, the simplest adjacency matrix for a directed graph is an 2D array of boolean:
boolean [][] adjacent = new boolean [n][n];
This requires each Node to be associated with an index. One way to do that is to use an array:
Node [] myNodes = new node [n];
Then, finding out if there is an edge between two nodes is simple:
public boolean areConnected (int a, int b) {
return adjacent [a][b];
}
And, they are easy to connect:
public void Connect (int from, int to) {
adjacent [from][to] = true;
}
If you need to search to find the index of a Node, you can add this code:
public int indexOf (Node node) {
int index = -1;
for (int i = 0; i < myNodes.length; ++i) {
if (node.equals (myNode [i]) {
index = i;
break;
}
}
}
public boolean areConnected (Node a, Node b) {
int locA = indexOf (a);
int locB = indexOf (b);
if (locA < 0 || locB < 0) {
return false;
}
return areConnected (locA, locB);
}
NOTE: Please seriously consider overriding equals (Object other) and hashCode() methods. Whether you use an adjacency matrix as described here, or a Collection, the default .equals (Object other) will be equivalent to this == other. That is, it will test if they refer to the same Object. If you want to know if the contents of Node Objects are the same, you need to override equals. If you override equals, you should also override hashCode().
Note: You will want to decide how to guard against the possibility that an invalid index or node will be passed to areConnected or Connect. You might, for example, want to throw an exception.
Note: This assumes a directed graph.
Comment: The beginning of this answer has this statement: "I'm going to assume the number of nodes needed will be known early." "Early" means "before the matrix is constructed". One way to represent an adjacency matrix in which the number of nodes is flexible is to use a List<List<Boolean>>. That can represent a 2D List.
An adjacency matrix is not restricted to boolean. For example, if each Node represented a city, the adjacency matrix might be an int [][] , where each entry represents the number of highway miles between cities. A double might represent the geographic distance. A BigDecimal [][] could represent the price of airfare between the cities. It could be an array of Objects of a class you create.
In my case, I want to create a binary tree by taking the depth as the input.
For example, if I do tree.create(3), there will generate a binary tree when the depth of the deepest left is three, which means there will be 2^3 - 1 nodes in the tree and the value of all of them will be 0. The index will be 0 - 6 accorndingly.
class Tree<T> {
int depth;
int index;
T value;
public Tree(int index,T value) {
this.index = index;
this.value = value;
}
public static <K> Tree<K> create(int depth){
if(depth >= 1) {
//return a tree with the inputting depth,
//but I don't know how to do in this step
return new Tree(...?)
}
}
}
Which part of knowledge shall I use to achieve this in Java? Thanks!
Usually when working with a binary tree data structure you need to write recursive methods. When writing a recursive method, you need to write a condition that terminates the recursion. In your case, the recursion will end if we have reached the desired depth. In order to determine if we have reached the desired depth, we need to know what that desired depth is and what the current depth is. I will assume that the depth of the root node in the binary tree is zero. This means that for a maximum depth of 3 (as in the example in your question), the deepest level will be level 2. If we have not reached the desired depth, then I need to add a left and a right child node and then call the same method on each of those children and make sure that the depth of the child nodes is one greater than the depth of the parent. So my recursive method needs three parameters:
a node to (possibly) add child nodes to
the current depth
the maximum depth
Note that I don't need to handle the value (or data) that each node contains because you state, in your question, that each node will have the same value. Hence I can simply copy the value from the parent node to each of its children.
Here is a complete example. Note that when I tested it, a depth greater than 25 causes an OutOfMemoryError because there is a limit on how many recursive method invocations can be done.
import java.util.ArrayList;
import java.util.List;
public class BinTree<T> {
BinTreeNode<T> root;
public static <T> BinTree<T> create(int depth, T val) {
if (depth > 0) {
BinTree<T> theTree = new BinTree<>();
theTree.root = new BinTreeNode<>(val);
theTree.addLevel(theTree.root, 0, depth);
return theTree;
}
else {
throw new IllegalArgumentException("Invalid depth: " + depth);
}
}
public void getAllElements(BinTreeNode<T> aNode, List<T> list) {
if (aNode == null) {
return;
}
else {
getAllElements(aNode.left, list);
list.add(aNode.genericObject);
getAllElements(aNode.right, list);
}
}
private void addLevel(BinTreeNode<T> theNode, int level, int deepest) {
if (level == deepest - 1) {
return;
}
theNode.left = new BinTreeNode<>(theNode.genericObject);
theNode.right = new BinTreeNode<>(theNode.genericObject);
addLevel(theNode.left, level + 1, deepest);
addLevel(theNode.right, level + 1, deepest);
}
public static void main(String[] args) {
BinTree<String> aTree = BinTree.create(3, "George"); // max depth = 25
List<String> list = new ArrayList<>();
aTree.getAllElements(aTree.root, list);
System.out.println(list.size());
}
}
class BinTreeNode<T> {
BinTreeNode<T> left, right;
T genericObject;
public BinTreeNode(T obj) {
genericObject = obj;
}
}
Note that I added method getAllElements as a test to see whether the correct number of nodes were created.
I am absolutely confused on what to do. I'm trying to code off of the pseudo code that wikipedia has on Dijkstra's with priority queues, but I'm having a tough time making the adjustments to fit what i need to find. This is my (incomplete) code so far, and any help would be very much appreciated.
public int doDijkstras (String startVertexName, String endVertexName, ArrayList< String > shortestPath) {
PriorityQueue<QEntry> q = new PriorityQueue<QEntry>();
int cost = 0;
int newCost;
QEntry pred = null;
for (String s : this.getVertices()) {
if (!s.equals(startVertexName)) {
cost = Integer.MAX_VALUE;
pred = null;
}
q.add(new QEntry(s, cost, pred, adjacencyMap.get(s)));
}
while (!q.isEmpty()) {
QEntry curr = getMin(q);
for (String s : curr.adj.keySet()) {
newCost = curr.cost + this.getCost(curr.name, s);
QEntry v = this.getVert(q, s);
if (newCost < v.cost) {
v.cost = newCost;
v.pred = curr;
if (!q.contains(curr)) {
q.add(curr);
}
}
}
}
}
private QEntry getMin(PriorityQueue<QEntry> q) {
QEntry min = q.peek();
for (QEntry temp : q) {
if (min.cost > temp.cost) {
min = temp;
}
}
return min;
}
private QEntry getVert(PriorityQueue<QEntry> q, String s) {
for (QEntry temp : q) {
if (temp.name.equals(s)) {
return temp;
}
}
return null;
}
class QEntry {
String name;
int cost;
QEntry pred;
TreeMap<String, Integer> adj;
public QEntry(String name, int cost, QEntry pred, TreeMap<String, Integer> adj) {
this.name = name;
this.cost = cost;
this.adj = adj;
this.pred = pred;
}
}
You are overlooking an important part of the algorithm: when to stop.
The pseudocode on Wikipedia is for the variation on Dijkstra's algorithm that computes the shortest path from the start node to every node connected to it. Commentary immediately following the big pseudocode block explains how to modify the algorithm to find only the path to a specific target, and after that is a shorter block explaining how to extract paths.
In English, though, as you're processing your priority queue, you need to watch for the target element being the one selected. When (if ever) it is, you know that no shorter path to it can be discovered than the one having the cost recorded in the target's queue entry, and represented (in reverse order) by that entry and its chain of predecessors. You fill the path list by walking the chain of predecessors, and you return the value that was recorded in the target queue entry.
Note, however, that in your code, in the event that the start and target vertexes are not connected in the graph (including if the target is not in the graph at all), you will eventually drain the queue and fall out the bottom of the while loop without ever reaching the target. You have to decide what to do with the path list and what to return in that case.
Note, too, that your code appears to have several errors, among them:
In the event that the start vertex name is not the first one in the iteration order of this.getVertices(), its queue entry will not be initialized with cost 0, and will not likely be the first element chosen from the queue.
If the specified start vertex is not in the graph at all then your code will run, and may emit a path, but its output in that case is bogus.
Your queue elements (type QEntry) do not have a natural order; to create a PriorityQueue whose elements have such a type, you must provide a Comparator that defines their relative priorities.
You are using your priority queue as a plain list. That in itself will not make your code produce wrong results, but it does increase its asymptotic complexity.
Be aware, however, that if you use the standard PriorityQueue as a priority queue, then you must never modify an enqueued object in a way that could change its order relative to any other enqueued object; instead, remove it from the queue first, modify it, then enqueue it again.
I have an undirected weighted graph (G) composed of at least 15 vertices. Given a set of vertices (G'), where G' ⊆ G, I need to calculate the shortest path required to traverse G' given a start vertex (v).
I am stuck!
I tried the following:
for (Vertex vx : G'):
computeShortestPath (v, vx)//using Dijkstra's algo
The shortest of all the paths generated between V and any vertex from G' will then constitute the initialized path (P).I then remove all vertices from G' that have been visited in P
G'.remove (P)
Recursively compute P until:
G'.size () == 0
My algorithm seems inefficient at times though! Any suggestions into different remedies to this problem?
Edit: I need to visit every node in G' only once.
If I understand your problem correctly then it is, essentially, the Travelling Salesman problem which has been proven to be NP-hard: there is no efficient solution to this problem. Any solution you come up with requires resources that increase exponentially with the number of nodes. There are efficient solutions for returning the most likely shortest path or to iterate towards the shortest path. There are algorithms for determining if there is a path before starting the search.
Djikstra's algorithm is used to find the shortest path through the graph rather than the shortest path that visits all nodes.
For small numbers of nodes the easiest solution is an exhaustive search of all paths. This will look something like:
class PathFinder {
Path shortestPath;
public void findShortestPath(Path currentPath, List<Node> remainingNodes) {
if (remainingNodes.isEmpty()) {
if (currentPath.isShorterThan(shortestPath)) {
shortestPath = currentPath;
}
} else {
for (Node node: currentPath.possibleNextNodes(remainingNodes)) {
remainingNodes.remove(node);
currentPath.add(node);
findShortestPath(currentPath, remainingNodes);
currentPath.remove(node);
remainingNodes.add(node);
}
}
}
}
This algorithm does not copy the path or list of remaining nodes for efficiency reasons. It will work find for graphs of 15 nodes. For thousands of nodes not so much.
This requires you to implement Path and Node classes. Here is a possible partial implementation of them:
public class Node {
private class Link {
private final Node destination;
private final int weight;
private Link(Node destination, int weight) {
this.destination = destination;
this.weight = weight;
}
private final List<Link> links;
public void addLink(Node destination, int weight) {
if (!connectsTo(destination)) {
Link link = new Link(destination, weight);
destination.addLink(this, weight);
}
}
public boolean connectsTo(Node node) {
return links.stream.anyMatch(link -> link.destination.equals(node));
}
public int weightTo(Node node) {
return links.stream.filter(link -> link.destination.equals(node))
.findAny().orElse(0);
}
}
public class Path {
private int length;
private List<Node> nodes;
private Node lastNode() {
return nodes.get(nodes.size() - 1);
}
public List<Node> possibleNextNodes(List<Node> possibleNodes) {
if (nodes.isEmpty());
return possibleNodes;
return possibleNodes.stream()
.filter(node -> lastNode().connectsTo(node))
.filter(node -> !nodes.contains(node))
.collect(Collectors.toList());
}
public boolean isShorterThan(Path other) {
return this.length < other.length;
}
public void add(Node node) {
length += lastNode().distanceTo(node);
nodes.add(node);
}
}
I'm trying to implement A-Star in Java based on OSM Data. My problem is that my implementation is not working correctly. First of all the path is not the shortest. Second the closedlist contains more 1/3 more nodes in the end as Dijkstra. Thats actuall not that what I expected.
Here is my A-Star code which is based on Wikipedia Pseudocode
public Object[] executeAstar(ArrayList<Arclistentry> data, NodeD start, NodeD dest,long[] nodenur)
{
openlist = new PriorityQueue<NodeD>(1,comp);
closedlist.clear();
openlist.offer(start);
start.setg(0);
start.seth(calccost(start, dest));
start.setf(start.getg()+start.geth());
while(!openlist.isEmpty())
{
NodeD currentnode = openlist.poll();
if(currentnode.getnodenumber() == dest.getpredessor())
{
closedlist.add(currentnode);
return drawway(closedlist, start, dest);
}
closedlist.add(currentnode);
ArrayList<Arclistentry> entries = neighbors.get((int)currentnode.getnodenumber()-1);
for(Arclistentry aentry:entries)
{
NodeD successor = new NodeD(aentry.getnode(),aentry.getstart(), aentry.getcoorddest());
float tentative_g = currentnode.getg()+calccost(currentnode,successor);//+aentry.getcost();
if(contains(successor, closedlist))
{
continue;
}
if((contains(successor,openlist))&& tentative_g >= aentry.getcost())
{
continue;
}
if(!contains(successor, openlist))
{
successor.setpredessor(currentnode.getnodenumber());
successor.setg(tentative_g);
successor.seth(calccost(successor, dest));
successor.setf(successor.getg()+successor.geth());
openlist.offer(successor);
}
else
{
openlist.remove(successor);
successor.setpredessor(currentnode.getnodenumber());
successor.setg(tentative_g);
successor.seth(calccost(successor, dest));
successor.setf(successor.getg()+successor.geth());
openlist.offer(successor);
}
}
}
return drawway(closedlist,start, dest);
}
My Heuristics will be calculated by using the euclidian distance. But to consider also the cost of the node, the costs are multiplied with the heuristics result. My Data structure contains the following:
private long nodenumber;
private long predessor;
private float label;
private float f;
private float g;
private float h;
private double[] coord = new double[2];
public NodeD(long nodenr, long predessor, double[] coor)
{
this.nodenumber = nodenr;
this.predessor = predessor;
this.coord = coor;
}
public NodeD(long nodenr, long predessor, float label)
{
this.nodenumber = nodenr;
this.predessor = predessor;
this.label = label;
}
and for the arclist I use the following:
private long start;
private long dest_node;
private float cost_;
private double[]coordstart = new double[2];
private double[]coorddest = new double[2];
Contains Function for Priority Queue:
public boolean contains(NodeD o, PriorityQueue<NodeD> al)
{
Iterator<NodeD> e = al.iterator();
if (o==null)
{
while (e.hasNext())
{
if (e.next()==null)
{
return true;
}
}
}
else
{
while (e.hasNext())
{
NodeD t = e.next();
if(t.equals(null))
{
return false;
}
if (((o.getnodenumber()==t.getnodenumber()) & (o.getpredessor()==t.getpredessor()))||(o.getnodenumber()==t.getpredessor() & o.getpredessor()==t.getnodenumber()))
{
return true;
}
}
return false;
}
return false;
}
and contains for ArrayList (because it was not detecting right with the ArrayList.contains function
public boolean contains(NodeD o, ArrayList<NodeD> al) {
return indexOf(o,al) >= 0;
}
public int indexOf(NodeD o, ArrayList<NodeD> al) {
if (o == null) {
for (int i = 0; i < al.size(); i++)
if (al.get(i)==null)
return i;
} else {
for (int i = 0; i < al.size(); i++)
{
if ((o.getpredessor()==al.get(i).getpredessor())) //(o.getnodenumber()==al.get(i).getnodenumber()) &&
{
return i;
}
else if((o.getpredessor()==al.get(i).getnodenumber())&&(o.getnodenumber()==al.get(i).getpredessor()))
{
return i;
}
}
}
return -1;
}
The problem is that the algorithm is visiting all nodes. The other problem is the sorted openlist, which is pushing neighbors of the currentnode up, because they have a lower f value. So what I'm duing wrong by implementing this algorithm?
Recap of all our previous answers:
Make sure the A* estimation is a lower estimate otherwise it will wrongly skip parts
Do not iterate over all nodes to determine the index of the edges of your current node's edge set in an array
When creating new objects to put in your queue/sets, checks should be done on the properties of the nodes
If your focus is on speed, avoid as much work as possible by aborting non-interesting searches as soon as possible
I'm still unsure about this line:
if((contains(successor,openlist))&& tentative_g >= aentry.getcost())
What I think you are trying to do is to avoid adding a new node to the queue when you already have a better value for it in there. However, tentative_g is the length of the path from your starting node to your current node while aentry.getcost seems to be the length of the edge you are relaxing. That doesn't seem right to me... Try to retrieve the correct (old) value to compare against your new tentative label.
Lastly, for your current code, I would also make the following changes:
Use HashSet for your closedlist. Every time you check if a node is in there, you have to go over them all, which is not that efficient... Try using a HashSet by overriding the hash function of your NodeD objects. The built-in contains-function is much faster than your current approach. A similar argument can be made for your openlist. You cannot change the PQ to a set but you could omit the contains-checks. If you add a node with a bad priority, you will always first poll the correct priority (because it PQ) and could then, when polling the bad priority, just skip it. That's a small optimisation that trades off size of PQ to PQ lookup-operations
avoid recalculating stuff (mainly calccost()) by calculating it once and reusing the value when you need it (small time gain but nicer code).
try to avoid multiple lines with the same code by placing them on the correct line (e.g. 2 closedlist.add function can be merged to 1 add-call placed above the if condition, if you have something like if(..){doA();doB()}else{doA();doC();} try to put doA() before the if for legibility)