@2112-lab/central-plant 0.1.0 → 0.1.2

package/dist/cjs/node_modules/@2112-lab/pathfinder/dist/index.esm.js

@@ -0,0 +1,1448 @@
+'use strict';
+
+Object.defineProperty(exports, '__esModule', { value: true });
+
+/**
+ * A simple 3D vector class for internal use
+ * @class Vector3
+ */
+class Vector3 {
+    /**
+     * Create a new Vector3
+     * @param {number} [x=0] - The x component
+     * @param {number} [y=0] - The y component
+     * @param {number} [z=0] - The z component
+     */
+    constructor(x = 0, y = 0, z = 0) {
+        this.x = x;
+        this.y = y;
+        this.z = z;
+    }
+
+    /**
+     * Create a copy of this vector
+     * @returns {Vector3} A new vector with the same components
+     */
+    clone() {
+        return new Vector3(this.x, this.y, this.z);
+    }
+
+    /**
+     * Convert the vector to an array
+     * @returns {Array<number>} Array representation [x, y, z]
+     */
+    toArray() {
+        return [this.x, this.y, this.z];
+    }
+
+    /**
+     * Normalize this vector (make it unit length)
+     * @returns {Vector3} This vector (for chaining)
+     */
+    normalize() {
+        const length = Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
+        if (length > 0) {
+            this.x /= length;
+            this.y /= length;
+            this.z /= length;
+        }
+        return this;
+    }
+
+    /**
+     * Subtract another vector from this one
+     * @param {Vector3} v - Vector to subtract
+     * @returns {Vector3} This vector (for chaining)
+     */
+    sub(v) {
+        this.x -= v.x;
+        this.y -= v.y;
+        this.z -= v.z;
+        return this;
+    }
+
+    /**
+     * Multiply this vector by a scalar value
+     * @param {number} scalar - The scalar value to multiply by
+     * @returns {Vector3} This vector (for chaining)
+     */
+    multiplyScalar(scalar) {
+        this.x *= scalar;
+        this.y *= scalar;
+        this.z *= scalar;
+        return this;
+    }
+
+    /**
+     * Add another vector to this one
+     * @param {Vector3} v - Vector to add
+     * @returns {Vector3} This vector (for chaining)
+     */
+    add(v) {
+        this.x += v.x;
+        this.y += v.y;
+        this.z += v.z;
+        return this;
+    }
+}
+
+/**
+ * Manages scene operations including object finding, position calculations, and hierarchy traversal
+ */
+class SceneManager {
+    /**
+     * Create a new SceneManager instance
+     * @param {Object} scene - The scene configuration object
+     */
+    constructor(scene) {
+        this.scene = scene;
+    }
+
+    /**
+     * Get the root scene object
+     * @returns {Object} The root scene object
+     */
+    getRoot() {
+        return this.scene.object;
+    }
+
+    /**
+     * Get world position from worldBoundingBox center
+     * @param {Object} object - Scene object
+     * @returns {Vector3} World position
+     */
+    getWorldPosition(object) {
+        if (!object.userData?.worldBoundingBox) {
+            console.log(`[DEBUG] Object ${object.uuid} has no worldBoundingBox in userData`);
+            return new Vector3();
+        }
+
+        const { min, max } = object.userData.worldBoundingBox;
+        return new Vector3(
+            (min[0] + max[0]) / 2,
+            (min[1] + max[1]) / 2,
+            (min[2] + max[2]) / 2
+        );
+    }
+
+    /**
+     * Get bounding box from worldBoundingBox in userData
+     * @param {Object} object - Scene object
+     * @returns {{min: Vector3, max: Vector3}|null} Bounding box or null if no worldBoundingBox
+     */
+    getBoundingBox(object) {
+        if (!object.userData?.worldBoundingBox) {
+            console.log(`[DEBUG] Object ${object.uuid} has no worldBoundingBox in userData`);
+            return null;
+        }
+
+        const { min, max } = object.userData.worldBoundingBox;
+        return {
+            min: new Vector3(min[0], min[1], min[2]),
+            max: new Vector3(max[0], max[1], max[2])
+        };
+    }
+
+    /**
+     * Find an object by UUID in the scene hierarchy
+     * @param {string} uuid - UUID to search for
+     * @returns {Object|null} Found object or null
+     */
+    findObjectByUUID(uuid) {
+        return this._findObjectByUUIDRecursive(this.getRoot(), uuid);
+    }
+
+    /**
+     * Recursive helper for findObjectByUUID
+     * @private
+     * @param {Object} node - Current node to search
+     * @param {string} uuid - UUID to search for
+     * @returns {Object|null} Found object or null
+     */
+    _findObjectByUUIDRecursive(node, uuid) {
+        if (node.uuid === uuid) {
+            return node;
+        }
+        if (node.children) {
+            for (const child of node.children) {
+                const found = this._findObjectByUUIDRecursive(child, uuid);
+                if (found) return found;
+            }
+        }
+        return null;
+    }
+
+    /**
+     * Find the parent object of a given object in the scene hierarchy
+     * @param {Object} target - Target object to find parent for
+     * @returns {Object|null} Parent object or null if not found or if parent is the scene
+     */
+    findParentObject(target) {
+        if (target.userData && Array.isArray(target.userData.direction)) {
+            return { isDirectionParent: true };
+        }
+        
+        return this._findParentObjectRecursive(this.getRoot(), target);
+    }
+
+    /**
+     * Recursive helper for findParentObject
+     * @private
+     * @param {Object} root - Root object to search from
+     * @param {Object} target - Target object to find parent for
+     * @returns {Object|null} Parent object or null
+     */
+    _findParentObjectRecursive(root, target) {
+        if (root.children) {
+            for (const child of root.children) {
+                if (child.uuid === target.uuid) {
+                    if (root.type === 'Scene') {
+                        return null;
+                    }
+                    return root;
+                }
+            }
+            
+            for (const child of root.children) {
+                const parent = this._findParentObjectRecursive(child, target);
+                if (parent) {
+                    return parent;
+                }
+            }
+        }
+        
+        return null;
+    }
+
+    /**
+     * Check if a voxel is occupied by any mesh object
+     * @param {string} voxelKey - Voxel key to check
+     * @param {number} gridSize - Size of each grid cell
+     * @returns {boolean} True if the voxel is occupied by a mesh object
+     */
+    isVoxelOccupiedByMesh(voxelKey, gridSize) {
+        const [x, y, z] = voxelKey.split(',').map(Number);
+        const worldPos = new Vector3(
+            x * gridSize,
+            y * gridSize,
+            z * gridSize
+        );
+        
+        return this._checkObjectOccupancy(this.getRoot(), worldPos);
+    }
+
+    /**
+     * Recursive helper for isVoxelOccupiedByMesh
+     * @private
+     * @param {Object} object - Object to check
+     * @param {Vector3} worldPos - World position to check
+     * @returns {boolean} True if the position is occupied
+     */
+    _checkObjectOccupancy(object, worldPos) {
+        if (object.type === 'Mesh') {
+            const bbox = this.getBoundingBox(object);
+            if (bbox) {
+                if (worldPos.x >= bbox.min.x && worldPos.x <= bbox.max.x &&
+                    worldPos.y >= bbox.min.y && worldPos.y <= bbox.max.y &&
+                    worldPos.z >= bbox.min.z && worldPos.z <= bbox.max.z) {
+                    return true;
+                }
+            }
+        }
+        
+        if (object.children) {
+            for (const child of object.children) {
+                if (this._checkObjectOccupancy(child, worldPos)) return true;
+            }
+        }
+        
+        return false;
+    }
+}
+
+/**
+ * Manages grid operations including voxel conversions, neighbor calculations, and distance metrics
+ */
+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;
+    }
+}
+
+/**
+ * Manages connector-related operations for the pathfinding system
+ * @class ConnectorManager
+ */
+class ConnectorManager {
+    /**
+     * Create a new ConnectorManager instance
+     * @param {Object} config - The configuration object
+     * @param {Array<Object>} config.connections - Array of connections between objects
+     * @param {string} config.connections[].from - UUID of the source object
+     * @param {string} config.connections[].to - UUID of the target object
+     * @param {number} minSegmentLength - Minimum length for straight pipe segments in world units
+     * @param {SceneManager} sceneManager - Instance of SceneManager for scene operations
+     * @param {GridSystem} gridSystem - Instance of GridSystem for grid operations
+     */
+    constructor(config, minSegmentLength, sceneManager, gridSystem) {
+        this.config = config;
+        this.MIN_SEGMENT_LENGTH = minSegmentLength;
+        this.sceneManager = sceneManager;
+        this.gridSystem = gridSystem;
+    }
+
+    /**
+     * Check if an object is a connector by checking if it's mentioned in connections
+     * @param {Object} object - Scene object
+     * @returns {boolean} True if the object is a connector
+     */
+    isConnector(object) {
+        if (!this.config.connections) return false;
+        
+        // Check if the object's UUID appears in any connection
+        return this.config.connections.some(conn => 
+            conn.from === object.uuid || conn.to === object.uuid
+        );
+    }
+
+    /**
+     * Calculate direction vector for a connector
+     * @param {Object} connector - Connector object
+     * @returns {Vector3|null} Direction vector or null if no parent or direction
+     */
+    getConnectorDirection(connector) {
+        // First check if direction is directly specified in userData
+        if (connector.userData && Array.isArray(connector.userData.direction)) {
+            const [x, y, z] = connector.userData.direction;
+            return new Vector3(x, y, z);
+        }
+        
+        // If no direction in userData and no parent, return null
+        if (!connector.parent) {
+            return null;
+        }
+        
+        // For backward compatibility, calculate direction from parent if no userData.direction
+        console.log(`[WARNING] No direction in userData for ${connector.name || connector.uuid} - using parent-based calculation (legacy mode)`);
+        
+        // Get world position of the connector
+        const connectorPos = this.sceneManager.getWorldPosition(connector);
+        
+        // For the parent, use the center of its bounding box if available
+        let parentPos;
+        const parentBBox = this.sceneManager.getBoundingBox(connector.parent);
+        
+        if (parentBBox) {
+            // Calculate the center of the parent's bounding box
+            parentPos = new Vector3(
+                (parentBBox.min.x + parentBBox.max.x) / 2,
+                (parentBBox.min.y + parentBBox.max.y) / 2,
+                (parentBBox.min.z + parentBBox.max.z) / 2
+            );
+        } else {
+            // Fall back to parent's position if bounding box is not available
+            parentPos = this.sceneManager.getWorldPosition(connector.parent);
+        }
+        
+        // Calculate the direction vector from parent to connector
+        const direction = new Vector3(
+            connectorPos.x - parentPos.x,
+            connectorPos.y - parentPos.y,
+            connectorPos.z - parentPos.z
+        ).normalize();
+        
+        // Enforce orthogonality by aligning with the dominant axis
+        const absX = Math.abs(direction.x);
+        const absY = Math.abs(direction.y);
+        const absZ = Math.abs(direction.z);
+        
+        if (absX >= absY && absX >= absZ) {
+            // X is dominant
+            direction.y = 0;
+            direction.z = 0;
+            direction.x = Math.sign(direction.x);
+        } else if (absY >= absX && absY >= absZ) {
+            // Y is dominant
+            direction.x = 0;
+            direction.z = 0;
+            direction.y = Math.sign(direction.y);
+        } else {
+            // Z is dominant
+            direction.x = 0;
+            direction.y = 0;
+            direction.z = Math.sign(direction.z);
+        }
+        
+        return direction;
+    }
+
+    /**
+     * Create a virtual segment for a connector
+     * @param {Object} connector - Connector object
+     * @param {Vector3} connectorPos - World position of the connector
+     * @returns {Object|null} Virtual segment info or null if no parent
+     */
+    createVirtualSegment(connector, connectorPos) {
+        const direction = this.getConnectorDirection(connector);
+        if (!direction) {
+            return null;
+        }
+        
+        // Create a virtual segment starting from the connector position
+        // and extending in the direction vector for MIN_SEGMENT_LENGTH
+        const endPos = new Vector3(
+            connectorPos.x + direction.x * this.MIN_SEGMENT_LENGTH,
+            connectorPos.y + direction.y * this.MIN_SEGMENT_LENGTH,
+            connectorPos.z + direction.z * this.MIN_SEGMENT_LENGTH
+        );
+        
+        // Convert to voxel keys
+        const startKey = this.gridSystem.voxelKey(connectorPos);
+        const endKey = this.gridSystem.voxelKey(endPos);
+        
+        return {
+            startPos: connectorPos,
+            endPos: endPos,
+            direction: direction,
+            startKey: startKey,
+            endKey: endKey
+        };
+    }
+
+    /**
+     * Process all connectors in the scene and mark their virtual segments as occupied
+     * @param {Object} sceneRoot - Root scene object
+     * @param {Set<string>} pathOccupied - Set of occupied voxel keys
+     * @param {string} excludeUUID1 - UUID of first object to exclude
+     * @param {string} excludeUUID2 - UUID of second object to exclude
+     */
+    processConnectors(sceneRoot, pathOccupied, excludeUUID1, excludeUUID2) {
+        this._processConnectorsRecursive(sceneRoot, pathOccupied, excludeUUID1, excludeUUID2);
+    }
+
+    /**
+     * Recursive helper for processConnectors
+     * @private
+     * @param {Object} object - Current object to process
+     * @param {Set<string>} pathOccupied - Set of occupied voxel keys
+     * @param {string} excludeUUID1 - UUID of first object to exclude
+     * @param {string} excludeUUID2 - UUID of second object to exclude
+     */
+    _processConnectorsRecursive(object, pathOccupied, excludeUUID1, excludeUUID2) {
+        // Process current object if it's a mesh
+        if (object.type === 'Mesh') {
+            // Skip the excluded objects
+            if (object.uuid === excludeUUID1 || object.uuid === excludeUUID2) {
+                return;
+            }
+            
+            const connectorPos = this.sceneManager.getWorldPosition(object);
+            const segment = this.createVirtualSegment(object, connectorPos);
+            
+            if (segment) {
+                this._markVirtualSegmentAsOccupied(segment, connectorPos, pathOccupied);
+            }
+        }
+        
+        // Process children recursively
+        if (object.children) {
+            for (const child of object.children) {
+                this._processConnectorsRecursive(child, pathOccupied, excludeUUID1, excludeUUID2);
+            }
+        }
+    }
+
+    /**
+     * Mark voxels in a virtual segment as occupied
+     * @private
+     * @param {Object} segment - Virtual segment info
+     * @param {Vector3} connectorPos - Position of the connector
+     * @param {Set<string>} pathOccupied - Set of occupied voxel keys
+     */
+    _markVirtualSegmentAsOccupied(segment, connectorPos, pathOccupied) {
+        const direction = segment.direction;
+        const distance = this.gridSystem.worldToVoxelDistance(this.MIN_SEGMENT_LENGTH + (this.gridSystem.safetyMargin * 2));
+        
+        for (let i = 0; i <= distance; i++) {
+            const x = Math.round(connectorPos.x / this.gridSystem.gridSize + direction.x * i);
+            const y = Math.round(connectorPos.y / this.gridSystem.gridSize + direction.y * i);
+            const z = Math.round(connectorPos.z / this.gridSystem.gridSize + direction.z * i);
+            const key = `${x},${y},${z}`;
+            
+            pathOccupied.add(key);
+        }
+    }
+
+    /**
+     * Cluster connections by shared objects
+     * @param {Array<Object>} connections - Array of connections between objects
+     * @returns {Array<Object>} Array of clusters
+     */
+    clusterConnections(connections) {
+        const clusters = new Map(); // Map of object UUID to its cluster
+        const clusterMap = new Map(); // Map of cluster ID to set of object UUIDs
+        let nextClusterId = 0;
+
+        // First pass: create initial clusters for each object
+        connections.forEach(conn => {
+            const { from, to } = conn;
+            
+            // If neither object is in a cluster, create new cluster
+            if (!clusters.has(from) && !clusters.has(to)) {
+                const clusterId = nextClusterId++;
+                clusters.set(from, clusterId);
+                clusters.set(to, clusterId);
+                clusterMap.set(clusterId, new Set([from, to]));
+            }
+            // If only 'from' is in a cluster, add 'to' to that cluster
+            else if (clusters.has(from) && !clusters.has(to)) {
+                const clusterId = clusters.get(from);
+                clusters.set(to, clusterId);
+                clusterMap.get(clusterId).add(to);
+            }
+            // If only 'to' is in a cluster, add 'from' to that cluster
+            else if (!clusters.has(from) && clusters.has(to)) {
+                const clusterId = clusters.get(to);
+                clusters.set(from, clusterId);
+                clusterMap.get(clusterId).add(from);
+            }
+            // If both are in different clusters, merge the clusters
+            else if (clusters.has(from) && clusters.has(to)) {
+                const fromCluster = clusters.get(from);
+                const toCluster = clusters.get(to);
+                
+                if (fromCluster !== toCluster) {
+                    // Merge toCluster into fromCluster
+                    const toClusterObjects = clusterMap.get(toCluster);
+                    toClusterObjects.forEach(objId => {
+                        clusters.set(objId, fromCluster);
+                        clusterMap.get(fromCluster).add(objId);
+                    });
+                    clusterMap.delete(toCluster);
+                }
+            }
+        });
+
+        // Convert clusters to array format
+        const clustersArray = Array.from(clusterMap.entries()).map(([clusterId, objects]) => ({
+            clusterId,
+            objects: Array.from(objects)
+        }));
+        
+        // Filter out clusters containing objects with "GATEWAY" in their UUID
+        const filteredClusters = clustersArray.filter(cluster => 
+            !cluster.objects.some(uuid => uuid.includes('GATEWAY'))
+        );
+        
+        // Enrich clusters with point and direction information
+        filteredClusters.forEach(cluster => {
+            cluster.objectPoints = cluster.objects.map(uuid => {
+                const object = this.sceneManager.findObjectByUUID(uuid);
+                if (object && object.userData.worldBoundingBox) {
+                    const center = this.sceneManager.getWorldPosition(object);
+                    
+                    return {
+                        uuid,
+                        point: center,
+                        direction: object.userData.direction ? 
+                            new Vector3(...object.userData.direction) : null
+                    };
+                }
+                return null;
+            }).filter(point => point !== null);
+
+            // Add displaced points to each cluster
+            cluster.displacedPoints = cluster.objectPoints.map(objPoint => {
+                const displacedPoint = objPoint.direction 
+                    ? objPoint.point.clone().add(objPoint.direction.multiplyScalar(0.5))
+                    : objPoint.point.clone();
+                    
+                return {
+                    uuid: objPoint.uuid,
+                    originalPoint: objPoint.point,
+                    displacedPoint: displacedPoint,
+                    direction: objPoint.direction
+                };
+            });
+        });
+        
+
+        return filteredClusters;
+    }
+}
+
+/**
+ * Manages path-related operations for the pathfinding system
+ * @class PathManager
+ */
+class PathManager {
+    /**
+     * Create a new PathManager instance
+     * @param {SceneManager} sceneManager - Instance of SceneManager for scene operations
+     * @param {GridSystem} gridSystem - Instance of GridSystem for grid operations
+     * @param {ConnectorManager} connectorManager - Instance of ConnectorManager for connector operations
+     * @param {number} minSegmentLength - Minimum length for straight pipe segments in world units
+     * @param {number} astarTimeout - Timeout for A* pathfinding in milliseconds
+     */
+    constructor(sceneManager, gridSystem, connectorManager, minSegmentLength, astarTimeout) {
+        this.sceneManager = sceneManager;
+        this.gridSystem = gridSystem;
+        this.connectorManager = connectorManager;
+        this.MIN_SEGMENT_LENGTH = minSegmentLength;
+        this.ASTAR_TIMEOUT = astarTimeout;
+    }
+
+    /**
+     * A* pathfinding algorithm
+     * @private
+     * @param {string} start - Start voxel key
+     * @param {string} goal - Goal voxel key
+     * @param {Set<string>} occupied - Set of occupied voxel keys
+     * @returns {string[]|null} Path as array of voxel keys or null if no path found
+     */
+    astar(start, goal, occupied) {
+        const open = new Set([start]);
+        const cameFrom = {};
+        const gScore = {[start]: 0};
+        const fScore = {[start]: this.gridSystem.manhattan(start, goal)};
+        
+        // Add timeout to prevent infinite loops
+        const startTime = Date.now();
+
+        while (open.size > 0) {
+            // Check for timeout
+            if (Date.now() - startTime > this.ASTAR_TIMEOUT) {
+                console.log('[Warning] Pathfinding timed out');
+                return null;
+            }
+
+            // Get node in open with lowest fScore
+            let current = null;
+            let minF = Infinity;
+            for (const node of open) {
+                if (fScore[node] < minF) {
+                    minF = fScore[node];
+                    current = node;
+                }
+            }
+            if (current === goal) {
+                // Reconstruct path
+                const path = [current];
+                while (cameFrom[current]) {
+                    current = cameFrom[current];
+                    path.unshift(current);
+                }
+                return path;
+            }
+            open.delete(current);
+            for (const neighbor of this.gridSystem.getNeighbors(current)) {
+                if (occupied.has(neighbor)) continue;
+                const tentativeG = gScore[current] + 1;
+                if (tentativeG < (gScore[neighbor] ?? Infinity)) {
+                    cameFrom[neighbor] = current;
+                    gScore[neighbor] = tentativeG;
+                    fScore[neighbor] = tentativeG + this.gridSystem.manhattan(neighbor, goal);
+                    open.add(neighbor);
+                }
+            }
+        }
+        return null;
+    }
+
+    /**
+     * Find a path respecting virtual segments
+     * @private
+     * @param {string} startKey - Start voxel key
+     * @param {string} endKey - End voxel key
+     * @param {Set<string>} occupied - Set of occupied voxel keys
+     * @param {Object} startSegment - Virtual segment for start connector (optional)
+     * @param {Object} endSegment - Virtual segment for end connector (optional)
+     * @returns {string[]|null} Path as array of voxel keys or null if no path found
+     */
+    findPathWithVirtualSegments(startKey, endKey, occupied, startSegment, endSegment) {
+        // If we have virtual segments, we need to ensure the path starts and ends with them
+        let actualStartKey = startKey;
+        let actualEndKey = endKey;
+        
+        // Create a copy of occupied set to avoid modifying the original
+        const pathOccupied = new Set(occupied);
+        
+        // If we have a start segment, use its end as the actual start for pathfinding
+        // and mark the segment as occupied
+        if (startSegment) {
+            actualStartKey = startSegment.endKey;
+            // Mark all voxels in the start segment as occupied
+            const startPos = startSegment.startPos;
+            const direction = startSegment.direction;
+            const distance = this.gridSystem.worldToVoxelDistance(this.MIN_SEGMENT_LENGTH);
+            // Note: we go up to distance-1 to exclude the connecting end point
+            for (let i = 0; i < distance; i++) {
+                const x = Math.round(startPos.x / this.gridSystem.gridSize + direction.x * i);
+                const y = Math.round(startPos.y / this.gridSystem.gridSize + direction.y * i);
+                const z = Math.round(startPos.z / this.gridSystem.gridSize + direction.z * i);
+                const key = `${x},${y},${z}`;
+                pathOccupied.add(key);
+            }
+        }
+        
+        // If we have an end segment, use its end as the actual end for pathfinding
+        // and mark the segment as occupied
+        if (endSegment) {
+            actualEndKey = endSegment.endKey;
+            // Mark all voxels in the end segment as occupied
+            const endPos = endSegment.startPos;
+            const direction = endSegment.direction;
+            const distance = this.gridSystem.worldToVoxelDistance(this.MIN_SEGMENT_LENGTH);
+            // Note: we go up to distance-1 to exclude the connecting end point
+            for (let i = 0; i < distance; i++) {
+                const x = Math.round(endPos.x / this.gridSystem.gridSize + direction.x * i);
+                const y = Math.round(endPos.y / this.gridSystem.gridSize + direction.y * i);
+                const z = Math.round(endPos.z / this.gridSystem.gridSize + direction.z * i);
+                const key = `${x},${y},${z}`;
+                pathOccupied.add(key);
+            }
+        }
+        
+        // Find a path between the actual start and end
+        const middlePath = this.astar(actualStartKey, actualEndKey, pathOccupied);
+        
+        if (!middlePath) {
+            return null;
+        }
+        
+        // Construct the full path
+        let fullPath = [];
+        
+        // Add the start segment if we have one
+        if (startSegment) {
+            fullPath.push(startSegment.startKey);
+        }
+        
+        // Add the middle path
+        fullPath = fullPath.concat(middlePath);
+        
+        // Add the end segment if we have one
+        if (endSegment) {
+            fullPath.push(endSegment.startKey);
+        }
+        
+        return fullPath;
+    }
+
+    /**
+     * Mark all mesh objects' voxels as occupied in the scene
+     * @private
+     * @param {Object} sceneRoot - Root scene object
+     * @returns {Object} Object containing occupied sets and maps
+     */
+    markAllMeshesAsOccupiedVoxels(sceneRoot) {
+        const occupied = new Set();
+        const occupiedByUUID = new Map();
+
+        const processObject = (object) => {
+            // Skip non-mesh objects
+            if (object.type !== 'Mesh') return;
+                        
+            // Get object's bounding box
+            const bbox = this.sceneManager.getBoundingBox(object);
+            if (!bbox) return;
+            
+            // Get all voxels in the bounding box plus safety margin
+            const voxels = this.gridSystem.getVoxelsInBoundingBox(bbox);
+            
+            // Create a set for this object's occupied voxels
+            const objectOccupied = new Set(voxels);
+            
+            // Add all voxels to the global occupied set
+            for (const key of voxels) {
+                occupied.add(key);
+            }
+            
+            // Store the object's occupied voxels in the map
+            occupiedByUUID.set(object.uuid, objectOccupied);
+
+            // Process children recursively
+            if (object.children) {
+                for (const child of object.children) {
+                    processObject(child);
+                }
+            }
+        };
+
+        // Process all objects in the scene's children array
+        if (sceneRoot.children) {
+            for (const object of sceneRoot.children) {
+                processObject(object);
+            }
+        }
+
+        return { occupied, occupiedByUUID };
+    }
+
+    /**
+     * Process a single connection and find its path
+     * @private
+     * @param {Object} connection - Connection object
+     * @param {Set<string>} occupied - Set of occupied voxel keys
+     * @param {Map<string, Set<string>>} occupiedByUUID - Map of occupied voxels by UUID
+     * @returns {Object|null} Path result object or null if connection is invalid
+     */
+    processConnection(connection, occupied, occupiedByUUID) {        
+        const fromObject = this.sceneManager.findObjectByUUID(connection.from);
+        const toObject = this.sceneManager.findObjectByUUID(connection.to);
+        
+        if (!fromObject || !toObject) {
+            console.log(`[ERROR] Could not find objects for connection ${connection.from} - ${connection.to}`);
+            return null;
+        }
+
+        // Verify that both objects are meshes
+        if (fromObject.type !== 'Mesh' || toObject.type !== 'Mesh') {
+            console.log(`[ERROR] Connection endpoints must be meshes: ${fromObject.type} - ${toObject.type}`);
+            return null;
+        }
+
+        // Store the parent for each object to access later
+        fromObject.parent = this.sceneManager.findParentObject(fromObject);
+        toObject.parent = this.sceneManager.findParentObject(toObject);
+
+        // Get world positions for both objects
+        const worldPos1 = this.sceneManager.getWorldPosition(fromObject);
+        const worldPos2 = this.sceneManager.getWorldPosition(toObject);
+        
+        // Create virtual segments for both connectors
+        const startSegment = this.connectorManager.createVirtualSegment(fromObject, worldPos1);
+        const endSegment = this.connectorManager.createVirtualSegment(toObject, worldPos2);
+
+        // Create a copy of occupied set for this path
+        const pathOccupied = new Set(occupied);
+        
+        // Debug copy 1: Initial state after copying from global occupied
+        const debugOccupiedInitial = Array.from(pathOccupied)
+            .map(key => this.gridSystem.voxelToVec3(key));
+        
+        // Get bounding boxes for the two objects
+        const fromBbox = this.sceneManager.getBoundingBox(fromObject);
+        const toBbox = this.sceneManager.getBoundingBox(toObject);
+
+        if (!fromBbox || !toBbox) return null;
+        
+        // Clear voxels in bounding boxes only for objects that do NOT have virtual segments
+        if (!startSegment) {
+            const voxels = this.gridSystem.getVoxelsInBoundingBox(fromBbox);
+            for (const key of voxels) {
+                pathOccupied.delete(key);
+            }
+        }
+        
+        if (!endSegment) {
+            const voxels = this.gridSystem.getVoxelsInBoundingBox(toBbox);
+            for (const key of voxels) {
+                pathOccupied.delete(key);
+            }
+        }
+        
+        // Also clear the voxels for the virtual segments if they exist
+        if (startSegment) {
+            const direction = startSegment.direction;
+            const distance = this.gridSystem.worldToVoxelDistance(this.MIN_SEGMENT_LENGTH + (this.gridSystem.safetyMargin * 2));
+            
+            for (let i = 0; i <= distance; i++) {
+                const x = Math.round(worldPos1.x / this.gridSystem.gridSize + direction.x * i);
+                const y = Math.round(worldPos1.y / this.gridSystem.gridSize + direction.y * i);
+                const z = Math.round(worldPos1.z / this.gridSystem.gridSize + direction.z * i);
+                pathOccupied.delete(`${x},${y},${z}`);
+            }
+        }
+        
+        if (endSegment) {
+            const direction = endSegment.direction;
+            const distance = this.gridSystem.worldToVoxelDistance(this.MIN_SEGMENT_LENGTH + (this.gridSystem.safetyMargin * 2));
+            
+            for (let i = 0; i <= distance; i++) {
+                const x = Math.round(worldPos2.x / this.gridSystem.gridSize + direction.x * i);
+                const y = Math.round(worldPos2.y / this.gridSystem.gridSize + direction.y * i);
+                const z = Math.round(worldPos2.z / this.gridSystem.gridSize + direction.z * i);
+                pathOccupied.delete(`${x},${y},${z}`);
+            }
+        }
+        
+        // Process all other connectors in the scene
+        this.connectorManager.processConnectors(this.sceneManager.getRoot(), pathOccupied, fromObject.uuid, toObject.uuid);
+        
+        // Debug copy 3: After clearing virtual segments and adding back previous paths
+        const debugOccupiedAfterSegments = Array.from(pathOccupied)
+            .map(key => this.gridSystem.voxelToVec3(key));
+
+        // Get start and end voxel keys
+        const startKey = this.gridSystem.voxelKey(worldPos1);
+        const endKey = this.gridSystem.voxelKey(worldPos2);
+
+        // Find a path with virtual segments
+        const pathKeys = this.findPathWithVirtualSegments(
+            startKey, 
+            endKey, 
+            pathOccupied, 
+            startSegment, 
+            endSegment
+        );
+        
+        const occupiedVoxels = Array.from(pathOccupied)
+            .map(key => this.gridSystem.voxelToVec3(key));
+        
+        if (pathKeys) {
+            const path = pathKeys.map(key => this.gridSystem.voxelToVec3(key));
+            
+            // Create a set for this path's occupied voxels
+            const pathOccupied = new Set();
+            
+            // Mark path points as occupied for next routes
+            for (const p of path) {
+                const key = this.gridSystem.voxelKey(p);
+                occupied.add(key);
+                pathOccupied.add(key);
+            }
+            
+            // Store the path's occupied voxels in the map with a special key
+            const pathKey = `path_${connection.from}_${connection.to}`;
+            occupiedByUUID.set(pathKey, pathOccupied);
+            
+            return {
+                from: connection.from,
+                to: connection.to,
+                path: path,
+                occupied: occupiedVoxels,
+                debug: {
+                    initial: debugOccupiedInitial,
+                    afterSegments: debugOccupiedAfterSegments
+                },
+                occupiedByUUID: occupiedByUUID // Include the map in the result
+            };
+        } else {
+            console.log(`[ERROR] No orthogonal path found for ${connection.from}-${connection.to}`);
+            return {
+                from: connection.from,
+                to: connection.to,
+                path: null,
+                start: worldPos1,
+                end: worldPos2,
+                occupied: occupiedVoxels,
+                debug: {
+                    initial: debugOccupiedInitial,
+                    afterSegments: debugOccupiedAfterSegments
+                },
+                occupiedByUUID: occupiedByUUID // Include the map in the result
+            };
+        }
+    }
+
+    /**
+     * Find paths for all connections
+     * @param {Array<Object>} connections - Array of connections to process
+     * @returns {Array<Object>} Array of path results
+     */
+    findPaths(connections) {
+        console.log('[DEBUG] Starting findPaths()');
+        
+        // Validate scene structure
+        if (!this.sceneManager.getRoot()) {
+            console.log('[ERROR] Invalid scene structure: missing scene root');
+            return [];
+        }
+
+        // Process all objects in the scene
+        const { occupied, occupiedByUUID } = this.markAllMeshesAsOccupiedVoxels(this.sceneManager.getRoot());
+
+        const paths = [];
+
+        // For each connection in the config, find a path
+        for (const connection of connections) {
+            const result = this.processConnection(connection, occupied, occupiedByUUID);
+            if (result) {
+                paths.push(result);
+            }
+        }
+
+        return paths;
+    }
+}
+
+/**
+ * Manages Steiner tree calculations and Minimum Spanning Tree (MST) operations
+ * @class SteinerTreeManager
+ */
+class SteinerTreeManager {
+    constructor() {}
+
+    /**
+     * 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 Steiner point using Fermat-Torricelli point approximation
+     * @param {Array<Vector3>} points - Array of points to connect
+     * @returns {Vector3|null} Steiner point or null if not possible
+     */
+    findSteinerPoint(points) {
+        if (points.length <= 2) {
+            return null;
+        }
+        
+        // Find MST first
+        const mst = this.findMST(points);
+        
+        if (mst.length === 0) {
+            return null;
+        }
+        
+        // Find the shortest edge in MST
+        let shortestEdge = null;
+        let minEdgeLength = Infinity;
+        
+        mst.forEach(([i, j]) => {
+            const edgeLength = this.distance(points[i], points[j]);
+            if (edgeLength < minEdgeLength) {
+                minEdgeLength = edgeLength;
+                shortestEdge = [points[i], points[j]];
+            }
+        });
+        
+        if (!shortestEdge) {
+            return null;
+        }
+        
+        // Place Steiner point at the midpoint of the shortest edge
+        const steinerPoint = new Vector3(
+            (shortestEdge[0].x + shortestEdge[1].x) * 0.5,
+            (shortestEdge[0].y + shortestEdge[1].y) * 0.5,
+            (shortestEdge[0].z + shortestEdge[1].z) * 0.5
+        );
+        
+        // Snap to 0.5 grid
+        const snapToGrid = (value) => Math.round(value * 2) / 2;
+        steinerPoint.x = snapToGrid(steinerPoint.x);
+        steinerPoint.y = snapToGrid(steinerPoint.y);
+        steinerPoint.z = snapToGrid(steinerPoint.z);
+        
+        return steinerPoint;
+    }
+}
+
+/**
+ * A 3D pathfinding system that finds orthogonal paths between objects
+ * @class Pathfinder
+ */
+class Pathfinder {
+    /**
+     * Create a new Pathfinding instance
+     * @param {Object} [config] - Optional configuration object
+     * @param {Object} [config.grid] - Grid system configuration
+     * @param {number} [config.grid.size=0.5] - Size of each grid cell in world units
+     * @param {number} [config.grid.safetyMargin=0] - Safety margin around obstacles in world units
+     * @param {number} [config.grid.minSegmentLength=0.5] - Minimum length for straight pipe segments in world units
+     * @param {number} [config.grid.timeout=1000] - Timeout for A* pathfinding in milliseconds
+     */
+    constructor(config = {}) {
+        this.config = config;
+        
+        // Grid system settings with defaults
+        const gridConfig = config.grid || {};
+        this.gridSystem = new GridSystem(
+            gridConfig.size ?? 0.5,
+            gridConfig.safetyMargin ?? 0
+        );
+        this.MIN_SEGMENT_LENGTH = gridConfig.minSegmentLength ?? 0.5;
+        this.ASTAR_TIMEOUT = gridConfig.timeout ?? 1000;
+
+        // Initialize connector manager
+        this.connectorManager = new ConnectorManager(
+            { grid: gridConfig }, // Only pass grid config
+            this.MIN_SEGMENT_LENGTH,
+            null, // sceneManager will be set in findPaths
+            this.gridSystem
+        );
+
+        // Initialize path manager
+        this.pathManager = new PathManager(
+            null, // sceneManager will be set in findPaths
+            this.gridSystem,
+            this.connectorManager,
+            this.MIN_SEGMENT_LENGTH,
+            this.ASTAR_TIMEOUT
+        );
+
+        // Initialize Steiner tree manager
+        this.steinerTreeManager = new SteinerTreeManager();
+    }
+
+    /**
+     * Find paths for all connections
+     * @param {Object} scene - The scene configuration
+     * @param {Object} scene.object - The scene object containing children
+     * @param {string} scene.object.uuid - Unique identifier for the scene object
+     * @param {string} scene.object.type - Type of the scene object (typically "Scene")
+     * @param {Array<Object>} scene.object.children - Array of scene objects
+     * @param {string} scene.object.children[].uuid - Unique identifier for the child object
+     * @param {string} scene.object.children[].type - Type of the child object (must be "Mesh")
+     * @param {Object} scene.object.children[].userData - User data for the child object
+     * @param {Object} scene.object.children[].userData.worldBoundingBox - Bounding box data
+     * @param {Array<number>} scene.object.children[].userData.worldBoundingBox.min - Minimum coordinates [x,y,z]
+     * @param {Array<number>} scene.object.children[].userData.worldBoundingBox.max - Maximum coordinates [x,y,z]
+     * @param {Array<number>} [scene.object.children[].userData.direction] - Optional direction vector [x,y,z]
+     * @param {Array<Object>} connections - Array of connections between objects
+     * @param {string} connections[].from - UUID of the source object
+     * @param {string} connections[].to - UUID of the target object
+     * @returns {Object} Result object containing paths, rewired connections, and gateways
+     * @property {Array<Object>} paths - Array of path results
+     * @property {string} paths[].from - Source object UUID
+     * @property {string} paths[].to - Target object UUID
+     * @property {Array<Vector3>|null} paths[].path - Array of path points or null if no path found
+     * @property {Array<Vector3>} [paths[].occupied] - Array of occupied points
+     * @property {Vector3} [paths[].start] - Start position (if no path found)
+     * @property {Vector3} [paths[].end] - End position (if no path found)
+     * @property {Array<Object>} rewiredConnections - Array of connections after gateway optimization
+     * @property {string} rewiredConnections[].from - Source object UUID
+     * @property {string} rewiredConnections[].to - Target object UUID
+     * @property {Array<Object>} gateways - Array of gateway information
+     * @property {string} gateways[].clusterId - ID of the cluster this gateway belongs to
+     * @property {number} gateways[].id - Unique identifier for the gateway
+     * @property {Vector3} gateways[].position - Position of the gateway in 3D space
+     */
+    findPaths(scene, connections) {
+        // Create scene manager with the provided scene
+        const sceneManager = new SceneManager(scene);
+        
+        // Update scene manager references
+        this.connectorManager.sceneManager = sceneManager;
+        this.pathManager.sceneManager = sceneManager;
+
+        // Cluster the connections
+        const clusters = this.connectorManager.clusterConnections(connections);
+
+        // Filter clusters to only include those with more than 2 objects
+        const filteredClusters = clusters.filter(cluster => cluster.objects.length > 2);
+        
+        // Calculate Steiner points and MST for each cluster
+        filteredClusters.forEach((cluster, index) => {
+            const points = cluster.displacedPoints.map(p => p.displacedPoint);
+            cluster.mst = this.steinerTreeManager.findMST(points);
+            cluster.steinerPoint = this.steinerTreeManager.findSteinerPoint(points);
+        });
+
+        let gatewayCounter = 1;
+        const gatewayMap = new Map();
+
+        filteredClusters.forEach(cluster => {
+            if (cluster.steinerPoint) {
+                const gatewayId = gatewayCounter++;
+                gatewayMap.set(cluster.clusterId, gatewayId);
+            }
+        });
+
+        // Create virtual gateway objects in the scene
+        filteredClusters.forEach(cluster => {
+            if (cluster.steinerPoint) {
+                const gatewayId = gatewayMap.get(cluster.clusterId);
+                const gatewayObject = {
+                    uuid: `GATEWAY-${gatewayId}`,
+                    type: 'Mesh',
+                    userData: {
+                        worldBoundingBox: {
+                            min: [
+                                cluster.steinerPoint.x - 0.25,
+                                cluster.steinerPoint.y - 0.25,
+                                cluster.steinerPoint.z - 0.25
+                            ],
+                            max: [
+                                cluster.steinerPoint.x + 0.25,
+                                cluster.steinerPoint.y + 0.25,
+                                cluster.steinerPoint.z + 0.25
+                            ]
+                        }
+                    }
+                };
+                scene.object.children.push(gatewayObject);
+            }
+        });
+
+        // Rewire connections through gateways
+        const rewiredConnections = [];
+        const connectionSet = new Set(); // Track unique connections
+
+        // Find direct connections (not part of clusters with >2 objects)
+        const directConnections = connections.filter(conn => {
+            const fromCluster = clusters.find(cluster => cluster.objects.includes(conn.from));
+            const toCluster = clusters.find(cluster => cluster.objects.includes(conn.to));
+            return (!fromCluster || !toCluster || fromCluster.objects.length <= 2 || toCluster.objects.length <= 2);
+        });
+
+        // First add direct connections
+        directConnections.forEach(conn => {
+            if (!connectionSet.has(JSON.stringify(conn))) {
+                rewiredConnections.push(conn);
+                connectionSet.add(JSON.stringify(conn));
+            }
+        });
+
+        // Then add gateway connections
+        const gatewayConnections = [];
+        connections.forEach(conn => {
+            const fromCluster = clusters.find(cluster => 
+                cluster.objects.includes(conn.from)
+            )?.clusterId;
+            const toCluster = clusters.find(cluster => 
+                cluster.objects.includes(conn.to)
+            )?.clusterId;
+            
+            if (fromCluster !== undefined && toCluster !== undefined) {
+                const fromGateway = `GATEWAY-${gatewayMap.get(fromCluster)}`;
+                const toGateway = `GATEWAY-${gatewayMap.get(toCluster)}`;
+                
+                if (fromGateway && toGateway) {
+                    // If both objects are in the same cluster, connect through their gateway
+                    if (fromCluster === toCluster) {
+                        const conn1 = { from: conn.from, to: fromGateway };
+                        const conn2 = { from: fromGateway, to: conn.to };
+                        
+                        // Only add if not already present
+                        if (!connectionSet.has(JSON.stringify(conn1))) {
+                            gatewayConnections.push(conn1);
+                            connectionSet.add(JSON.stringify(conn1));
+                        }
+                        if (!connectionSet.has(JSON.stringify(conn2))) {
+                            gatewayConnections.push(conn2);
+                            connectionSet.add(JSON.stringify(conn2));
+                        }
+                    } else {
+                        // If objects are in different clusters, connect through both gateways
+                        const conn1 = { from: conn.from, to: fromGateway };
+                        const conn2 = { from: fromGateway, to: toGateway };
+                        const conn3 = { from: toGateway, to: conn.to };
+                        
+                        // Only add if not already present
+                        if (!connectionSet.has(JSON.stringify(conn1))) {
+                            gatewayConnections.push(conn1);
+                            connectionSet.add(JSON.stringify(conn1));
+                        }
+                        if (!connectionSet.has(JSON.stringify(conn2))) {
+                            gatewayConnections.push(conn2);
+                            connectionSet.add(JSON.stringify(conn2));
+                        }
+                        if (!connectionSet.has(JSON.stringify(conn3))) {
+                            gatewayConnections.push(conn3);
+                            connectionSet.add(JSON.stringify(conn3));
+                        }
+                    }
+                }
+            }
+        });
+        
+        // Sort gateway connections by edge length
+        gatewayConnections.sort((a, b) => {
+            const objA1 = sceneManager.findObjectByUUID(a.from);
+            const objA2 = sceneManager.findObjectByUUID(a.to);
+            const objB1 = sceneManager.findObjectByUUID(b.from);
+            const objB2 = sceneManager.findObjectByUUID(b.to);
+            
+            if (!objA1 || !objA2 || !objB1 || !objB2) return 0;
+            
+            const distA = this.steinerTreeManager.distance(sceneManager.getWorldPosition(objA1), sceneManager.getWorldPosition(objA2));
+            const distB = this.steinerTreeManager.distance(sceneManager.getWorldPosition(objB1), sceneManager.getWorldPosition(objB2));
+            
+            return distA - distB;
+        });
+        
+        // Add sorted gateway connections to rewiredConnections
+        rewiredConnections.push(...gatewayConnections);
+
+        const paths = this.pathManager.findPaths(rewiredConnections || []);
+        
+        return {
+            paths,
+            rewiredConnections,
+            gateways: Array.from(gatewayMap.entries()).map(([clusterId, gatewayId]) => ({
+                clusterId,
+                id: gatewayId,
+                position: clusters.find(c => c.clusterId === clusterId)?.steinerPoint
+            }))
+        };
+    }
+}
+
+exports.Pathfinder = Pathfinder;