@2112-lab/pathfinder/src/GridSystem.js

Pure JavaScript 3D pathfinding algorithm library for industrial plant pipe routing

import { Vector3 } from './Vector3.js';

/**
 * Manages grid operations including voxel conversions, neighbor calculations, and distance metrics
 */
export class GridSystem {
    /**
     * Create a new GridSystem instance
     * @param {number} gridSize - Size of each grid cell in world units
     * @param {number} safetyMargin - Safety margin around obstacles in world units
     */
    constructor(gridSize = 0.5, safetyMargin = 0) {
        this.gridSize = gridSize;
        this.safetyMargin = safetyMargin;
    }

    /**
     * Convert a position to a voxel key
     * @param {Vector3|Object} pos - Position to convert
     * @param {number} pos.x - X coordinate
     * @param {number} pos.y - Y coordinate
     * @param {number} pos.z - Z coordinate
     * @returns {string} Voxel key in format "x,y,z"
     */
    voxelKey(pos) {
        return [
            Math.round(pos.x / this.gridSize),
            Math.round(pos.y / this.gridSize),
            Math.round(pos.z / this.gridSize)
        ].join(',');
    }

    /**
     * Convert a voxel key to a Vector3
     * @param {string} key - Voxel key
     * @returns {Vector3} Vector3 position
     */
    voxelToVec3(key) {
        const [x, y, z] = key.split(',').map(Number);
        return new Vector3(x * this.gridSize, y * this.gridSize, z * this.gridSize);
    }

    /**
     * Get neighboring voxels (axis-aligned)
     * @param {string} vox - Voxel key
     * @returns {string[]} Array of neighboring voxel keys
     */
    getNeighbors(vox) {
        const [x, y, z] = vox.split(',').map(Number);
        return [
            [x+1, y, z], [x-1, y, z],
            [x, y+1, z], [x, y-1, z],
            [x, y, z+1], [x, y, z-1]
        ].map(arr => arr.join(','));
    }

    /**
     * Calculate Manhattan distance between two voxels
     * @param {string} a - First voxel key
     * @param {string} b - Second voxel key
     * @returns {number} Manhattan distance
     */
    manhattan(a, b) {
        const [ax, ay, az] = a.split(',').map(Number);
        const [bx, by, bz] = b.split(',').map(Number);
        return Math.abs(ax-bx) + Math.abs(ay-by) + Math.abs(az-bz);
    }

    /**
     * Calculate the number of voxels needed for a given world distance
     * @param {number} worldDistance - Distance in world units
     * @returns {number} Number of voxels
     */
    worldToVoxelDistance(worldDistance) {
        return Math.ceil(worldDistance / this.gridSize);
    }

    /**
     * Calculate the world distance for a given number of voxels
     * @param {number} voxelCount - Number of voxels
     * @returns {number} Distance in world units
     */
    voxelToWorldDistance(voxelCount) {
        return voxelCount * this.gridSize;
    }

    /**
     * Get the safety margin in voxel units
     * @returns {number} Safety margin in voxels
     */
    getSafetyMarginVoxels() {
        return this.worldToVoxelDistance(this.safetyMargin);
    }

    /**
     * Check if a voxel is within the safety margin of a bounding box
     * @param {string} voxelKey - Voxel key to check
     * @param {Object} bbox - Bounding box
     * @param {Vector3} bbox.min - Minimum coordinates
     * @param {Vector3} bbox.max - Maximum coordinates
     * @returns {boolean} True if the voxel is within the safety margin
     */
    isVoxelInSafetyMargin(voxelKey, bbox) {
        const voxelPos = this.voxelToVec3(voxelKey);
        const margin = this.safetyMargin;

        return (
            voxelPos.x >= bbox.min.x - margin && voxelPos.x <= bbox.max.x + margin &&
            voxelPos.y >= bbox.min.y - margin && voxelPos.y <= bbox.max.y + margin &&
            voxelPos.z >= bbox.min.z - margin && voxelPos.z <= bbox.max.z + margin
        );
    }

    /**
     * Get all voxel keys within a bounding box plus safety margin
     * @param {Object} bbox - Bounding box
     * @param {Vector3} bbox.min - Minimum coordinates
     * @param {Vector3} bbox.max - Maximum coordinates
     * @returns {string[]} Array of voxel keys
     */
    getVoxelsInBoundingBox(bbox) {
        const margin = this.getSafetyMarginVoxels();
        const min = bbox.min;
        const max = bbox.max;

        const voxels = [];
        for (let x = Math.round(min.x / this.gridSize) - margin; x <= Math.round(max.x / this.gridSize) + margin; x++) {
            for (let y = Math.round(min.y / this.gridSize) - margin; y <= Math.round(max.y / this.gridSize) + margin; y++) {
                for (let z = Math.round(min.z / this.gridSize) - margin; z <= Math.round(max.z / this.gridSize) + margin; z++) {
                    voxels.push(`${x},${y},${z}`);
                }
            }
        }
        return voxels;
    }
}