Server/src/engine/gameManager/NavigationManager.java

package engine.gameManager;

import engine.Enum;
import engine.math.Vector3fImmutable;
import engine.mobileAI.MobAI;
import engine.net.client.msg.MoveToPointMsg;
import engine.objects.*;
import java.awt.geom.Path2D;
import java.util.ArrayList;


public class NavigationManager {

    private static final int cellGap = 1;
    private static final int stepHeight = 2;

    public static void pathfind(AbstractCharacter character, Vector3fImmutable goal) {
        if(!pathBlocked(character.loc,goal)){
            character.destination = goal;
            MobAI.directMove((Mob)character,character.combatTarget != null);
        }
        try {
            ArrayList<Vector3fImmutable> path = getPath(character.loc, goal);//getOptimizedPath(getPath(character.loc, goal), getPath(goal, character.loc));
            if (path.isEmpty()) {
                MobAI.directMove((Mob)character,character.combatTarget != null);
                return; //no points to walk to
            }
            character.navPath = path;

        } catch (Exception e) {
            //something failed
        }
    }

    public static ArrayList<Vector3fImmutable> getOptimizedPath(ArrayList<Vector3fImmutable> startToGoal, ArrayList<Vector3fImmutable> goalToStart) {
        ArrayList<Vector3fImmutable> optimalPath = new ArrayList<>();
        optimalPath.add(startToGoal.get(0));
        for (Vector3fImmutable point : startToGoal) {
            if (goalToStart.contains(point) && !optimalPath.contains(point)) {
                optimalPath.add(point);
            }

        }

        //optimize the path to its smallest possible amount of points


        return optimalPath;
    }

    private static ArrayList<Vector3fImmutable> getPath(Vector3fImmutable start, Vector3fImmutable goal) {
        ArrayList<Vector3fImmutable> path = new ArrayList<>();
        path.add(start);
        Vector3fImmutable current = start;
        boolean obstructed = false;
        int count = 0;
        while (current.distanceSquared(goal) > 9 && count < 250) {
            //gather the 8 cells around the player
            ArrayList<Vector3fImmutable> surroundingCells = new ArrayList<>();
            surroundingCells.add(current.add(new Vector3fImmutable(cellGap, 0, 0)));
            surroundingCells.add(current.add(new Vector3fImmutable(cellGap, 0, cellGap)));
            surroundingCells.add(current.add(new Vector3fImmutable(0, 0, cellGap)));
            surroundingCells.add(current.add(new Vector3fImmutable(-cellGap, 0, 0)));
            surroundingCells.add(current.add(new Vector3fImmutable(-cellGap, 0, -cellGap)));
            surroundingCells.add(current.add(new Vector3fImmutable(0, 0, -cellGap)));
            surroundingCells.add(current.add(new Vector3fImmutable(-cellGap, 0, cellGap)));
            surroundingCells.add(current.add(new Vector3fImmutable(cellGap, 0, -cellGap)));
            Vector3fImmutable cheapest = new Vector3fImmutable(Vector3fImmutable.ZERO);
            for (Vector3fImmutable point : surroundingCells) {
                count++;

                if (path.contains(point))
                    continue;

                if (pointIsBlocked(point)) {
                    obstructed = true;
                    continue;
                }

                if (getCost(cheapest, current, goal) > getCost(point, current, goal))
                    cheapest = point;
            }
            current = cheapest;
            path.add(cheapest);
        }
        if (obstructed) {
            return path;
        } else {
            ArrayList<Vector3fImmutable> goalPath = new ArrayList<>();
            goalPath.add(start);
            goalPath.add(start.moveTowards(goal,32));
            goalPath.add(goal);
            return goalPath; //if the path isn't obstructed we can walk directly from start to the goal

        }
    }

    public static float getCost(Vector3fImmutable point, Vector3fImmutable start, Vector3fImmutable goal) {
        float gCost = start.distanceSquared(point);
        float hCost = goal.distanceSquared(point);
        return gCost + hCost;
    }

    public static boolean pathBlocked(Vector3fImmutable start, Vector3fImmutable end){
        Zone zone = ZoneManager.findSmallestZone(start);
        if(zone != null) {
            for (Path2D.Float obstacle : zone.navObstacles)
                if(obstacle.intersects(start.x,start.z,end.x,end.z))
                    return true;
        }
        return false;
    }
    public static boolean pointIsBlocked(Vector3fImmutable point) {
    Zone zone = ZoneManager.findSmallestZone(point);
    if(zone != null){
        for(Path2D.Float obstacle : zone.navObstacles)
            if (obstacle.contains(point.x,point.z)) {
                return true;
            }
    }

        //Building building = BuildingManager.getBuildingAtLocation(point);
        //if(building != null) {
            //for (Path2D.Float mesh : building.meshes) {
                //if (mesh.contains(point.x,point.z)) {
                    //return true;
                //}
            //}
            //for (Regions region : building.getBounds().getRegions()) {
                //if (region.isPointInPolygon(point))
                    //if (Math.abs(region.lerpY(point) - point.y) > stepHeight) // get the height distance between current height and target location height
                        //return true;
            //}
        //}
        return false;
    }
}