lakebane/src/engine/gameManager/NavigationManager.java

package engine.gameManager;

import engine.math.Vector3fImmutable;
import engine.objects.*;

import java.awt.*;
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 = getOptimizedPath(getPath(character.loc, goal), getPath(goal, character.loc));
            if (path.isEmpty() || path.size() < 2) {
                character.destination = goal;
                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))
            {
                continue;
            }
            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(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 (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;
            }
            boolean pointBlocked = false;
            for (Path2D.Float mesh : building.meshes) {
                if (mesh.contains((double)point.x,(double)point.z)) {
                    pointBlocked = true;
                }
                return pointBlocked;
            }
        }
        return false;
    }
}