orthogonal-path-finding
Version:
Compute an orthogonal line from start to end with obstacles
424 lines (423 loc) • 13.4 kB
JavaScript
const { floor: z } = Math, { min: Q } = Math, u = function(e, t) {
return e < t ? -1 : e > t ? 1 : 0;
}, B = function(e, t, s = 0, n = null, i = u) {
let o;
if (s < 0)
throw new Error("lo must be non-negative");
for (n == null && (n = e.length); s < n; )
o = z((s + n) / 2), i(t, e[o]) < 0 ? n = o : s = o + 1;
return e.splice(s, 0, t), t;
}, $ = function(e, t, s = u) {
e.push(t), j(e, 0, e.length - 1, s);
}, v = function(e, t = u) {
let s;
const n = e.pop();
return e.length ? (s = e[0], e[0] = n, k(e, 0, t)) : s = n, s;
}, q = function(e, t, s = u) {
const n = e[0];
return e[0] = t, k(e, 0, s), n;
}, P = function(e, t, s = u) {
return e.length && s(e[0], t) < 0 && ([e[0], t] = [t, e[0]], k(e, 0, s)), t;
}, W = function(e, t = u) {
const s = z(e.length / 2);
for (let n = s - 1; n >= 0; n--)
k(e, n, t);
}, S = function(e, t, s = u) {
const n = e.indexOf(t);
n !== -1 && (j(e, 0, n, s), k(e, n, s));
}, H = function(e, t, s = u) {
if (t <= 0) return [];
const n = e.slice(0, t);
if (!n.length)
return n;
W(n, s);
for (const i of e.slice(t))
P(n, i, s);
return n.sort(s).reverse();
}, J = function(e, t, s = u) {
if (t <= 0) return [];
if (t * 10 <= e.length) {
const i = e.slice(0, t).sort(s);
if (!i.length)
return i;
let o = i[i.length - 1];
for (const l of e.slice(t))
s(l, o) < 0 && (B(i, l, 0, null, s), i.pop(), o = i[i.length - 1]);
return i;
}
W(e, s);
const n = [];
for (let i = 0; i < Q(t, e.length); i++)
n.push(v(e, s));
return n;
}, j = function(e, t, s, n = u) {
const i = e[s];
for (; s > t; ) {
const o = s - 1 >> 1, l = e[o];
if (n(i, l) < 0) {
e[s] = l, s = o;
continue;
}
break;
}
e[s] = i;
}, k = function(e, t, s = u) {
const n = e.length, i = t, o = e[t];
let l = 2 * t + 1;
for (; l < n; ) {
let r = l + 1;
r < n && !(s(e[l], e[r]) < 0) && (l = r), e[t] = e[l], t = l, l = 2 * t + 1;
}
e[t] = o, j(e, i, t, s);
};
class E {
constructor(t = u) {
this.insert = this.push, this.top = this.peek, this.front = this.peek, this.has = this.contains, this.copy = this.clone, this.cmp = t, this.nodes = [];
}
push(t) {
$(this.nodes, t, this.cmp);
}
pop() {
return v(this.nodes, this.cmp);
}
peek() {
return this.nodes[0];
}
contains(t) {
return this.nodes.indexOf(t) !== -1;
}
replace(t) {
return q(this.nodes, t, this.cmp);
}
pushpop(t) {
return P(this.nodes, t, this.cmp);
}
heapify() {
W(this.nodes, this.cmp);
}
updateItem(t) {
S(this.nodes, t, this.cmp);
}
clear() {
this.nodes = [];
}
empty() {
return this.nodes.length === 0;
}
size() {
return this.nodes.length;
}
clone() {
const t = new E(this.cmp);
return t.nodes = this.nodes.slice(0), t;
}
toArray() {
return this.nodes.slice(0);
}
static push(t, s, n = u) {
$(t, s, n);
}
static pop(t, s = u) {
return v(t, s);
}
static replace(t, s, n = u) {
return q(t, s, n);
}
static pushpop(t, s, n = u) {
return P(t, s, n);
}
static heapify(t, s = u) {
W(t, s);
}
static updateItem(t, s, n = u) {
S(t, s, n);
}
static nlargest(t, s, n = u) {
return H(t, s, n);
}
static nsmallest(t, s, n = u) {
return J(t, s, n);
}
}
class F {
/**
* @constructor
* @param {number} x - The x coordinate of the node on the grid.
* @param {number} y - The y coordinate of the node on the grid.
* @param {boolean} [walkable] - Whether this node is walkable.
*/
constructor(t, s, n = !0) {
this.x = t, this.y = s, this.walkable = n;
}
}
var x = /* @__PURE__ */ ((e) => (e[e.always = 1] = "always", e[e.never = 2] = "never", e[e.ifAtMostOneObstacle = 3] = "ifAtMostOneObstacle", e[e.onlyWhenNoObstacles = 4] = "onlyWhenNoObstacles", e))(x || {});
class R {
/**
* @constructor
* @param {number | Array<Array<number | boolean>>} width_or_matrix - Number of columns of the grid, or matrix
* @param {number} [height] - Number of rows of the grid.
* @param {Array<Array<number | boolean>>} [matrix] - A 0-1 matrix representing the walkable status of the nodes
* (0 or false for walkable). If the matrix is not supplied, all the nodes will be walkable.
*/
constructor(t, s, n) {
let i;
typeof t != "object" ? i = t : (s = t.length, i = t[0].length, n = t), this.width = i, this.height = s, this.nodes = this._buildNodes(i, s, n);
}
/**
* Build and return the nodes.
* @private
* @param {number} width
* @param {number} height
* @param {Array<Array<number | boolean>>} [matrix] - A 0-1 matrix representing the walkable status of the nodes.
* @see Grid
*/
_buildNodes(t, s, n) {
const i = new Array(s);
for (let o = 0; o < s; ++o) {
i[o] = new Array(t);
for (let l = 0; l < t; ++l)
i[o][l] = new F(l, o);
}
if (n === void 0)
return i;
if (n.length !== s || n[0].length !== t)
throw new Error("Matrix size does not fit");
for (let o = 0; o < s; ++o)
for (let l = 0; l < t; ++l)
n[o][l] && (i[o][l].walkable = !1);
return i;
}
getNodeAt(t, s) {
return this.nodes[s][t];
}
/**
* Determine whether the node at the given position is walkable.
* (Also returns false if the position is outside the grid.)
* @param {number} x - The x coordinate of the node.
* @param {number} y - The y coordinate of the node.
* @return {boolean} - The walkability of the node.
*/
isWalkableAt(t, s) {
return this.isInside(t, s) && !!this.nodes[s][t].walkable;
}
/**
* Determine whether the position is inside the grid.
* XXX: `grid.isInside(x, y)` is weird to read.
* It should be `(x, y) is inside grid`, but I failed to find a better
* name for this method.
* @param {number} x
* @param {number} y
* @return {boolean}
*/
isInside(t, s) {
return t >= 0 && t < this.width && s >= 0 && s < this.height;
}
/**
* Set whether the node on the given position is walkable.
* NOTE: throws exception if the coordinate is not inside the grid.
* @param {number} x - The x coordinate of the node.
* @param {number} y - The y coordinate of the node.
* @param {boolean} walkable - Whether the position is walkable.
*/
setWalkableAt(t, s, n) {
this.nodes[s][t].walkable = n;
}
/**
* Get the neighbors of the given node.
*
* offsets diagonalOffsets:
* +---+---+---+ +---+---+---+
* | | 0 | | | 0 | | 1 |
* +---+---+---+ +---+---+---+
* | 3 | | 1 | | | | |
* +---+---+---+ +---+---+---+
* | | 2 | | | 3 | | 2 |
* +---+---+---+ +---+---+---+
*
* When allowDiagonal is true, if offsets[i] is valid, then
* diagonalOffsets[i] and
* diagonalOffsets[(i + 1) % 4] is valid.
* @param {Node} node
* @param {DiagonalMovement} diagonalMovement
*/
getNeighbors(t, s) {
const { x: n, y: i } = t, o = [];
let l = !1, r = !1, a = !1, d = !1, p = !1, b = !1, f = !1, g = !1;
const { nodes: h } = this;
if (this.isWalkableAt(n, i - 1) && (o.push(h[i - 1][n]), l = !0), this.isWalkableAt(n + 1, i) && (o.push(h[i][n + 1]), a = !0), this.isWalkableAt(n, i + 1) && (o.push(h[i + 1][n]), p = !0), this.isWalkableAt(n - 1, i) && (o.push(h[i][n - 1]), f = !0), s === x.never)
return o;
if (s === x.onlyWhenNoObstacles)
r = f && l, d = l && a, b = a && p, g = p && f;
else if (s === x.ifAtMostOneObstacle)
r = f || l, d = l || a, b = a || p, g = p || f;
else if (s === x.always)
r = !0, d = !0, b = !0, g = !0;
else
throw new Error("Incorrect value of diagonalMovement");
return r && this.isWalkableAt(n - 1, i - 1) && o.push(h[i - 1][n - 1]), d && this.isWalkableAt(n + 1, i - 1) && o.push(h[i - 1][n + 1]), b && this.isWalkableAt(n + 1, i + 1) && o.push(h[i + 1][n + 1]), g && this.isWalkableAt(n - 1, i + 1) && o.push(h[i + 1][n - 1]), o;
}
/**
* Get a clone of this grid.
* @return {Grid} Cloned grid.
*/
clone() {
const { width: t, height: s, nodes: n } = this, i = new R(t, s), o = new Array(s);
for (let l = 0; l < s; ++l) {
o[l] = new Array(t);
for (let r = 0; r < t; ++r)
o[l][r] = new F(r, l, n[l][r].walkable);
}
return i.nodes = o, i;
}
}
function K(e) {
const t = [[e.x, e.y]];
for (; e.parent; )
e = e.parent, t.push([e.x, e.y]);
return t.reverse();
}
function U(e) {
if (e.length < 3)
return e;
const t = [], s = e[0][0], n = e[0][1];
let i = e[1][0], o = e[1][1], l = i - s, r = o - n, a, d, p, b, f, g;
for (f = Math.sqrt(l * l + r * r), l /= f, r /= f, t.push([s, n]), g = 2; g < e.length; g++)
a = i, d = o, p = l, b = r, i = e[g][0], o = e[g][1], l = i - a, r = o - d, f = Math.sqrt(l * l + r * r), l /= f, r /= f, (l !== p || r !== b) && t.push([a, d]);
return t.push([i, o]), t;
}
const I = {
/**
* Manhattan distance.
* @param {number} dx - Difference in x.
* @param {number} dy - Difference in y.
* @return {number} dx + dy
*/
manhattan(e, t) {
return e + t;
},
/**
* Euclidean distance.
* @param {number} dx - Difference in x.
* @param {number} dy - Difference in y.
* @return {number} sqrt(dx * dx + dy * dy)
*/
euclidean(e, t) {
return Math.sqrt(e * e + t * t);
},
/**
* Octile distance.
* @param {number} dx - Difference in x.
* @param {number} dy - Difference in y.
* @return {number} sqrt(dx * dx + dy * dy) for grids
*/
octile(e, t) {
const s = Math.SQRT2 - 1;
return e < t ? s * e + t : s * t + e;
},
/**
* Chebyshev distance.
* @param {number} dx - Difference in x.
* @param {number} dy - Difference in y.
* @return {number} max(dx, dy)
*/
chebyshev(e, t) {
return Math.max(e, t);
}
};
class V {
constructor(t = {}) {
this.allowDiagonal = t.allowDiagonal || !1, this.dontCrossCorners = t.dontCrossCorners || !1, this.heuristic = t.heuristic || I.manhattan, this.weight = t.weight || 1, this.diagonalMovement = t.diagonalMovement || x.never, this.diagonalMovement || (this.allowDiagonal ? this.dontCrossCorners ? this.diagonalMovement = x.onlyWhenNoObstacles : this.diagonalMovement = x.ifAtMostOneObstacle : this.diagonalMovement = x.never), this.diagonalMovement === x.never ? this.heuristic = t.heuristic || I.manhattan : this.heuristic = t.heuristic || I.octile;
}
getTurnWeight(t) {
const { parent: s } = t;
if (!s) return 0;
const n = s.parent;
if (!n) return 0;
let i = 0;
return n.x === s.x && t.x !== s.x && (i += 1), n.y === s.y && t.y !== s.y && (i += 1), i;
}
/**
* Find and return the path.
* @return {Array<Array<number>>} The path, including both start and
* end positions.
*/
findPath(t, s, n, i, o) {
const l = new E((G, L) => G.f - L.f), r = o.getNodeAt(t, s), a = o.getNodeAt(n, i), { heuristic: d } = this, { diagonalMovement: p } = this, { weight: b } = this, { abs: f, SQRT2: g } = Math;
let h, w, c, y, A, M, N, O, T = 0;
for (r.g = 0, r.f = 0, r.t = 0, l.push(r), r.opened = !0; !l.empty(); ) {
if (h = l.pop(), h.closed = !0, h === a)
return K(a);
for (w = o.getNeighbors(h, p), y = 0, A = w.length; y < A; ++y)
c = w[y], !c.closed && (M = c.x, N = c.y, O = h.g + (M - h.x === 0 || N - h.y === 0 ? 1 : g), (!c.opened || O < c.g) && (c.parent = h, c.g = O, c.h = c.h || b * d(f(M - n), f(N - i)), c.t = h.t + this.getTurnWeight(c), c.t !== 0 && (T += 0.01, c.t += T), c.f = c.g + c.h + c.t, c.opened ? l.updateItem(c) : (l.push(c), c.opened = !0)));
}
return [];
}
}
const Z = (e) => e.reduce((t, s, n) => n === 0 ? `M${s.x}, ${s.y}` : `${t} L${s.x}, ${s.y}`, ""), X = (e, t) => ({
x: e.x - t,
y: e.y - t,
width: e.width + t * 2,
height: e.height + t * 2
}), Y = (e, t) => e.x >= t.x && e.x <= t.x + t.width && e.y >= t.y && e.y <= t.y + t.height, D = (e, t, s = 0) => {
const { x: n, y: i } = e;
return t.some((o) => Y({ x: n, y: i }, X(o, s)));
}, m = (e, t) => {
let s = 0, n = e.length - 1;
for (; s <= n; ) {
const i = Math.floor((s + n) / 2);
if (e[i] === t)
return i;
e[i] < t ? s = i + 1 : n = i - 1;
}
return -1;
};
let C;
const _ = (e, t, s, n = {}) => {
n = Object.assign({
debug: !1,
padding: 5
}, n);
const i = [
...s.flatMap((h) => [h.x, h.x + h.width]),
e.x,
e.x,
t.x,
t.x
].sort((h, w) => h - w), o = [
...s.flatMap((h) => [h.y, h.y + h.height]),
e.y,
e.y,
t.y,
t.y
].sort((h, w) => h - w), l = i.slice(1).map((h, w) => (h + i[w]) / 2), r = o.slice(1).map((h, w) => (h + o[w]) / 2), a = {
x: m(l, e.x),
y: m(r, e.y)
}, d = {
x: m(l, t.x),
y: m(r, t.y)
}, p = new R(l.length, r.length), b = [];
l.forEach((h, w) => {
r.forEach((c, y) => {
const A = { x: h, y: c };
D(A, s, n.padding) ? p.setWalkableAt(w, y, !1) : b.push(A);
});
}), C || (C = new V());
const f = U(C.findPath(a.x, a.y, d.x, d.y, p)).map((h) => ({ x: l[h[0]], y: r[h[1]] })), g = Z(f);
return {
...n.debug ? {
walkablePoints: b,
xs: i,
ys: o,
indexGrid: p,
startIndexPoint: a,
endIndexPoint: d,
xCenters: l,
yCenters: r
} : {},
path: f,
d: g
};
};
export {
_ as orthogonalPathFinding
};