3D Voxel Pathfinding Class

I have not made this pretty so take it how you will, but here is the working PathFinder class from my new game Sands of Osiris (sandofosiris.tumblr.com)

When instantiated, the code uses a new thread to place ‘Cells’ all over the map in points where a unit might be able to go. Then, you can call the function GetPath with a start and an end position and it will spit back out the list of Cells that will get you from point A to point B. In my unit code, the unit always tries to go to the nearest cell and if he ever gets within a certain distance, the cell is destroyed so he can chase after the next one. It works pretty good. I am releasing this in the public domain. Have fun!

[java]package mygame;

import java.util.ArrayList;
import java.util.Collection;
import java.util.List;
import java.util.ListIterator;

import com.jme3.collision.CollisionResults;
import com.jme3.material.Material;
import com.jme3.math.ColorRGBA;
import com.jme3.math.Ray;
import com.jme3.math.Vector3f;
import com.jme3.scene.Geometry;
import com.jme3.scene.Node;
import com.jme3.scene.shape.Box;

import mygame.assets.voxelengine.MarchingVoxelTerrain;
import mygame.assets.voxelengine.Vector3Int;
import mygame.gameobjects.Building;
import mygame.gameobjects.Selectable;
import mygame.gameobjects.Unit;
import mygame.states.GameState.Scene;

public class PathFinder implements Runnable {

MarchingVoxelTerrain terrain;
Thread runner;

public PathFinder( MarchingVoxelTerrain terrain) {
	
	this.terrain = terrain;
	runner = new Thread(this, "pathfinder thread");
	runner.setPriority(Thread.MIN_PRIORITY);
	runner.start();

}

long last_millis;

public void run() {// this is hogging all of the processor! processor spends
					// 50% of its time here! WHAT

	initialize();

	last_millis = System.currentTimeMillis();

	/*while (true) {

		long new_millis = System.currentTimeMillis();
		float tpf = (float) (new_millis - last_millis) / 1000f;
		last_millis = new_millis;

		if (tpf > 0.2f) {

			Debugger.log("Server proc loop lagged: " + tpf, "lag");
		}

		try {

			//update(tpf);

		} catch (Exception e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

		try {
			Thread.sleep(100);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	 */
}

boolean ready = false;

private void initialize() {

	// define all of the nav cells
	int step = 2;
	for (int x = 0; x < terrain.getMapSize(); x += step) {
		for (int z = 0; z < terrain.getMapSize(); z += step) {
			for (int y = 0; y < terrain.getMapHeight(); y++) {
				if (blockAtLocIsValidCell(terrain, x, y, z)) {
					cells.add(new Cell(x, y, z));

				}
			}
		}
	}

	// link all nav cells together properly to make a complex 2D-ish grid
	ListIterator<Cell> allCells = cells.listIterator();

	while (allCells.hasNext()) {
		Cell cell = allCells.next();
		ListIterator<Cell> possibleNeighbors = cells.listIterator();
		while (possibleNeighbors.hasNext()) {
			Cell neighbor = possibleNeighbors.next();
			if (cell.otherCellIsNeighbor(neighbor)) {
				cell.addNeighboringCell(neighbor);
			}
		}
	}

	ready = true;

}


private boolean blockAtLocIsValidCell(MarchingVoxelTerrain terrain, int x, int y, int z) {
	if (!terrain.BlockAtLocIsCollidable(x, y - 1, z)) {// block under must
														// be ground
		return false;
	}
	for (int dx = -1; dx <= 1; dx++) {
		for (int dz = -1; dz <= 1; dz++) {
			for (int dy = 0; dy <= 2; dy++) {
				if (terrain.BlockAtLocIsCollidable(x, y, z)) {//space above must be air
					return false;
				}
			}
		}
	}

	return true;

}


public List<Cell> getPath(Vector3f physicsLocation, Vector3f moveCommandPos) {

	if (ready) {

		List<Cell> openlist = new ArrayList<Cell>();
		List<Cell> closedlist = new ArrayList<Cell>();

		List<ParentEntry> parentslist = new ArrayList<ParentEntry>();

		Cell startCell = getClosestCell(SharedData.getBlockLocAtPosition(physicsLocation));
		Cell endCell = getClosestCell(SharedData.getBlockLocAtPosition(moveCommandPos));
		closedlist.add(startCell);
		
		
		for(Cell neighbor : startCell.neighbors){
			if(terrain.getBuildingAtLoc(neighbor.getLoc())==null){
				openlist.add(neighbor);
			}
		}
		


		Cell bestOpenCell = null;
		if (openlist.iterator().hasNext()) {

			do {
				bestOpenCell = getBestCell(openlist, startCell, endCell);

				while (bestOpenCell.getValidNeighbors(openlist, closedlist).iterator().hasNext()) {
					Cell cell = bestOpenCell.getValidNeighbors(openlist, closedlist).iterator().next();
					setCellParent(cell, bestOpenCell, parentslist);
					openlist.add(cell);
				}
				// bestOpenCell.setAsParentOfNeighbors();

				openlist.remove(bestOpenCell);// switch to closed list
				closedlist.add(bestOpenCell);

				// stop if the endCell was found or if the openlist becomes
				// empty
			} while (!bestOpenCell.equals(endCell) && openlist.iterator().hasNext());
		}

		if (!openlist.iterator().hasNext()) {
			Debugger.log("Could not find any path!", "ai");
		}

	

		if (bestOpenCell != null && bestOpenCell.equals(endCell)) {
			List<Cell> finalPath = new ArrayList<Cell>();

			Cell cellInPath = endCell;
			finalPath.add(endCell);
			while (cellInPath != null && !cellInPath.equals(startCell)) {
				// System.out.println(cellInPath.parent);
				cellInPath = getParentOfCell(cellInPath, parentslist, closedlist);
				finalPath.add(cellInPath);

			}

			return finalPath;

			

		}
	}

	return null; 

}

private Cell getBestCell(List<Cell> list, Cell startCell, Cell endCell) {
	Cell answer = null;
	float bestOpenFScore = 0;

	ListIterator<Cell> openCells = list.listIterator();
	while (openCells.hasNext()) {
		Cell openCell = openCells.next();
		float fScore = openCell.getFScore(startCell, openCell, endCell);
		if (answer == null || fScore < bestOpenFScore) {
			answer = openCell;
			bestOpenFScore = fScore;
		}
	}

	return answer;
}

private Cell getClosestCell(Vector3Int desiredPos) {
	Cell nearest = null;
	float bestDistance = 0;

	ListIterator<Cell> allCells = cells.listIterator();

	while (allCells.hasNext()) {
		Cell cell = allCells.next();
		if (nearest == null || cell.distance(desiredPos) < bestDistance) {
			nearest = cell;
			bestDistance = cell.distance(desiredPos);
		}
	}

	return nearest;
}

List<Cell> cells = new ArrayList<Cell>();

public class Cell {
	int x;
	int y;
	int z;
	List<Cell> neighbors = new ArrayList<Cell>();

	// Cell parent = null;///for A*

	public Cell(int x, int y, int z) {
		this.x = x;
		this.y = y;
		this.z = z;

	}

	public Vector3Int getLoc() {
		return new Vector3Int(x,y,z);
	}

	public List<Cell> getValidNeighbors(List<Cell> openlist, List<Cell> closedlist) {

		List<Cell> answer = new ArrayList<Cell>();

		for (Cell neighbor : getNeighbors()) {
			if (!neighbor.isListed(openlist, closedlist)) {
				answer.add(neighbor);
			}
		}

		return answer;
	}

	private boolean isListed(List<Cell> openlist, List<Cell> closedlist) {
		for (Cell openCell : openlist) {
			if (this.equals(openCell)) {
				return true;
			}
		}
		for (Cell closedCell : closedlist) {
			if (this.equals(closedCell)) {
				return true;
			}
		}

		return false;
	}

	public float getFScore(Cell startCell, Cell previousCell, Cell endCell) {
		// G is distance from me to the start
		float g = this.distance(startCell);

		// H is the ESTIMATED distance from me to the end
		float h = this.distance(endCell);

		float v = 5 * Math.abs(previousCell.y - endCell.y);// vertical
															// differential
															// is
															// bad, we want
															// to
															// prefer
															// lateral
															// travel
		
		float b = 0;//if node is in building, avoid it
		if(terrain.getBlock(x, y+1, z) > 0 || terrain.getBlock(x, y, z) > 0){
			b+=100;
		}

		// float g = 10*this.distance(neighbor);//cost of going to that
		// neighbor

		return h + g + v + b;
		// we use f/2 because of the cell step size of 2 !
	}

	private float distance(Cell endCell) {
		// TODO Auto-generated method stub
		return SharedData.pythagorean(this.x - endCell.x, this.y - endCell.y, this.z - endCell.z);
	}

	public float distance(Vector3Int desiredPos) {
		return SharedData.pythagorean(this.x - desiredPos.x, this.y - desiredPos.y, this.z - desiredPos.z);
	}

	// a neighbor is a cell that I can travel TO! That does not means
	// somebody at the neighbor can travel to me! CAREFUL
	public boolean otherCellIsNeighbor(Cell neighbor) {
		if (neighbor.equals(this)) {
			return false;
		}

		// I can travel to a cell from here if that cell is somewhat nearby
		// on the XY plane and is either below me (can fall) or slightly
		// higher than me (can jump)
		if (SharedData.pythagorean(this.x - neighbor.x, this.z - neighbor.z) <= 2) {
			if ((neighbor.y <= this.y + 1 && SharedData.pythagorean(this.x - neighbor.x, this.z - neighbor.z) > 1)
					|| (neighbor.y <= this.y)) {

				return true;
			}
		}
		return false;
	}

	public void addNeighboringCell(Cell neighbor) {
		neighbors.add(neighbor);
	}

	public List<Cell> getNeighbors() {// this works perfectly
		return neighbors;
	}

	public String toString() {
		return (x + "," + y + "," + z);
	}

	public boolean equals(Cell cell) {
		if (cell == null) {
			return false;
		}
		return x == cell.x && y == cell.y && z == cell.z;
	}

	public Vector3f getPhysicsLocation() {
		// TODO Auto-generated method stub
		return SharedData.getPositionAtBlockLoc(new Vector3Int(x, y, z));
	}

	Geometry geom;

	public void debug() {
		Box b = new Box(1, 1, 1); // create a 1x1x1 box shape
		geom = new Geometry("Box", b); // create a cube geometry from the
										// box shape
		Material mat = MainApp.getAssetPreloader().defaultmat.clone(); // create
																	// a
																	// simple
																	// material
		mat.setColor("Color", ColorRGBA.Blue); // set color of material to
												// blue
		geom.setMaterial(mat); // set the cube geometry 's material
		geom.setLocalTranslation(getPhysicsLocation());
		MainApp.getGameState().queueNodeAttachment(geom, Scene.ROOT);
	}

	public void unDebug() {
		if (geom != null && geom.hasAncestor(MainApp.getGameState().getRootNode())) {
			MainApp.getGameState().queueNodeDetachment(geom, Scene.ROOT);
		}
	}

}

class ParentEntry {

	Cell child;

	Cell parent;// /from temp open list

	ParentEntry(Cell cell, Cell parent) {
		this.child = cell;
		this.parent = parent;
	}

	public boolean equals(Cell cell) {
		// TODO Auto-generated method stub
		return this.child.equals(cell);
	}

	public boolean hasParent(Cell parent) {
		// TODO Auto-generated method stub
		return this.parent.equals(parent);
	}

	public String toString() {
		return (child + " with parent " + parent);
	}
}

private void setCellParent(Cell cell, Cell parent, List<ParentEntry> parentslist) {

	for (ParentEntry entry : parentslist) {
		if (entry.equals(cell)) {
			// entry.set(cell,parent);
			return;
		}
	}
	ParentEntry newEntry = new ParentEntry(cell, parent);
	parentslist.add(newEntry);
	// System.out.println("set parent of :" + newEntry);
}

private Cell getParentOfCell(Cell cell, List<ParentEntry> parentslist, List<Cell> cellslist) {

	for (ParentEntry entry : parentslist) {
		if (entry.equals(cell)) {

			for (Cell possibleParent : cellslist) {
				if (entry.parent.equals(possibleParent)) {
					return possibleParent;
				}
			}

		}
	}

	return null;

}

}
[/java]

3 Likes

Thanks for sharing! You chose the right community to be a voxel tinkerer in :wink:

Currently your website is empty though :frowning: Would have liked to have a closer look at what you’re up to.