pathfinding work

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
 
 
 

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

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<PathfinderNode> 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<PathfinderNode> rVal = new LinkedList<PathfinderNode>();
+
+        //create sets
+        PriorityQueue<PathfinderNode> openSet = new PriorityQueue<PathfinderNode>();
+        Map<Long,PathfinderNode> openSetLookup = new HashMap<Long,PathfinderNode>();
+        Map<Long,PathfinderNode> closetSet = new HashMap<Long,PathfinderNode>();
+
+        //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

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