Bukkit simple railroad pathfinder

Pathfinder.java

import org.bukkit.Location;
import org.bukkit.Material;
import org.bukkit.World;
import org.bukkit.util.Vector;

import javax.swing.tree.DefaultMutableTreeNode;
import javax.swing.tree.TreeNode;
import java.util.*;

public class Pathfinder {

    private World w;
    private Vector p1, p2;
    private Material b;

    private DefaultMutableTreeNode end = null;
    private Set<Vector> used = new HashSet<>();
    private Vector best1, best2;

    public Pathfinder(World w, Vector p1, Vector p2, Material b) {
        this.w = w;
        this.p1 = floor(p1);
        this.p2 = floor(p2);
        this.b = b;
    }

    private Vector floor(Vector v){
        return new Vector(v.getBlockX(), v.getBlockY(), v.getBlockZ());
    }

    public Pathfinder build(){
        Vector diff = p2.clone().subtract(p1);
        DefaultMutableTreeNode node = new DefaultMutableTreeNode(p1);

        int dx = diff.getBlockX();
        int dz = diff.getBlockZ();

        if(dx == 0 && dz == 0) {   //p1 == p2 for X and Z
            end = node;
            return this;
        } else if(dx == 0){
            best1 = new Vector(0, 0, dz > 0 ? 1 : -1);
            best2 = new Vector(1, 0, 0);
        } else if(dz == 0){
            best1 = new Vector(dx > 0 ? 1 : -1, 0, 0);
            best2 = new Vector(0, 0, 1);
        } else if(abs(dx) < abs(dz)){     //p2 is closer for X
            best1 = new Vector(dx > 0 ? 1 : -1, 0, 0);
            best2 = new Vector(0, 0, dz > 0 ? 1 : -1);
        } else {      //abs(dz) < abs(dx) (p2 is closer for Z)
            best1 = new Vector(0, 0, dz > 0 ? 1 : -1);
            best2 = new Vector(dx > 0 ? 1 : -1, 0, 0);
        }

        step(p1, node);
        return this;
    }

    private int abs(int n){
        return Math.abs(n);
    }

    private void step(Vector p, DefaultMutableTreeNode node){
        if(     //exit if one of them is true
                step0(p, node, best1) ||
                step0(p, node, best2) ||
                step0(p, node, invert(best1)) ||
                step0(p, node, invert(best2))
        ){}
    }

    public Vector invert(Vector v){
        return v.clone().multiply(-1);
    }

    private boolean step0(Vector p, DefaultMutableTreeNode node, Vector add){
        if(end != null)
            return true;

        step1(p.clone().add(add), node);
        return false;
    }

    private boolean step1(Vector n, DefaultMutableTreeNode node){
        Location loc = new Location(w, n.getX(), n.getY(), n.getZ());
        if(loc.getBlock().getType() == b && !used.contains(n)) {
            used.add(n);
            DefaultMutableTreeNode nnode = new DefaultMutableTreeNode(n);
            node.add(nnode);

            if(n.equals(p2)) {
                end = nnode;
                return true;
            }

            step(n, nnode);
        }

        return false;
    }

    public Path get(int p1, int p2) {
        if(end == null)
            return Path.EMPTY_PATH;

        List<Vector> list = new ArrayList<>();
        TreeNode[] path = end.getPath();
        for(TreeNode node : path){
            Vector v = (Vector) ((DefaultMutableTreeNode)node).getUserObject();
            list.add(v);
        }

        if(list.size() == 1)
            return Path.EMPTY_PATH;      //start = end

        return new Path(p1, p2, list);
    }

}

Looks like its a bug somewhere here, cannot remember where exactly