From e33c57f983bd9decac4628c69981cc2888c1cc0d Mon Sep 17 00:00:00 2001 From: austin Date: Sat, 3 May 2025 19:44:11 -0400 Subject: [PATCH] pathfinding work --- docs/src/progress/renderertodo.md | 2 + .../client/ui/menu/debug/ImGuiAI.java | 23 ++- .../pathfinding/voxel/VoxelPathfinder.java | 186 ++++++++++++++++++ .../electrosphere/util/math/HashUtils.java | 9 +- 4 files changed, 218 insertions(+), 2 deletions(-) diff --git a/docs/src/progress/renderertodo.md b/docs/src/progress/renderertodo.md index 351b2d5f..d776c324 100644 --- a/docs/src/progress/renderertodo.md +++ b/docs/src/progress/renderertodo.md @@ -1655,6 +1655,8 @@ Refactor recast pathfinding classes (05/03/2025) Fix voxel pathfinding logic Remove several usages of concurrent datastructures +Fix block chunk data allocation explosion +Fix pathfinding voxel hashing calculating 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 675dc26f..0d3d696e 100644 --- a/src/main/java/electrosphere/client/ui/menu/debug/ImGuiAI.java +++ b/src/main/java/electrosphere/client/ui/menu/debug/ImGuiAI.java @@ -118,7 +118,28 @@ public class ImGuiAI { displayEntity.add(newEnt); } } - ImGui.text("Current pos: " + solvedPosition); + if(ImGui.button("Draw Open Set (" + 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.aStarStepOpen(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); + } + } } } diff --git a/src/main/java/electrosphere/server/pathfinding/voxel/VoxelPathfinder.java b/src/main/java/electrosphere/server/pathfinding/voxel/VoxelPathfinder.java index fa26ca56..5650644e 100644 --- a/src/main/java/electrosphere/server/pathfinding/voxel/VoxelPathfinder.java +++ b/src/main/java/electrosphere/server/pathfinding/voxel/VoxelPathfinder.java @@ -534,6 +534,192 @@ public class VoxelPathfinder { return rVal; } + /** + * 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 aStarStepOpen(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; + + int iteration = 0; + while(openSet.size() > 0 && !foundGoal && iteration < closetSetSize){ + + //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(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); + } + + iteration++; + } + + rVal.addAll(openSet); + + return rVal; + } + /** * Performs string pulling on the closed set to get the optimized path * @param voxelCellManager The voxel cell manager diff --git a/src/main/java/electrosphere/util/math/HashUtils.java b/src/main/java/electrosphere/util/math/HashUtils.java index 8ecb0737..bc92f2ce 100644 --- a/src/main/java/electrosphere/util/math/HashUtils.java +++ b/src/main/java/electrosphere/util/math/HashUtils.java @@ -60,7 +60,14 @@ public class HashUtils { * @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)); + return ( + ((long)chunkX & 0xFFl) | + ((((long) chunkY) << 8 ) & 0xFF00l) | + ((((long) chunkZ) << 16) & 0xFF0000l) | + ((((long) voxelX) << 24) & 0xFF000000l) | + ((((long) voxelY) << 32) & 0xFF00000000l) | + ((((long) voxelZ) << 40) & 0xFF0000000000l) + ); } /**