385 lines
13 KiB
JavaScript
385 lines
13 KiB
JavaScript
|
"use strict";
|
|||
|
|
|
|||
|
|
Object.defineProperty(exports, "__esModule", {
|
|||
|
|
value: true
|
|||
|
|
});
|
|||
|
|
exports.pathFinder = exports.octolinearCfg = void 0;
|
|||
|
|
var _g6Core = require("@antv/g6-core");
|
|||
|
|
var _util = require("@antv/util");
|
|||
|
|
var _polylineUtil = require("./polyline-util");
|
|||
|
|
/**
|
|||
|
|
* 通过配置不同的 costFunc, distFunc, constraints 可以得到不同效果的 router
|
|||
|
|
* generalRouter: 不限制搜索时的移动方向，避开障碍即可
|
|||
|
|
* orthogonal: 线必须沿着竖直或水平方向（4个方向）
|
|||
|
|
* octolinearRouter: 线沿着竖直、水平、对角线方向（8个方向）
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
var manhattanDist = function manhattanDist(p1, p2) {
|
|||
|
|
return Math.abs(p1.x - p2.x) + Math.abs(p1.y - p2.y);
|
|||
|
|
};
|
|||
|
|
var eucliDist = function eucliDist(p1, p2) {
|
|||
|
|
return Math.sqrt(Math.pow(p1.x - p2.x, 2) + Math.pow(p1.y - p2.y, 2));
|
|||
|
|
};
|
|||
|
|
var straightPath = function straightPath(start, end) {
|
|||
|
|
// console.warn('fallbackRoute: straight path');
|
|||
|
|
return [start, end];
|
|||
|
|
};
|
|||
|
|
var simplePolyline = function simplePolyline(start, end, startNode, endNode, cfg) {
|
|||
|
|
return (0, _polylineUtil.simplifyPolyline)((0, _polylineUtil.getPolylinePoints)(start, end, startNode, endNode, cfg.offset));
|
|||
|
|
};
|
|||
|
|
// getPolylinePoints
|
|||
|
|
var defaultCfg = {
|
|||
|
|
offset: 20,
|
|||
|
|
maxAllowedDirectionChange: Math.PI / 2,
|
|||
|
|
maximumLoops: 2000,
|
|||
|
|
gridSize: 10,
|
|||
|
|
directions: [{
|
|||
|
|
stepX: 1,
|
|||
|
|
stepY: 0
|
|||
|
|
}, {
|
|||
|
|
stepX: -1,
|
|||
|
|
stepY: 0
|
|||
|
|
}, {
|
|||
|
|
stepX: 0,
|
|||
|
|
stepY: 1
|
|||
|
|
}, {
|
|||
|
|
stepX: 0,
|
|||
|
|
stepY: -1
|
|||
|
|
} // top
|
|||
|
|
],
|
|||
|
|
get penalties() {
|
|||
|
|
return {
|
|||
|
|
0: 0,
|
|||
|
|
45: this.gridSize / 2,
|
|||
|
|
90: this.gridSize / 2
|
|||
|
|
};
|
|||
|
|
},
|
|||
|
|
distFunc: manhattanDist,
|
|||
|
|
fallbackRoute: simplePolyline
|
|||
|
|
};
|
|||
|
|
var octolinearCfg = exports.octolinearCfg = {
|
|||
|
|
maxAllowedDirectionChange: Math.PI / 4,
|
|||
|
|
// 8 个方向: 上下左右 + 45度斜线方向
|
|||
|
|
directions: [{
|
|||
|
|
stepX: 1,
|
|||
|
|
stepY: 0
|
|||
|
|
}, {
|
|||
|
|
stepX: 1,
|
|||
|
|
stepY: 1
|
|||
|
|
}, {
|
|||
|
|
stepX: 0,
|
|||
|
|
stepY: 1
|
|||
|
|
}, {
|
|||
|
|
stepX: -1,
|
|||
|
|
stepY: 1
|
|||
|
|
}, {
|
|||
|
|
stepX: -1,
|
|||
|
|
stepY: 0
|
|||
|
|
}, {
|
|||
|
|
stepX: -1,
|
|||
|
|
stepY: -1
|
|||
|
|
}, {
|
|||
|
|
stepX: 0,
|
|||
|
|
stepY: -1
|
|||
|
|
}, {
|
|||
|
|
stepX: 1,
|
|||
|
|
stepY: -1
|
|||
|
|
}],
|
|||
|
|
distFunc: eucliDist,
|
|||
|
|
fallbackRoute: straightPath
|
|||
|
|
};
|
|||
|
|
var pos2GridIx = function pos2GridIx(pos, gridSize) {
|
|||
|
|
var gridIx = Math.round(Math.abs(pos / gridSize));
|
|||
|
|
var sign = pos < 0 ? -1 : 1;
|
|||
|
|
return gridIx < 0 ? 0 : sign * gridIx;
|
|||
|
|
};
|
|||
|
|
var getObstacleMap = function getObstacleMap(items, gridSize, offset) {
|
|||
|
|
var map = {};
|
|||
|
|
items.forEach(function (item) {
|
|||
|
|
// create-edge 时，当边类型为 polyline 时 endNode 为 null
|
|||
|
|
if (!item) return;
|
|||
|
|
var bbox = (0, _polylineUtil.getExpandedBBox)(item.getBBox(), offset);
|
|||
|
|
for (var x = pos2GridIx(bbox.minX, gridSize); x <= pos2GridIx(bbox.maxX, gridSize); x += 1) {
|
|||
|
|
for (var y = pos2GridIx(bbox.minY, gridSize); y <= pos2GridIx(bbox.maxY, gridSize); y += 1) {
|
|||
|
|
map["".concat(x, "|||").concat(y)] = true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
});
|
|||
|
|
return map;
|
|||
|
|
};
|
|||
|
|
/**
|
|||
|
|
* 方向角：计算从 p1 到 p2 的射线与水平线形成的夹角度数（顺时针从右侧0°转到该射线的角度）
|
|||
|
|
* @param p1 PolyPoint
|
|||
|
|
* @param p2 PolyPoint
|
|||
|
|
*/
|
|||
|
|
var getDirectionAngle = function getDirectionAngle(p1, p2) {
|
|||
|
|
var deltaX = p2.x - p1.x;
|
|||
|
|
var deltaY = p2.y - p1.y;
|
|||
|
|
if (deltaX || deltaY) {
|
|||
|
|
return Math.atan2(deltaY, deltaX);
|
|||
|
|
}
|
|||
|
|
return 0;
|
|||
|
|
};
|
|||
|
|
/**
|
|||
|
|
* 方向角的改变，取小于180度角
|
|||
|
|
* @param angle1
|
|||
|
|
* @param angle2
|
|||
|
|
*/
|
|||
|
|
var getAngleDiff = function getAngleDiff(angle1, angle2) {
|
|||
|
|
var directionChange = Math.abs(angle1 - angle2);
|
|||
|
|
return directionChange > Math.PI ? 2 * Math.PI - directionChange : directionChange;
|
|||
|
|
// return directionChange > 180 ? 360 - directionChange : directionChange;
|
|||
|
|
};
|
|||
|
|
// Path finder //
|
|||
|
|
var estimateCost = function estimateCost(from, endPoints, distFunc) {
|
|||
|
|
var min = Infinity;
|
|||
|
|
for (var i = 0, len = endPoints.length; i < len; i++) {
|
|||
|
|
var cost = distFunc(from, endPoints[i]);
|
|||
|
|
if (cost < min) {
|
|||
|
|
min = cost;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return min;
|
|||
|
|
};
|
|||
|
|
// 计算考虑 offset 后的 BBox 上的连接点
|
|||
|
|
var getBoxPoints = function getBoxPoints(point,
|
|||
|
|
// 被 gridSize 格式化后的位置（anchorPoint）
|
|||
|
|
oriPoint,
|
|||
|
|
// 未被 gridSize 格式化的位置（anchorPoint）
|
|||
|
|
node,
|
|||
|
|
// 原始节点，用于获取 bbox
|
|||
|
|
anotherPoint,
|
|||
|
|
// 另一端被 gridSize 格式化后的位置
|
|||
|
|
cfg) {
|
|||
|
|
var points = [];
|
|||
|
|
// create-edge 生成边的过程中，endNode 为 null
|
|||
|
|
if (!node) {
|
|||
|
|
return [point];
|
|||
|
|
}
|
|||
|
|
var directions = cfg.directions,
|
|||
|
|
offset = cfg.offset;
|
|||
|
|
var bbox = node.getBBox();
|
|||
|
|
var isInside = oriPoint.x > bbox.minX && oriPoint.x < bbox.maxX && oriPoint.y > bbox.minY && oriPoint.y < bbox.maxY;
|
|||
|
|
var expandBBox = (0, _polylineUtil.getExpandedBBox)(bbox, offset);
|
|||
|
|
for (var i in expandBBox) {
|
|||
|
|
expandBBox[i] = pos2GridIx(expandBBox[i], cfg.gridSize);
|
|||
|
|
}
|
|||
|
|
if (isInside) {
|
|||
|
|
// 如果 anchorPoint 在节点内部，允许第一段线穿过节点
|
|||
|
|
for (var _i = 0, directions_1 = directions; _i < directions_1.length; _i++) {
|
|||
|
|
var dir = directions_1[_i];
|
|||
|
|
var bounds = [[{
|
|||
|
|
x: expandBBox.minX,
|
|||
|
|
y: expandBBox.minY
|
|||
|
|
}, {
|
|||
|
|
x: expandBBox.maxX,
|
|||
|
|
y: expandBBox.minY
|
|||
|
|
}], [{
|
|||
|
|
x: expandBBox.minX,
|
|||
|
|
y: expandBBox.minY
|
|||
|
|
}, {
|
|||
|
|
x: expandBBox.minX,
|
|||
|
|
y: expandBBox.maxY
|
|||
|
|
}], [{
|
|||
|
|
x: expandBBox.maxX,
|
|||
|
|
y: expandBBox.minY
|
|||
|
|
}, {
|
|||
|
|
x: expandBBox.maxX,
|
|||
|
|
y: expandBBox.maxY
|
|||
|
|
}], [{
|
|||
|
|
x: expandBBox.minX,
|
|||
|
|
y: expandBBox.maxY
|
|||
|
|
}, {
|
|||
|
|
x: expandBBox.maxX,
|
|||
|
|
y: expandBBox.maxY
|
|||
|
|
}]];
|
|||
|
|
for (var i = 0; i < 4; i++) {
|
|||
|
|
var boundLine = bounds[i];
|
|||
|
|
var insterctP_1 = _g6Core.Util.getLineIntersect(point, {
|
|||
|
|
x: point.x + dir.stepX * expandBBox.width,
|
|||
|
|
y: point.y + dir.stepY * expandBBox.height
|
|||
|
|
}, boundLine[0], boundLine[1]);
|
|||
|
|
if (insterctP_1 && !(0, _polylineUtil.isSegmentCrossingBBox)(point, insterctP_1, bbox)) {
|
|||
|
|
insterctP_1.id = "".concat(insterctP_1.x, "|||").concat(insterctP_1.y);
|
|||
|
|
points.push(insterctP_1);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return points;
|
|||
|
|
}
|
|||
|
|
// 如果 anchorPoint 在节点上，只有一个可选方向
|
|||
|
|
var insterctP = (0, _polylineUtil.getExpandedBBoxPoint)(expandBBox, point, anotherPoint);
|
|||
|
|
insterctP.id = "".concat(insterctP.x, "|||").concat(insterctP.y);
|
|||
|
|
return [insterctP];
|
|||
|
|
};
|
|||
|
|
var getDirectionChange = function getDirectionChange(current, neighbor, cameFrom, scaleStartPoint) {
|
|||
|
|
var directionAngle = getDirectionAngle(current, neighbor);
|
|||
|
|
var currentCameFrom = cameFrom[current.id];
|
|||
|
|
if (!currentCameFrom) {
|
|||
|
|
var startAngle = getDirectionAngle(scaleStartPoint, current);
|
|||
|
|
return getAngleDiff(startAngle, directionAngle);
|
|||
|
|
}
|
|||
|
|
var prevDirectionAngle = getDirectionAngle({
|
|||
|
|
x: currentCameFrom.x,
|
|||
|
|
y: currentCameFrom.y
|
|||
|
|
}, current);
|
|||
|
|
return getAngleDiff(prevDirectionAngle, directionAngle);
|
|||
|
|
};
|
|||
|
|
var getControlPoints = function getControlPoints(current, cameFrom, scaleStartPoint, endPoint, startPoint, scaleEndPoint, gridSize) {
|
|||
|
|
var controlPoints = [endPoint];
|
|||
|
|
var pointZero = endPoint;
|
|||
|
|
var currentId = current.id;
|
|||
|
|
var currentX = current.x;
|
|||
|
|
var currentY = current.y;
|
|||
|
|
var lastPoint = {
|
|||
|
|
x: currentX,
|
|||
|
|
y: currentY,
|
|||
|
|
id: currentId
|
|||
|
|
};
|
|||
|
|
if (getDirectionChange(lastPoint, scaleEndPoint, cameFrom, scaleStartPoint)) {
|
|||
|
|
pointZero = {
|
|||
|
|
x: scaleEndPoint.x === endPoint.x ? endPoint.x : lastPoint.x * gridSize,
|
|||
|
|
y: scaleEndPoint.y === endPoint.y ? endPoint.y : lastPoint.y * gridSize
|
|||
|
|
};
|
|||
|
|
controlPoints.unshift(pointZero);
|
|||
|
|
}
|
|||
|
|
var currentCameFrom = cameFrom[currentId];
|
|||
|
|
while (currentCameFrom && currentCameFrom.id !== currentId) {
|
|||
|
|
var point = {
|
|||
|
|
x: currentX,
|
|||
|
|
y: currentY,
|
|||
|
|
id: currentId
|
|||
|
|
};
|
|||
|
|
var prePoint = {
|
|||
|
|
x: currentCameFrom.x,
|
|||
|
|
y: currentCameFrom.y,
|
|||
|
|
id: currentCameFrom.id
|
|||
|
|
};
|
|||
|
|
var directionChange = getDirectionChange(prePoint, point, cameFrom, scaleStartPoint);
|
|||
|
|
if (directionChange) {
|
|||
|
|
pointZero = {
|
|||
|
|
x: prePoint.x === point.x ? pointZero.x : prePoint.x * gridSize,
|
|||
|
|
y: prePoint.y === point.y ? pointZero.y : prePoint.y * gridSize
|
|||
|
|
};
|
|||
|
|
controlPoints.unshift(pointZero);
|
|||
|
|
}
|
|||
|
|
currentId = prePoint.id;
|
|||
|
|
currentX = prePoint.x;
|
|||
|
|
currentY = prePoint.y;
|
|||
|
|
currentCameFrom = cameFrom[currentId];
|
|||
|
|
}
|
|||
|
|
// 和startNode对齐
|
|||
|
|
controlPoints[0].x = currentX === scaleStartPoint.x ? startPoint.x : pointZero.x;
|
|||
|
|
controlPoints[0].y = currentY === scaleStartPoint.y ? startPoint.y : pointZero.y;
|
|||
|
|
controlPoints.unshift(startPoint);
|
|||
|
|
return controlPoints;
|
|||
|
|
};
|
|||
|
|
var pathFinder = exports.pathFinder = function pathFinder(startPoint, endPoint, startNode, endNode, routerCfg) {
|
|||
|
|
if (isNaN(startPoint.x) || isNaN(endPoint.x)) return [];
|
|||
|
|
var cfg = (0, _util.deepMix)(defaultCfg, routerCfg);
|
|||
|
|
cfg.obstacles = cfg.obstacles || [];
|
|||
|
|
var penalties = cfg.penalties,
|
|||
|
|
gridSize = cfg.gridSize;
|
|||
|
|
var map = getObstacleMap(cfg.obstacles.concat([startNode, endNode]), gridSize, cfg.offset);
|
|||
|
|
var scaleStartPoint = {
|
|||
|
|
x: pos2GridIx(startPoint.x, gridSize),
|
|||
|
|
y: pos2GridIx(startPoint.y, gridSize)
|
|||
|
|
};
|
|||
|
|
var scaleEndPoint = {
|
|||
|
|
x: pos2GridIx(endPoint.x, gridSize),
|
|||
|
|
y: pos2GridIx(endPoint.y, gridSize)
|
|||
|
|
};
|
|||
|
|
startPoint.id = "".concat(scaleStartPoint.x, "|||").concat(scaleStartPoint.y);
|
|||
|
|
endPoint.id = "".concat(scaleEndPoint.x, "|||").concat(scaleEndPoint.y);
|
|||
|
|
var startPoints = getBoxPoints(scaleStartPoint, startPoint, startNode, scaleEndPoint, cfg);
|
|||
|
|
var endPoints = getBoxPoints(scaleEndPoint, endPoint, endNode, scaleStartPoint, cfg);
|
|||
|
|
startPoints.forEach(function (point) {
|
|||
|
|
delete map[point.id];
|
|||
|
|
});
|
|||
|
|
endPoints.forEach(function (point) {
|
|||
|
|
delete map[point.id];
|
|||
|
|
});
|
|||
|
|
var openSet = {};
|
|||
|
|
var closedSet = {};
|
|||
|
|
var cameFrom = {};
|
|||
|
|
// 从起点到当前点已产生的 cost, default: Infinity
|
|||
|
|
var gScore = {};
|
|||
|
|
// 起点经过当前点到达终点预估的 cost, default: Infinity
|
|||
|
|
var fScore = {};
|
|||
|
|
var sortedOpenSet = new _polylineUtil.SortedArray();
|
|||
|
|
// initialize
|
|||
|
|
for (var i = 0; i < startPoints.length; i++) {
|
|||
|
|
var firstStep = startPoints[i];
|
|||
|
|
openSet[firstStep.id] = firstStep;
|
|||
|
|
gScore[firstStep.id] = 0;
|
|||
|
|
fScore[firstStep.id] = estimateCost(firstStep, endPoints, cfg.distFunc);
|
|||
|
|
sortedOpenSet.add({
|
|||
|
|
id: firstStep.id,
|
|||
|
|
value: fScore[firstStep.id]
|
|||
|
|
});
|
|||
|
|
}
|
|||
|
|
var remainLoops = cfg.maximumLoops;
|
|||
|
|
var current, direction, neighbor, neighborCost, costFromStart, directionChange;
|
|||
|
|
var curCost = Infinity;
|
|||
|
|
var endPointMap = {};
|
|||
|
|
endPoints.forEach(function (point) {
|
|||
|
|
endPointMap["".concat(point.x, "|||").concat(point.y)] = true;
|
|||
|
|
});
|
|||
|
|
Object.keys(openSet).forEach(function (key) {
|
|||
|
|
var id = openSet[key].id;
|
|||
|
|
if (fScore[id] <= curCost) {
|
|||
|
|
curCost = fScore[id];
|
|||
|
|
current = openSet[id];
|
|||
|
|
}
|
|||
|
|
});
|
|||
|
|
while (Object.keys(openSet).length > 0 && remainLoops > 0) {
|
|||
|
|
var minId = sortedOpenSet.minId((remainLoops + 1) % 30 === 0);
|
|||
|
|
if (minId) {
|
|||
|
|
current = openSet[minId];
|
|||
|
|
} else {
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
// 如果 fScore 最小的点就是终点
|
|||
|
|
if (endPointMap["".concat(current.x, "|||").concat(current.y)]) {
|
|||
|
|
return getControlPoints(current, cameFrom, scaleStartPoint, endPoint, startPoint, scaleEndPoint, gridSize);
|
|||
|
|
}
|
|||
|
|
delete openSet[current.id];
|
|||
|
|
sortedOpenSet.remove(current.id);
|
|||
|
|
closedSet[current.id] = true;
|
|||
|
|
// 获取符合条件的下一步的候选连接点
|
|||
|
|
// 沿候选方向走一步
|
|||
|
|
for (var i = 0; i < cfg.directions.length; i++) {
|
|||
|
|
direction = cfg.directions[i];
|
|||
|
|
var neighborId = "".concat(Math.round(current.x) + direction.stepX, "|||").concat(Math.round(current.y) + direction.stepY);
|
|||
|
|
neighbor = {
|
|||
|
|
x: current.x + direction.stepX,
|
|||
|
|
y: current.y + direction.stepY,
|
|||
|
|
id: neighborId
|
|||
|
|
};
|
|||
|
|
if (closedSet[neighborId]) continue;
|
|||
|
|
directionChange = getDirectionChange(current, neighbor, cameFrom, scaleStartPoint);
|
|||
|
|
if (directionChange > cfg.maxAllowedDirectionChange) continue;
|
|||
|
|
if (map[neighborId]) continue; // 如果交叉则跳过
|
|||
|
|
// 将候选点加入 openSet, 并计算每个候选点的 cost
|
|||
|
|
if (!openSet[neighborId]) {
|
|||
|
|
openSet[neighborId] = neighbor;
|
|||
|
|
}
|
|||
|
|
var directionPenalties = penalties[directionChange];
|
|||
|
|
neighborCost = cfg.distFunc(current, neighbor) + (isNaN(directionPenalties) ? gridSize : directionPenalties);
|
|||
|
|
costFromStart = gScore[current.id] + neighborCost;
|
|||
|
|
var neighborGScore = gScore[neighborId];
|
|||
|
|
if (neighborGScore && costFromStart >= neighborGScore) {
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
cameFrom[neighborId] = current;
|
|||
|
|
gScore[neighborId] = costFromStart;
|
|||
|
|
fScore[neighborId] = costFromStart + estimateCost(neighbor, endPoints, cfg.distFunc);
|
|||
|
|
sortedOpenSet.add({
|
|||
|
|
id: neighborId,
|
|||
|
|
value: fScore[neighborId]
|
|||
|
|
});
|
|||
|
|
}
|
|||
|
|
remainLoops -= 1;
|
|||
|
|
}
|
|||
|
|
return cfg.fallbackRoute(startPoint, endPoint, startNode, endNode, cfg);
|
|||
|
|
};
|