Server/src/engine/gameManager/NavigationManager.java

package engine.gameManager;

import engine.Enum;
import engine.math.Vector3fImmutable;
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) {
        try {
            ArrayList<Vector3fImmutable> path = getPath(character.loc, goal);//getOptimizedPath(getPath(character.loc, goal), getPath(goal, character.loc));
            if (path.isEmpty()) {
                MoveToPointMsg msg = new MoveToPointMsg();

                msg.setSourceType(Enum.GameObjectType.Mob.ordinal());
                msg.setSourceID(character.getObjectUUID());
                msg.setStartCoord(character.loc);
                msg.setEndCoord(goal);
                Regions region = Regions.getRegionAtLocation(goal);
                if (region != null) {
                    msg.setInBuildingFloor(region.room);
                    msg.setInBuilding(region.level);
                    msg.setStartLocType(0);
                    msg.setInBuildingUUID(region.parentBuildingID);
                } else {
                    msg.setInBuildingFloor(-1);
                    msg.setInBuilding(-1);
                    msg.setStartLocType(0);
                    msg.setInBuildingUUID(0);
                }
                return; //no points to walk to
            }

            //character.destination = path.get(1);
            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 pointIsBlocked(Vector3fImmutable point) {

        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;
    }
}