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

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

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

/**
 * Manages tree path calculations and Minimum Spanning Tree (MST) operations
 * @class TreePathManager
 */
export class TreePathManager {
    constructor(gridSystem = null, sceneManager = null) {
        this.gridSystem = gridSystem;
        this.sceneManager = sceneManager;
    }

    /**
     * Calculate distance between two points
     * @param {Vector3} p1 - First point
     * @param {Vector3} p2 - Second point
     * @returns {number} Distance between points
     */
    distance(p1, p2) {
        const dx = p2.x - p1.x;
        const dy = p2.y - p1.y;
        const dz = p2.z - p1.z;
        return Math.sqrt(dx * dx + dy * dy + dz * dz);
    }
    
    /**
     * Find Minimum Spanning Tree using Prim's algorithm
     * @param {Array<Vector3>} points - Array of points to connect
     * @returns {Array<Array<number>>} Array of edges in the MST
     */
    findMST(points) {
        const n = points.length;
        const mst = [];
        const visited = new Set();
        
        // Start with the first point
        visited.add(0);
        
        while (visited.size < n) {
            let minDist = Infinity;
            let minEdge = null;
            
            // Find the minimum edge from visited to unvisited
            for (const i of visited) {
                for (let j = 0; j < n; j++) {
                    if (!visited.has(j)) {
                        const dist = this.distance(points[i], points[j]);
                        if (dist < minDist) {
                            minDist = dist;
                            minEdge = [i, j];
                        }
                    }
                }
            }
            
            if (minEdge) {
                mst.push(minEdge);
                visited.add(minEdge[1]);
            } else {
                break; // Break if we can't find any more edges
            }
        }
        
        return mst;
    }

    /**
     * Find the nearest free voxel to a given position
     * @param {Vector3} position - Position to find free voxel near
     * @param {number} maxSearchRadius - Maximum search radius in voxel units
     * @returns {Vector3|null} Nearest free position or null if none found
     */
    findNearestFreeVoxel(position, maxSearchRadius = 5) {
        if (!this.gridSystem || !this.sceneManager) {
            return position; // Return original position if systems not available
        }

        const originalVoxelKey = this.gridSystem.voxelKey(position);
        
        // Check if original position is free
        if (!this.sceneManager.isVoxelOccupiedByMesh(originalVoxelKey, this.gridSystem.gridSize)) {
            return position;
        }

        // Search in expanding rings around the position
        for (let radius = 1; radius <= maxSearchRadius; radius++) {
            const [x, y, z] = originalVoxelKey.split(',').map(Number);
            
            // Generate all voxels at this radius
            const candidates = [];
            
            // Generate all combinations of offsets at this radius
            for (let dx = -radius; dx <= radius; dx++) {
                for (let dy = -radius; dy <= radius; dy++) {
                    for (let dz = -radius; dz <= radius; dz++) {
                        // Only consider voxels exactly at this radius
                        if (Math.abs(dx) + Math.abs(dy) + Math.abs(dz) === radius) {
                            candidates.push([x + dx, y + dy, z + dz]);
                        }
                    }
                }
            }
            
            // Check each candidate
            for (const [cx, cy, cz] of candidates) {
                const candidateKey = `${cx},${cy},${cz}`;
                if (!this.sceneManager.isVoxelOccupiedByMesh(candidateKey, this.gridSystem.gridSize)) {
                    return this.gridSystem.voxelToVec3(candidateKey);
                }
            }
        }
        
        return null;
    }
    
    /**
     * Find joint point 
     * @param {Array<Vector3>} points - Array of points to connect
     * @returns {Vector3|null} Joint point or null if not possible
     */
    findJointPoint(points) {
        if (points.length <= 2) {
            return null;
        }
        
        // Find MST first
        const mst = this.findMST(points);
        
        if (mst.length === 0) {
            return null;
        }
        
        // Find the best edge in MST
        let bestEdge = null;
        let maxScore = -1;
        
        mst.forEach(([i, j], edgeIndex) => {
            const pointA = points[i];
            const pointB = points[j];
            
            // Calculate edge components
            const dx = Math.abs(pointB.x - pointA.x);
            const dy = Math.abs(pointB.y - pointA.y);
            const dz = Math.abs(pointB.z - pointA.z);
            
            // Calculate total length
            const totalLength = dx + dy + dz;
            
            if (totalLength > 0) {
                // Calculate score
                // Higher score = more orthogonal (closer to being axis-aligned) relative to the total length of the edge
                const maxComponent = Math.max(dx, dy, dz);
                const score = (maxComponent / totalLength) / totalLength;
                
                // Check if this edge has a better orthogonality score
                if (score > maxScore) {
                    maxScore = score;
                    bestEdge = [pointA, pointB];
                }
            }
        });
        
        if (!bestEdge) {
            return null;
        }
        
        // Place joint point at the midpoint of the most orthogonal edge
        const jointPoint = new Vector3(
            (bestEdge[0].x + bestEdge[1].x) * 0.5,
            (bestEdge[0].y + bestEdge[1].y) * 0.5,
            (bestEdge[0].z + bestEdge[1].z) * 0.5
        );
        
        // Snap to 0.5 grid
        const snapToGrid = (value) => Math.round(value * 2) / 2;
        jointPoint.x = snapToGrid(jointPoint.x);
        jointPoint.y = snapToGrid(jointPoint.y);
        jointPoint.z = snapToGrid(jointPoint.z);
        
        // Check if the joint point is in an occupied voxel and move it if necessary
        const freeJointPoint = this.findNearestFreeVoxel(jointPoint);
        
        if (freeJointPoint) {
            return freeJointPoint;
        } else {
            return jointPoint; // Return original if no free position found
        }
    }
}