Libgdx A* A Star path finding example

astar.java

package org.jrenner.tac.ai;

import com.badlogic.gdx.math.Vector3;
import com.badlogic.gdx.utils.Array;
import com.badlogic.gdx.utils.BinaryHeap;
import com.badlogic.gdx.utils.DelayedRemovalArray;
import com.badlogic.gdx.utils.GdxRuntimeException;
import com.badlogic.gdx.utils.ObjectFloatMap;
import com.badlogic.gdx.utils.ObjectMap;
import com.badlogic.gdx.utils.ObjectSet;
import com.badlogic.gdx.utils.Pools;
import org.jrenner.tac.Main;
import org.jrenner.tac.Unit;
import org.jrenner.tac.ai.NavGraph.Node;
import org.jrenner.tac.ai.NavGraph.Link;
import org.jrenner.tac.utils.Time;

import java.util.Comparator;

public class AStar {
	ObjectSet<Node> closed = new ObjectSet<>();
	ObjectFloatMap<Node> gScores = new ObjectFloatMap<>();
	ObjectFloatMap<Node> fScores = new ObjectFloatMap<>();
	ObjectMap<Node, Node> cameFrom = new ObjectMap<>();
	Array<Node> path = new Array<>();
	BinaryHeap<NodeWrapper> open = new BinaryHeap<>();
	ObjectSet<Node> contentsOfOpen = new ObjectSet<>();
	Node goal;
	public Time time = new Time();

	public void findPathForUnit(Unit unit, Node start, Node goal) {
		// don't let any unit queue up more than once
		pathingQueue.begin();
		for (int i = 0; i < pathingQueue.size; i++) {
			PathingRequest req = pathingQueue.get(i);
			if (req.unit == unit) {
				pathingQueue.removeIndex(i);
			}
		}
		pathingQueue.end();
		PathingRequest req = Pools.get(PathingRequest.class).obtain();
		req.unit = unit;
		req.start = start;
		req.goal = goal;
		pathingQueue.add(req);
	}

	public void update() {
		int consumeRate = 1;
		for (int i = 0; i < consumeRate && pathingQueue.size > 0; i++) {
			PathingRequest req = pathingQueue.removeIndex(0);
			Array<Node> path = findPath(req.start, req.goal);
			if (path != null) {
				req.unit.setPathByNodes(path);
			}
		}
		if (pathingQueue.size > 0) {
			System.out.println(pathingQueue.size + " units left in pathing queue");
		}
	}

	public Array<Node> findPath(Node start, Node goal) {
		//time.start("findPath");
		reset(start, goal);
		while (open.size != 0) {
			Node current = getLowestFScoreFromOpen();
			//System.out.println("working with node: " + current);
			if (current == goal) {
				//time.stop();
				return reconstructPath();
			}
			closed.add(current);
			Link[] links = current.getLinks();
			for (int i = 0; i < links.length; i++) {
				Link link = links[i];
				//System.out.println("link: " + i);
				if (link == null) continue;
				Node neighbor = link.dest;
				if (neighbor.blocked)
					closed.add(neighbor);
				if (closed.contains(neighbor))
					continue;
				float tentativeGScore = getGScore(current) + calcDistance(link);
				boolean neighborInOpen = contentsOfOpen.contains(neighbor);
				boolean tentativeIsLower = false;
				if (neighborInOpen) {
					tentativeIsLower = tentativeGScore < getGScore(neighbor);
				}
				if (!neighborInOpen || tentativeIsLower) {
					cameFrom.put(neighbor, current);
					//System.out.println("tentativeGScore: " + tentativeGScore);
					gScores.put(neighbor, tentativeGScore);
					float fScore = getGScore(neighbor) + calcHScore(neighbor);
					fScores.put(neighbor, fScore);
					if (!neighborInOpen) {
						addToOpen(neighbor);
					}
				}
			}
		}
		System.out.println("AStar: Failed to find path from " + start + " to " + goal);
		//time.stop();
		return null;
	}

	private Array<Node> reconstructPath() {
		path.clear();
		Node current = goal;
		while (current != null) {
			path.add(current);
			current = cameFrom.get(current, null);
		}
		path.reverse();
		return path;
	}

	private float calcDistance(Link link) {
		float weightFactor = 2f;
		float result = (link.weight * weightFactor) * Main.heightMapModel.heightMap.getWidthScale();
		return result;
	}

	private float getGScore(Node node) {
		if (!gScores.containsKey(node)) {
			throw new GdxRuntimeException("no GScore found for node");
		}
		float gScore = gScores.get(node, 0f);
		return gScore;
	}

	private float calcHScore(Node neighbor) {
		float h = Vector3.dst(neighbor.x, neighbor.y, neighbor.z, goal.x, goal.y, goal.z);
		return h;
	}

	private void reset(Node start, Node goal) {
		closed.clear();
		open.clear();
		contentsOfOpen.clear();
		gScores.clear();
		fScores.clear();
		cameFrom.clear();
		path.clear();
		this.goal = goal;
		addToOpen(start);
		gScores.put(start, 0f);
	}

	private void addToOpen(Node node) {
		NodeWrapper wrapper = new NodeWrapper(getFScore(node), node);
		open.add(wrapper);
		contentsOfOpen.add(node);
	}

	private Node getLowestFScoreFromOpen() {
		// TODO optimize with binary heap
		Node lowest = open.pop().navNode;
		contentsOfOpen.remove(lowest);
		return lowest;
	}

	private float getFScore(Node n) {
		float score = gScores.get(n, 0f) + calcHScore(n);
		return score;
	}


	// misc stuff
	Comparator<Node> rankByFScore = new Comparator<Node>() {
		@Override
		public int compare(Node o1, Node o2) {
			float diff = getFScore(o1) - getFScore(o2);
			if (diff > 0) return 1;
			if (diff < 0) return -1;
			return 0;
		}
	};

	private class NodeWrapper extends BinaryHeap.Node {
		public NavGraph.Node navNode;

		public NodeWrapper(float value, NavGraph.Node navNode) {
			super(value);
			setNavNode(navNode);
		}

		public void setNavNode(NavGraph.Node navNode) {
			this.navNode = navNode;
		}
	}

	private DelayedRemovalArray<PathingRequest> pathingQueue = new DelayedRemovalArray<>();

	public static class PathingRequest {
		Unit unit;
		NavGraph.Node start;
		NavGraph.Node goal;

		public PathingRequest() {}
	}

}