@2112-lab/pathfinder
Version:
A lightweight 3D pathfinding library for finding orthogonal paths between objects in 3D space
1,326 lines (1,164 loc) • 55.6 kB
JavaScript
'use strict';
/**
* 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: [
c