Need some pathfinding help

S0, I need some pathfinding help for a bot I'm making. Currently, it takes *really* long and times out the server.
Main pathfinding code:
Note: Ignore the println's ^_^

Code:java

 
 
System.out.println("Starting");
nodes.clear();
 
MovementNode start = createNode(startLoc);
MovementNode goal = createNode(goalLoc);
if (start.getLocation().equals(goal.getLocation())) {
return constructPath(start);
}
 
Queue<MovementNode> queue = new PriorityQueue<MovementNode>();
queue.add(start);
 
List<MovementNode> bestPath = null;
 
System.out.println("Stage 1");
while (!queue.isEmpty()) {
if (logged < 10) {
System.out.println("Get MovementNode");
}
MovementNode current = queue.poll();
if (current.wasVisited()) {
if (logged < 10) {
System.out.println("Visited");
}
continue;
}
 
if (current.isBlocked()) {
if (logged < 10) {
System.out.println("Blocked");
}
continue;
}
 
if (current == goal) {
bestPath = constructPath(goal);
break;
}
 
for (MovementNode neighbor : current.getNeighbors()) {
if (logged < 10) {
System.out.println("Got neighbor");
}
if (neighbor.wasVisited()) {
if (logged < 10) {
System.out.println("Visited");
}
continue;
}
if (neighbor.isBlocked()) {
if (logged < 10) {
System.out.println("Blocked");
}
continue;
}
 
int distance = current.getPathDistance()
+ (int) current.getLocation().distance(
neighbor.getLocation());
if (neighbor.getParent() != null
&& distance >= neighbor.getPathDistance()) {
if (logged < 10) {
System.out.println("Neighbor != null");
}
continue;
}
 
neighbor.setPathDistance(distance);
neighbor.setHeuristicDistance((int) neighbor.getLocation()
.distance(goal.getLocation()) + distance);
if (neighbor.getParent() == null) {
if (logged < 10) {
System.out.println("Parent is null");
}
neighbor.setPriority(neighbor.getHeuristicDistance());
queue.add(neighbor);
} else {
if (logged < 10) {
System.out.println("Parent isn't null");
}
neighbor.setPriority(neighbor.getHeuristicDistance());
}
neighbor.setParent(current);
if (logged < 10) {
System.out.println("Done with that node");
logged++;
}
}
}
 
System.out.println("Done!");
 
return bestPath == null ? null : bestPath;
 
 

MovementNode:

Code:java

 
public class MovementNode implements Comparable<MovementNode> {
private final Location loc;
 
private MovementNode parent = null;
 
private Set<MovementNode> neighbors;
 
private final MovementPathfinder pathfinder;
 
private int pathDistance;
private boolean visited;
private int heuristicDistance;
 
private int priority = 0;
 
public MovementNode(MovementPathfinder pathfinder, Location loc) {
this.pathfinder = pathfinder;
this.loc = loc;
}
 
public MovementNode getParent() {
return parent;
}
 
public void setParent(MovementNode parent) {
this.parent = parent;
}
 
public boolean wasVisited() {
return visited;
}
 
public void setVisited(boolean visited) {
this.visited = visited;
}
 
public Location getLocation() {
return loc;
}
 
/*
* public boolean isTouching(MovementNode other) { Location otherLoc =
* other.getLocation(); int x = otherLoc.getBlockX() - loc.getBlockX(); int
* y = otherLoc.getBlockY() - loc.getBlockY(); int z = otherLoc.getBlockZ()
* - loc.getBlockZ(); return ((x == 0 || x == 1 || x == -1) && (y == 0 || y
* == 1 || y == -1) && (z == 0 || z == 1 || z == -1)); }
*
* public boolean isDiagonal(MovementNode other) { Location otherLoc =
* other.getLocation(); int x = Math.abs(otherLoc.getBlockX() -
* loc.getBlockX()); int z = Math.abs(otherLoc.getBlockZ() -
* loc.getBlockZ()); return x == z; }
*/
 
public boolean isBlocked() {
Block block = loc.getBlock();
if (!(BlockUtils.canOccupy(block))) {
// System.out.println("Not air");
// Must be air
return true;
}
Block above = block.getRelative(BlockFace.UP);
if (!(BlockUtils.canOccupy(above))) {
// System.out.println("Above not air");
// Must be air
return true;
}
Block below = block.getRelative(BlockFace.DOWN);
if (!(BlockUtils.canStandOn(below))) {
// System.out.println("Below cannot be stood on");
return true;
}
return false;
}
 
public Set<MovementNode> getNeighbors() {
if (neighbors != null) {
return neighbors;
}
neighbors = new HashSet<MovementNode>(24);
//System.out.println("Creating neighbors");
for (Block b : BlockUtils.getHollowCube(getLocation(), 1)) {
//System.out.println("neighbor: " + b.toString());
Location loc = b.getLocation();
neighbors.add(pathfinder.getNode(loc));
}
return neighbors;
}
 
/*
* public int getCostFromStart() { if (costFromStart == -1) { costFromStart
* = MovementNode.getEstimatedCostTo(helper.getStart(), this); } return
* costFromStart; }
*/
 
public int getPathDistance() {
return pathDistance;
}
 
public void setPathDistance(int pathDistance) {
this.pathDistance = pathDistance;
}
 
public int getHeuristicDistance() {
return heuristicDistance;
}
 
public void setHeuristicDistance(int heuristicDistance) {
this.heuristicDistance = heuristicDistance;
}
 
public static int getEstimatedCostTo(MovementNode node, MovementNode other) {
Validate.notNull(node, "Node cannot be null!");
Validate.notNull(other, "Other cannot be null!");
 
Location loc = node.getLocation();
Location otherLoc = other.getLocation();
 
return (int) Math.sqrt(Math.pow(loc.getBlockX() - otherLoc.getBlockX(),
2)
+ Math.pow(loc.getBlockY() - otherLoc.getBlockY(), 2)
+ Math.pow(loc.getBlockZ() - otherLoc.getBlockZ(), 2));
}
 
public static String getHash(Location loc) {
StringBuilder builder = new StringBuilder(5);
return builder.append(loc.getBlockX()).append("_")
.append(loc.getBlockY()).append("_").append(loc.getBlockZ())
.toString();
}
 
public int getPriority() {
return priority;
}
 
public void setPriority(int priority) {
this.priority = priority;
}
 
@Override
public int compareTo(MovementNode node) {
if (node.getPriority() == priority) {
return 0;
}
return node.getPriority() < priority ? -1 : 1;
}
 
}
 

And the actual tick method, called every tick.

Code:java

 
public void tick() {
// if (to == null) {
// return;
// }
if (toMove == null) {
return;
}
if (!(++ticks % 20 == 0)) {
return;
}
Iterator<MovementNode> it = toMove.iterator();
if (sprint) {
for (int i = 0; i <= 5 && it.hasNext(); i++) {
MovementNode node = it.next();
Location loc = node.getLocation();
npc.setPosition(loc.getX(), loc.getY(), loc.getZ());
npc.lookAtPoint(loc);
it.remove();
}
} else {
for (int i = 0; i <= 4 && it.hasNext(); i++) {
MovementNode node = it.next();
Location loc = node.getLocation();
npc.setPosition(loc.getX(), loc.getY(), loc.getZ());
npc.lookAtPoint(loc);
it.remove();
}
}
if (toMove.isEmpty()) {
toMove = null;
}
}
 

Tell me if you need any code, and thanks in advance!

 

tips48
Pathfinding is one of the basics of game AI, there should be tons of documentation about it out there. I personally don't have much experience with it, but I'm willing to help out a bit.
But first I'd like you to explain what algorithm you're making here (I do see something with heuristics) and where are you exactly using it for. What would also help is reposting the code with proper indentation (not sure if that fell away when copying, else you really need to start using it).

 

The code seems to lose it's indentation when it's pasted, unfortunately.
I'm attempting the use A* to move a custom NPC class (Which extends net.minecraft.server.EntityPlayer) to a given Location. Each NPC has a MovementHelper, and the movement helper's tick() method get's called every tick.

 

tips48
Ok, this goes a bit over my head. If we were in the same room you could take me through your code, and you might even find an error yourself then. Right now I can't really make much out of this, probably also because of the lack of indentation and comments. Actually even the relationship of the first two pastes are not totally clear to me.
Your tick method seems pretty simple and since it only works once a second it seems unlikely that that part is causing the lag (unless it's called for like 100ths of NPC's at the same time pherhaps), so something might be wrong with the pathfinder.
If I read it right, your heuristic is a straight line. This is safe, because it's impossible to get a shorter path, but I do know A* is more efficient when the heuristic is as high as possible. When it's too high, you risk selecting a path that's not the shortest, but this might not be a big problem. Seems unlikely that a inefficient heuristic would time out your server, though.

 

JayEffKay Through those println's, I have found it is something with the pathfinder, but for the life of me I can't figure out what :/