major pathfinding work

docs/src/progress/renderertodo.md

@@ -1648,6 +1648,7 @@ Fix bug where sync messages eternally bounce if the entity was already deleted
 Fix blocks not saving to disk when being ejected from cache
 Block chunk memory pooling
 Rename MoveToTree
+Major pathfinding work -- breaking MoteToTree
 
 
 

src/main/java/electrosphere/engine/Globals.java

@@ -84,6 +84,7 @@ import electrosphere.server.datacell.EntityDataCellMapper;
 import electrosphere.server.datacell.RealmManager;
 import electrosphere.server.db.DatabaseController;
 import electrosphere.server.entity.poseactor.PoseModel;
+import electrosphere.server.pathfinding.Pathfinder;
 import electrosphere.server.saves.Save;
 import electrosphere.server.simulation.MacroSimulation;
 import electrosphere.server.simulation.MicroSimulation;
@@ -430,6 +431,9 @@ public class Globals {
     public static Entity draggedItem = null;
     public static Object dragSourceInventory = null;
 
+    //pathfinder
+    public static Pathfinder pathfinder;
+    
     
     
     
@@ -518,6 +522,9 @@ public class Globals {
         gameConfigCurrent = gameConfigDefault;
         NetConfig.readNetConfig();
 
+        //pathfinder
+        Globals.pathfinder = new Pathfinder();
+
         //
         //Values that depend on the loaded config
         Globals.clientSelectedVoxelType = (VoxelType)gameConfigCurrent.getVoxelData().getTypes().toArray()[1];

src/main/java/electrosphere/server/ai/blackboard/BlackboardKeys.java

@@ -70,4 +70,9 @@ public class BlackboardKeys {
      */
     public static final String HARVEST_TARGET_TYPE = "harvestTargetType";
 
+    /**
+     * The pathfinding data
+     */
+    public static final String PATHFINDING_DATA = "pathfindingData";
+
 }

src/main/java/electrosphere/server/ai/nodes/plan/PathfindingNode.java

@@ -0,0 +1,98 @@
+package electrosphere.server.ai.nodes.plan;
+
+import java.util.List;
+
+import org.joml.Vector3d;
+
+import electrosphere.engine.Globals;
+import electrosphere.entity.Entity;
+import electrosphere.entity.EntityUtils;
+import electrosphere.server.ai.blackboard.Blackboard;
+import electrosphere.server.ai.blackboard.BlackboardKeys;
+import electrosphere.server.ai.nodes.AITreeNode;
+import electrosphere.server.datacell.Realm;
+import electrosphere.server.datacell.interfaces.PathfindingManager;
+import electrosphere.server.pathfinding.PathingProgressiveData;
+
+/**
+ * A node that performs pathfinding
+ */
+public class PathfindingNode implements AITreeNode {
+
+
+    /**
+     * The blackboard key to lookup the target entity under
+     */
+    String targetEntityKey;
+
+    /**
+     * 
+     * @param targetEntityKey
+     */
+    public static PathfindingNode createPathEntity(String targetEntityKey){
+        PathfindingNode rVal = new PathfindingNode();
+        rVal.targetEntityKey = targetEntityKey;
+        return rVal;
+    }
+
+
+    @Override
+    public AITreeNodeResult evaluate(Entity entity, Blackboard blackboard) {
+        if(!PathfindingNode.hasPathfindingData(blackboard)){
+            Vector3d targetPos = null;
+            if(this.targetEntityKey != null){
+                Entity targetEnt = (Entity)blackboard.get(targetEntityKey);
+                targetPos = EntityUtils.getPosition(targetEnt);
+            } else {
+                throw new Error("Target position is null!");
+            }
+
+            Realm realm = Globals.realmManager.getEntityRealm(entity);
+            PathfindingManager pathfindingManager = realm.getPathfindingManager();
+
+            Vector3d entityPos = EntityUtils.getPosition(entity);
+
+            List<Vector3d> path = pathfindingManager.findPath(entityPos, targetPos);
+            PathingProgressiveData pathingProgressiveData = new PathingProgressiveData(path);
+            PathfindingNode.setPathfindingData(blackboard, pathingProgressiveData);
+        }
+
+        return AITreeNodeResult.SUCCESS;
+    }
+
+    /**
+     * Sets the pathfinding data in the blackboard
+     * @param blackboard The blackboard
+     * @param pathfindingData The pathfinding data
+     */
+    public static void setPathfindingData(Blackboard blackboard, PathingProgressiveData pathfindingData){
+        blackboard.put(BlackboardKeys.PATHFINDING_DATA, pathfindingData);
+    }
+
+    /**
+     * Gets the current pathfinding data
+     * @param blackboard The blackboard
+     * @return The pathfinding data if it exists, null otherwise
+     */
+    public static PathingProgressiveData getPathfindingData(Blackboard blackboard){
+        return (PathingProgressiveData)blackboard.get(BlackboardKeys.PATHFINDING_DATA);
+    }
+
+    /**
+     * Checks if the blackboard has pathfinding data
+     * @param blackboard The blackboard
+     * @return true if it has pathfinding data, false otherwise
+     */
+    public static boolean hasPathfindingData(Blackboard blackboard){
+        return blackboard.has(BlackboardKeys.PATHFINDING_DATA);
+    }
+
+    /**
+     * Clears the pathfinding data
+     * @param blackboard The pathfinding data
+     */
+    public static void clearPathfindingData(Blackboard blackboard){
+        blackboard.delete(BlackboardKeys.PATHFINDING_DATA);
+    }
+    
+}

src/main/java/electrosphere/server/ai/trees/creature/MoveToTree.java

@@ -10,6 +10,7 @@ import electrosphere.server.ai.nodes.meta.collections.SelectorNode;
 import electrosphere.server.ai.nodes.meta.collections.SequenceNode;
 import electrosphere.server.ai.nodes.meta.decorators.RunnerNode;
 import electrosphere.server.ai.nodes.meta.decorators.SucceederNode;
+import electrosphere.server.ai.nodes.plan.PathfindingNode;
 
 /**
  * Moves to a target
@@ -38,7 +39,7 @@ public class MoveToTree {
 
             //not in range of target, keep moving towards it
             new SequenceNode(
-                //check that dependencies exist
+                PathfindingNode.createPathEntity(targetKey),
                 new FaceTargetNode(targetKey),
                 new RunnerNode(new MoveStartNode(MovementRelativeFacing.FORWARD))
             )

src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellManager.java

@@ -15,6 +15,7 @@ import java.util.concurrent.locks.ReentrantLock;
 
 import org.joml.Vector3d;
 import org.joml.Vector3i;
+import org.recast4j.detour.MeshData;
 
 import electrosphere.client.block.BlockChunkData;
 import electrosphere.client.terrain.data.TerrainChunkData;
@@ -364,7 +365,11 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager
         ServerDataCell serverDataCell = this.groundDataCells.get(key);
         GriddedDataCellTrackingData trackingData = this.cellTrackingMap.get(serverDataCell);
         if(terrainMeshData.getVertices().length > 0){
-            trackingData.setNavMeshData(NavMeshConstructor.constructNavmesh(terrainMeshData));
+            MeshData pathingMeshData = NavMeshConstructor.constructNavmesh(terrainMeshData);
+            if(pathingMeshData == null){
+                throw new Error("Failed to build pathing data from existing vertices!");
+            }
+            trackingData.setNavMeshData(pathingMeshData);
         }
 
         loadedCellsLock.lock();
@@ -802,7 +807,11 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager
 
                 //create pathfinding mesh
                 if(terrainMeshData.getVertices().length > 0){
-                    trackingData.setNavMeshData(NavMeshConstructor.constructNavmesh(terrainMeshData));
+                    MeshData pathingMeshData = NavMeshConstructor.constructNavmesh(terrainMeshData);
+                    if(pathingMeshData == null){
+                        throw new Error("Failed to build pathing data from existing vertices!");
+                    }
+                    trackingData.setNavMeshData(pathingMeshData);
                 }
 
                 //set ready
@@ -1104,7 +1113,18 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager
 
     @Override
     public List<Vector3d> findPath(Vector3d start, Vector3d end) {
-        throw new UnsupportedOperationException("Unimplemented method 'findPath'");
+        Vector3i startChunkPos = ServerWorldData.convertRealToChunkSpace(start);
+        ServerDataCell cell = this.getCellAtWorldPosition(startChunkPos);
+        GriddedDataCellTrackingData trackingData = this.cellTrackingMap.get(cell);
+        if(trackingData == null){
+            throw new Error("Failed to find tracking data for " + start);
+        }
+        MeshData trackingMeshData = trackingData.getNavMeshData();
+        if(trackingMeshData == null){
+            throw new Error("Tracking mesh data is null!");
+        }
+        List<Vector3d> points = Globals.pathfinder.solve(trackingMeshData, start, end);
+        return points;
     }
 
     /**

src/main/java/electrosphere/server/pathfinding/NavMeshConstructor.java

@@ -1,111 +1,131 @@
 package electrosphere.server.pathfinding;
 
+import org.joml.Vector3d;
 import org.recast4j.detour.MeshData;
 import org.recast4j.detour.NavMeshBuilder;
 import org.recast4j.detour.NavMeshDataCreateParams;
 import org.recast4j.recast.AreaModification;
+import org.recast4j.recast.CompactHeightfield;
+import org.recast4j.recast.Heightfield;
 import org.recast4j.recast.PolyMesh;
 import org.recast4j.recast.PolyMeshDetail;
 import org.recast4j.recast.RecastBuilder;
 import org.recast4j.recast.RecastBuilder.RecastBuilderResult;
-import org.recast4j.recast.RecastConstants.PartitionType;
 import org.recast4j.recast.RecastBuilderConfig;
 import org.recast4j.recast.RecastConfig;
+import org.recast4j.recast.RecastConstants;
+import org.recast4j.recast.RecastConstants.PartitionType;
+import org.recast4j.recast.Span;
 import org.recast4j.recast.geom.SingleTrimeshInputGeomProvider;
 
-import electrosphere.client.terrain.data.TerrainChunkData;
+import electrosphere.entity.state.collidable.TriGeomData;
+import electrosphere.util.math.GeomUtils;
 
 /**
  * Constructor methods for nav meshes
  */
 public class NavMeshConstructor {
 
+    /**
+     * Minimum size of geometry aabb
+     */
+    public static final float AABB_MIN_SIZE = 0.01f;
+
     /**
      * Size of a recast cell
      */
-    static final float RECAST_CELL_SIZE = 1.0f;
+    static final float RECAST_CELL_SIZE = 0.3f;
 
     /**
      * Height of a recast cell
      */
-    static final float RECAST_CELL_HEIGHT = 1.0f;
+    static final float RECAST_CELL_HEIGHT = 0.2f;
-
-    /**
-     * Size of a recast agent
-     */
-    static final float RECAST_AGENT_SIZE = 1.0f;
 
     /**
      * Height of a recast agent
      */
     static final float RECAST_AGENT_HEIGHT = 1.0f;
 
+    /**
+     * Size of a recast agent
+     */
+    static final float RECAST_AGENT_SIZE = 0.5f;
+
     /**
      * Maximum height a recast agent can climb
      */
-    static final float RECAST_AGENT_MAX_CLIMB = 0.1f;
+    static final float RECAST_AGENT_MAX_CLIMB = 0.9f;
 
     /**
      * Maximum slope a recast agent can handle
      */
-    static final float RECAST_AGENT_MAX_SLOPE = 0.4f;
+    static final float RECAST_AGENT_MAX_SLOPE = 60.0f;
 
     /**
      * Minimum size of a recast region
      */
-    static final int RECAST_MIN_REGION_SIZE = 1;
+    static final int RECAST_MIN_REGION_SIZE = 0;
 
     /**
      * Merge size of a recast region
      */
-    static final int RECAST_REGION_MERGE_SIZE = 1;
+    static final int RECAST_REGION_MERGE_SIZE = 0;
 
-    static final float RECAST_REGION_EDGE_MAX_LEN = 1.0f;
+    static final float RECAST_REGION_EDGE_MAX_LEN = 20.0f;
 
-    static final float RECAST_REGION_EDGE_MAX_ERROR = 1.0f;
+    static final float RECAST_REGION_EDGE_MAX_ERROR = 3.3f;
 
-    static final int RECAST_VERTS_PER_POLY = 3;
+    static final int RECAST_VERTS_PER_POLY = 6;
 
-    static final float RECAST_DETAIL_SAMPLE_DIST = 0.1f;
+    static final float RECAST_DETAIL_SAMPLE_DIST = 1.0f;
 
-    static final float RECAST_DETAIL_SAMPLE_MAX_ERROR = 0.1f;
+    static final float RECAST_DETAIL_SAMPLE_MAX_ERROR = 1.0f;
     
     /**
      * Constructs a navmesh
      * @param terrainChunk The terrain chunk
      * @return the MeshData
      */
-    public static MeshData constructNavmesh(TerrainChunkData terrainChunkData){
+    public static MeshData constructNavmesh(TriGeomData geomData){
         MeshData rVal = null;
 
         try {
-            RecastConfig recastConfig = new RecastConfig(
+
-                PartitionType.WATERSHED,
+            //build polymesh
-                RECAST_CELL_SIZE,
+            RecastBuilderResult recastBuilderResult = NavMeshConstructor.buildPolymesh(geomData);
-                RECAST_CELL_HEIGHT,
-                RECAST_AGENT_HEIGHT,
-                RECAST_AGENT_SIZE,
-                RECAST_AGENT_MAX_CLIMB,
-                RECAST_AGENT_MAX_SLOPE,
-                RECAST_MIN_REGION_SIZE,
-                RECAST_REGION_MERGE_SIZE,
-                RECAST_REGION_EDGE_MAX_LEN,
-                RECAST_REGION_EDGE_MAX_ERROR,
-                RECAST_VERTS_PER_POLY,
-                RECAST_DETAIL_SAMPLE_DIST,
-                RECAST_DETAIL_SAMPLE_MAX_ERROR,
-                new AreaModification(0)
-            );
-            SingleTrimeshInputGeomProvider geomProvider = new SingleTrimeshInputGeomProvider(terrainChunkData.getVertices(), terrainChunkData.getFaceElements());
-            RecastBuilderConfig recastBuilderConfig = new RecastBuilderConfig(recastConfig, geomProvider.getMeshBoundsMin(), geomProvider.getMeshBoundsMax());
-            RecastBuilder recastBuilder = new RecastBuilder();
-            RecastBuilderResult recastBuilderResult = recastBuilder.build(geomProvider, recastBuilderConfig);
             PolyMesh polyMesh = recastBuilderResult.getMesh();
+            
+            Vector3d polyMin = new Vector3d(polyMesh.bmin[0],polyMesh.bmin[1],polyMesh.bmin[2]);
+            Vector3d polyMax = new Vector3d(polyMesh.bmax[0],polyMesh.bmax[1],polyMesh.bmax[2]);
+            if(polyMin.x < 0 || polyMin.y < 0 || polyMin.z < 0){
+                String message = "Min bound is less than 0\n" +
+                NavMeshConstructor.polyMeshToString(polyMesh);
+                throw new Error(message);
+            }
+
+            if(polyMin.distance(polyMax) < AABB_MIN_SIZE){
+                String message = "Bounding box is too small for polymesh\n" +
+                NavMeshConstructor.polyMeshToString(polyMesh);
+                throw new Error(message);
+            }
+
+            //error check the built result
+            if(polyMesh.nverts < 1){
+                String message = "Failed to generate verts in poly mesh\n" +
+                NavMeshConstructor.polyMeshToString(polyMesh);
+                throw new Error(message);
+            }
+            if(polyMesh.npolys < 1){
+                String message = "Failed to generate polys in poly mesh\n" +
+                NavMeshConstructor.polyMeshToString(polyMesh);
+                throw new Error(message);
+            }
+
+            //set flags
             for(int i = 0; i < polyMesh.npolys; i++){
                 polyMesh.flags[i] = 1;
             }
 
-
             //set params
             NavMeshDataCreateParams params = new NavMeshDataCreateParams();
             params.verts = polyMesh.verts;
@@ -163,4 +183,120 @@ public class NavMeshConstructor {
         return rVal;
     }
 
+    /**
+     * Builds a builder result from a geom data
+     * @param geomData The geom data
+     * @return The builder result
+     */
+    protected static RecastBuilderResult buildPolymesh(TriGeomData geomData){
+        //create the geometry provider and error check
+        SingleTrimeshInputGeomProvider geomProvider = NavMeshConstructor.getSingleTrimeshInputGeomProvider(geomData);
+
+        //build configs
+        RecastConfig recastConfig = new RecastConfig(
+            PartitionType.WATERSHED,
+            RECAST_CELL_SIZE,
+            RECAST_CELL_HEIGHT,
+            RECAST_AGENT_HEIGHT,
+            RECAST_AGENT_SIZE,
+            RECAST_AGENT_MAX_CLIMB,
+            RECAST_AGENT_MAX_SLOPE,
+            RECAST_MIN_REGION_SIZE,
+            RECAST_REGION_MERGE_SIZE,
+            RECAST_REGION_EDGE_MAX_LEN,
+            RECAST_REGION_EDGE_MAX_ERROR,
+            RECAST_VERTS_PER_POLY,
+            RECAST_DETAIL_SAMPLE_DIST,
+            RECAST_DETAIL_SAMPLE_MAX_ERROR,
+            new AreaModification(1)
+        );
+        float[] boundMax = new float[]{
+            geomProvider.getMeshBoundsMax()[0],
+            geomProvider.getMeshBoundsMax()[1] + RECAST_AGENT_HEIGHT,
+            geomProvider.getMeshBoundsMax()[2]
+        };
+        RecastBuilderConfig recastBuilderConfig = new RecastBuilderConfig(recastConfig, geomProvider.getMeshBoundsMin(), boundMax);
+        RecastBuilder recastBuilder = new RecastBuilder();
+
+        //actually build polymesh
+        RecastBuilderResult recastBuilderResult = recastBuilder.build(geomProvider, recastBuilderConfig);
+
+        return recastBuilderResult;
+    }
+
+    /**
+     * Creates the geom provider from a given trimesh
+     * @return The geom provider
+     */
+    protected static SingleTrimeshInputGeomProvider getSingleTrimeshInputGeomProvider(TriGeomData geomData){
+        //error check input
+        if(!GeomUtils.isWindingClockwise(geomData.getVertices(), geomData.getFaceElements())){
+            throw new Error("Geometry is not wound clockwise!");
+        }
+        //create the geometry provider and error check
+        SingleTrimeshInputGeomProvider geomProvider = new SingleTrimeshInputGeomProvider(geomData.getVertices(), geomData.getFaceElements());
+        //check the bounding box
+        Vector3d aabbStart = new Vector3d(geomProvider.getMeshBoundsMin()[0],geomProvider.getMeshBoundsMin()[1],geomProvider.getMeshBoundsMin()[2]);
+        Vector3d aabbEnd = new Vector3d(geomProvider.getMeshBoundsMax()[0],geomProvider.getMeshBoundsMax()[1],geomProvider.getMeshBoundsMax()[2]);
+        if(aabbStart.distance(aabbEnd) < AABB_MIN_SIZE){
+            throw new Error("Geometry provider's AABB is too small " + aabbStart.distance(aabbEnd));
+        }
+        return geomProvider;
+    }
+
+    /**
+     * Counts the spans of a building result
+     * @param recastBuilderResult The result
+     * @return The number of spans
+     */
+    protected static int countSpans(RecastBuilderResult recastBuilderResult){
+        Heightfield heightfield = recastBuilderResult.getSolidHeightfield();
+        int count = 0;
+        int w = heightfield.width;
+        int h = heightfield.height;
+        for(int y = 0; y < h; ++y) {
+            for(int x = 0; x < w; ++x) {
+                for(Span s = heightfield.spans[x + y * w]; s != null; s = s.next) {
+                    if(s.area != RecastConstants.RC_NULL_AREA){
+                        count++;
+                    }
+                }
+            }
+        }
+        return count;
+    }
+
+
+    /**
+     * Counts the walkable spans in the compact heightfield
+     * @param recastBuilderResult The build result
+     * @return The number of walkable spans
+     */
+    protected static int countWalkableSpans(RecastBuilderResult recastBuilderResult){
+        CompactHeightfield chf = recastBuilderResult.getCompactHeightfield();
+        int count = 0;
+        for (int i = 0; i < chf.spanCount; i++){
+            if(chf.spans[i] != null){
+                count++;
+            }
+        }
+        return count;
+    }
+
+    /**
+     * Converts a polymesh to a string
+     * @param polyMesh The polymesh
+     * @return The string
+     */
+    protected static String polyMeshToString(PolyMesh polyMesh){
+        return "" +
+        "nverts: " + polyMesh.nverts + "\n" +
+        "verts.length: " + polyMesh.verts.length + "\n" +
+        "npolys: " + polyMesh.npolys + "\n" +
+        "polys.length: " + polyMesh.polys.length + "\n" +
+        "bmin: " + polyMesh.bmin[0] + "," + polyMesh.bmin[1] + "," + polyMesh.bmin[2] + "\n" +
+        "bmin: " + polyMesh.bmax[0] + "," + polyMesh.bmax[1] + "," + polyMesh.bmax[2] + "\n" +
+        "";
+    }
+
 }

src/main/java/electrosphere/server/pathfinding/Pathfinder.java

@@ -2,8 +2,10 @@ package electrosphere.server.pathfinding;
 
 import java.util.LinkedList;
 import java.util.List;
+import java.util.Objects;
 
 import org.joml.Vector3d;
+import org.joml.Vector3f;
 import org.recast4j.detour.DefaultQueryFilter;
 import org.recast4j.detour.FindNearestPolyResult;
 import org.recast4j.detour.MeshData;
@@ -11,8 +13,11 @@ import org.recast4j.detour.NavMesh;
 import org.recast4j.detour.NavMeshQuery;
 import org.recast4j.detour.QueryFilter;
 import org.recast4j.detour.Result;
+import org.recast4j.detour.Status;
 import org.recast4j.detour.StraightPathItem;
 
+import electrosphere.server.physics.terrain.manager.ServerTerrainChunk;
+
 /**
  * Performs pathfinding
  */
@@ -33,6 +38,10 @@ public class Pathfinder {
     public List<Vector3d> solve(MeshData mesh, Vector3d startPos, Vector3d endPos){
         List<Vector3d> rVal = new LinkedList<Vector3d>();
 
+        if(mesh == null){
+            throw new Error("Mesh data is null!");
+        }
+
         //construct objects
         NavMesh navMesh = new NavMesh(mesh,6,0);
         NavMeshQuery query = new NavMeshQuery(navMesh);
@@ -41,11 +50,11 @@ public class Pathfinder {
         //convert points to correct datatypes
         float[] startArr = new float[]{(float)startPos.x, (float)startPos.y, (float)startPos.z};
         float[] endArr = new float[]{(float)endPos.x, (float)endPos.y, (float)endPos.z};
-        float[] polySearchBounds = new float[]{10,10,10};
+        float[] polySearchBounds = new float[]{ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET,ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET,ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET};
 
         //find start poly
         Result<FindNearestPolyResult> startPolyResult = query.findNearestPoly(startArr, polySearchBounds, filter);
-        if(startPolyResult.failed()){
+        if(!startPolyResult.succeeded()){
             String message = "Failed to solve for start polygon!\n" +
             startPolyResult.message
             ;
@@ -55,7 +64,7 @@ public class Pathfinder {
 
         //find end poly
         Result<FindNearestPolyResult> endPolyResult = query.findNearestPoly(endArr, polySearchBounds, filter);
-        if(endPolyResult.failed()){
+        if(!endPolyResult.succeeded()){
             String message = "Failed to solve for end polygon!\n" +
             endPolyResult.message
             ;
@@ -63,12 +72,29 @@ public class Pathfinder {
         }
         long endRef = endPolyResult.result.getNearestRef();
 
+        if(startRef == 0){
+            throw new Error("Start ref is 0!");
+        }
+        if(endRef == 0){
+            throw new Error("End ref is 0!");
+        }
+
         //solve path
         Result<List<Long>> pathResult = query.findPath(startRef, endRef, startArr, endArr, filter);
         if(pathResult.failed()){
             String message = "Failed to solve for path!\n" +
-            pathResult.message
+            pathResult.message + "\n" +
+            pathResult.status + "\n" +
+            ""
             ;
+            if(pathResult.status == Status.FAILURE_INVALID_PARAM){
+                message = "Failed to solve for path -- invalid param!\n" +
+                "Message: " + pathResult.message + "\n" +
+                "Status: " + pathResult.status + "\n" +
+                Pathfinder.checkInvalidParam(navMesh,startRef,endRef,startArr,endArr) + "\n" +
+                ""
+                ;
+            }
             throw new Error(message);
         }
 
@@ -89,4 +115,28 @@ public class Pathfinder {
         return rVal;
     }
 
+    /**
+     * Checks params to a path query
+     * @param mesh The mesh
+     * @param startRef The start ref
+     * @param endRef The end ref
+     * @param startPos The start pos
+     * @param endPos THe end pos
+     * @return The string containing the data
+     */
+    private static String checkInvalidParam(NavMesh mesh, long startRef, long endRef, float[] startPos, float[] endPos){
+        //none of these should be true
+        return "" +
+        "startRef: " + startRef + "\n" +
+        "endRef:   " + endRef + "\n" +
+        "StartRef poly area succeeded: " + mesh.getPolyArea(startRef).succeeded() + "\n" +
+        "EndRef poly area succeeded:   " + mesh.getPolyArea(endRef).succeeded() + "\n" +
+        "StartPos is null: " + Objects.isNull(startPos) + "\n" +
+        "StartPos is finite: " + !new Vector3f(startPos[0],startPos[1],startPos[2]).isFinite() + "\n" +
+        "EndPos is null:   " + Objects.isNull(endPos) + "\n" +
+        "EndPos is finite:   " + !new Vector3f(endPos[0],endPos[1],endPos[2]).isFinite() + "\n" +
+        ""
+        ;
+    }
+
 }

src/main/java/electrosphere/server/pathfinding/PathingProgressiveData.java

@@ -0,0 +1,65 @@
+package electrosphere.server.pathfinding;
+
+import java.util.List;
+
+import org.joml.Vector3d;
+
+/**
+ * Data tracking moving along a solved path
+ */
+public class PathingProgressiveData {
+    
+    /**
+     * The list of points that represent the path
+     */
+    List<Vector3d> points;
+
+    /**
+     * The current point to move towards (ie all previous points have already been pathed to)
+     */
+    int currentPoint;
+
+    /**
+     * Constructor
+     * @param points The points for the path
+     */
+    public PathingProgressiveData(List<Vector3d> points){
+        this.points = points;
+        this.currentPoint = 0;
+    }
+
+    /**
+     * Gets the points that define the path
+     * @return The points
+     */
+    public List<Vector3d> getPoints() {
+        return points;
+    }
+
+    /**
+     * Sets the points that define the path
+     * @param points The points
+     */
+    public void setPoints(List<Vector3d> points) {
+        this.points = points;
+    }
+
+    /**
+     * Gets the current point to move towards
+     * @return The current point's index
+     */
+    public int getCurrentPoint() {
+        return currentPoint;
+    }
+
+    /**
+     * Sets the current point to move towards
+     * @param currentPoint The current point's index
+     */
+    public void setCurrentPoint(int currentPoint) {
+        this.currentPoint = currentPoint;
+    }
+
+    
+
+}

src/main/java/electrosphere/util/math/GeomUtils.java

@@ -511,4 +511,64 @@ public class GeomUtils {
         }
     }
 
+    /**
+     * Checks the winding of a given set of geometry
+     * @param verts The vertices
+     * @param indices The indices
+     */
+    public static void checkWinding(float[] verts, int[] indices) {
+        for (int i = 0; i < indices.length; i += 3) {
+            int ia = indices[i] * 3;
+            int ib = indices[i + 1] * 3;
+            int ic = indices[i + 2] * 3;
+    
+            float ax = verts[ia];
+            float az = verts[ia + 2];
+            float bx = verts[ib];
+            float bz = verts[ib + 2];
+            float cx = verts[ic];
+            float cz = verts[ic + 2];
+    
+            // Compute signed area of the triangle in XZ plane
+            float signedArea = (bx - ax) * (cz - az) - (cx - ax) * (bz - az);
+    
+            if (signedArea < 0) {
+                System.out.println("Triangle " + (i / 3) + " is wound CLOCKWISE (probably incorrect)");
+            } else if (signedArea > 0) {
+                System.out.println("Triangle " + (i / 3) + " is wound COUNTER-CLOCKWISE (likely correct)");
+            } else {
+                System.out.println("Triangle " + (i / 3) + " is degenerate (zero area)");
+            }
+        }
+    }
+
+    /**
+     * Checks that the winding of the geometry it clockwise
+     * @param verts The verts
+     * @param indices The indices
+     * @return true if all triangles are wound clockwise, false otherwise
+     */
+    public static boolean isWindingClockwise(float[] verts, int[] indices){
+        for (int i = 0; i < indices.length; i += 3) {
+            int ia = indices[i] * 3;
+            int ib = indices[i + 1] * 3;
+            int ic = indices[i + 2] * 3;
+    
+            float ax = verts[ia];
+            float az = verts[ia + 2];
+            float bx = verts[ib];
+            float bz = verts[ib + 2];
+            float cx = verts[ic];
+            float cz = verts[ic + 2];
+    
+            // Compute signed area of the triangle in XZ plane
+            float signedArea = (bx - ax) * (cz - az) - (cx - ax) * (bz - az);
+    
+            if (signedArea > 0) {
+                return false;
+            }
+        }
+        return true;
+    }
+    
 }

src/test/java/electrosphere/server/pathfinding/NavMeshConstructorTests.java

@@ -0,0 +1,182 @@
+package electrosphere.server.pathfinding;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+
+import org.joml.Vector3d;
+import org.recast4j.detour.MeshData;
+import org.recast4j.recast.ContourSet;
+import org.recast4j.recast.RecastBuilder.RecastBuilderResult;
+import org.recast4j.recast.geom.SingleTrimeshInputGeomProvider;
+
+import electrosphere.entity.state.collidable.TriGeomData;
+import electrosphere.test.annotations.UnitTest;
+import electrosphere.util.math.GeomUtils;
+
+/**
+ * Tests for navmesh constructor
+ */
+public class NavMeshConstructorTests {
+
+    /**
+     * Expected size of the geom 1 aabb
+     */
+    static final int GEOM_1_EXPECTED_AABB = 1;
+
+    /**
+     * Number of spans expected for geom 1
+     */
+    static final int GEOM_1_SPAN_COUNT = 496;
+    
+    /**
+     * Test constructing a simple navmesh
+     */
+    @UnitTest
+    public void test_constructNavmesh_geom1(){
+        TriGeomData geom = new TriGeomData() {
+            @Override
+            public float[] getVertices() {
+                return new float[]{
+                    0f, 0f, 0f,
+                    10f, 0f, 0f,
+                    0f, 0f, 10f
+                };
+            }
+            @Override
+            public int[] getFaceElements() {
+                return new int[]{
+                    2, 1, 0
+                };
+            }
+        };
+
+        MeshData meshData = NavMeshConstructor.constructNavmesh(geom);
+
+        assertNotNull(meshData);
+    }
+
+    /**
+     * Test constructing a simple navmesh
+     */
+    @UnitTest
+    public void test_countSpans_geom1(){
+        TriGeomData geom = new TriGeomData() {
+            @Override
+            public float[] getVertices() {
+                return new float[]{
+                    0f, 0f, 0f,
+                    10f, 0f, 0f,
+                    0f, 0f, 10f
+                };
+            }
+            @Override
+            public int[] getFaceElements() {
+                return new int[]{
+                    2, 1, 0
+                };
+            }
+        };
+
+        //actually build polymesh
+        RecastBuilderResult recastBuilderResult = NavMeshConstructor.buildPolymesh(geom);
+        int spanCount = NavMeshConstructor.countSpans(recastBuilderResult);
+
+        assertEquals(GEOM_1_SPAN_COUNT, spanCount);
+    }
+
+    /**
+     * Test constructing a simple navmesh
+     */
+    @UnitTest
+    public void test_countWalkableSpans_geom1(){
+        TriGeomData geom = new TriGeomData() {
+            @Override
+            public float[] getVertices() {
+                return new float[]{
+                    0f, 0f, 0f,
+                    10f, 0f, 0f,
+                    0f, 0f, 10f
+                };
+            }
+            @Override
+            public int[] getFaceElements() {
+                return new int[]{
+                    2, 1, 0
+                };
+            }
+        };
+
+        //actually build polymesh
+        RecastBuilderResult recastBuilderResult = NavMeshConstructor.buildPolymesh(geom);
+        int spanCount = NavMeshConstructor.countWalkableSpans(recastBuilderResult);
+
+        assertEquals(GEOM_1_SPAN_COUNT, spanCount);
+    }
+
+    /**
+     * Test constructing a simple navmesh
+     */
+    @UnitTest
+    public void test_ContourCount_geom1(){
+        TriGeomData geom = new TriGeomData() {
+            @Override
+            public float[] getVertices() {
+                return new float[]{
+                    0f, 0f, 0f,
+                    10f, 0f, 0f,
+                    0f, 0f, 10f
+                };
+            }
+            @Override
+            public int[] getFaceElements() {
+                return new int[]{
+                    2, 1, 0
+                };
+            }
+        };
+
+        //actually build polymesh
+        RecastBuilderResult recastBuilderResult = NavMeshConstructor.buildPolymesh(geom);
+
+        GeomUtils.checkWinding(geom.getVertices(), geom.getFaceElements());
+
+        ContourSet contourSet = recastBuilderResult.getContourSet();
+        assertNotEquals(0,contourSet.width);
+        assertNotEquals(0,contourSet.height);
+        assertNotEquals(0,contourSet.conts.size());
+    }
+
+    /**
+     * Test constructing a simple navmesh
+     */
+    @UnitTest
+    public void test_getSingleTrimeshInputGeomProvider_geom1(){
+        TriGeomData geom = new TriGeomData() {
+            @Override
+            public float[] getVertices() {
+                return new float[]{
+                    0f, 0f, 0f,
+                    10f, 0f, 0f,
+                    0f, 0f, 10f
+                };
+            }
+            @Override
+            public int[] getFaceElements() {
+                return new int[]{
+                    2, 1, 0
+                };
+            }
+        };
+
+        SingleTrimeshInputGeomProvider geomProvider = NavMeshConstructor.getSingleTrimeshInputGeomProvider(geom);
+        assertNotNull(geomProvider);
+
+        Vector3d aabbStart = new Vector3d(geomProvider.getMeshBoundsMin()[0],geomProvider.getMeshBoundsMin()[1],geomProvider.getMeshBoundsMin()[2]);
+        Vector3d aabbEnd = new Vector3d(geomProvider.getMeshBoundsMax()[0],geomProvider.getMeshBoundsMax()[1],geomProvider.getMeshBoundsMax()[2]);
+
+        boolean greaterThan = aabbStart.distance(aabbEnd) > GEOM_1_EXPECTED_AABB;
+        assertEquals(true, greaterThan);
+    }
+
+}