so, here’s a working A* algorithm + path reconstruction (no physics control needed)
A* pathfinding:
public static void aStarSearch(GameObject actor){
HashSet<Vector3f> openSet = new HashSet();
HashSet<Vector3f> closedSet = new HashSet();
LinkedHashMap<Vector3f,Vector3f> cameFrom = new LinkedHashMap();// 1 - node / 2 - nodes parent
HashMap<Vector3f,Float> gScore = new HashMap();
HashMap<Vector3f,Float> fScore = new HashMap();
gScore.put(actor.getStartingNode(), 0f);
fScore.put(actor.getStartingNode(), (abs(actor.getStartingNode().x-actor.getEndingNodePos().x)+abs(actor.getStartingNode().z-actor.getEndingNodePos().z)));
openSet.add(actor.getStartingNode());
// long sum = 0;
// System.out.println("ending node " + actor.getEndingNodePos());
while(!actor.getFoundPath() && !openSet.isEmpty() && !actor.hasLineOfSight){ // # 4 a) b)
float lowestF = Collections.min(fScore.values());
Vector3f currentNode = null;
// long time1 = System.currentTimeMillis();
for(Object entry: fScore.entrySet()) {
Entry<Vector3f,Float>entry1 = (Entry) entry;
if(entry1.getValue().equals(lowestF)) {
currentNode = entry1.getKey();
break;
}
}
// long time2 = System.currentTimeMillis()-time1;
// sum += time2;
if(currentNode.equals(actor.getEndingNodePos())){
actor.setFoundPath(true);
reconstructPath(cameFrom,currentNode);
break;
}
openSet.remove(currentNode);
closedSet.add(currentNode);
for(Object adjacentToCurrent : adjacencyMatrix.get(currentNode)){
Vector3f neighbor = (Vector3f) adjacentToCurrent;
if(!closedSet.contains(neighbor)){
float tentativeScore = gScore.get(currentNode)+(abs(currentNode.x-neighbor.x)+abs(currentNode.z-neighbor.z));
boolean tentativeScoreIsBetter = false;
if(!openSet.contains(neighbor)){
// cameFrom.putIfAbsent(neighbor, currentNode);
// gScore.putIfAbsent(neighbor, tentativeScore);
// fScore.putIfAbsent(neighbor,tentativeScore+(abs(neighbor.x-actor.getEndingNodePos().x)+abs(neighbor.z-actor.getEndingNodePos().z)));
openSet.add(neighbor);
tentativeScoreIsBetter = true;
// System.out.println("dodaje");
} else if(tentativeScore<gScore.get(neighbor)){
tentativeScoreIsBetter = true;
}
if(tentativeScoreIsBetter){
if(cameFrom.keySet().contains(neighbor)){
cameFrom.replace(neighbor, currentNode);
gScore.replace(neighbor, tentativeScore);
fScore.replace(neighbor,tentativeScore+(abs(neighbor.x-actor.getEndingNodePos().x)+abs(neighbor.z-actor.getEndingNodePos().z)));
}else{
cameFrom.put(neighbor,currentNode);
gScore.put(neighbor, tentativeScore);
fScore.put(neighbor,tentativeScore+(abs(neighbor.x-actor.getEndingNodePos().x)+abs(neighbor.z-actor.getEndingNodePos().z)));
}
}
}
}
fScore.remove(currentNode);
gScore.remove(currentNode);
}
}
path reconstruction:
public static void reconstructPath(GameObject actor,HashMap cameFrom,Vector3f currentNode){
LinkedList<Vector3f>finalPath = new LinkedList();
while(cameFrom.keySet().contains(currentNode)){
currentNode = (Vector3f)cameFrom.get(currentNode);
finalPath.addFirst(currentNode);
}
actor.setPath(finalPath);
}
example of end result:
But you may ask: what is dynamic about it?
note that the first node (the one on the top left,blue spheres represent the nodes) is notably closer to the second one, and the other ones are spaced evenly. that is because you can add it to the
adjacencyMatrix
and check adjacent nodes to the node you are adding by checking distances (for example, nodes are considered adjacent if distance from each other is <= 2).
I will post full code soon so if someone wants simple and efficient pathfinding they’ll be able to copy- paste it and adjust it to their need.
as for now, can i ask you for advice on optimizing the algorithm? (it works really fast but in pathfinding every milisecond counts)
