A* implementation issue

I am using A* for pathfinding in a Java project that I am working on. My attempt at implementing it though, has some issues. Not only is it slightly slow, but it sometimes runs into infinite loop issues where it never finds a path. I believe this to be the result of a bug somewhere in my code. The pathfinder operates on 3d space, and uses a basic heuristic (I can provide the code if necessary, however, it does little more than just calculate the distance squared between the two points).

The code is here:

public class BinaryPathFinder extends PathFinder {
  private final PriorityBuffer<PathNode> paths;
  private final HashMap<Point, Integer> mindists = new HashMap<Point, Integer>();
  private final ComparableComparator<PathNode> comparator = new ComparableComparator<PathNode>();
  private Path lastPath;
  private Point start, end;
  private final byte SIZE_INCREMENT = 20;

  public BinaryPathFinder(PathHeuristic heuristic,
      Pathplayer player, PathWorld pathWorld) {
    super(heuristic, player, pathWorld);
    this.world = pathWorld.getWorld();
    paths = new PriorityBuffer<PathNode>(8000, comparator);
  }

  @Override
  public boolean find() {
    try {
      PathNode root = new PathNode();
      root.point = start;
      calculateTotalCost(root, start, start, false);
      expand(root);
      while (true) {
        PathNode p = paths.remove();
        if (p == null)
          return false;
        Point last = p.point;
        if (isGoal(last)) {
          calculatePath(p); // Iterate back.
          this.mindists.clear();
          this.paths.clear();
          return true;
        }
        expand(p);
      }
    } catch (Exception e) {
      e.printStackTrace();
    }
    this.mindists.clear();
    this.paths.clear();
    return false;
  }

  @Override
  public Path path() {
    return this.lastPath;
  }

  @Override
  public void recalculate(Point start, Point end) {
    this.start = start;
    this.end = end;
    this.lastPath = null;
  }

  private void expand(PathNode path) {
    Point p = path.point;
    Integer min = mindists.get(p);
    if (min == null || min > path.totalCost)
      mindists.put(p, path.totalCost);
    else
      return;
    Point[] successors = generateSuccessors(p);
    for (Point t : successors) {
      if (t == null)
        continue;
      PathNode newPath = new PathNode(path);
      newPath.point = t;
      calculateTotalCost(newPath, p, t, false);
      paths.add(newPath);
    }
  }

  private void calculatePath(PathNode p) {
    Point[] retPath = new Point[20];
    Point[] copy = null;
    short added = 0;
    for (PathNode i = p; i != null; i = i.parent) {
      if (added >= retPath.length) {
        copy = new Point[retPath.length + SIZE_INCREMENT];
        System.arraycopy(retPath, 0, copy, 0, retPath.length);
        retPath = copy;
      }
      retPath[added++]开发者_开发知识库 = i.point;
    }
    this.lastPath = new Path(retPath);
  }

  private int calculateHeuristic(Point start, Point end, boolean endPoint) {
    return this.heuristic.calculate(start, end, this.pathWorld,
        this.player, endPoint);
  }

  private int calculateTotalCost(PathNode p, Point from, Point to,
      boolean endPoint) {
    int g = (calculateHeuristic(from, to, endPoint) + ((p.parent != null) ? p.parent.cost
        : 0));
    int h = calculateHeuristic(from, to, endPoint);
    p.cost = g;
    p.totalCost = (g + h);
    return p.totalCost;
  }

  private Point[] generateSuccessors(Point point) {
    Point[] points = new Point[27];
    Point temp = null;
    byte counter = -1;
    for (int x = point.x - 1; x <= point.x + 1; ++x) {
      for (int y = point.y + 1; y >= point.y - 1; --y) {
        for (int z = point.z - 1; z <= point.z + 1; ++z) {
          ++counter;
          if (x == 0 && y == 0 && z == 0)
            continue;
          temp = new Point(x, y, z);
          if (valid(temp))
            points[counter] = temp;
        }
      }
    }
    return points;
  }

  private boolean isGoal(Point last) {
    return last.equals(this.end);
  }
}

PathNode is here:

public class PathNode implements Comparable<PathNode> {
  public Point point;
  public final PathNode parent;
  public int cost;
  public int totalCost;

  public PathNode(Point point, PathNode parent, short cost, short totalCost) {
    this.point = point;
    this.parent = parent;
    this.cost = cost;
    this.totalCost = totalCost;
  }

  public PathNode() {
    this.point = null;
    this.parent = null;
    this.cost = this.totalCost = 0;
  }

  public PathNode(PathNode path) {
    this.parent = path;
    this.cost = path.cost;
    this.totalCost = path.totalCost;
  }

  @Override
  public int compareTo(PathNode node) {
    int result = node.totalCost - node.cost;
    if (result > node.totalCost)
      return 1;
    else if (result == 0)
      return 0;
    else
      return -1;
  }
}

Point is just a wrapper class that defines integer x, y, z values.

I am using the Apache Commons PriorityBuffer for storing path nodes. Help would be appreciated - thanks in advance!

---

just from a programming point of view - are you sure that for(PathNode i = p; i != null; i = i.parent) always terminates? this looks like the only place it could really hang.