diff --git a/docs/src/progress/renderertodo.md b/docs/src/progress/renderertodo.md index 7a7c7ec9..0b9d5b0d 100644 --- a/docs/src/progress/renderertodo.md +++ b/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 diff --git a/src/main/java/electrosphere/engine/Globals.java b/src/main/java/electrosphere/engine/Globals.java index 34e87306..e65e7b99 100644 --- a/src/main/java/electrosphere/engine/Globals.java +++ b/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; @@ -429,6 +430,9 @@ public class Globals { //drag item state 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]; diff --git a/src/main/java/electrosphere/server/ai/blackboard/BlackboardKeys.java b/src/main/java/electrosphere/server/ai/blackboard/BlackboardKeys.java index c5395e98..935155b6 100644 --- a/src/main/java/electrosphere/server/ai/blackboard/BlackboardKeys.java +++ b/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"; + } diff --git a/src/main/java/electrosphere/server/ai/nodes/plan/PathfindingNode.java b/src/main/java/electrosphere/server/ai/nodes/plan/PathfindingNode.java new file mode 100644 index 00000000..b7f1b610 --- /dev/null +++ b/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 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); + } + +} diff --git a/src/main/java/electrosphere/server/ai/trees/creature/MoveToTree.java b/src/main/java/electrosphere/server/ai/trees/creature/MoveToTree.java index 9e79166f..fe1322f0 100644 --- a/src/main/java/electrosphere/server/ai/trees/creature/MoveToTree.java +++ b/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)) ) diff --git a/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellManager.java b/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellManager.java index f53c5a5c..9997fddb 100644 --- a/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellManager.java +++ b/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 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 points = Globals.pathfinder.solve(trackingMeshData, start, end); + return points; } /** diff --git a/src/main/java/electrosphere/server/pathfinding/NavMeshConstructor.java b/src/main/java/electrosphere/server/pathfinding/NavMeshConstructor.java index 649a6174..66bbaaa0 100644 --- a/src/main/java/electrosphere/server/pathfinding/NavMeshConstructor.java +++ b/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; - - /** - * Size of a recast agent - */ - static final float RECAST_AGENT_SIZE = 1.0f; + static final float RECAST_CELL_HEIGHT = 0.2f; /** * 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, - 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(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); + + //build polymesh + RecastBuilderResult recastBuilderResult = NavMeshConstructor.buildPolymesh(geomData); 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" + + ""; + } + } diff --git a/src/main/java/electrosphere/server/pathfinding/Pathfinder.java b/src/main/java/electrosphere/server/pathfinding/Pathfinder.java index f766587e..6b9533ce 100644 --- a/src/main/java/electrosphere/server/pathfinding/Pathfinder.java +++ b/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 solve(MeshData mesh, Vector3d startPos, Vector3d endPos){ List rVal = new LinkedList(); + 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 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 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> 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" + + "" + ; + } + } diff --git a/src/main/java/electrosphere/server/pathfinding/PathingProgressiveData.java b/src/main/java/electrosphere/server/pathfinding/PathingProgressiveData.java new file mode 100644 index 00000000..b16964d3 --- /dev/null +++ b/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 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 points){ + this.points = points; + this.currentPoint = 0; + } + + /** + * Gets the points that define the path + * @return The points + */ + public List getPoints() { + return points; + } + + /** + * Sets the points that define the path + * @param points The points + */ + public void setPoints(List 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; + } + + + +} diff --git a/src/main/java/electrosphere/util/math/GeomUtils.java b/src/main/java/electrosphere/util/math/GeomUtils.java index 098e7aef..4b6813fe 100644 --- a/src/main/java/electrosphere/util/math/GeomUtils.java +++ b/src/main/java/electrosphere/util/math/GeomUtils.java @@ -510,5 +510,65 @@ 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; + } } diff --git a/src/test/java/electrosphere/server/pathfinding/NavMeshConstructorTests.java b/src/test/java/electrosphere/server/pathfinding/NavMeshConstructorTests.java new file mode 100644 index 00000000..ce80efc0 --- /dev/null +++ b/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); + } + +}