@2112-lab/pathfinder
Version:
Pure JavaScript 3D pathfinding algorithm library for industrial plant pipe routing
823 lines (703 loc) • 36 kB
JavaScript
import { Vector3 } from './Vector3.js';
/**
* Manages path-related operations for the pathfinding system
* @class PathManager
*/
export 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
* @param {number} protectedSegmentCount - Number of segments adjacent to connectors to protect from merging (default: 1)
*/
constructor(sceneManager, gridSystem, connectorManager, minSegmentLength, astarTimeout, protectedSegmentCount = 1) {
this.sceneManager = sceneManager;
this.gridSystem = gridSystem;
this.connectorManager = connectorManager;
this.MIN_SEGMENT_LENGTH = minSegmentLength;
this.ASTAR_TIMEOUT = astarTimeout;
this.PROTECTED_SEGMENT_COUNT = protectedSegmentCount;
}
/**
* A* pathfinding algorithm with bend minimization
* @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;
// Calculate cost with bend penalty
const tentativeG = this.calculateGScore(current, neighbor, cameFrom[current], gScore[current]);
// 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;
}
/**
* Calculate G score with bend penalty
* @private
* @param {string} current - Current voxel key
* @param {string} neighbor - Neighbor voxel key
* @param {string} parent - Parent voxel key (null if current is start)
* @param {number} currentG - Current G score
* @returns {number} G score with bend penalty
*/
calculateGScore(current, neighbor, parent, currentG) {
let cost = currentG + 1; // Base movement cost
if (parent) {
// Check if this move creates a bend
const [px, py, pz] = parent.split(',').map(Number);
const [cx, cy, cz] = current.split(',').map(Number);
const [nx, ny, nz] = neighbor.split(',').map(Number);
// Direction from parent to current
const prevDir = { x: cx - px, y: cy - py, z: cz - pz };
// Direction from current to neighbor
const nextDir = { x: nx - cx, y: ny - cy, z: nz - cz };
// If directions are different, add bend penalty
if (prevDir.x !== nextDir.x || prevDir.y !== nextDir.y || prevDir.z !== nextDir.z) {
cost += 0.00001; // Bend penalty - adjust this value to control how much bends are avoided
}
}
return cost;
}
/**
* 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);
let startSegmentVoxels = 0;
// 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);
startSegmentVoxels++;
}
}
// 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);
let endSegmentVoxels = 0;
// 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);
endSegmentVoxels++;
}
}
// 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 objects that aren't components or connectors (e.g., Groups, Cameras, Lights)
// Check for userData.objectType to identify relevant objects
if (!object.userData || !object.userData.objectType) 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 using pure coordinates (new API)
* @param {Object} startPos - Start position {x, y, z}
* @param {Object} endPos - End position {x, y, z}
* @param {Object|null} startDirection - Optional start direction {x, y, z}
* @param {Object|null} endDirection - Optional end direction {x, y, z}
* @param {Set<string>} occupied - Set of occupied voxel keys
* @param {Map<string, Set<string>>} occupiedByUUID - Map of occupied voxels by UUID
* @param {string} fromUUID - Source connector UUID (for tracking)
* @param {string} toUUID - Target connector UUID (for tracking)
* @returns {Array<Object>|null} Array of path points {x, y, z} or null if no path found
*/
processConnectionWithCoordinates(startPos, endPos, startDirection, endDirection, occupied, occupiedByUUID, fromUUID, toUUID) {
// Create a copy of occupied set for this path
const pathOccupied = new Set(occupied);
// Calculate virtual segments inline if directions are provided
let actualStartKey = this.gridSystem.voxelKey(startPos);
let actualEndKey = this.gridSystem.voxelKey(endPos);
// Process start direction constraint
if (startDirection) {
// Normalize direction
const length = Math.sqrt(startDirection.x ** 2 + startDirection.y ** 2 + startDirection.z ** 2);
const dir = length > 0 ? {
x: startDirection.x / length,
y: startDirection.y / length,
z: startDirection.z / length
} : { x: 0, y: 0, z: 0 };
// Calculate end point of virtual segment
const distance = this.gridSystem.worldToVoxelDistance(this.MIN_SEGMENT_LENGTH);
const endPoint = {
x: startPos.x + dir.x * this.MIN_SEGMENT_LENGTH,
y: startPos.y + dir.y * this.MIN_SEGMENT_LENGTH,
z: startPos.z + dir.z * this.MIN_SEGMENT_LENGTH
};
actualStartKey = this.gridSystem.voxelKey(endPoint);
// Mark virtual segment voxels as occupied
for (let i = 0; i < distance; i++) {
const x = Math.round(startPos.x / this.gridSystem.gridSize + dir.x * i);
const y = Math.round(startPos.y / this.gridSystem.gridSize + dir.y * i);
const z = Math.round(startPos.z / this.gridSystem.gridSize + dir.z * i);
const key = `${x},${y},${z}`;
pathOccupied.add(key);
}
// Clear the virtual segment area for pathfinding
const clearDistance = this.gridSystem.worldToVoxelDistance(this.MIN_SEGMENT_LENGTH + (this.gridSystem.safetyMargin * 2));
for (let i = 0; i <= clearDistance; i++) {
const x = Math.round(startPos.x / this.gridSystem.gridSize + dir.x * i);
const y = Math.round(startPos.y / this.gridSystem.gridSize + dir.y * i);
const z = Math.round(startPos.z / this.gridSystem.gridSize + dir.z * i);
pathOccupied.delete(`${x},${y},${z}`);
}
}
// Process end direction constraint
if (endDirection) {
// Normalize direction
const length = Math.sqrt(endDirection.x ** 2 + endDirection.y ** 2 + endDirection.z ** 2);
const dir = length > 0 ? {
x: endDirection.x / length,
y: endDirection.y / length,
z: endDirection.z / length
} : { x: 0, y: 0, z: 0 };
// Calculate end point of virtual segment
const distance = this.gridSystem.worldToVoxelDistance(this.MIN_SEGMENT_LENGTH);
const endPoint = {
x: endPos.x + dir.x * this.MIN_SEGMENT_LENGTH,
y: endPos.y + dir.y * this.MIN_SEGMENT_LENGTH,
z: endPos.z + dir.z * this.MIN_SEGMENT_LENGTH
};
actualEndKey = this.gridSystem.voxelKey(endPoint);
// Mark virtual segment voxels as occupied
for (let i = 0; i < distance; i++) {
const x = Math.round(endPos.x / this.gridSystem.gridSize + dir.x * i);
const y = Math.round(endPos.y / this.gridSystem.gridSize + dir.y * i);
const z = Math.round(endPos.z / this.gridSystem.gridSize + dir.z * i);
const key = `${x},${y},${z}`;
pathOccupied.add(key);
}
// Clear the virtual segment area for pathfinding
const clearDistance = this.gridSystem.worldToVoxelDistance(this.MIN_SEGMENT_LENGTH + (this.gridSystem.safetyMargin * 2));
for (let i = 0; i <= clearDistance; i++) {
const x = Math.round(endPos.x / this.gridSystem.gridSize + dir.x * i);
const y = Math.round(endPos.y / this.gridSystem.gridSize + dir.y * i);
const z = Math.round(endPos.z / this.gridSystem.gridSize + dir.z * i);
pathOccupied.delete(`${x},${y},${z}`);
}
}
// Run A* pathfinding
const pathKeys = this.astar(actualStartKey, actualEndKey, pathOccupied);
if (!pathKeys) {
return null;
}
// Build full path with virtual segments
let fullPath = [];
// Add start virtual segment if direction provided
if (startDirection) {
fullPath.push(this.gridSystem.voxelKey(startPos));
}
// Add middle path
fullPath = fullPath.concat(pathKeys);
// Add end virtual segment if direction provided
if (endDirection) {
fullPath.push(this.gridSystem.voxelKey(endPos));
}
// Convert voxel keys to coordinates
return fullPath.map(key => this.gridSystem.voxelToVec3(key));
}
/**
* Process a single connection and find its path (legacy API - wraps coordinate-based method)
* @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 have userData (are valid connectors/components)
if (!fromObject.userData || !toObject.userData) {
console.log(`[ERROR] Connection endpoints must have userData: ${fromObject.uuid} - ${toObject.uuid}`);
return null;
}
// Extract objectType for later use in path splitting logic
const fromObjectType = fromObject.userData.objectType;
const toObjectType = toObject.userData.objectType;
// Store the parent for each object to access later
let fromParent = this.sceneManager.findParentObject(fromObject);
if (fromParent && fromParent.isDirectionParent) {
fromParent = this.sceneManager._findParentObjectRecursive(this.sceneManager.getRoot(), fromObject);
}
fromObject.parent = fromParent;
let toParent = this.sceneManager.findParentObject(toObject);
if (toParent && toParent.isDirectionParent) {
toParent = this.sceneManager._findParentObjectRecursive(this.sceneManager.getRoot(), toObject);
}
toObject.parent = toParent;
// Determine if endpoints have direction constraints (connectors with direction)
// Only connectors with explicit direction need segment protection
// Gateways and connectors without direction can have adjacent segments merged
const fromHasDirection = fromObject.userData?.direction !== undefined;
const toHasDirection = toObject.userData?.direction !== undefined;
// Get world positions for both objects
const worldPos1 = this.sceneManager.getWorldPosition(fromObject);
const worldPos2 = this.sceneManager.getWorldPosition(toObject);
// Get directions from connectors
const startDirection = this.connectorManager.getConnectorDirection(fromObject);
const endDirection = this.connectorManager.getConnectorDirection(toObject);
// Get bounding boxes for the two objects
const fromBbox = this.sceneManager.getBoundingBox(fromObject);
const toBbox = this.sceneManager.getBoundingBox(toObject);
if (!fromBbox || !toBbox) return null;
// 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));
// Clear voxels in bounding boxes (to allow pathfinding through connectors)
const fromVoxels = this.gridSystem.getVoxelsInBoundingBox(fromBbox);
const toVoxels = this.gridSystem.getVoxelsInBoundingBox(toBbox);
for (const key of fromVoxels) {
pathOccupied.delete(key);
}
for (const key of toVoxels) {
pathOccupied.delete(key);
}
// Process all other connectors in the scene
this.connectorManager.processConnectors(this.sceneManager.getRoot(), pathOccupied, fromObject.uuid, toObject.uuid);
// Debug copy 2: After clearing and processing other connectors
const debugOccupiedAfterSegments = Array.from(pathOccupied)
.map(key => this.gridSystem.voxelToVec3(key));
// Call the new coordinate-based API
const path = this.processConnectionWithCoordinates(
worldPos1,
worldPos2,
startDirection,
endDirection,
pathOccupied,
occupiedByUUID,
connection.from,
connection.to
);
const occupiedVoxels = Array.from(pathOccupied)
.map(key => this.gridSystem.voxelToVec3(key));
if (path) {
// [CUSTOM SEGMENT FIX]
// If the start/end points are component connectors, attempt to rectify the path
// to maintain orthogonality while reaching exact offsets.
const fromIsComponent = fromObject.parent && fromObject.parent.userData && fromObject.parent.userData.objectType === 'component';
const toIsComponent = toObject.parent && toObject.parent.userData && toObject.parent.userData.objectType === 'component';
// Also apply to manual segments if they are not components but we still want to connect
// Check if objects are manual segment connectors
const fromIsManual = fromObject.userData?.isManualSegmentConnector || fromObject.userData?.objectType === 'gateway';
const toIsManual = toObject.userData?.isManualSegmentConnector || toObject.userData?.objectType === 'gateway';
// We want to rectify if it's a component OR a manual connector/gateway
// This ensures we connect exactly to the target point
const shouldRectifyStart = fromIsComponent || fromIsManual;
const shouldRectifyEnd = toIsComponent || toIsManual;
let rectifiedSegments = new Set();
if (shouldRectifyStart || shouldRectifyEnd) {
rectifiedSegments = this.rectifyPathEndpoints(path, worldPos1, worldPos2, shouldRectifyStart, shouldRectifyEnd);
}
// Create a set for this path's occupied voxels
const pathOccupiedSet = new Set();
// Mark path points as occupied for next routes
for (const p of path) {
const key = this.gridSystem.voxelKey(p);
occupied.add(key);
pathOccupiedSet.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, pathOccupiedSet);
return {
from: connection.from,
to: connection.to,
fromObjectType: fromObjectType,
toObjectType: toObjectType,
fromHasDirection: fromHasDirection,
toHasDirection: toHasDirection,
path: path,
occupied: occupiedVoxels,
rectifiedSegments: Array.from(rectifiedSegments), // Pass to result
debug: {
initial: debugOccupiedInitial,
afterSegments: debugOccupiedAfterSegments
},
occupiedByUUID: occupiedByUUID
};
} else {
console.log(`[ERROR] No orthogonal path found for ${connection.from}-${connection.to}`);
return {
from: connection.from,
to: connection.to,
fromObjectType: fromObjectType,
toObjectType: toObjectType,
fromHasDirection: fromHasDirection,
toHasDirection: toHasDirection,
path: null,
start: worldPos1,
end: worldPos2,
occupied: occupiedVoxels,
debug: {
initial: debugOccupiedInitial,
afterSegments: debugOccupiedAfterSegments
},
occupiedByUUID: occupiedByUUID
};
}
}
/**
* Merges colinear segments in a single path to optimize pipe creation
* @private
* @param {Array<Object|Vector3>} path - Array of path points
* @param {Number} tolerance - Angular tolerance for considering segments colinear (default: ~0.57 degrees)
* @param {boolean} fromHasDirection - Whether start connector has a direction constraint
* @param {boolean} toHasDirection - Whether end connector has a direction constraint
* @returns {Array<Object|Vector3>} Optimized path with merged colinear segments
*/
mergeColinearSegments(path, tolerance = 0.001, fromHasDirection = false, toHasDirection = false) {
if (!path || path.length <= 2) {
return path; // Skip paths with 0-2 points (no optimization needed)
}
console.log('[mergeColinearSegments] Starting merge:', {
inputLength: path.length,
fromHasDirection,
toHasDirection
});
// Convert path points to Vector3 objects for easier processing
const points = path.map(point => {
if (point instanceof Vector3) {
return point.clone();
} else if (Array.isArray(point)) {
return new Vector3(point[0], point[1], point[2]);
} else if (point.x !== undefined && point.y !== undefined && point.z !== undefined) {
return new Vector3(point.x, point.y, point.z);
} else {
console.warn('[Warning] Unknown point format:', point);
return new Vector3(0, 0, 0);
}
});
// If only 1 point, don't merge (no segments to optimize)
if (points.length <= 1) {
console.log('[mergeColinearSegments] Path too short (<=1 points), skipping merge');
return path;
}
// Determine if we should protect segments adjacent to connectors
// Only protect segments next to connectors with explicit direction constraints
// Gateways and connectors without direction can have their adjacent segments merged
const protectStartSegment = fromHasDirection;
const protectEndSegment = toHasDirection;
console.log('[mergeColinearSegments] Protection settings:', {
protectStartSegment,
protectEndSegment,
willKeepPoint1: protectStartSegment,
willKeepPointN2: protectEndSegment
});
const optimized = [points[0]]; // Always keep first point (connector)
// Keep the initial segment points if connector has direction constraint
if (protectStartSegment) {
for (let i = 1; i <= Math.min(this.PROTECTED_SEGMENT_COUNT, points.length - 1); i++) {
optimized.push(points[i]);
}
}
// Determine loop bounds based on endpoint protection
const startIndex = protectStartSegment ? Math.min(this.PROTECTED_SEGMENT_COUNT + 1, points.length - 1) : 1;
const endIndex = protectEndSegment ? Math.max(0, points.length - this.PROTECTED_SEGMENT_COUNT - 1) : points.length - 1;
console.log('[mergeColinearSegments] Loop bounds:', {
startIndex,
endIndex,
totalPoints: points.length,
loopRange: `${startIndex} to ${endIndex - 1}`
});
// Iterate through points, only keeping direction changes
for (let i = startIndex; i < endIndex; i++) {
const prev = points[i - 1];
const curr = points[i];
const next = points[i + 1];
// Direction from prev to curr
const dir1 = new Vector3(curr.x - prev.x, curr.y - prev.y, curr.z - prev.z);
const len1 = Math.sqrt(dir1.x ** 2 + dir1.y ** 2 + dir1.z ** 2);
// Direction from curr to next
const dir2 = new Vector3(next.x - curr.x, next.y - curr.y, next.z - curr.z);
const len2 = Math.sqrt(dir2.x ** 2 + dir2.y ** 2 + dir2.z ** 2);
// Skip if either segment is zero-length
if (len1 < 1e-6 || len2 < 1e-6) {
continue;
}
// Normalize directions
dir1.x /= len1; dir1.y /= len1; dir1.z /= len1;
dir2.x /= len2; dir2.y /= len2; dir2.z /= len2;
// Calculate dot product (1.0 means same direction)
const dot = dir1.x * dir2.x + dir1.y * dir2.y + dir1.z * dir2.z;
// If directions are different (not colinear), keep this point
if (Math.abs(dot - 1) >= tolerance) {
optimized.push(curr);
}
}
// Keep the final segment points if connector has direction constraint
if (protectEndSegment) {
const startProtectedIndex = Math.max(0, points.length - this.PROTECTED_SEGMENT_COUNT - 1);
for (let i = startProtectedIndex; i < points.length - 1; i++) {
// Avoid duplicates - check if this point is already in optimized
const lastPoint = optimized[optimized.length - 1];
if (lastPoint.x !== points[i].x || lastPoint.y !== points[i].y || lastPoint.z !== points[i].z) {
optimized.push(points[i]);
}
}
}
optimized.push(points[points.length - 1]); // Always keep last point (connector)
console.log('[mergeColinearSegments] Result:', {
inputLength: path.length,
outputLength: optimized.length,
reduction: path.length - optimized.length,
reductionPercent: (((path.length - optimized.length) / path.length) * 100).toFixed(1) + '%'
});
// Convert back to original format
const outputPath = optimized.map(point => {
if (Array.isArray(path[0])) {
return [point.x, point.y, point.z];
} else if (path[0] && path[0].x !== undefined) {
return { x: point.x, y: point.y, z: point.z };
} else {
return point;
}
});
return outputPath;
}
/**
* Rectify path endpoints to maintain orthogonality with off-grid connectors
* @private
* @param {Array<Object>} path - Array of path points (Vector3)
* @param {Vector3} startTarget - Exact start position
* @param {Vector3} endTarget - Exact end position
* @param {boolean} adjustStart - Whether to adjust the start
* @param {boolean} adjustEnd - Whether to adjust the end
*/
rectifyPathEndpoints(path, startTarget, endTarget, adjustStart, adjustEnd) {
// Helper to check if two values are effectively equal
const isClose = (a, b) => Math.abs(a - b) < 0.001;
const rectifiedIndices = new Set();
if (adjustEnd && path.length >= 2) {
const current = path[path.length - 1];
const diff = {
x: endTarget.x - current.x,
y: endTarget.y - current.y,
z: endTarget.z - current.z
};
// For each axis with specific offset
['x', 'y', 'z'].forEach(axis => {
if (Math.abs(diff[axis]) > 0.001) {
// Search backwards for a segment parallel to this axis
for (let i = path.length - 2; i >= 0; i--) {
// Check if segment i -> i+1 moves along this axis
const p1 = path[i];
const p2 = path[i + 1];
// We look for a segment that has length along the axis we need to adjust
// and is effectively orthogonal (minimal change on other axes).
// Since A* grid paths are orthogonal, checking if it has length on this axis is usually sufficient.
const isParallel = !isClose(p1[axis], p2[axis]);
if (isParallel) {
// Apply offset to all points from i+1 to end
for (let j = i + 1; j < path.length; j++) {
path[j][axis] = endTarget[axis];
}
// Mark segment i as rectified (the one that absorbed the offset)
rectifiedIndices.add(i);
break;
}
}
}
});
// Finally force the last point to match exactly (in case no parallel segment found, at least connect)
path[path.length - 1] = endTarget;
}
if (adjustStart && path.length >= 2) {
const current = path[0];
const diff = {
x: startTarget.x - current.x,
y: startTarget.y - current.y,
z: startTarget.z - current.z
};
// For each axis with specific offset
['x', 'y', 'z'].forEach(axis => {
if (Math.abs(diff[axis]) > 0.001) {
// Search forwards
for (let i = 0; i < path.length - 1; i++) {
const p1 = path[i];
const p2 = path[i + 1];
const isParallel = !isClose(p1[axis], p2[axis]);
if (isParallel) {
// Apply offset to all points from 0 to i
for (let j = 0; j <= i; j++) {
path[j][axis] = startTarget[axis];
}
// Mark segment i as rectified
rectifiedIndices.add(i);
break;
}
}
}
});
// Force first point
path[0] = startTarget;
}
return rectifiedIndices;
}
/**
* 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) {
// Optimize path by merging colinear segments before adding to results
if (result.path && result.path.length > 0) {
result.path = this.mergeColinearSegments(
result.path,
0.001,
result.fromHasDirection,
result.toHasDirection
);
}
paths.push(result);
}
}
return paths;
}
}