@2112-lab/pathfinder
Version:
Pure JavaScript 3D pathfinding algorithm library for industrial plant pipe routing
539 lines (466 loc) • 27.1 kB
JavaScript
import { Vector3 } from './Vector3.js';
import { SceneManager } from './SceneManager.js';
import { GridSystem } from './GridSystem.js';
import { ConnectorManager } from './ConnectorManager.js';
import { PathManager } from './PathManager.js';
import { TreePathManager } from './TreePathManager.js';
/**
* Generate a deterministic ID based on cluster properties for stable pathfinding
* @private
* @param {Object} cluster - The cluster object
* @returns {string} A deterministic unique ID string
*/
function generateDeterministicId(cluster) {
// Create a deterministic hash from cluster properties
const sortedObjects = [...cluster.objects].sort();
const jointPoint = cluster.jointPoint;
const hashString = `${sortedObjects.join('-')}-${Math.round(jointPoint.x * 100)}-${Math.round(jointPoint.y * 100)}-${Math.round(jointPoint.z * 100)}`;
// Simple hash function to create a short ID
let hash = 0;
for (let i = 0; i < hashString.length; i++) {
const char = hashString.charCodeAt(i);
hash = ((hash << 5) - hash) + char;
hash = hash & hash; // Convert to 32-bit integer
}
return Math.abs(hash).toString(36).substring(0, 8);
}
/**
* A 3D pathfinding system that finds orthogonal paths between objects
*
* @class Pathfinder
* @version __VERSION__
* @updated 2025-10-22
*
* @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
* @param {number} [config.connectorBBoxSize=0.1] - Default bounding box size for position-based connectors
*/
export class Pathfinder {
/**
* Create a new Pathfinding instance
*
* @private
*
*/
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;
// Connector bounding box size for position-based format
this.CONNECTOR_BBOX_SIZE = config.connectorBBoxSize ?? 0.1;
// 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 tree path manager
this.treePathManager = new TreePathManager(this.gridSystem, null);
}
/**
* Detect non-orthogonal segments in paths
* @private
* @param {Array<Object>} paths - Array of path results
* @returns {Object} Detection results
* @property {boolean} hasNonOrthogonal - Whether any non-orthogonal segments were found
* @property {Array<Object>} segments - Array of detected non-orthogonal segments
*/
_detectNonOrthogonalSegments(paths) {
const detectedSegments = [];
paths.forEach((pathResult, pathIndex) => {
if (!pathResult.path || pathResult.path.length < 3) {
return; // Need at least 3 points to detect non-orthogonal
}
const path = pathResult.path;
// Check each consecutive triplet of points
for (let i = 0; i < path.length - 2; i++) {
const p1 = path[i];
const p2 = path[i + 1];
const p3 = path[i + 2];
// Calculate vectors between points
const v1 = {
x: p2.x - p1.x,
y: p2.y - p1.y,
z: p2.z - p1.z
};
const v2 = {
x: p3.x - p2.x,
y: p3.y - p2.y,
z: p3.z - p2.z
};
// Calculate dot product to check if vectors are orthogonal
const dotProduct = v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
// Calculate magnitudes
const mag1 = Math.sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z);
const mag2 = Math.sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z);
// Check if vectors are parallel (colinear) or perpendicular (orthogonal)
const epsilon = 1e-6;
if (Math.abs(dotProduct) < epsilon) {
// Orthogonal - this is expected, skip
continue;
}
// Normalize dot product for angle calculation
const normalizedDot = dotProduct / (mag1 * mag2);
// Determine type of non-orthogonal segment
let segmentType;
if (Math.abs(Math.abs(normalizedDot) - 1.0) < epsilon) {
// Vectors are parallel (colinear)
segmentType = 'colinear';
} else {
// Vectors form a diagonal
segmentType = 'diagonal';
}
detectedSegments.push({
pathIndex,
from: pathResult.from,
to: pathResult.to,
pointIndices: [i, i + 1, i + 2],
points: [
{ x: p1.x, y: p1.y, z: p1.z },
{ x: p2.x, y: p2.y, z: p2.z },
{ x: p3.x, y: p3.y, z: p3.z }
],
type: segmentType,
dotProduct,
normalizedDot
});
}
});
return {
hasNonOrthogonal: detectedSegments.length > 0,
count: detectedSegments.length,
segments: detectedSegments
};
}
/**
* Find paths for all connections
* @param {Object} scene - The scene configuration
* @param {Array<Object>} scene.children - Array of scene objects
* @param {string} scene.children[].uuid - Unique identifier for the child object
* @param {Object} scene.children[].userData - User data for the child object
* @param {Object} scene.children[].userData.worldBoundingBox - Bounding box data
* @param {Array<number>} scene.children[].userData.worldBoundingBox.min - Minimum coordinates [x,y,z]
* @param {Array<number>} scene.children[].userData.worldBoundingBox.max - Maximum coordinates [x,y,z]
* @param {Array<number>} [scene.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, gateways, and non-orthogonal detection
* @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
* @property {Object} gateways[].connections - Connection mapping information
* @property {Array<Object>} gateways[].connections.removed - Array of original connections that were removed
* @property {string} gateways[].connections.removed[].from - Source object UUID
* @property {string} gateways[].connections.removed[].to - Target object UUID
* @property {Array<Object>} gateways[].connections.added - Array of new connections that were added
* @property {string} gateways[].connections.added[].from - Source object UUID
* @property {string} gateways[].connections.added[].to - Target object UUID
* @property {Object} nonOrthogonalSegments - Non-orthogonal segment detection results
* @property {boolean} nonOrthogonalSegments.hasNonOrthogonal - Whether any non-orthogonal segments were found
* @property {number} nonOrthogonalSegments.count - Number of non-orthogonal segments detected
* @property {Array<Object>} nonOrthogonalSegments.segments - Array of detected non-orthogonal segments
* @property {number} nonOrthogonalSegments.segments[].pathIndex - Index of the path containing the segment
* @property {string} nonOrthogonalSegments.segments[].from - Source object UUID
* @property {string} nonOrthogonalSegments.segments[].to - Target object UUID
* @property {Array<number>} nonOrthogonalSegments.segments[].pointIndices - Indices of the three points
* @property {Array<Vector3>} nonOrthogonalSegments.segments[].points - The three points forming the segment
* @property {string} nonOrthogonalSegments.segments[].type - Type: 'diagonal' or 'colinear'
* @property {number} nonOrthogonalSegments.segments[].dotProduct - Dot product of the two vectors
* @property {number} nonOrthogonalSegments.segments[].normalizedDot - Normalized dot product (for angle calculation)
*/
findPaths(scene, connections) {
const timing = {}
const t0 = typeof performance !== 'undefined' ? performance.now() : Date.now()
// Create scene manager with the provided scene and connector size
const sceneManager = new SceneManager(scene, this.CONNECTOR_BBOX_SIZE);
// Update scene manager references
this.connectorManager.sceneManager = sceneManager;
this.pathManager.sceneManager = sceneManager;
this.treePathManager.sceneManager = sceneManager;
timing.sceneManagerInit = (typeof performance !== 'undefined' ? performance.now() : Date.now()) - t0
// ── Clustering ─────────────────────────────────────────────────
const clusterStart = typeof performance !== 'undefined' ? performance.now() : Date.now()
// 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 joint points for each cluster
filteredClusters.forEach((cluster, index) => {
const points = cluster.displacedPoints.map(p => p.displacedPoint);
cluster.jointPoint = this.treePathManager.findJointPoint(points);
});
timing.clustering = (typeof performance !== 'undefined' ? performance.now() : Date.now()) - clusterStart
// ── Gateway Creation ───────────────────────────────────────────
const gatewayStart = typeof performance !== 'undefined' ? performance.now() : Date.now()
const gatewayMap = new Map();
filteredClusters.forEach(cluster => {
if (cluster.jointPoint) {
// Check if cluster contains an existing declared gateway
const existingGatewayUUID = cluster.objects.find(uuid => {
if (typeof uuid === 'string' && uuid.includes('Gateway')) {
const gatewayObj = sceneManager.findObjectByUUID(uuid);
// Reuse if it's a declared (manual) gateway
return gatewayObj && gatewayObj.userData?.isDeclared === true;
}
return false;
});
if (existingGatewayUUID) {
// Reuse existing declared gateway
console.log(`[Pathfinder] Reusing existing declared gateway: ${existingGatewayUUID}`);
gatewayMap.set(cluster.clusterId, existingGatewayUUID);
// Update the joint point to match the existing gateway's position
const existingGateway = sceneManager.findObjectByUUID(existingGatewayUUID);
if (existingGateway) {
const gatewayPos = sceneManager.getWorldPosition(existingGateway);
cluster.jointPoint = gatewayPos;
console.log(`[Pathfinder] Using existing gateway position: [${gatewayPos.x}, ${gatewayPos.y}, ${gatewayPos.z}]`);
}
} else {
// Generate a new gateway ID for computed gateways
const uuid = generateDeterministicId(cluster);
const gatewayId = `Gateway-${uuid}`;
gatewayMap.set(cluster.clusterId, gatewayId);
}
}
});
// Create virtual gateway objects in the scene (only for new computed gateways)
filteredClusters.forEach(cluster => {
if (cluster.jointPoint) {
const gatewayId = gatewayMap.get(cluster.clusterId);
// Check if this gateway already exists in the scene
const existingGateway = sceneManager.findObjectByUUID(gatewayId);
if (existingGateway) {
console.log(`[Pathfinder] Skipping duplicate gateway creation: ${gatewayId} (already exists)`);
return; // Skip creating duplicate
}
// Create new computed gateway object
const gatewayObject = {
uuid: gatewayId,
userData: {
worldBoundingBox: {
min: [
cluster.jointPoint.x - 0.25,
cluster.jointPoint.y - 0.25,
cluster.jointPoint.z - 0.25
],
max: [
cluster.jointPoint.x + 0.25,
cluster.jointPoint.y + 0.25,
cluster.jointPoint.z + 0.25
]
}
}
};
scene.children.push(gatewayObject);
}
});
// Rewire connections through gateways
const rewiredConnections = [];
const connectionSet = new Set(); // Track unique connections
const gatewayConnectionMappings = new Map(); // Track which original connections are replaced by which gateway 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 = gatewayMap.get(fromCluster);
const toGateway = gatewayMap.get(toCluster);
if (fromGateway !== undefined && toGateway !== undefined) {
const fromGatewayId = fromGateway;
const toGatewayId = toGateway;
// Track the original connection that will be replaced
const originalConnection = { from: conn.from, to: conn.to };
const addedConnections = [];
// If both objects are in the same cluster, connect through their gateway
if (fromCluster === toCluster) {
const conn1 = { from: conn.from, to: fromGatewayId };
const conn2 = { from: fromGatewayId, to: conn.to };
// Only add if not already present
if (!connectionSet.has(JSON.stringify(conn1))) {
gatewayConnections.push(conn1);
connectionSet.add(JSON.stringify(conn1));
addedConnections.push(conn1);
}
if (!connectionSet.has(JSON.stringify(conn2))) {
gatewayConnections.push(conn2);
connectionSet.add(JSON.stringify(conn2));
addedConnections.push(conn2);
}
} else {
// If objects are in different clusters, connect through both gateways
const conn1 = { from: conn.from, to: fromGatewayId };
const conn2 = { from: fromGatewayId, to: toGatewayId };
const conn3 = { from: toGatewayId, to: conn.to };
// Only add if not already present
if (!connectionSet.has(JSON.stringify(conn1))) {
gatewayConnections.push(conn1);
connectionSet.add(JSON.stringify(conn1));
addedConnections.push(conn1);
}
if (!connectionSet.has(JSON.stringify(conn2))) {
gatewayConnections.push(conn2);
connectionSet.add(JSON.stringify(conn2));
addedConnections.push(conn2);
}
if (!connectionSet.has(JSON.stringify(conn3))) {
gatewayConnections.push(conn3);
connectionSet.add(JSON.stringify(conn3));
addedConnections.push(conn3);
}
}
// Store the mapping for this connection
if (addedConnections.length > 0) {
gatewayConnectionMappings.set(JSON.stringify(originalConnection), {
removed: [originalConnection],
added: addedConnections
});
}
}
}
});
// 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.treePathManager.distance(sceneManager.getWorldPosition(objA1), sceneManager.getWorldPosition(objA2));
const distB = this.treePathManager.distance(sceneManager.getWorldPosition(objB1), sceneManager.getWorldPosition(objB2));
return distA - distB;
});
// Add sorted gateway connections to rewiredConnections
rewiredConnections.push(...gatewayConnections);
timing.gatewayAndRewiring = (typeof performance !== 'undefined' ? performance.now() : Date.now()) - gatewayStart
// ── A* Pathfinding ─────────────────────────────────────────────
const aStarStart = typeof performance !== 'undefined' ? performance.now() : Date.now()
const paths = this.pathManager.findPaths(rewiredConnections || []);
timing.aStarPathfinding = (typeof performance !== 'undefined' ? performance.now() : Date.now()) - aStarStart
// Aggregate connection mappings by gateway
const gatewayConnectionsMap = new Map();
// Initialize connection mappings for each gateway
Array.from(gatewayMap.entries()).forEach(([clusterId, gatewayId]) => {
gatewayConnectionsMap.set(gatewayId, {
removed: [],
added: []
});
});
// Populate connection mappings
gatewayConnectionMappings.forEach((mapping, connectionKey) => {
const originalConnection = JSON.parse(connectionKey);
const fromCluster = clusters.find(cluster =>
cluster.objects.includes(originalConnection.from)
)?.clusterId;
const toCluster = clusters.find(cluster =>
cluster.objects.includes(originalConnection.to)
)?.clusterId;
if (fromCluster !== undefined && toCluster !== undefined) {
const fromGateway = gatewayMap.get(fromCluster);
const toGateway = gatewayMap.get(toCluster);
if (fromGateway !== undefined && toGateway !== undefined) {
// Add to both gateways if they're different
if (fromGateway === toGateway) {
// Same gateway
const gatewayConnections = gatewayConnectionsMap.get(fromGateway);
gatewayConnections.removed.push(originalConnection);
gatewayConnections.added.push(...mapping.added);
} else {
// Different gateways - split the connections
const fromGatewayConnections = gatewayConnectionsMap.get(fromGateway);
const toGatewayConnections = gatewayConnectionsMap.get(toGateway);
// Add the original connection to both gateways' removed list
fromGatewayConnections.removed.push(originalConnection);
toGatewayConnections.removed.push(originalConnection);
// Split added connections by gateway
mapping.added.forEach(conn => {
if (conn.from === originalConnection.from || conn.to === fromGateway) {
fromGatewayConnections.added.push(conn);
}
if (conn.from === toGateway || conn.to === originalConnection.to) {
toGatewayConnections.added.push(conn);
}
});
}
}
}
});
// Detect non-orthogonal segments in paths
const nonOrthogonalDetection = this._detectNonOrthogonalSegments(paths);
timing.postProcessing = (typeof performance !== 'undefined' ? performance.now() : Date.now()) - aStarStart - timing.aStarPathfinding
timing.total = (typeof performance !== 'undefined' ? performance.now() : Date.now()) - t0
timing.connectionCount = connections.length
timing.rewiredConnectionCount = rewiredConnections.length
timing.clusterCount = filteredClusters.length
timing.pathCount = paths.length
const results = {
gateways: Array.from(gatewayMap.entries()).map(([clusterId, gatewayId]) => ({
clusterId,
id: gatewayId,
position: clusters.find(c => c.clusterId === clusterId)?.jointPoint,
connections: gatewayConnectionsMap.get(gatewayId) || { removed: [], added: [] }
})),
paths,
rewiredConnections,
nonOrthogonalSegments: nonOrthogonalDetection,
timing
}
console.log("findPaths results:", results);
return results;
}
}
// Export additional classes for use by consumers
export { GridSystem } from './GridSystem.js';
export { Vector3 } from './Vector3.js';
export { PathManager } from './PathManager.js';
export { ConnectorManager } from './ConnectorManager.js';
export { SceneManager } from './SceneManager.js';
export { TreePathManager } from './TreePathManager.js';