major pathfinding work

docs/src/progress/renderertodo.md

@@ -1647,6 +1647,8 @@ New AI behaviors
 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/AcquireItemTree.java

@@ -44,7 +44,7 @@ public class AcquireItemTree {
                     new SourcingTypeNode(SourcingType.PICKUP, BlackboardKeys.ITEM_TARGET_CATEGORY),
                     //logic to pick up the item
                     new TargetEntityCategoryNode(BlackboardKeys.ITEM_TARGET_CATEGORY),
-                    MoveToTarget.create(CollisionEngine.DEFAULT_INTERACT_DISTANCE, BlackboardKeys.ENTITY_TARGET),
+                    MoveToTree.create(CollisionEngine.DEFAULT_INTERACT_DISTANCE, BlackboardKeys.ENTITY_TARGET),
                     new CollectItemNode(),
                     new RunnerNode(null)
                 ),
@@ -60,7 +60,7 @@ public class AcquireItemTree {
                     //check if we should be sourcing this from harvesting foliage
                     new SourcingTypeNode(SourcingType.HARVEST, blackboardKey),
                     new TargetEntityCategoryNode(BlackboardKeys.HARVEST_TARGET_TYPE),
-                    MoveToTarget.create(CollisionEngine.DEFAULT_INTERACT_DISTANCE, BlackboardKeys.ENTITY_TARGET),
+                    MoveToTree.create(CollisionEngine.DEFAULT_INTERACT_DISTANCE, BlackboardKeys.ENTITY_TARGET),
                     new HarvestNode(),
                     new RunnerNode(null)
                 ),

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

@@ -10,11 +10,12 @@ 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
  */
-public class MoveToTarget {
+public class MoveToTree {
     
     /**
      * Name of the tree
@@ -38,7 +39,7 @@ public class MoveToTarget {
 
             //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/ai/trees/creature/melee/FellTree.java

@@ -16,7 +16,7 @@ import electrosphere.server.ai.nodes.meta.debug.PublishStatusNode;
 import electrosphere.server.ai.nodes.meta.decorators.RunnerNode;
 import electrosphere.server.ai.nodes.meta.decorators.TimerNode;
 import electrosphere.server.ai.nodes.meta.decorators.UntilNode;
-import electrosphere.server.ai.trees.creature.MoveToTarget;
+import electrosphere.server.ai.trees.creature.MoveToTree;
 import electrosphere.server.ai.trees.creature.inventory.EquipToolbarTree;
 
 /**
@@ -68,7 +68,7 @@ public class FellTree {
                 ),
 
                 //move to target
-                MoveToTarget.create(FellTree.FELL_RANGE, targetKey),
+                MoveToTree.create(FellTree.FELL_RANGE, targetKey),
 
                 //movement succeeded, but failed to attack -- tree is currently running
                 new RunnerNode(null)

src/main/java/electrosphere/server/ai/trees/struct/BuildStructureTree.java

@@ -13,7 +13,7 @@ import electrosphere.server.ai.nodes.meta.debug.PublishStatusNode;
 import electrosphere.server.ai.nodes.meta.decorators.SucceederNode;
 import electrosphere.server.ai.nodes.solvers.SolveBuildMaterialNode;
 import electrosphere.server.ai.trees.creature.AcquireItemTree;
-import electrosphere.server.ai.trees.creature.MoveToTarget;
+import electrosphere.server.ai.trees.creature.MoveToTree;
 
 /**
  * A tree to build whatever the current structure target is
@@ -44,7 +44,7 @@ public class BuildStructureTree {
                         //if we're within range to place the material
                         new SequenceNode(
                             //not in range, move to within range
-                            MoveToTarget.create(CollisionEngine.DEFAULT_INTERACT_DISTANCE, BlackboardKeys.STRUCTURE_TARGET),
+                            MoveToTree.create(CollisionEngine.DEFAULT_INTERACT_DISTANCE, BlackboardKeys.STRUCTURE_TARGET),
                             //equip the type of block to place
                             EquipToolbarNode.equipBlock(BlackboardKeys.BUILDING_MATERIAL_CURRENT),
                             //in range, place block

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