diff --git a/docs/src/progress/renderertodo.md b/docs/src/progress/renderertodo.md index 796a6997..7e644b3c 100644 --- a/docs/src/progress/renderertodo.md +++ b/docs/src/progress/renderertodo.md @@ -1652,6 +1652,9 @@ Major pathfinding work -- breaking MoteToTree Pathfinding tiling work Refactor recast pathfinding classes +(05/03/2025) +Fix voxel pathfinding logic + diff --git a/src/main/java/electrosphere/client/ui/menu/debug/ImGuiAI.java b/src/main/java/electrosphere/client/ui/menu/debug/ImGuiAI.java index a4083d5f..675dc26f 100644 --- a/src/main/java/electrosphere/client/ui/menu/debug/ImGuiAI.java +++ b/src/main/java/electrosphere/client/ui/menu/debug/ImGuiAI.java @@ -1,11 +1,27 @@ package electrosphere.client.ui.menu.debug; +import java.util.Arrays; +import java.util.LinkedList; +import java.util.List; + +import org.joml.Quaterniond; +import org.joml.Vector3d; + import electrosphere.engine.Globals; +import electrosphere.engine.assetmanager.AssetDataStrings; +import electrosphere.entity.ClientEntityUtils; import electrosphere.entity.Entity; +import electrosphere.entity.EntityCreationUtils; +import electrosphere.entity.EntityUtils; +import electrosphere.renderer.actor.Actor; +import electrosphere.renderer.actor.ActorTextureMask; import electrosphere.renderer.ui.imgui.ImGuiWindow; import electrosphere.renderer.ui.imgui.ImGuiWindow.ImGuiWindowCallback; import electrosphere.server.ai.AI; +import electrosphere.server.datacell.gridded.GriddedDataCellManager; import electrosphere.server.datacell.utils.EntityLookupUtils; +import electrosphere.server.pathfinding.voxel.VoxelPathfinder; +import electrosphere.server.pathfinding.voxel.VoxelPathfinder.PathfinderNode; import imgui.ImGui; /** @@ -13,14 +29,31 @@ import imgui.ImGui; */ public class ImGuiAI { - //window for viewing information about the ai state + /** + * window for viewing information about the ai state + */ protected static ImGuiWindow aiWindow; + /** + * The number of iterations to step through + */ + static int numIterations = 1; + + /** + * The position that was solved for to display + */ + static Vector3d solvedPosition = new Vector3d(); + + /** + * The entity to display debug info with + */ + static List displayEntity = new LinkedList(); + /** * Creates the windows in this file */ protected static void createAIWindows(){ - createAIDebugWindow(); + ImGuiAI.createAIDebugWindow(); } /** @@ -51,9 +84,42 @@ public class ImGuiAI { if(ImGui.collapsingHeader("Statuses")){ for(AI ai : Globals.aiManager.getAIList()){ - ImGui.text(ai.getParent().getId() + " - " + ai.getStatus()); + if(ImGui.collapsingHeader(ai.getParent().getId() + " - " + ai.getStatus())){ + if(ImGui.button("Draw current pathing")){ + throw new Error("Unsupported currently!"); + } + } } } + + if(ImGui.collapsingHeader("Debug Player AI")){ + if(ImGui.button("Reset iterations")){ + numIterations = 1; + } + if(ImGui.button("Increase pathfinding Iteration Cap (" + numIterations + ")")){ + numIterations = numIterations + 1; + VoxelPathfinder voxelPathfinder = new VoxelPathfinder(); + GriddedDataCellManager griddedDataCellManager = (GriddedDataCellManager)Globals.realmManager.getEntityRealm(serverPlayerEntity).getDataCellManager(); + Vector3d playerPos = new Vector3d(EntityUtils.getPosition(serverPlayerEntity)); + List closedSet = voxelPathfinder.aStarStep(griddedDataCellManager, playerPos, new Vector3d(playerPos).add(10,0,0), 1000, numIterations); + + if(displayEntity.size() > 0){ + for(Entity entity : displayEntity){ + ClientEntityUtils.destroyEntity(entity); + } + } + for(PathfinderNode node : closedSet){ + Entity newEnt = EntityCreationUtils.createClientSpatialEntity(); + EntityCreationUtils.makeEntityDrawable(newEnt, AssetDataStrings.UNITCUBE); + Actor blockCursorActor = EntityUtils.getActor(newEnt); + blockCursorActor.addTextureMask(new ActorTextureMask("cube", Arrays.asList(new String[]{AssetDataStrings.TEXTURE_RED_TRANSPARENT}))); + ClientEntityUtils.initiallyPositionEntity(newEnt, node.getPosition(), new Quaterniond()); + EntityUtils.getScale(newEnt).set(0.4f); + displayEntity.add(newEnt); + } + } + ImGui.text("Current pos: " + solvedPosition); + } } }); diff --git a/src/main/java/electrosphere/server/ai/blackboard/BlackboardKeys.java b/src/main/java/electrosphere/server/ai/blackboard/BlackboardKeys.java index 935155b6..4e8a7e06 100644 --- a/src/main/java/electrosphere/server/ai/blackboard/BlackboardKeys.java +++ b/src/main/java/electrosphere/server/ai/blackboard/BlackboardKeys.java @@ -75,4 +75,9 @@ public class BlackboardKeys { */ public static final String PATHFINDING_DATA = "pathfindingData"; + /** + * The pathfinding point + */ + public static final String PATHFINDING_POINT = "pathfindingPoint"; + } diff --git a/src/main/java/electrosphere/server/ai/nodes/meta/DataDeleteNode.java b/src/main/java/electrosphere/server/ai/nodes/meta/DataDeleteNode.java new file mode 100644 index 00000000..d0f079a8 --- /dev/null +++ b/src/main/java/electrosphere/server/ai/nodes/meta/DataDeleteNode.java @@ -0,0 +1,31 @@ +package electrosphere.server.ai.nodes.meta; + +import electrosphere.entity.Entity; +import electrosphere.server.ai.blackboard.Blackboard; +import electrosphere.server.ai.nodes.AITreeNode; + +/** + * A node that deletes a key + */ +public class DataDeleteNode implements AITreeNode { + + /** + * The key to push data into + */ + String destinationKey; + + /** + * Constructor + * @param destinationKey The key to push data into + */ + public DataDeleteNode(String destinationKey){ + this.destinationKey = destinationKey; + } + + @Override + public AITreeNodeResult evaluate(Entity entity, Blackboard blackboard) { + blackboard.delete(this.destinationKey); + return AITreeNodeResult.SUCCESS; + } + +} diff --git a/src/main/java/electrosphere/server/ai/nodes/plan/PathfindingNode.java b/src/main/java/electrosphere/server/ai/nodes/plan/PathfindingNode.java index 1a5591c2..107ecef1 100644 --- a/src/main/java/electrosphere/server/ai/nodes/plan/PathfindingNode.java +++ b/src/main/java/electrosphere/server/ai/nodes/plan/PathfindingNode.java @@ -12,6 +12,7 @@ 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.macro.structure.Structure; import electrosphere.server.pathfinding.recast.PathingProgressiveData; /** @@ -20,6 +21,11 @@ import electrosphere.server.pathfinding.recast.PathingProgressiveData; public class PathfindingNode implements AITreeNode { + /** + * The value used to check if the entity is close to a pathing point + */ + public static final double CLOSENESS_CHECK_BOUND = 0.3f; + /** * The blackboard key to lookup the target entity under */ @@ -39,12 +45,19 @@ public class PathfindingNode implements AITreeNode { @Override public AITreeNodeResult evaluate(Entity entity, Blackboard blackboard) { if(!PathfindingNode.hasPathfindingData(blackboard)){ + Object targetRaw = blackboard.get(this.targetEntityKey); Vector3d targetPos = null; - if(this.targetEntityKey != null){ - Entity targetEnt = (Entity)blackboard.get(targetEntityKey); - targetPos = EntityUtils.getPosition(targetEnt); + if(targetRaw == null){ + throw new Error("Target undefined!"); + } + if(targetRaw instanceof Vector3d){ + targetPos = (Vector3d)targetRaw; + } else if(targetRaw instanceof Entity){ + targetPos = EntityUtils.getPosition((Entity)targetRaw); + } else if(targetRaw instanceof Structure){ + targetPos = ((Structure)targetRaw).getPos(); } else { - throw new Error("Target position is null!"); + throw new Error("Unsupported target type " + targetRaw); } Realm realm = Globals.realmManager.getEntityRealm(entity); @@ -53,10 +66,34 @@ public class PathfindingNode implements AITreeNode { Vector3d entityPos = EntityUtils.getPosition(entity); List path = pathfindingManager.findPath(entityPos, targetPos); + path.add(targetPos); PathingProgressiveData pathingProgressiveData = new PathingProgressiveData(path); PathfindingNode.setPathfindingData(blackboard, pathingProgressiveData); } + if(!PathfindingNode.hasPathfindingData(blackboard)){ + throw new Error("Failed to find path! Unhandled"); + } + + Vector3d entityPos = EntityUtils.getPosition(entity); + + PathingProgressiveData pathingProgressiveData = PathfindingNode.getPathfindingData(blackboard); + Vector3d currentPathPos = null; + if(pathingProgressiveData.getCurrentPoint() < pathingProgressiveData.getPoints().size()){ + currentPathPos = pathingProgressiveData.getPoints().get(pathingProgressiveData.getCurrentPoint()); + } + double dist = currentPathPos.distance(entityPos); + while( + currentPathPos != null && + dist < CLOSENESS_CHECK_BOUND && + pathingProgressiveData.getCurrentPoint() < pathingProgressiveData.getPoints().size() - 1 + ){ + pathingProgressiveData.setCurrentPoint(pathingProgressiveData.getCurrentPoint() + 1); + currentPathPos = pathingProgressiveData.getPoints().get(pathingProgressiveData.getCurrentPoint()); + dist = currentPathPos.distance(entityPos); + } + PathfindingNode.setPathfindingPoint(blackboard, currentPathPos); + return AITreeNodeResult.SUCCESS; } @@ -94,5 +131,40 @@ public class PathfindingNode implements AITreeNode { public static void clearPathfindingData(Blackboard blackboard){ blackboard.delete(BlackboardKeys.PATHFINDING_DATA); } + + /** + * Sets the pathfinding point in the blackboard + * @param blackboard The blackboard + * @param pathfindingPoint The pathfinding point + */ + public static void setPathfindingPoint(Blackboard blackboard, Vector3d pathfindingPoint){ + blackboard.put(BlackboardKeys.PATHFINDING_POINT, pathfindingPoint); + } + + /** + * Gets the current pathfinding point + * @param blackboard The blackboard + * @return The pathfinding point if it exists, null otherwise + */ + public static Vector3d getPathfindingPoint(Blackboard blackboard){ + return (Vector3d)blackboard.get(BlackboardKeys.PATHFINDING_POINT); + } + + /** + * Checks if the blackboard has pathfinding point + * @param blackboard The blackboard + * @return true if it has pathfinding point, false otherwise + */ + public static boolean hasPathfindingPoint(Blackboard blackboard){ + return blackboard.has(BlackboardKeys.PATHFINDING_POINT); + } + + /** + * Clears the pathfinding point + * @param blackboard The pathfinding point + */ + public static void clearPathfindingPoint(Blackboard blackboard){ + blackboard.delete(BlackboardKeys.PATHFINDING_POINT); + } } 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 fe1322f0..665e2e05 100644 --- a/src/main/java/electrosphere/server/ai/trees/creature/MoveToTree.java +++ b/src/main/java/electrosphere/server/ai/trees/creature/MoveToTree.java @@ -1,11 +1,13 @@ package electrosphere.server.ai.trees.creature; import electrosphere.entity.state.movement.groundmove.ClientGroundMovementTree.MovementRelativeFacing; +import electrosphere.server.ai.blackboard.BlackboardKeys; import electrosphere.server.ai.nodes.AITreeNode; import electrosphere.server.ai.nodes.actions.move.FaceTargetNode; import electrosphere.server.ai.nodes.actions.move.MoveStartNode; import electrosphere.server.ai.nodes.actions.move.MoveStopNode; import electrosphere.server.ai.nodes.checks.spatial.TargetRangeCheckNode; +import electrosphere.server.ai.nodes.meta.DataDeleteNode; import electrosphere.server.ai.nodes.meta.collections.SelectorNode; import electrosphere.server.ai.nodes.meta.collections.SequenceNode; import electrosphere.server.ai.nodes.meta.decorators.RunnerNode; @@ -29,10 +31,15 @@ public class MoveToTree { * @return The root node of the move-to-target tree */ public static AITreeNode create(double dist, String targetKey){ + if(dist < PathfindingNode.CLOSENESS_CHECK_BOUND){ + throw new Error("Dist less than minimal amount! " + dist); + } return new SelectorNode( new SequenceNode( //check if in range of target new TargetRangeCheckNode(dist, targetKey), + new DataDeleteNode(BlackboardKeys.PATHFINDING_POINT), + new DataDeleteNode(BlackboardKeys.PATHFINDING_DATA), //if in range, stop moving fowards and return SUCCESS new SucceederNode(new MoveStopNode()) ), @@ -40,7 +47,7 @@ public class MoveToTree { //not in range of target, keep moving towards it new SequenceNode( PathfindingNode.createPathEntity(targetKey), - new FaceTargetNode(targetKey), + new FaceTargetNode(BlackboardKeys.PATHFINDING_POINT), new RunnerNode(new MoveStartNode(MovementRelativeFacing.FORWARD)) ) ); diff --git a/src/main/java/electrosphere/server/datacell/ServerWorldData.java b/src/main/java/electrosphere/server/datacell/ServerWorldData.java index 5f469482..2db7b087 100644 --- a/src/main/java/electrosphere/server/datacell/ServerWorldData.java +++ b/src/main/java/electrosphere/server/datacell/ServerWorldData.java @@ -282,8 +282,8 @@ public class ServerWorldData { * @param worldPos The world pos of the chunk * @return The real pos */ - public double convertVoxelToRealSpace(int voxelPos, int worldPos){ - return voxelPos + this.convertWorldToReal(worldPos); + public static double convertVoxelToRealSpace(int voxelPos, int worldPos){ + return voxelPos + ServerWorldData.convertWorldToReal(worldPos); } public double getRelativeLocation(double real, int world){ @@ -299,7 +299,7 @@ public class ServerWorldData { return convertRealToChunkSpace(real); } - public double convertWorldToReal(int world){ + public static double convertWorldToReal(int world){ return convertChunkToRealSpace(world); } @@ -324,14 +324,28 @@ public class ServerWorldData { ); } + /** + * Converts a real-space position to a voxel-space position + * @param position The real-space position + * @return The voxel-space position + */ public static Vector3i convertRealToVoxelSpace(Vector3d position){ return new Vector3i( - (int)Math.floor(position.x - convertChunkToRealSpace(convertRealToChunkSpace(position.x))), - (int)Math.floor(position.y - convertChunkToRealSpace(convertRealToChunkSpace(position.y))), - (int)Math.floor(position.z - convertChunkToRealSpace(convertRealToChunkSpace(position.z))) + ServerWorldData.convertRealToVoxelSpace(position.x), + ServerWorldData.convertRealToVoxelSpace(position.y), + ServerWorldData.convertRealToVoxelSpace(position.z) ); } + /** + * Converts a real-space position to a voxel-space position + * @param x The real-space position + * @return The voxel-space position + */ + public static int convertRealToVoxelSpace(double x){ + return (int)Math.floor(x - convertChunkToRealSpace(convertRealToChunkSpace(x))); + } + /** * Converts a real coordinate to a world space coordinate * @param position The real coordinate diff --git a/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellManager.java b/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellManager.java index f326f6fd..77c29020 100644 --- a/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellManager.java +++ b/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellManager.java @@ -15,10 +15,8 @@ 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; import electrosphere.engine.Globals; import electrosphere.entity.Entity; import electrosphere.entity.EntityCreationUtils; @@ -39,8 +37,7 @@ import electrosphere.server.datacell.interfaces.VoxelCellManager; import electrosphere.server.datacell.physics.PhysicsDataCell; import electrosphere.server.entity.ServerContentManager; import electrosphere.server.entity.serialization.ContentSerialization; -import electrosphere.server.pathfinding.recast.NavMeshConstructor; -import electrosphere.server.pathfinding.recast.RecastPathfinder; +import electrosphere.server.pathfinding.voxel.VoxelPathfinder; import electrosphere.server.physics.block.manager.ServerBlockManager; import electrosphere.server.physics.fluid.manager.ServerFluidChunk; import electrosphere.server.physics.fluid.manager.ServerFluidManager; @@ -163,7 +160,7 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager /** * The pathfinder for the manager */ - RecastPathfinder pathfinder; + VoxelPathfinder pathfinder; /** * Constructor @@ -194,7 +191,7 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager this.serverContentManager + " " ); } - this.pathfinder = new RecastPathfinder(); + this.pathfinder = new VoxelPathfinder(); } /** @@ -367,27 +364,6 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager cell.setBlockChunk(blockChunkData); cell.generatePhysics(); - //create pathfinding mesh - TerrainChunkData terrainMeshData = cell.getTerrainChunkData(); - ServerDataCell serverDataCell = this.groundDataCells.get(key); - GriddedDataCellTrackingData trackingData = this.cellTrackingMap.get(serverDataCell); - if(terrainMeshData.getVertices().length > 0){ - MeshData pathingMeshData = NavMeshConstructor.constructNavmesh(terrainMeshData); - if(pathingMeshData == null){ - throw new Error("Failed to build pathing data from existing vertices!"); - } - pathingMeshData.header.x = worldPos.x; - pathingMeshData.header.y = worldPos.z; - pathingMeshData.header.bmin[0] = worldPos.x; - pathingMeshData.header.bmin[1] = worldPos.y; - pathingMeshData.header.bmin[2] = worldPos.z; - pathingMeshData.header.bmax[0] = worldPos.x + ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; - pathingMeshData.header.bmax[1] = worldPos.y + ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; - pathingMeshData.header.bmax[2] = worldPos.z + ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; - pathfinder.addTile(pathingMeshData); - trackingData.setNavMeshData(pathingMeshData); - } - loadedCellsLock.lock(); posPhysicsMap.put(key, cell); loadedCellsLock.unlock(); @@ -770,8 +746,7 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager Map posPhysicsMap, Map groundDataCells, Map cellTrackingMap, - Realm realm, - RecastPathfinder pathfinder + Realm realm ){ //get data to generate with Vector3d realPos = new Vector3d( @@ -801,9 +776,6 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager ServerEntityUtils.destroyEntity(blockEntity); } - //get advanced tracking data for the data cell - GriddedDataCellTrackingData trackingData = cellTrackingMap.get(dataCell); - generationService.submit(() -> { try { BlockChunkData blockChunkData = realm.getServerWorldData().getServerBlockManager().getChunk(worldPos.x, worldPos.y, worldPos.z); @@ -811,33 +783,12 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager targetCell.setTerrainChunk(terrainChunk); targetCell.setBlockChunk(blockChunkData); - TerrainChunkData terrainMeshData; //create physics entities if(cell != null){ cell.retireCell(); cell.generatePhysics(); - terrainMeshData = cell.getTerrainChunkData(); } else { targetCell.generatePhysics(); - terrainMeshData = targetCell.getTerrainChunkData(); - } - - //create pathfinding mesh - if(terrainMeshData.getVertices().length > 0){ - MeshData pathingMeshData = NavMeshConstructor.constructNavmesh(terrainMeshData); - if(pathingMeshData == null){ - throw new Error("Failed to build pathing data from existing vertices!"); - } - pathingMeshData.header.x = worldPos.x; - pathingMeshData.header.y = worldPos.z; - pathingMeshData.header.bmin[0] = worldPos.x; - pathingMeshData.header.bmin[1] = worldPos.y; - pathingMeshData.header.bmin[2] = worldPos.z; - pathingMeshData.header.bmax[0] = worldPos.x + ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; - pathingMeshData.header.bmax[1] = worldPos.y + ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; - pathingMeshData.header.bmax[2] = worldPos.z + ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; - pathfinder.addTile(pathingMeshData); - trackingData.setNavMeshData(pathingMeshData); } //set ready @@ -887,7 +838,7 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager //generates physics for the cell in a dedicated thread then finally registers loadedCellsLock.lock(); PhysicsDataCell cell = posPhysicsMap.get(key); - GriddedDataCellManager.runPhysicsGenerationThread(localWorldPos,key,cell,this.posPhysicsMap,this.groundDataCells,this.cellTrackingMap,this.parent,this.pathfinder); + GriddedDataCellManager.runPhysicsGenerationThread(localWorldPos,key,cell,this.posPhysicsMap,this.groundDataCells,this.cellTrackingMap,this.parent); loadedCellsLock.unlock(); @@ -924,7 +875,7 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager * @return The ServerTerrainChunk of data at that position, or null if it is out of bounds or otherwise doesn't exist */ public ServerTerrainChunk getChunkAtPosition(Vector3i worldPosition) { - return serverTerrainManager.getChunk(worldPosition.x, worldPosition.y, worldPosition.z, ServerChunkCache.STRIDE_FULL_RES); + return this.getChunkAtPosition(worldPosition.x,worldPosition.y,worldPosition.z); } @Override @@ -1138,18 +1089,14 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager } @Override - public List findPath(Vector3d start, Vector3d end) { + public List findPath(Vector3d start, Vector3d end){ 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 = this.pathfinder.solve(trackingMeshData, start, end); + List points = this.pathfinder.findPath(this, start, end, VoxelPathfinder.DEFAULT_MAX_COST); return points; } @@ -1183,4 +1130,9 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager return rVal; } + @Override + public ServerTerrainChunk getChunkAtPosition(int worldX, int worldY, int worldZ) { + return serverTerrainManager.getChunk(worldX, worldY, worldZ, ServerChunkCache.STRIDE_FULL_RES); + } + } diff --git a/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellTrackingData.java b/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellTrackingData.java index e33b83cf..e00a0180 100644 --- a/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellTrackingData.java +++ b/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellTrackingData.java @@ -1,7 +1,5 @@ package electrosphere.server.datacell.gridded; -import org.recast4j.detour.MeshData; - /** * Data associated with a ServerDataCell by the GriddedDataCellManager */ @@ -17,11 +15,6 @@ public class GriddedDataCellTrackingData { */ double closestPlayer; - /** - * The nav mesh data - */ - MeshData navMeshData; - /** * Gets the distance from the cell to the closest player * @return The distance @@ -37,21 +30,5 @@ public class GriddedDataCellTrackingData { public void setClosestPlayer(double closestPlayer) { this.closestPlayer = closestPlayer; } - - /** - * Gets the nav mesh data - * @return The nav mesh data - */ - public MeshData getNavMeshData() { - return navMeshData; - } - - /** - * Sets the nav mesh data - * @param navMeshData The nav mesh data - */ - public void setNavMeshData(MeshData navMeshData) { - this.navMeshData = navMeshData; - } } diff --git a/src/main/java/electrosphere/server/datacell/interfaces/VoxelCellManager.java b/src/main/java/electrosphere/server/datacell/interfaces/VoxelCellManager.java index bc8e7000..c2d434f6 100644 --- a/src/main/java/electrosphere/server/datacell/interfaces/VoxelCellManager.java +++ b/src/main/java/electrosphere/server/datacell/interfaces/VoxelCellManager.java @@ -34,6 +34,15 @@ public interface VoxelCellManager { */ public ServerTerrainChunk getChunkAtPosition(Vector3i worldPosition); + /** + * Gets the chunk data at a given world position + * @param worldX The world x position + * @param worldY The world y position + * @param worldZ The world z position + * @return The ServerTerrainChunk of data at that position, or null if it is out of bounds or otherwise doesn't exist + */ + public ServerTerrainChunk getChunkAtPosition(int worldX, int worldY, int worldZ); + /** * Edits a single voxel * @param worldPosition The world position of the chunk to edit diff --git a/src/main/java/electrosphere/server/pathfinding/voxel/VoxelPathfinder.java b/src/main/java/electrosphere/server/pathfinding/voxel/VoxelPathfinder.java new file mode 100644 index 00000000..fa26ca56 --- /dev/null +++ b/src/main/java/electrosphere/server/pathfinding/voxel/VoxelPathfinder.java @@ -0,0 +1,789 @@ +package electrosphere.server.pathfinding.voxel; + +import java.util.Collections; +import java.util.HashMap; +import java.util.LinkedList; +import java.util.List; +import java.util.Map; +import java.util.PriorityQueue; +import java.util.stream.Collectors; + +import org.joml.Vector3d; +import org.joml.Vector3i; + +import electrosphere.server.datacell.ServerWorldData; +import electrosphere.server.datacell.interfaces.VoxelCellManager; +import electrosphere.server.physics.terrain.manager.ServerTerrainChunk; +import electrosphere.util.math.HashUtils; + +/** + * finds a path between two points given a voxel manager + */ +public class VoxelPathfinder { + + /** + * Maximum distance to allow searching + */ + public static final double MAX_DIST = 1000; + + /** + * Default maximum cost to tolerate + */ + public static final int DEFAULT_MAX_COST = 12000; + + /** + * The heuristic lookup table + */ + int[][][] heuristic = new int[3][3][3]; + + /** + * Finds a path between two points given a voxel cell manager + * @param voxelCellManager The voxel cell manager + * @param startPoint The start point + * @param endPoint The end point + * @param maxDist + * @return The path if it is solvable, null otherwise + */ + public List findPath(VoxelCellManager voxelCellManager, Vector3d startPoint, Vector3d endPoint, long maxCost){ + List rVal = null; + + if(startPoint.distance(endPoint) > MAX_DIST){ + throw new Error("Distance is outside range provided! " + startPoint.distance(endPoint) + " vs " + MAX_DIST); + } + + //create sets + PriorityQueue openSet = new PriorityQueue(); + Map openSetLookup = new HashMap(); + Map closetSet = new HashMap(); + + //add starting node + PathfinderNode startingNode = new PathfinderNode( + ServerWorldData.convertRealToChunkSpace(startPoint.x), ServerWorldData.convertRealToChunkSpace(startPoint.y), ServerWorldData.convertRealToChunkSpace(startPoint.z), + ServerWorldData.convertRealToVoxelSpace(startPoint.x), ServerWorldData.convertRealToVoxelSpace(startPoint.y), ServerWorldData.convertRealToVoxelSpace(startPoint.z), + 0, 0, 0 + ); + openSet.add(startingNode); + + //structures used throughout iteration + Vector3i worldPos = new Vector3i(); + Vector3i voxelPos = new Vector3i(); + + worldPos.set(ServerWorldData.convertRealToChunkSpace(endPoint)); + voxelPos.set(ServerWorldData.convertRealToVoxelSpace(endPoint)); + long goalHash = HashUtils.hashVoxel( + worldPos.x,worldPos.y,worldPos.z, + voxelPos.x,voxelPos.y,voxelPos.z + ); + + //set heuristic + this.setHeuristic(startPoint, endPoint); + + //main A* loop + this.aStar(voxelCellManager, openSet, openSetLookup, closetSet, endPoint, goalHash, maxCost); + + //string pulling + List stringPulled = this.stringPull(voxelCellManager, closetSet, goalHash); + + rVal = stringPulled.stream().map( + (PathfinderNode node) -> { + Vector3i currWorld = new Vector3i( + node.worldX, + node.worldY, + node.worldZ + ); + Vector3i currVoxel = new Vector3i( + node.voxelX, + node.voxelY, + node.voxelZ + ); + float weight = voxelCellManager.getVoxelWeightAtLocalPosition(currWorld,currVoxel); + // int type = voxelCellManager.getVoxelTypeAtLocalPosition(currWorld,currVoxel); + if(weight < 0){ + weight = 0; + } + currVoxel.y++; + float aboveWeight = voxelCellManager.getVoxelWeightAtLocalPosition(currWorld,currVoxel); + float total = (weight - aboveWeight); + float percentage = weight / total; + return new Vector3d( + ServerWorldData.convertVoxelToRealSpace(node.voxelX, node.worldX), + ServerWorldData.convertVoxelToRealSpace(node.voxelY, node.worldY) + percentage, + ServerWorldData.convertVoxelToRealSpace(node.voxelZ, node.worldZ) + ); + } + ).collect(Collectors.toList()); + + return rVal; + } + + + /** + * Sets the heuristic + * @param start The start pos + * @param end The end pos + */ + private void setHeuristic(Vector3d start, Vector3d end){ + //clear out array + for(int x = 0; x < 3; x++){ + for(int y = 0; y < 3; y++){ + for(int z = 0; z < 3; z++){ + this.heuristic[x][y][z] = 1; + } + } + } + + + if(start.x > end.x){ + for(int x = 0; x < 3; x++){ + for(int y = 0; y < 3; y++){ + this.heuristic[0][x][y] = this.heuristic[0][x][y] + 1; + this.heuristic[1][x][y] = this.heuristic[1][x][y] + 3; + this.heuristic[2][x][y] = this.heuristic[2][x][y] + 5; + } + } + } else { + for(int x = 0; x < 3; x++){ + for(int y = 0; y < 3; y++){ + this.heuristic[0][x][y] = this.heuristic[0][x][y] + 5; + this.heuristic[1][x][y] = this.heuristic[1][x][y] + 3; + this.heuristic[2][x][y] = this.heuristic[2][x][y] + 1; + } + } + } + + if(start.y > end.y){ + for(int x = 0; x < 3; x++){ + for(int y = 0; y < 3; y++){ + this.heuristic[x][0][y] = this.heuristic[x][0][y] + 1; + this.heuristic[x][1][y] = this.heuristic[x][1][y] + 3; + this.heuristic[x][2][y] = this.heuristic[x][2][y] + 5; + } + } + } else { + for(int x = 0; x < 3; x++){ + for(int y = 0; y < 3; y++){ + this.heuristic[x][0][y] = this.heuristic[x][0][y] + 5; + this.heuristic[x][1][y] = this.heuristic[x][1][y] + 3; + this.heuristic[x][2][y] = this.heuristic[x][2][y] + 1; + } + } + } + + if(start.z > end.z){ + for(int x = 0; x < 3; x++){ + for(int y = 0; y < 3; y++){ + this.heuristic[x][y][0] = this.heuristic[x][y][0] + 1; + this.heuristic[x][y][1] = this.heuristic[x][y][1] + 3; + this.heuristic[x][y][2] = this.heuristic[x][y][2] + 5; + } + } + } else { + for(int x = 0; x < 3; x++){ + for(int y = 0; y < 3; y++){ + this.heuristic[x][y][0] = this.heuristic[x][y][0] + 5; + this.heuristic[x][y][1] = this.heuristic[x][y][1] + 3; + this.heuristic[x][y][2] = this.heuristic[x][y][2] + 1; + } + } + } + + + } + + /** + * Solves for the A* closed set + * @param voxelCellManager The voxel manager + * @param openSet The open set + * @param openSetLookup The open set lookup table + * @param closetSet The closet set + * @param goalPos The goal position + * @param goalHash The goal hash + * @param maxCost The max allowable cost + */ + private void aStar( + VoxelCellManager voxelCellManager, + PriorityQueue openSet, + Map openSetLookup, + Map closetSet, + Vector3d goalPos, + long goalHash, + long maxCost + ){ + //tracks whether we've found the goal or not + boolean foundGoal = false; + int countConsidered = 0; + + int chunkPosX = 0; + int chunkPosY = 0; + int chunkPosZ = 0; + + int voxelPosX = 0; + int voxelPosY = 0; + int voxelPosZ = 0; + + while(openSet.size() > 0 && !foundGoal){ + + //pull from open set + PathfinderNode currentNode = openSet.poll(); + long currentCost = currentNode.cost; + openSetLookup.remove(currentNode.hash); + closetSet.put(currentNode.hash, currentNode); + countConsidered++; + + + + //scan all neighbors + for(int x = -1; x <= 1; x++){ + if(foundGoal){ + continue; + } + for(int y = -1; y <= 1; y++){ + if(foundGoal){ + continue; + } + for(int z = -1; z <= 1; z++){ + if(foundGoal){ + continue; + } + if(x == 0 && y == 0 && z == 0){ + continue; + } + + //calculate chunk offsets + voxelPosX = (currentNode.voxelX + x); + voxelPosY = (currentNode.voxelY + y); + voxelPosZ = (currentNode.voxelZ + z); + if(voxelPosX < 0){ + voxelPosX = -1; + } else { + voxelPosX = voxelPosX / ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; + } + if(voxelPosY < 0){ + voxelPosY = -1; + } else { + voxelPosY = voxelPosY / ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; + } + if(voxelPosZ < 0){ + voxelPosZ = -1; + } else { + voxelPosZ = voxelPosZ / ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; + } + //update world position + chunkPosX = currentNode.worldX + voxelPosX; + chunkPosY = currentNode.worldY + voxelPosY; + chunkPosZ = currentNode.worldZ + voxelPosZ; + voxelPosX = (currentNode.voxelX + x + ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET) % ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; + voxelPosY = (currentNode.voxelY + y + ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET) % ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; + voxelPosZ = (currentNode.voxelZ + z + ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET) % ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; + + //error/bounds check + if(chunkPosX < 0 || chunkPosY < 0 || chunkPosZ < 0){ + continue; + } + + + // + //checking if this is the goal + // + + //calculte hash for neighbor pos + long newHash = HashUtils.hashVoxel( + chunkPosX,chunkPosY,chunkPosZ, + voxelPosX,voxelPosY,voxelPosZ + ); + + //check if found goal + if(newHash == goalHash){ + foundGoal = true; + PathfinderNode newNode = new PathfinderNode( + chunkPosX, chunkPosY, chunkPosZ, + voxelPosX, voxelPosY, voxelPosZ, + 0, newHash, currentNode.hash + ); + closetSet.put(goalHash, newNode); + continue; + } + + // + //creating a new node + // + + //it's a solid block + if(!this.isWalkable(voxelCellManager, new Vector3i(chunkPosX,chunkPosY,chunkPosZ), new Vector3i(voxelPosX,voxelPosY,voxelPosZ))){ + continue; + } + + //calculate new cost + //TODO: apply heuristic here + Vector3d currPosReal = new Vector3d( + ServerWorldData.convertVoxelToRealSpace(voxelPosX, chunkPosX), + ServerWorldData.convertVoxelToRealSpace(voxelPosY, chunkPosY), + ServerWorldData.convertVoxelToRealSpace(voxelPosZ, chunkPosZ) + ); + long newCost = currentCost + (int)currPosReal.distance(goalPos); + + //check cost boundary + if(newCost > maxCost){ + continue; + } + + //push to open set + if(!closetSet.containsKey(newHash) && !openSetLookup.containsKey(newHash)){ + PathfinderNode newNode = new PathfinderNode( + chunkPosX, chunkPosY, chunkPosZ, + voxelPosX, voxelPosY, voxelPosZ, + newCost, newHash, currentNode.hash + ); + openSet.add(newNode); + openSetLookup.put(newHash, newNode); + } + } + } + } + + if(openSet.size() < 1){ + throw new Error("Open set ran out of nodes! " + countConsidered); + } + } + + if(!foundGoal){ + throw new Error("Failed to find goal " + countConsidered); + } + } + + /** + * Steps through the A* solver to a given number of closet set items + * @param voxelCellManager The voxel manager + * @param startPoint The start point + * @param endPoint The end point + * @param maxCost The max allowable cost + * @param closetSetSize The size of the closed set to stop at + * @return The closed set from iteration through A* + */ + public List aStarStep(VoxelCellManager voxelCellManager, Vector3d startPoint, Vector3d endPoint, long maxCost, int closetSetSize){ + if(startPoint.distance(endPoint) > MAX_DIST){ + throw new Error("Distance is outside range provided! " + startPoint.distance(endPoint) + " vs " + MAX_DIST); + } + + List rVal = new LinkedList(); + + //create sets + PriorityQueue openSet = new PriorityQueue(); + Map openSetLookup = new HashMap(); + Map closetSet = new HashMap(); + + //add starting node + PathfinderNode startingNode = new PathfinderNode( + ServerWorldData.convertRealToChunkSpace(startPoint.x), ServerWorldData.convertRealToChunkSpace(startPoint.y), ServerWorldData.convertRealToChunkSpace(startPoint.z), + ServerWorldData.convertRealToVoxelSpace(startPoint.x), ServerWorldData.convertRealToVoxelSpace(startPoint.y), ServerWorldData.convertRealToVoxelSpace(startPoint.z), + 0, 0, 0 + ); + openSet.add(startingNode); + + //structures used throughout iteration + int chunkPosX = 0; + int chunkPosY = 0; + int chunkPosZ = 0; + + int voxelPosX = 0; + int voxelPosY = 0; + int voxelPosZ = 0; + + Vector3i endWorldPos = ServerWorldData.convertRealToChunkSpace(endPoint); + Vector3i endVoxelPos = ServerWorldData.convertRealToVoxelSpace(endPoint); + + long goalHash = HashUtils.hashVoxel( + endWorldPos.x,endWorldPos.y,endWorldPos.z, + endVoxelPos.x,endVoxelPos.y,endVoxelPos.z + ); + + //set heuristic + this.setHeuristic(startPoint, endPoint); + + //tracks whether we've found the goal or not + boolean foundGoal = false; + int countConsidered = 0; + + while(openSet.size() > 0 && !foundGoal && rVal.size() < closetSetSize + 1){ + + //pull from open set + PathfinderNode currentNode = openSet.poll(); + long currentCost = currentNode.cost; + openSetLookup.remove(currentNode.hash); + closetSet.put(currentNode.hash, currentNode); + rVal.add(currentNode); + countConsidered++; + + + + //scan all neighbors + for(int x = -1; x <= 1; x++){ + if(foundGoal){ + continue; + } + for(int y = -1; y <= 1; y++){ + if(foundGoal){ + continue; + } + for(int z = -1; z <= 1; z++){ + if(foundGoal){ + continue; + } + if(x == 0 && y == 0 && z == 0){ + continue; + } + + //calculate chunk offsets + voxelPosX = (currentNode.voxelX + x); + voxelPosY = (currentNode.voxelY + y); + voxelPosZ = (currentNode.voxelZ + z); + if(voxelPosX < 0){ + voxelPosX = -1; + } else { + voxelPosX = voxelPosX / ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; + } + if(voxelPosY < 0){ + voxelPosY = -1; + } else { + voxelPosY = voxelPosY / ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; + } + if(voxelPosZ < 0){ + voxelPosZ = -1; + } else { + voxelPosZ = voxelPosZ / ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; + } + //update world position + chunkPosX = currentNode.worldX + voxelPosX; + chunkPosY = currentNode.worldY + voxelPosY; + chunkPosZ = currentNode.worldZ + voxelPosZ; + voxelPosX = (currentNode.voxelX + x + ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET) % ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; + voxelPosY = (currentNode.voxelY + y + ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET) % ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; + voxelPosZ = (currentNode.voxelZ + z + ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET) % ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET; + + //error/bounds check + if(chunkPosX < 0 || chunkPosY < 0 || chunkPosZ < 0){ + continue; + } + + + // + //checking if this is the goal + // + + //calculte hash for neighbor pos + long newHash = HashUtils.hashVoxel( + chunkPosX,chunkPosY,chunkPosZ, + voxelPosX,voxelPosY,voxelPosZ + ); + + //check if found goal + if(newHash == goalHash){ + foundGoal = true; + PathfinderNode newNode = new PathfinderNode( + chunkPosX, chunkPosY, chunkPosZ, + voxelPosX, voxelPosY, voxelPosZ, + 0, newHash, currentNode.hash + ); + closetSet.put(goalHash, newNode); + rVal.add(currentNode); + continue; + } + + // + //creating a new node + // + + //it's a solid block + if(!this.isWalkable(voxelCellManager, new Vector3i(chunkPosX,chunkPosY,chunkPosZ), new Vector3i(voxelPosX,voxelPosY,voxelPosZ))){ + continue; + } + + //calculate new cost + //TODO: apply heuristic here + Vector3d currPosReal = new Vector3d( + ServerWorldData.convertVoxelToRealSpace(voxelPosX, chunkPosX), + ServerWorldData.convertVoxelToRealSpace(voxelPosY, chunkPosY), + ServerWorldData.convertVoxelToRealSpace(voxelPosZ, chunkPosZ) + ); + long newCost = currentCost + (int)currPosReal.distance(endPoint); + + //check cost boundary + if(newCost > maxCost){ + continue; + } + + //push to open set + if(!closetSet.containsKey(newHash) && !openSetLookup.containsKey(newHash)){ + PathfinderNode newNode = new PathfinderNode( + chunkPosX, chunkPosY, chunkPosZ, + voxelPosX, voxelPosY, voxelPosZ, + newCost, newHash, currentNode.hash + ); + openSet.add(newNode); + openSetLookup.put(newHash, newNode); + } + } + } + } + + if(openSet.size() < 1){ + throw new Error("Open set ran out of nodes! " + countConsidered); + } + } + + return rVal; + } + + /** + * Performs string pulling on the closed set to get the optimized path + * @param voxelCellManager The voxel cell manager + * @param closetSet The closed set + * @param goalHash The goal hash + * @return The list of minimal points + */ + private List stringPull( + VoxelCellManager voxelCellManager, + Map closetSet, + long goalHash + ){ + List rVal = new LinkedList(); + //reverse the raw path + List pathRaw = new LinkedList(); + PathfinderNode current = closetSet.get(goalHash); + if(current == null){ + throw new Error("End node not stored in closed set!"); + } + while(current.prevNode != current.hash){ + pathRaw.add(current); + current = closetSet.get(current.prevNode); + if(current == null){ + throw new Error("Node undefined!"); + } + } + Collections.reverse(pathRaw); + + int i = 0; + while(i < pathRaw.size() - 1){ + rVal.add(pathRaw.get(i)); + int j = i + 1; + for(; j < pathRaw.size() - 1; j++){ + PathfinderNode from = pathRaw.get(i); + PathfinderNode to = pathRaw.get(j); + if(this.hasLineOfSight(voxelCellManager, from, to)) { + break; + } + } + i = j; + } + + return rVal; + } + + /** + * Performs LOS checks for string pulling + * @param voxelCellManager The voxel cell manager + * @param from The cell to start string pulling from + * @param to THe cell to start string pulling towards + * @return true if we can keep string pulling, false otherwise + */ + private boolean hasLineOfSight( + VoxelCellManager voxelCellManager, + PathfinderNode from, + PathfinderNode to + ){ + double x0 = ServerWorldData.convertVoxelToRealSpace(from.voxelX,from.worldX); + double y0 = ServerWorldData.convertVoxelToRealSpace(from.voxelY,from.worldY); + double z0 = ServerWorldData.convertVoxelToRealSpace(from.voxelZ,from.worldZ); + + double x1 = ServerWorldData.convertVoxelToRealSpace(to.voxelX,to.worldX); + double y1 = ServerWorldData.convertVoxelToRealSpace(to.voxelY,to.worldY); + double z1 = ServerWorldData.convertVoxelToRealSpace(to.voxelZ,to.worldZ); + + double dx = Math.abs(x1 - x0); + double dy = Math.abs(y1 - x0); + double dz = Math.abs(z1 - z0); + int sx = Double.compare(x1, x0); + int sy = Double.compare(y1, y0); + int sz = Double.compare(z1, z0); + + Vector3d realPos = new Vector3d(); + double err1, err2; + if(dx >= dy && dx >= dz){ + err1 = 2 * dy - dx; + err2 = 2 * dz - dx; + for(; x0 != x1; x0 += sx){ + realPos.set(x0,y0,z0); + if(!this.isWalkable( + voxelCellManager, + ServerWorldData.convertRealToChunkSpace(realPos), + ServerWorldData.convertRealToVoxelSpace(realPos) + )){ + return false; + } + if(err1 > 0){ + y0 += sy; + err1 -= 2 * dx; + } + if(err2 > 0){ + z0 += sz; + err2 -= 2 * dx; + } + err1 += 2 * dy; + err2 += 2 * dz; + } + } else if(dy >= dx && dy >= dz){ + err1 = 2 * dx - dy; + err2 = 2 * dz - dy; + for(; y0 != y1; y0 += sy){ + realPos.set(x0,y0,z0); + if(!this.isWalkable( + voxelCellManager, + ServerWorldData.convertRealToChunkSpace(realPos), + ServerWorldData.convertRealToVoxelSpace(realPos) + )){ + return false; + } + if(err1 > 0){ + x0 += sx; + err1 -= 2 * dy; + } + if(err2 > 0){ + z0 += sz; + err2 -= 2 * dy; + } + err1 += 2 * dx; + err2 += 2 * dz; + } + } else { + err1 = 2 * dy - dz; + err2 = 2 * dx - dz; + for(; z0 != z1; z0 += sz){ + realPos.set(x0,y0,z0); + if(!this.isWalkable( + voxelCellManager, + ServerWorldData.convertRealToChunkSpace(realPos), + ServerWorldData.convertRealToVoxelSpace(realPos) + )){ + return false; + } + if(err1 > 0){ + y0 += sy; + err1 -= 2 * dz; + } + if(err2 > 0){ + x0 += sx; + err2 -= 2 * dz; + } + err1 += 2 * dy; + err2 += 2 * dx; + } + } + return true; + } + + /** + * Checks if a voxel is passable + * @param voxelCellManager The voxel cell manager + * @param chunkPos The position of the chunk + * @param voxelPos The position of the boxel + * @return true if it is passable, false otherwise + */ + private boolean isWalkable(VoxelCellManager voxelCellManager, Vector3i chunkPos, Vector3i voxelPos){ + int voxelType = voxelCellManager.getVoxelTypeAtLocalPosition(chunkPos, voxelPos); + float voxelWeight = voxelCellManager.getVoxelWeightAtLocalPosition(chunkPos, voxelPos); + + //get the type of voxel above the current voxel + int aboveChunk = chunkPos.y + ((voxelPos.y + 1) / ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET); + int aboveVoxel = ((voxelPos.y + 1) % ServerTerrainChunk.CHUNK_PLACEMENT_OFFSET); + ServerTerrainChunk chunk = voxelCellManager.getChunkAtPosition(chunkPos.x, aboveChunk, chunkPos.z); + int aboveType = chunk.getType(voxelPos.x, aboveVoxel, voxelPos.z); + float aboveWeight = chunk.getWeight(voxelPos.x, aboveVoxel, voxelPos.z); + + return voxelType != ServerTerrainChunk.VOXEL_TYPE_AIR && voxelWeight > 0 && (aboveType == ServerTerrainChunk.VOXEL_TYPE_AIR || aboveWeight <= 0); + } + + /** + * A node to use during searching + */ + public static class PathfinderNode implements Comparable { + + /** + * The world x position + */ + int worldX; + + /** + * The world y position + */ + int worldY; + + /** + * The world z position + */ + int worldZ; + + /** + * The voxel x position + */ + int voxelX; + + /** + * The voxel y position + */ + int voxelY; + + /** + * The voxel z position + */ + int voxelZ; + + /** + * Cost to get to this node + */ + long cost = 0; + + /** + * The hash of this node + */ + long hash = 0; + + /** + * The previous node + */ + long prevNode = 0; + + public PathfinderNode( + int worldX, int worldY, int worldZ, + int voxelX, int voxelY, int voxelZ, + long cost, long hash, long prevNode + ){ + this.worldX = worldX; + this.worldY = worldY; + this.worldZ = worldZ; + this.voxelX = voxelX; + this.voxelY = voxelY; + this.voxelZ = voxelZ; + this.cost = cost; + this.hash = hash; + this.prevNode = prevNode; + } + + @Override + public int compareTo(PathfinderNode o) { + return (int)(this.cost - o.cost); + } + + /** + * Gets the position of the node + * @return The position of the node + */ + public Vector3d getPosition(){ + return new Vector3d( + ServerWorldData.convertVoxelToRealSpace(voxelX, worldX), + ServerWorldData.convertVoxelToRealSpace(voxelY, worldY), + ServerWorldData.convertVoxelToRealSpace(voxelZ, worldZ) + ); + } + } + +} diff --git a/src/main/java/electrosphere/util/math/HashUtils.java b/src/main/java/electrosphere/util/math/HashUtils.java index 29b5dd61..8ecb0737 100644 --- a/src/main/java/electrosphere/util/math/HashUtils.java +++ b/src/main/java/electrosphere/util/math/HashUtils.java @@ -49,6 +49,20 @@ public class HashUtils { return ((long) x) | ((long) y << SHIFT_Y) | ((long) z << SHIFT_Z); } + /** + * Hashes a voxel position + * @param chunkX The chunk x position + * @param chunkY The chunk y position + * @param chunkZ The chunk z position + * @param voxelX THe voxel x position + * @param voxelY THe voxel y position + * @param voxelZ THe voxel z position + * @return The resultant hashed long + */ + public static long hashVoxel(int chunkX, int chunkY, int chunkZ, int voxelX, int voxelY, int voxelZ){ + return ((long)chunkX | ((long) chunkY << 8) | ((long) chunkZ << 16) | ((long) voxelX << 24) | ((long) voxelY << 32) | ((long) voxelZ << 40)); + } + /** * Unhashes an ivec-hashed value * @param hash The hash