pathfinder

package com.streak324.pathfinding;

import java.util.Comparator;
import java.util.HashSet;
import java.util.PriorityQueue;

import com.badlogic.gdx.utils.Array;

public class PathFinder {

	private boolean foundTarget;
	private int width, height;
	private Array<PathNode> path;
	private PathNode[][] nodes;
	private PriorityQueue<PathNode> open;
	private HashSet<PathNode> closed;
	private PathNode start, target, current;
	private int range = 1;

	public PathFinder(int width, int height, boolean map[][]) {
		this.width = width;
		this.height = height;
		nodes = new PathNode[width][height];

		for(int i=0; i<width; i++) {
			for(int j=0; j<height; j++) {
				nodes[i][j] = new PathNode(i, j);
				if(map[i][j] != true)
					nodes[i][j].passable = false;
			}
		}
		open = new PriorityQueue<PathNode>(new CostComparator());
		closed = new HashSet<PathNode>();
	}

	public Array<PathNode> getPath(int sx, int sy ,int tx, int ty) {
		path = new Array<PathNode>();
		open.clear();
		closed.clear();

		start = nodes[sx][sy];
		start.movementCost = 0;
		target = nodes[tx][ty];
		addToOpenList(start);

		while(foundTarget != true) {
			if(open.size() == 0) { return null; }

			current = open.poll();
			addToClosedList(current);

			checkNeighbors(current);
		}

		traceBack();

		return path;
	}

	private void traceBack() {
		while(current != start) {
			path.add(current);
			current = current.parent;
		}
	}

	private void checkNeighbors(PathNode node) {
		for(int i = node.x - range; i <= (node.x + range); i++) {
			if(i >= width || i < 0) { continue; }
			for(int j = node.y - range; j <= (node.y + range); j++) {
				if( j >= height || j < 0) { continue; }

				if((i == node.x && j == node.y) )  { continue; }

				PathNode neighbor = nodes[i][j];

				identifyNode(neighbor);

			}
		}
	}

	private void identifyNode(PathNode node) {
		if(!node.passable || closed.contains(node) ) return;

		if(node == target) {
			foundTarget = true;
			System.out.println("Target Found: " + node.x + ", " + node.y);
			return;
		}
		else if(!open.contains(node)) {
			addToOpenList(node);
			calcHeuristic(node);
			updateNode(node, current);
		}
		else {
			checkForReparenting(node);
		}
	}

	private void checkForReparenting(PathNode node) {
		float cost = node.movementCost;
		float reCost = calcMovementCost(node, current);

		if(reCost < cost) {
			System.out.println("Reparenting");
			updateNode(node, current);
		}
	}

	private void updateNode(PathNode child, PathNode parent) {
		child.parent = parent;
		child.movementCost = calcMovementCost(child, parent);
		child.totalCost = child.movementCost + child.heuristic;
	}

	private float calcMovementCost(PathNode n1, PathNode n2) {
		float dx = n1.x - n2.x;
		float dy = n1.y - n2.y;
		return (float) Math.sqrt( (dx*dx + dy*dy) ) + n2.movementCost;
	}

	private float calcHeuristic(PathNode node) {
		float dx = node.x - target.x;
		float dy = node.y - target.y;
		return (float) Math.sqrt( (dx*dx + dy*dy) );
	}

	private void addToOpenList(PathNode node) {
		if(!open.contains(node) && !closed.contains(node)) {
			open.add(node);
		}
	}

	private void addToClosedList(PathNode node) {
		if(!closed.contains(node)) {
			closed.add(node);
		}
	}

	public class PathNode {
		public int x, y;
		public PathNode parent;
		public float movementCost, heuristic, totalCost;
		public boolean passable;

		public PathNode(int x, int y) {
			this.x = x;
			this.y = y;
			passable = true;
		}

	}

	private class CostComparator implements Comparator<PathNode> {

		@Override
		public int compare(PathNode a, PathNode b) {

			if(a.totalCost < b.totalCost) return 1;
			else return -1;
		}

	}

}