Entity Pathfinding and Targetting

Discussion in 'Resources' started by Jnorr44, Sep 30, 2012.

Thread Status:
Not open for further replies.
  1. Offline

    Jnorr44

    James Norris' Pathfinding/Targetting Algorithm

    Alright, so I decided to show off my pathfinding and targetting system to allow movement to a target. This system hopefully can be used to help many devs out in the future.

    DOES NOT REQUIRE OBC/NMS!

    Pathfinder class, used to create a new path to a target:
    Code:
    public class Pathfinder {
        /**
        * Calculates and returns the path to the target from the starting point.
        * This also accounts for pitch and yaw toward the target.
        *
        * @param start The location to start the path at
        * @param target The location to find the path towards, starting at the start
        * @return The path from the start to the target
        */
        public static Path calculate(Location start, Location target) {
            HashMap<Integer, double[]> locations = new HashMap<Integer, double[]>();
            World world = start.getWorld();
            Location current = start.subtract(0, 1, 0);
            locations.put(0, getCoordinates(setLocationDirection(current, target)));
            for (int n = 1; n <= 1000; n++) {
                double H = Double.MAX_VALUE;
                Location correct = null;
                for (int x = -1; x <= 1; x++) {
                    for (int z = -1; z <= 1; z++) {
                        Location check = current.clone().add(x, 0, z);
                        double newH = check.distanceSquared(target);
                        if (!check.getBlock().isEmpty()) {
                            if (check.clone().add(0, 1, 0).getBlock().isEmpty() && check.clone().add(0, 2, 0).getBlock().isEmpty()) {
                                check = check.clone().add(0, 1, 0);
                                newH = check.distanceSquared(target);
                            } else {
                                newH += 400;// 20 squared
                            }
                        }
                        if (newH < H) {
                            H = newH;
                            correct = check;
                        }
                    }
                }
                boolean newNode = correct != null && H < Double.MAX_VALUE && !MiscUtil.locationMatch(correct, current);
                Location found = setLocationDirection(newNode ? correct : current, target);
                locations.put(n, getCoordinates(found));
                if (!newNode) {
                    break;
                }
                current = correct;
                if (MiscUtil.locationMatch(target, current, 2)) {
                    break;// target reached
                }
            }
            return new Path(world, locations);
        }
     
        /**
        * Gets a serializable array of coordinates for this location.
        *
        * [0] = x<br>
        * [1] = y<br>
        * [2] = z<br>
        * [3] = yaw<br>
        * [4] = pitch
        *
        * @param found The location to get the coordinates array for
        */
        public static double[] getCoordinates(Location found) {
            double[] coordinates = new double[5];
            coordinates[0] = found.getX();
            coordinates[1] = found.getY();
            coordinates[2] = found.getZ();
            coordinates[3] = found.getYaw();
            coordinates[4] = found.getPitch();
            return coordinates;
        }
     
        public static boolean pathReaches(Path path, Location loc) {
            for (int i = 0; i <= path.getRawNodesMap().keySet().size(); i++) {
                Location node = path.getNode(i);
                if (node == null || !MiscUtil.locationMatch(loc, node)) {
                    continue;
                }
                return true;
            }
            return true;
        }
     
        public static boolean pathReaches(Path path, Location loc, int radiusDistance) {
            for (int i = 0; i <= path.getRawNodesMap().keySet().size(); i++) {
                Location node = path.getNode(i);
                if (node == null) {
                    continue;
                }
                if (!MiscUtil.locationMatch(loc, node, radiusDistance)) {
                    return false;
                }
            }
            return true;
        }
     
        public static Location setLocationDirection(Location loc, Location lookat) {// TODO not working?
            loc = loc.clone();
            // double b = lookat.getX() - loc.getX();
            // double d = lookat.getY() - loc.getY();
            // double a = lookat.getZ() - loc.getZ();
            // double c = Math.sqrt(Math.pow(a, 2) + Math.pow(b, 2));
            // double e = Math.sqrt(Math.pow(c, 2) + Math.pow(d, 2));
            // loc.setYaw((float) Math.toDegrees(Math.asin(a / c)));
            // loc.setPitch((float) Math.toDegrees(Math.asin(d / e)));
            // or... -----------------------------------------------------
            double dx = lookat.getX() - loc.getX();
            double dy = lookat.getY() - loc.getY();
            double dz = lookat.getZ() - loc.getZ();
            if (dx != 0) {
                if (dx < 0) {
                    loc.setYaw((float) (1.5 * Math.PI));
                } else {
                    loc.setYaw((float) (0.5 * Math.PI));
                }
                loc.setYaw(loc.getYaw() - (float) Math.atan(dz / dx));
            } else if (dz < 0) {
                loc.setYaw((float) Math.PI);
            }
            double dxz = Math.sqrt(Math.pow(dx, 2) + Math.pow(dz, 2));
            loc.setPitch((float) -Math.atan(dy / dxz));
            loc.setYaw(-loc.getYaw() * 180f / (float) Math.PI);
            loc.setPitch(loc.getPitch() * 180f / (float) Math.PI);
            return loc;
        }
    }
    Path class, created by the pathfinder to be used later:
    Code:
    public class Path {
        private HashMap<Integer, double[]> locations = new HashMap<Integer, double[]>();
        private World world;
     
        public Path(World world, HashMap<Integer, double[]> locations) {
            this.world = world;
            this.locations = locations;
        }
     
        public Location getEndNode() {
            for (int i = locations.size(); i > 0; i--) {
                Location node = getNode(i);
                if (node != null) {
                    return node;
                }
            }
            return null;
        }
     
        /**
        * Gets the location related to the number given.
        * The numbers are in sequence towards the target.
        *
        * @param nodeNumber The number of the point to get
        * @return The location related to the number given
        */
        public Location getNode(int nodeNumber) {
            if (locations.get(nodeNumber) != null) {
                double[] coords = locations.get(nodeNumber);
                return new Location(world, coords[0], coords[1], coords[2], (float) coords[3], (float) coords[4]);
            }
            return null;
        }
     
        public double getPitch(int nodeNumber) {
            return locations.get(nodeNumber)[4];
        }
     
        public HashMap<Integer, double[]> getRawNodesMap() {
            return locations;
        }
     
        public double getX(int nodeNumber) {
            return locations.get(nodeNumber)[0];
        }
     
        public double getY(int nodeNumber) {
            return locations.get(nodeNumber)[1];
        }
     
        public double getYaw(int nodeNumber) {
            return locations.get(nodeNumber)[3];
        }
     
        public double getZ(int nodeNumber) {
            return locations.get(nodeNumber)[2];
        }
    }
    MobTargettingThread class, used to move the mob along the path. Please note that this is used in one of my plugins, and therefore contains some references you may not be familiar with. I will describe those below:

    Code:
    public class MobTargettingThread implements ZARepeatingTask {
        private Creature creature;
        private DataContainer data = Ablockalypse.getData();
        private int interval = 1, count = 0, nodeNum = 0, standStill = 0, stillTime = 15;
        private HashMap<Integer, double[]> locations = new HashMap<Integer, double[]>();
        private double nodesPerTick = .08D, activeNodesPerTick = 0;
        private Path path;
        private boolean runThrough = false;
        private Location target, previous;
     
        public MobTargettingThread(Creature creature, Location target, double nodesPerTick, boolean autorun) {
            this.creature = creature;
            this.target = target;
            this.nodesPerTick = nodesPerTick;
            setTarget(target);
            runThrough = autorun;
            addToThreads();
        }
     
        @Override public int getCount() {
            return count;
        }
     
        @Override public int getInterval() {
            return interval;
        }
     
        public double getNodesPerTick() {
            return nodesPerTick;
        }
     
        public Path getPath() {
            return path;
        }
     
        public int getStandStillAllowance() {
            return stillTime / 20;
        }
     
        public int getStandStillTime() {
            return standStill / 20;
        }
     
        public Location getTarget() {
            return target;
        }
     
        @Override public void remove() {
            data.objects.remove(this);
        }
     
        @Override public void run() {
            if (creature == null || creature.isDead()) {
                remove();
                return;
            }
            Location creatureLoc = getCreatureMovementLocation(creature);
            activeNodesPerTick += nodesPerTick;
            if (activeNodesPerTick >= interval) {
                nodeNum++;
                previous = target;
                activeNodesPerTick = 0;
            }
            double currentDist = 256;// 16 squared
            Player closestPlayer = null;
            for (Player player : Bukkit.getServer().getOnlinePlayers()) {
                if (player.getLocation().distanceSquared(creatureLoc) < currentDist) {
                    closestPlayer = player;
                }
            }
            boolean distantBarrierTarget = target instanceof Location && data.isBarrier(target) && !data.getBarrier(target).isWithinRadius(creature, 1);
            boolean usingRegularPathfinder = closestPlayer != null ? creatureLoc.distanceSquared(closestPlayer.getLocation()) <= 225 /* 15 squared */&& creature.getTarget() != null && !creature.getTarget().isDead() : false;
            double[] coords = locations.get(nodeNum);
            if (coords != null && (!usingRegularPathfinder || distantBarrierTarget)) {
                previous = creatureLoc;
                double[] futureCoords = locations.get(nodeNum + 1);
                double Xadd = 0, Yadd = 0, Zadd = 0, pitchAdd = 0, yawAdd = 0;
                if (futureCoords != null) {
                    Xadd = (futureCoords[0] - coords[0]) * activeNodesPerTick;
                    Yadd = (futureCoords[1] - coords[1]) * activeNodesPerTick;
                    Zadd = (futureCoords[2] - coords[2]) * activeNodesPerTick;
                    yawAdd = (futureCoords[3] - coords[3]) * activeNodesPerTick;
                    pitchAdd = (futureCoords[4] - coords[4]) * activeNodesPerTick;
                }
                Location move = new Location(target.getWorld(), coords[0] + Xadd, coords[1] + Yadd, coords[2] + Zadd, (float) (coords[3] + yawAdd), (float) (coords[4] + pitchAdd));
                if (move.clone().subtract(0, 1, 0).getBlock().isEmpty()) {
                    recalculate(move, target);
                }
                creature.teleport(move);
            }
            standStill = previous != null && MiscUtil.locationMatch(previous, creatureLoc) ? ++standStill : 0;
            if (standStill >= stillTime * 20 / interval) {// same spot for "stillTime" seconds
                standStill = 0;
                recalculate(creatureLoc, target);
            }
        }
     
        @Override public boolean runThrough() {
            return runThrough;
        }
     
        @Override public void setCount(int i) {
            count = i;
        }
     
        @Override public void setInterval(int i) {
            interval = i;
        }
     
        public void setNodesPerTick(double nodesPerTick) {
            this.nodesPerTick = nodesPerTick;
        }
     
        @Override public void setRunThrough(boolean tf) {
            runThrough = tf;
        }
     
        public void setStandStillAllowance(int seconds) {
            stillTime = seconds;
        }
     
        public void setTarget(Location loc) {
            recalculate(getCreatureMovementLocation(creature), loc);
        }
     
        private synchronized void addToThreads() {
            data.objects.add(this);
        }
     
        private Location getCreatureMovementLocation(LivingEntity entity) {
            return MiscUtil.floorLivingEntity(entity);
        }
     
        protected void recalculate(Location start, Location finish) {
            path = Pathfinder.calculate(start, finish);
            target = finish;
            nodeNum = 0;
            activeNodesPerTick = 0;
            locations = path.getRawNodesMap();
        }
    }
    ZARepeatingTask - A class that contained a run method ran by a thread that runs per tick. The interval is the number of ticks before the run method is triggered, and the count is the number of ticks passed since the last interval. A remove method is required.

    DataContainer - Holds all of the data for my plugin. Really the brain of the operation.

    Ablockalypse - The main class of the plugin.

    MiscUtil - Just a miscellaneous utility class. The most prominent use in the targetter is MiscUtil.locationMatch(), which simply checks if the 2 arguments provided are the same location (not considering pitch and yaw). If an integer argument is provided for locationMatch(), it will allow that much difference in blocks between the arguments.
     
  2. Offline

    iKeirNez

    Awesome, glad you figured it out!
     
  3. Offline

    Uniclaw

    Yay, awesome :D !!
     
  4. Offline

    Jnorr44

    Just made it account for walls and stuff. It automatically jumps and everything if the entity actually has a target nearby.
     
  5. Offline

    iKeirNez

    Now that..... is awesome!
     
  6. Offline

    Jnorr44

    yep, now I made it jump automatically if there is a block at its feet also, even if it doesn't have a target.
     
  7. Offline

    hawkfalcon

    Path finding?
     
    SkillPvPz likes this.
  8. Offline

    Jnorr44

    Tried that, this can be used without creating your own entity. P:athfinding will throw an NPE if you set the radius greater than 16.
     
  9. Offline

    fletch_to_99

    What about using A* to find a path? http://en.wikipedia.org/wiki/A*_search_algorithm You could then use this to find the shortest realistic path assuming it is implemented correctly. Here is a simple demo of how it could be put to piratical use:

    Basically accounting for block changes in minectaft. However I'm not sure how you would handle the constantly changing multiple planes of minecraft. All of this is just in theory though, there is probably a better pathfinding algorithm out there more suited for multi-plane traversal.

    I have found the following video which may come to your use:
     
  10. Offline

    Jnorr44

    I could use this, but I believe if I can actually do the same thing as MC pathfinding it will look more natural, and may work a bit better for the blocks I have to deal with. All I really need is for the targets to be set from any distance that can be set.
     
  11. Offline

    hawkfalcon

    I can use this to set an irongolem to pathfind a tree, right?
     
  12. Offline

    Jnorr44

    yes, but only if you switch the second arg from a player to a location.

    EDIT: And you may want to use my updated version, which tries to account for walls and blocks to jump over, although it isn't yet 100% accurate.
     
    hawkfalcon likes this.
  13. Offline

    V10lator

    I coupled your model with NMS pathfinding (quick&dirty, untested!):
    Code:java
    1. public class MobTargetter {
    2. private final Plugin plugin; // Better use this instead of statics in bukkit.
    3. private Location loc; // Player to Location.
    4. private Creature c; // Entity to Creature.
    5. private int id = -1;
    6. private boolean hasTarget = false;
    7. private float speed = 0.05F; // Double to speed for NMS compatibility.
    8.  
    9. public MobTargetter(Plugin plugin,Creature c, Location loc) { // Removed autorun disabling.
    10. this.e = e;
    11. this.p = p;
    12. setTarget(loc);
    13. }
    14.  
    15. public void setTarget(Location loc)
    16. {
    17. cancel();
    18. if(loc != null)
    19. {
    20. this.loc = loc;
    21. target();
    22. }
    23. }
    24.  
    25. private void target() { //Public to private. Others should use SetTarget(Location loc) instead.
    26. hasTarget = true;
    27. id = plugin.getServer().getScheduler().scheduleSyncRepeatingTask(Ablockalypse.instance, new Runnable() {
    28. public void run() {
    29. if (!(e.isDead())) {
    30. moveMob();
    31. } else {
    32. cancel();
    33. }
    34. }
    35. }, 1, 1);
    36. }
    37.  
    38. /**
    39.   * Cancels the thread.
    40.   */
    41. protected void cancel() {
    42. if(id > -1)
    43. {
    44. hasTarget = false;
    45. plugin.getServer().getScheduler().cancelTask(id);
    46. id = -1;
    47. }
    48. }
    49.  
    50. //This has been taken from a PoC:
    51. private void moveMob()
    52. {
    53. EntityCreature notchMob = (EntityCreature) ((CraftCreature)e).getHandle();
    54. PathEntity path = notchMob.world.a(notchMob, loc.getBlockX(), loc.getBlockY(), loc.getBlockZ(), 100.0F, true, false, false, true);
    55. notchMob.setPathEntity(path); // Old AI ...
    56. notchMob.getNavigation().a(path, speed); // new AI ...
    57. }
    58.  
    59. public boolean hasTarget()
    60. {
    61. return hasTarget;
    62. }
    63. }

    As you see it uses the new and the old mob AI. While this seems stupid it isn't: A mob with the new AI ignores the old and vice-versa.
    Also I haven't tested anything with the max. distance. It may be the 100.0F at the PathEntity.

    And before you argument that a scheduler isn't needed if using NMS: It's still needed, the AI is designed to change paths / forget targets randomly.
     
  14. Offline

    Jnorr44

    This works, but it moves in a weird way. It looks like it is just teleporting the entity a pathpoint away, without client correction.
     
  15. Offline

    V10lator

    What the...? It's NMS code used to target players, for example... :confused:
    Maybe try to reduce the execution of the runnable, so make it from one to 10 ticks, for example.
     
  16. Offline

    Jnorr44

    Thats not it, I changes the float argument of the pathentity to 32, and it works. With such a high number, it appears to not work. 32 is ok though, but it would be nice if I could re-write the way they do pathfinding for a much larger area. That may end up being a bit too complicated though...
     
  17. Offline

    Jnorr44

    BUMPED - Updated to a new, smarter, better system.
     
  18. Offline

    Iroh Retired Staff

    Moved to resources.
     
    Jnorr44 likes this.
  19. Offline

    Robin Bi

    Jnorr44 Sorry that i pick up your old thread here, but I'm currently trying to work with your pathfinding classes and in addition i'm relatively new to all of this.

    So I've created the three classes and I've copied in all of your code, but as you mentioned above, i'm missing some stuff like MiscUtil or the ZARepeatingTask. Do i have to get them now or can i just replace them?


    Hope you're still watching this thread :3
     
  20. Offline

    Jnorr44

    This has been replaced by MCPath, you can find it in this resources section. It currently just contains the pathfinding, not the targetting, although it should contain the targetting soon.
     
Thread Status:
Not open for further replies.

Share This Page