@rxflow/manhattan
Version:
Manhattan routing algorithm for ReactFlow - generates orthogonal paths with obstacle avoidance
330 lines (291 loc) • 14.1 kB
JavaScript
;
Object.defineProperty(exports, "__esModule", {
value: true
});
exports.findRoute = findRoute;
var _geometry = require("../geometry");
var _SortedSet = require("./SortedSet");
var _utils = require("../utils");
/**
* Generate smart points based on position using extensionDistance and binary search
*
* Algorithm:
* 1. First try extensionDistance in the specified direction
* 2. If blocked, use binary search starting from step, halving until finding accessible point
* 3. For target points, approach from opposite direction
*/
function generateSmartPoints(anchor, bbox, position, grid, map, options, isTarget = false) {
const directionMap = {
'right': {
x: 1,
y: 0
},
'left': {
x: -1,
y: 0
},
'top': {
x: 0,
y: -1
},
'bottom': {
x: 0,
y: 1
}
};
const direction = directionMap[position];
if (!direction) {
console.warn(`[generateSmartPoints] Unknown position: ${position}, falling back to anchor`);
return [anchor];
}
// Both source and target extend in the specified direction
// - Source: extends away from node in sourcePosition direction
// - Target: extends away from node in targetPosition direction (path approaches from this direction)
const actualDirection = direction;
const points = [];
// 1. First try extensionDistance
const extensionPoint = new _geometry.Point(anchor.x + actualDirection.x * options.extensionDistance, anchor.y + actualDirection.y * options.extensionDistance).round(options.precision);
console.log(`[generateSmartPoints] ${isTarget ? 'Target' : 'Source'} position=${position}, trying extension point: (${extensionPoint.x}, ${extensionPoint.y})`);
if (map.isAccessible(extensionPoint)) {
points.push(extensionPoint);
console.log(`[generateSmartPoints] Extension point is accessible`);
return points;
}
console.log(`[generateSmartPoints] Extension point blocked, using step-based search`);
// 2. Step-based search with binary refinement
// First, extend outward by step increments until we find an accessible point
let stepMultiplier = 1;
let maxSteps = 20; // Prevent infinite loop
let foundAccessibleDistance = -1;
console.log(`[generateSmartPoints] Starting outward search with step=${options.step}`);
while (stepMultiplier <= maxSteps) {
const distance = stepMultiplier * options.step;
const testPoint = new _geometry.Point(anchor.x + actualDirection.x * distance, anchor.y + actualDirection.y * distance).round(options.precision);
console.log(`[generateSmartPoints] Testing ${stepMultiplier}*step (distance=${distance}): (${testPoint.x}, ${testPoint.y})`);
if (map.isAccessible(testPoint)) {
foundAccessibleDistance = distance;
console.log(`[generateSmartPoints] Found accessible point at ${stepMultiplier}*step (distance=${distance})`);
break;
}
stepMultiplier++;
}
// 3. If we found an accessible point, refine by binary search within the last step interval
if (foundAccessibleDistance > 0) {
const outerDistance = foundAccessibleDistance;
const innerDistance = foundAccessibleDistance - options.step;
console.log(`[generateSmartPoints] Refining between ${innerDistance} and ${outerDistance}`);
// Binary search within the last step interval to find the closest accessible point
let left = innerDistance;
let right = outerDistance;
let bestDistance = outerDistance;
// Binary search with precision of 1px
while (right - left > 1) {
const mid = (left + right) / 2;
const testPoint = new _geometry.Point(anchor.x + actualDirection.x * mid, anchor.y + actualDirection.y * mid).round(options.precision);
console.log(`[generateSmartPoints] Binary search testing distance ${mid.toFixed(1)}: (${testPoint.x}, ${testPoint.y})`);
if (map.isAccessible(testPoint)) {
bestDistance = mid;
right = mid;
console.log(`[generateSmartPoints] Point accessible, searching closer (right=${right})`);
} else {
left = mid;
console.log(`[generateSmartPoints] Point blocked, searching further (left=${left})`);
}
}
// Use the best distance found
const finalPoint = new _geometry.Point(anchor.x + actualDirection.x * bestDistance, anchor.y + actualDirection.y * bestDistance).round(options.precision);
points.push(finalPoint);
console.log(`[generateSmartPoints] Final point at distance ${bestDistance}: (${finalPoint.x}, ${finalPoint.y})`);
} else {
// 4. If no accessible point found after maxSteps, use anchor as fallback
console.log(`[generateSmartPoints] No accessible point found after ${maxSteps} steps, using anchor: (${anchor.x}, ${anchor.y})`);
points.push(anchor);
}
return points;
}
/**
* Find route between two points using A* algorithm
*/
function findRoute(sourceBBox, targetBBox, sourceAnchor, targetAnchor, map, options) {
const precision = options.precision;
// Round anchor points
const sourceEndpoint = (0, _utils.round)(sourceAnchor.clone(), precision);
const targetEndpoint = (0, _utils.round)(targetAnchor.clone(), precision);
// Get grid for this route
const grid = (0, _utils.getGrid)(options.step, sourceEndpoint, targetEndpoint);
// Get pathfinding points
const startPoint = sourceEndpoint;
const endPoint = targetEndpoint;
// Get start and end points around rectangles
// Use smart point generation based on position if available
let startPoints;
let endPoints;
// Generate smart start points based on sourcePosition
if (options.sourcePosition) {
startPoints = generateSmartPoints(startPoint, sourceBBox, options.sourcePosition, grid, map, options, false);
console.log('[findRoute] Start points from smart generation:', startPoints.map(p => `(${p.x}, ${p.y})`));
} else {
startPoints = (0, _utils.getRectPoints)(startPoint, sourceBBox, options.startDirections, grid, options);
console.log('[findRoute] Start points from getRectPoints:', startPoints.map(p => `(${p.x}, ${p.y})`));
// Take into account only accessible rect points
startPoints = startPoints.filter(p => map.isAccessible(p));
}
// Generate smart end points based on targetPosition
if (options.targetPosition) {
endPoints = generateSmartPoints(targetEndpoint, targetBBox, options.targetPosition, grid, map, options, true);
console.log('[findRoute] End points from smart generation:', endPoints.map(p => `(${p.x}, ${p.y})`));
} else {
endPoints = (0, _utils.getRectPoints)(targetEndpoint, targetBBox, options.endDirections, grid, options);
console.log('[findRoute] End points from getRectPoints:', endPoints.map(p => `(${p.x}, ${p.y})`));
// Take into account only accessible rect points
endPoints = endPoints.filter(p => map.isAccessible(p));
}
console.log('[findRoute] Start points after filter:', startPoints.map(p => `(${p.x}, ${p.y})`));
console.log('[findRoute] End points after filter:', endPoints.map(p => `(${p.x}, ${p.y})`));
// Ensure we always have at least the anchor points
// This handles edge cases where anchor is on the node boundary
if (startPoints.length === 0) {
startPoints = [(0, _utils.round)(startPoint, precision)];
}
if (endPoints.length === 0) {
endPoints = [(0, _utils.round)(endPoint, precision)];
}
// Initialize A* data structures
const openSet = new _SortedSet.SortedSet();
const points = new Map();
const parents = new Map();
const costs = new Map();
// Add all start points to open set
for (const startPoint of startPoints) {
const key = (0, _utils.getKey)(startPoint);
openSet.add(key, (0, _utils.getCost)(startPoint, endPoints));
points.set(key, startPoint);
costs.set(key, 0);
}
const previousRouteDirectionAngle = options.previousDirectionAngle;
const isPathBeginning = previousRouteDirectionAngle === undefined;
// Get directions with grid offsets
const directions = (0, _utils.getGridOffsets)(grid, options);
const numDirections = directions.length;
// Create set of end point keys for quick lookup
const endPointsKeys = new Set(endPoints.map(p => (0, _utils.getKey)(p)));
// Check if start and end points are the same
const sameStartEndPoints = startPoints.length === endPoints.length && startPoints.every((sp, i) => sp.equals(endPoints[i]));
// Main A* loop
let loopsRemaining = options.maxLoopCount;
while (!openSet.isEmpty() && loopsRemaining > 0) {
// Get the closest item and mark it CLOSED
const currentKey = openSet.pop();
if (!currentKey) break;
const currentPoint = points.get(currentKey);
const currentParent = parents.get(currentKey);
const currentCost = costs.get(currentKey);
const isStartPoint = currentPoint.equals(startPoint);
const isRouteBeginning = currentParent === undefined;
// Calculate previous direction angle
let previousDirectionAngle;
if (!isRouteBeginning) {
previousDirectionAngle = (0, _utils.getDirectionAngle)(currentParent, currentPoint, numDirections, grid, options);
} else if (!isPathBeginning) {
previousDirectionAngle = previousRouteDirectionAngle;
} else if (!isStartPoint) {
previousDirectionAngle = (0, _utils.getDirectionAngle)(startPoint, currentPoint, numDirections, grid, options);
} else {
previousDirectionAngle = null;
}
// Check if we reached any endpoint
const skipEndCheck = isRouteBeginning && sameStartEndPoints;
if (!skipEndCheck && endPointsKeys.has(currentKey)) {
options.previousDirectionAngle = previousDirectionAngle;
return (0, _utils.reconstructRoute)(parents, points, currentPoint, startPoint, endPoint);
}
// Explore neighbors in all directions
for (const direction of directions) {
const directionAngle = direction.angle;
const directionChange = (0, _utils.getDirectionChange)(previousDirectionAngle ?? 0, directionAngle);
// Don't use the point if direction changed too rapidly
if (!(isPathBeginning && isStartPoint) && directionChange > options.maxDirectionChange) {
continue;
}
// Calculate neighbor point and align to global grid
const rawNeighbor = currentPoint.clone().translate(direction.gridOffsetX || 0, direction.gridOffsetY || 0);
// Align to global grid for consistent path alignment
const neighborPoint = new _geometry.Point(Math.round(rawNeighbor.x / grid.x) * grid.x, Math.round(rawNeighbor.y / grid.y) * grid.y).round(precision);
const neighborKey = (0, _utils.getKey)(neighborPoint);
// Skip if closed or not accessible
if (openSet.isClose(neighborKey) || !map.isAccessible(neighborPoint)) {
continue;
}
// Check if we can reach any end point directly from this neighbor
// This allows connecting to end points that are not on the grid
let canReachEndPoint = false;
let reachableEndPoint = null;
for (const endPt of endPoints) {
const distanceToEnd = neighborPoint.manhattanDistance(endPt);
// If close enough to end point (within step distance), try direct connection
if (distanceToEnd < options.step * 1.5) {
// Check if we can move directly to the end point
const dx = endPt.x - neighborPoint.x;
const dy = endPt.y - neighborPoint.y;
// Allow direct connection if it's orthogonal or close to orthogonal
const isOrthogonal = Math.abs(dx) < 0.1 || Math.abs(dy) < 0.1;
if (isOrthogonal && map.isAccessible(endPt)) {
canReachEndPoint = true;
reachableEndPoint = endPt;
break;
}
}
}
// If we can reach an end point directly, add it as the final step
if (canReachEndPoint && reachableEndPoint) {
const endKey = (0, _utils.getKey)(reachableEndPoint);
const endCost = neighborPoint.manhattanDistance(reachableEndPoint);
const totalCost = currentCost + direction.cost + endCost;
if (!openSet.isOpen(endKey) || totalCost < (costs.get(endKey) || Infinity)) {
points.set(endKey, reachableEndPoint);
parents.set(endKey, neighborPoint);
costs.set(endKey, totalCost);
// Also add the neighbor point if not already added
if (!points.has(neighborKey)) {
points.set(neighborKey, neighborPoint);
parents.set(neighborKey, currentPoint);
costs.set(neighborKey, currentCost + direction.cost);
}
// Check if this is our target end point
if (endPointsKeys.has(endKey)) {
options.previousDirectionAngle = directionAngle;
return (0, _utils.reconstructRoute)(parents, points, reachableEndPoint, startPoint, endPoint);
}
}
}
// Check if neighbor is an end point (exact match)
if (endPointsKeys.has(neighborKey)) {
const isEndPoint = neighborPoint.equals(endPoint);
if (!isEndPoint) {
const endDirectionAngle = (0, _utils.getDirectionAngle)(neighborPoint, endPoint, numDirections, grid, options);
const endDirectionChange = (0, _utils.getDirectionChange)(directionAngle, endDirectionAngle);
if (endDirectionChange > options.maxDirectionChange) {
continue;
}
}
}
// Calculate costs
const neighborCost = direction.cost;
const neighborPenalty = isStartPoint ? 0 : options.penalties[directionChange] || 0;
const costFromStart = currentCost + neighborCost + neighborPenalty;
// Update if not in open set or found better path
if (!openSet.isOpen(neighborKey) || costFromStart < (costs.get(neighborKey) || Infinity)) {
points.set(neighborKey, neighborPoint);
parents.set(neighborKey, currentPoint);
costs.set(neighborKey, costFromStart);
openSet.add(neighborKey, costFromStart + (0, _utils.getCost)(neighborPoint, endPoints));
}
}
loopsRemaining -= 1;
}
// No path found, try fallback
if (options.fallbackRoute) {
return options.fallbackRoute(startPoint, endPoint);
}
return null;
}