907 lines
31 KiB
TypeScript
907 lines
31 KiB
TypeScript
import type { BufferGeometry } from "three";
|
|
|
|
export const PARTY_NAVIGATION_WELD_TOLERANCE = 0.001;
|
|
|
|
export type PartyNavigationPosition = readonly [x: number, y: number, z: number];
|
|
export type PartyNavigationPoint = PartyNavigationPosition | Readonly<{ x: number; y: number; z: number }>;
|
|
|
|
export interface PartyNavigationEdge {
|
|
readonly to: number;
|
|
readonly cost: number;
|
|
}
|
|
|
|
export interface PartyNavigationNode {
|
|
readonly id: number;
|
|
readonly position: PartyNavigationPosition;
|
|
readonly neighbors: readonly PartyNavigationEdge[];
|
|
}
|
|
|
|
export type PartyNavigationTriangle = readonly [a: number, b: number, c: number];
|
|
export type PartyNavigationPortal = readonly [a: number, b: number];
|
|
|
|
export interface PartyNavigationTriangleEdge {
|
|
readonly to: number;
|
|
readonly portal: PartyNavigationPortal;
|
|
readonly cost: number;
|
|
}
|
|
|
|
export interface PartyNavigationTriangleNode {
|
|
readonly id: number;
|
|
readonly centroid: PartyNavigationPosition;
|
|
readonly neighbors: readonly PartyNavigationTriangleEdge[];
|
|
}
|
|
|
|
export interface PartyNavigationGraph {
|
|
readonly nodes: readonly PartyNavigationNode[];
|
|
/** Triangle node indices retained by the builder from the largest connected surface. */
|
|
readonly triangles?: readonly PartyNavigationTriangle[];
|
|
/** Shared-edge adjacency for the retained triangles, in matching index order. */
|
|
readonly triangleNodes?: readonly PartyNavigationTriangleNode[];
|
|
readonly weldTolerance: number;
|
|
readonly sourceVertexCount: number;
|
|
readonly sourceTriangleCount: number;
|
|
/** Number of welded nodes before disconnected islands were discarded. */
|
|
readonly weldedNodeCount: number;
|
|
readonly componentCount: number;
|
|
readonly discardedNodeCount: number;
|
|
}
|
|
|
|
export interface PartyNavigationBuildOptions {
|
|
readonly weldTolerance?: number;
|
|
/** Defensive runtime ceiling; checked before graph allocations begin. */
|
|
readonly maxSourceTriangles?: number;
|
|
readonly offMeshLinks?: readonly {
|
|
readonly start: PartyNavigationPosition;
|
|
readonly end: PartyNavigationPosition;
|
|
readonly bidirectional: boolean;
|
|
}[];
|
|
}
|
|
|
|
export interface PartyNavigationNearestNode {
|
|
readonly node: PartyNavigationNode;
|
|
readonly distance: number;
|
|
}
|
|
|
|
export interface PartyNavigationPathOptions {
|
|
/** Reject endpoints farther than this distance from the retained navigation surface. */
|
|
readonly maxSnapDistance?: number;
|
|
/** Optional start-only override for maxSnapDistance. */
|
|
readonly maxStartSnapDistance?: number;
|
|
/** Optional end-only override for maxSnapDistance. */
|
|
readonly maxEndSnapDistance?: number;
|
|
/** Maximum 3D deviation when removing redundant, collinear waypoints. */
|
|
readonly simplifyTolerance?: number;
|
|
/** Optional collision/clearance validation for each reconstructed corridor segment. */
|
|
readonly routeSegmentAllowed?: (
|
|
start: PartyNavigationPosition,
|
|
end: PartyNavigationPosition,
|
|
) => boolean;
|
|
}
|
|
|
|
interface MutableWeldedNode {
|
|
readonly representative: [number, number, number];
|
|
sumX: number;
|
|
sumY: number;
|
|
sumZ: number;
|
|
samples: number;
|
|
readonly neighbors: Set<number>;
|
|
}
|
|
|
|
interface OpenEntry {
|
|
readonly id: number;
|
|
readonly g: number;
|
|
readonly h: number;
|
|
readonly f: number;
|
|
}
|
|
|
|
export interface PartyNavigationSurfaceMatch {
|
|
readonly position: PartyNavigationPosition;
|
|
readonly triangleId: number;
|
|
readonly distance: number;
|
|
}
|
|
|
|
const DISTANCE_EPSILON = 1e-9;
|
|
|
|
function pointComponents(point: PartyNavigationPoint): PartyNavigationPosition {
|
|
const values: PartyNavigationPosition = "x" in point
|
|
? [point.x, point.y, point.z]
|
|
: [point[0], point[1], point[2]];
|
|
if (!values.every(Number.isFinite)) {
|
|
throw new Error("Party navigation points must contain finite coordinates.");
|
|
}
|
|
return values;
|
|
}
|
|
|
|
function distanceSquared(left: PartyNavigationPosition, right: PartyNavigationPosition): number {
|
|
const dx = right[0] - left[0];
|
|
const dy = right[1] - left[1];
|
|
const dz = right[2] - left[2];
|
|
return dx * dx + dy * dy + dz * dz;
|
|
}
|
|
|
|
function distance(left: PartyNavigationPosition, right: PartyNavigationPosition): number {
|
|
return Math.sqrt(distanceSquared(left, right));
|
|
}
|
|
|
|
function bucketCoordinate(value: number, tolerance: number): number {
|
|
return Math.floor(value / tolerance);
|
|
}
|
|
|
|
function bucketKey(x: number, y: number, z: number): string {
|
|
return `${x},${y},${z}`;
|
|
}
|
|
|
|
function validatedTolerance(value: number | undefined): number {
|
|
const tolerance = value ?? PARTY_NAVIGATION_WELD_TOLERANCE;
|
|
if (!Number.isFinite(tolerance) || tolerance <= 0) {
|
|
throw new Error("Party navigation weld tolerance must be a positive finite number.");
|
|
}
|
|
return tolerance;
|
|
}
|
|
|
|
function connectedComponents(nodes: readonly MutableWeldedNode[]): number[][] {
|
|
const visited = new Uint8Array(nodes.length);
|
|
const components: number[][] = [];
|
|
|
|
for (let seed = 0; seed < nodes.length; seed += 1) {
|
|
if (visited[seed]) continue;
|
|
visited[seed] = 1;
|
|
const pending = [seed];
|
|
const component: number[] = [];
|
|
|
|
while (pending.length) {
|
|
const id = pending.pop()!;
|
|
component.push(id);
|
|
const neighbors = Array.from(nodes[id].neighbors).sort((left, right) => right - left);
|
|
for (const neighbor of neighbors) {
|
|
if (visited[neighbor]) continue;
|
|
visited[neighbor] = 1;
|
|
pending.push(neighbor);
|
|
}
|
|
}
|
|
|
|
component.sort((left, right) => left - right);
|
|
components.push(component);
|
|
}
|
|
|
|
return components;
|
|
}
|
|
|
|
function buildPartyNavigationTriangleNodes(
|
|
triangles: readonly PartyNavigationTriangle[],
|
|
positions: readonly PartyNavigationPosition[],
|
|
): readonly PartyNavigationTriangleNode[] {
|
|
const centroids = triangles.map(([a, b, c]): PartyNavigationPosition => Object.freeze([
|
|
(positions[a][0] + positions[b][0] + positions[c][0]) / 3,
|
|
(positions[a][1] + positions[b][1] + positions[c][1]) / 3,
|
|
(positions[a][2] + positions[b][2] + positions[c][2]) / 3,
|
|
]) as PartyNavigationPosition);
|
|
const edgeOwners = new Map<string, Array<{ triangleId: number; portal: PartyNavigationPortal }>>();
|
|
triangles.forEach(([a, b, c], triangleId) => {
|
|
for (const [left, right] of [[a, b], [b, c], [c, a]] as const) {
|
|
const portal = Object.freeze([
|
|
Math.min(left, right),
|
|
Math.max(left, right),
|
|
]) as PartyNavigationPortal;
|
|
const key = `${portal[0]},${portal[1]}`;
|
|
const owners = edgeOwners.get(key);
|
|
if (owners) owners.push({ triangleId, portal });
|
|
else edgeOwners.set(key, [{ triangleId, portal }]);
|
|
}
|
|
});
|
|
|
|
const neighbors: PartyNavigationTriangleEdge[][] = triangles.map(() => []);
|
|
for (const owners of edgeOwners.values()) {
|
|
for (let leftIndex = 0; leftIndex < owners.length; leftIndex += 1) {
|
|
for (let rightIndex = leftIndex + 1; rightIndex < owners.length; rightIndex += 1) {
|
|
const left = owners[leftIndex];
|
|
const right = owners[rightIndex];
|
|
const cost = distance(centroids[left.triangleId], centroids[right.triangleId]);
|
|
neighbors[left.triangleId].push(Object.freeze({
|
|
to: right.triangleId,
|
|
portal: left.portal,
|
|
cost,
|
|
}));
|
|
neighbors[right.triangleId].push(Object.freeze({
|
|
to: left.triangleId,
|
|
portal: right.portal,
|
|
cost,
|
|
}));
|
|
}
|
|
}
|
|
}
|
|
|
|
return Object.freeze(centroids.map((centroid, id): PartyNavigationTriangleNode => Object.freeze({
|
|
id,
|
|
centroid,
|
|
neighbors: Object.freeze(neighbors[id].sort((left, right) => (
|
|
left.to - right.to
|
|
|| left.portal[0] - right.portal[0]
|
|
|| left.portal[1] - right.portal[1]
|
|
))),
|
|
})));
|
|
}
|
|
|
|
/**
|
|
* Converts an indexed or non-indexed triangle geometry into a deterministic
|
|
* navigation graph. Export seams within one millimeter are welded, then only
|
|
* the largest connected surface is retained so an isolated island cannot win
|
|
* a nearest-node query.
|
|
*/
|
|
export function buildPartyNavigationGraph(
|
|
geometry: BufferGeometry,
|
|
options: PartyNavigationBuildOptions = {},
|
|
): PartyNavigationGraph {
|
|
const tolerance = validatedTolerance(options.weldTolerance);
|
|
const position = geometry.getAttribute("position");
|
|
if (!position || position.itemSize < 3 || position.count < 3) {
|
|
throw new Error("Party navigation geometry requires at least three 3D positions.");
|
|
}
|
|
|
|
const index = geometry.getIndex();
|
|
const indexCount = index?.count ?? position.count;
|
|
if (indexCount < 3 || indexCount % 3 !== 0) {
|
|
throw new Error("Party navigation geometry must contain complete triangles.");
|
|
}
|
|
const sourceTriangleCount = indexCount / 3;
|
|
if (
|
|
options.maxSourceTriangles !== undefined
|
|
&& (
|
|
!Number.isInteger(options.maxSourceTriangles)
|
|
|| options.maxSourceTriangles <= 0
|
|
)
|
|
) {
|
|
throw new Error("Party navigation maximum source triangles must be a positive integer.");
|
|
}
|
|
if (
|
|
options.maxSourceTriangles !== undefined
|
|
&& sourceTriangleCount > options.maxSourceTriangles
|
|
) {
|
|
throw new Error(
|
|
`Party navigation geometry has ${sourceTriangleCount} triangles; maximum is ${options.maxSourceTriangles}.`,
|
|
);
|
|
}
|
|
|
|
const sourceToWelded = new Int32Array(position.count);
|
|
const welded: MutableWeldedNode[] = [];
|
|
const buckets = new Map<string, number[]>();
|
|
const toleranceSquared = tolerance * tolerance;
|
|
|
|
for (let sourceId = 0; sourceId < position.count; sourceId += 1) {
|
|
const x = position.getX(sourceId);
|
|
const y = position.getY(sourceId);
|
|
const z = position.getZ(sourceId);
|
|
if (![x, y, z].every(Number.isFinite)) {
|
|
throw new Error(`Party navigation vertex ${sourceId} contains a non-finite coordinate.`);
|
|
}
|
|
|
|
const bucketX = bucketCoordinate(x, tolerance);
|
|
const bucketY = bucketCoordinate(y, tolerance);
|
|
const bucketZ = bucketCoordinate(z, tolerance);
|
|
let bestId = -1;
|
|
let bestDistanceSquared = Number.POSITIVE_INFINITY;
|
|
|
|
for (let offsetX = -1; offsetX <= 1; offsetX += 1) {
|
|
for (let offsetY = -1; offsetY <= 1; offsetY += 1) {
|
|
for (let offsetZ = -1; offsetZ <= 1; offsetZ += 1) {
|
|
const candidates = buckets.get(bucketKey(
|
|
bucketX + offsetX,
|
|
bucketY + offsetY,
|
|
bucketZ + offsetZ,
|
|
));
|
|
if (!candidates) continue;
|
|
for (const candidateId of candidates) {
|
|
const candidate = welded[candidateId].representative;
|
|
const dx = candidate[0] - x;
|
|
const dy = candidate[1] - y;
|
|
const dz = candidate[2] - z;
|
|
const candidateDistanceSquared = dx * dx + dy * dy + dz * dz;
|
|
if (candidateDistanceSquared > toleranceSquared + DISTANCE_EPSILON) continue;
|
|
if (
|
|
candidateDistanceSquared < bestDistanceSquared - DISTANCE_EPSILON
|
|
|| (Math.abs(candidateDistanceSquared - bestDistanceSquared) <= DISTANCE_EPSILON
|
|
&& candidateId < bestId)
|
|
) {
|
|
bestId = candidateId;
|
|
bestDistanceSquared = candidateDistanceSquared;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (bestId < 0) {
|
|
bestId = welded.length;
|
|
welded.push({
|
|
representative: [x, y, z],
|
|
sumX: x,
|
|
sumY: y,
|
|
sumZ: z,
|
|
samples: 1,
|
|
neighbors: new Set(),
|
|
});
|
|
const key = bucketKey(bucketX, bucketY, bucketZ);
|
|
const contents = buckets.get(key);
|
|
if (contents) contents.push(bestId);
|
|
else buckets.set(key, [bestId]);
|
|
} else {
|
|
const node = welded[bestId];
|
|
node.sumX += x;
|
|
node.sumY += y;
|
|
node.sumZ += z;
|
|
node.samples += 1;
|
|
}
|
|
sourceToWelded[sourceId] = bestId;
|
|
}
|
|
|
|
const sourceIndex = (offset: number): number => {
|
|
const value = index ? index.getX(offset) : offset;
|
|
if (!Number.isInteger(value) || value < 0 || value >= position.count) {
|
|
throw new Error(`Party navigation index ${offset} references an invalid vertex.`);
|
|
}
|
|
return value;
|
|
};
|
|
const connect = (left: number, right: number) => {
|
|
if (left === right) return;
|
|
welded[left].neighbors.add(right);
|
|
welded[right].neighbors.add(left);
|
|
};
|
|
const weldedTriangles: PartyNavigationTriangle[] = [];
|
|
const blockedOffMeshDirections = new Set<string>();
|
|
|
|
for (let offset = 0; offset < indexCount; offset += 3) {
|
|
const a = sourceToWelded[sourceIndex(offset)];
|
|
const b = sourceToWelded[sourceIndex(offset + 1)];
|
|
const c = sourceToWelded[sourceIndex(offset + 2)];
|
|
connect(a, b);
|
|
connect(b, c);
|
|
connect(c, a);
|
|
if (a !== b && b !== c && c !== a) {
|
|
weldedTriangles.push(Object.freeze([a, b, c]));
|
|
}
|
|
}
|
|
|
|
const nearestWeldedNode = (point: PartyNavigationPosition): number => {
|
|
let nearest = 0;
|
|
let nearestDistance = Number.POSITIVE_INFINITY;
|
|
welded.forEach((node, id) => {
|
|
const candidate = distanceSquared(node.representative, point);
|
|
if (candidate < nearestDistance) {
|
|
nearest = id;
|
|
nearestDistance = candidate;
|
|
}
|
|
});
|
|
return nearest;
|
|
};
|
|
for (const link of options.offMeshLinks ?? []) {
|
|
const start = nearestWeldedNode(link.start);
|
|
const end = nearestWeldedNode(link.end);
|
|
connect(start, end); // Weak connectivity determines which authored surfaces are retained.
|
|
if (!link.bidirectional) blockedOffMeshDirections.add(`${end}:${start}`);
|
|
}
|
|
|
|
const components = connectedComponents(welded);
|
|
const largest = components.reduce<number[]>((best, candidate) => {
|
|
if (candidate.length > best.length) return candidate;
|
|
if (candidate.length < best.length) return best;
|
|
return (candidate[0] ?? Number.POSITIVE_INFINITY) < (best[0] ?? Number.POSITIVE_INFINITY)
|
|
? candidate
|
|
: best;
|
|
}, []);
|
|
if (!largest.length) throw new Error("Party navigation geometry produced no usable graph nodes.");
|
|
|
|
const oldToNew = new Int32Array(welded.length);
|
|
oldToNew.fill(-1);
|
|
largest.forEach((oldId, newId) => { oldToNew[oldId] = newId; });
|
|
|
|
const retainedPositions = largest.map((oldId): PartyNavigationPosition => {
|
|
const node = welded[oldId];
|
|
return Object.freeze([
|
|
node.sumX / node.samples,
|
|
node.sumY / node.samples,
|
|
node.sumZ / node.samples,
|
|
]) as PartyNavigationPosition;
|
|
});
|
|
const nodes = largest.map((oldId, id): PartyNavigationNode => {
|
|
const neighbors = Array.from(welded[oldId].neighbors)
|
|
.filter((neighbor) => oldToNew[neighbor] >= 0)
|
|
.filter((neighbor) => !blockedOffMeshDirections.has(`${oldId}:${neighbor}`))
|
|
.sort((left, right) => left - right)
|
|
.map((neighbor): PartyNavigationEdge => {
|
|
const to = oldToNew[neighbor];
|
|
return Object.freeze({ to, cost: distance(retainedPositions[id], retainedPositions[to]) });
|
|
});
|
|
return Object.freeze({
|
|
id,
|
|
position: retainedPositions[id],
|
|
neighbors: Object.freeze(neighbors),
|
|
});
|
|
});
|
|
const triangles = weldedTriangles
|
|
.filter(([a, b, c]) => oldToNew[a] >= 0 && oldToNew[b] >= 0 && oldToNew[c] >= 0)
|
|
.map(([a, b, c]): PartyNavigationTriangle => Object.freeze([
|
|
oldToNew[a],
|
|
oldToNew[b],
|
|
oldToNew[c],
|
|
]));
|
|
if (!triangles.length) {
|
|
throw new Error("Party navigation geometry produced no usable retained triangles.");
|
|
}
|
|
const usesOffMeshLinks = Boolean(options.offMeshLinks?.length);
|
|
const triangleNodes = usesOffMeshLinks ? undefined : buildPartyNavigationTriangleNodes(triangles, retainedPositions);
|
|
|
|
return Object.freeze({
|
|
nodes: Object.freeze(nodes),
|
|
...(usesOffMeshLinks ? {} : {
|
|
triangles: Object.freeze(triangles),
|
|
triangleNodes,
|
|
}),
|
|
weldTolerance: tolerance,
|
|
sourceVertexCount: position.count,
|
|
sourceTriangleCount,
|
|
weldedNodeCount: welded.length,
|
|
componentCount: components.length,
|
|
discardedNodeCount: welded.length - nodes.length,
|
|
});
|
|
}
|
|
|
|
export function findNearestPartyNavigationNode(
|
|
graph: PartyNavigationGraph,
|
|
point: PartyNavigationPoint,
|
|
maxDistance = Number.POSITIVE_INFINITY,
|
|
): PartyNavigationNearestNode | null {
|
|
const target = pointComponents(point);
|
|
const limit = Number.isFinite(maxDistance) ? Math.max(0, maxDistance) : Number.POSITIVE_INFINITY;
|
|
let nearest: PartyNavigationNode | null = null;
|
|
let nearestDistanceSquared = limit * limit;
|
|
|
|
for (const node of graph.nodes) {
|
|
const candidateDistanceSquared = distanceSquared(target, node.position);
|
|
if (
|
|
candidateDistanceSquared < nearestDistanceSquared - DISTANCE_EPSILON
|
|
|| (Math.abs(candidateDistanceSquared - nearestDistanceSquared) <= DISTANCE_EPSILON
|
|
&& (nearest === null || node.id < nearest.id))
|
|
) {
|
|
nearest = node;
|
|
nearestDistanceSquared = candidateDistanceSquared;
|
|
}
|
|
}
|
|
|
|
return nearest ? Object.freeze({ node: nearest, distance: Math.sqrt(nearestDistanceSquared) }) : null;
|
|
}
|
|
|
|
function closestPointOnSegment(
|
|
point: PartyNavigationPosition,
|
|
start: PartyNavigationPosition,
|
|
end: PartyNavigationPosition,
|
|
): PartyNavigationPosition {
|
|
const segmentX = end[0] - start[0];
|
|
const segmentY = end[1] - start[1];
|
|
const segmentZ = end[2] - start[2];
|
|
const lengthSquared = segmentX * segmentX + segmentY * segmentY + segmentZ * segmentZ;
|
|
if (lengthSquared <= DISTANCE_EPSILON) return [start[0], start[1], start[2]];
|
|
const projection = Math.max(0, Math.min(1, (
|
|
(point[0] - start[0]) * segmentX
|
|
+ (point[1] - start[1]) * segmentY
|
|
+ (point[2] - start[2]) * segmentZ
|
|
) / lengthSquared));
|
|
return [
|
|
start[0] + segmentX * projection,
|
|
start[1] + segmentY * projection,
|
|
start[2] + segmentZ * projection,
|
|
];
|
|
}
|
|
|
|
/** Returns the closest point on a filled 3D triangle, including its edges. */
|
|
function closestPointOnTriangle(
|
|
point: PartyNavigationPosition,
|
|
a: PartyNavigationPosition,
|
|
b: PartyNavigationPosition,
|
|
c: PartyNavigationPosition,
|
|
): PartyNavigationPosition {
|
|
const ab: PartyNavigationPosition = [b[0] - a[0], b[1] - a[1], b[2] - a[2]];
|
|
const ac: PartyNavigationPosition = [c[0] - a[0], c[1] - a[1], c[2] - a[2]];
|
|
const crossX = ab[1] * ac[2] - ab[2] * ac[1];
|
|
const crossY = ab[2] * ac[0] - ab[0] * ac[2];
|
|
const crossZ = ab[0] * ac[1] - ab[1] * ac[0];
|
|
if (crossX * crossX + crossY * crossY + crossZ * crossZ <= DISTANCE_EPSILON) {
|
|
const candidates = [
|
|
closestPointOnSegment(point, a, b),
|
|
closestPointOnSegment(point, b, c),
|
|
closestPointOnSegment(point, c, a),
|
|
];
|
|
return candidates.reduce((best, candidate) => (
|
|
distanceSquared(point, candidate) < distanceSquared(point, best) - DISTANCE_EPSILON
|
|
? candidate
|
|
: best
|
|
));
|
|
}
|
|
|
|
const ap: PartyNavigationPosition = [point[0] - a[0], point[1] - a[1], point[2] - a[2]];
|
|
const d1 = ab[0] * ap[0] + ab[1] * ap[1] + ab[2] * ap[2];
|
|
const d2 = ac[0] * ap[0] + ac[1] * ap[1] + ac[2] * ap[2];
|
|
if (d1 <= 0 && d2 <= 0) return [a[0], a[1], a[2]];
|
|
|
|
const bp: PartyNavigationPosition = [point[0] - b[0], point[1] - b[1], point[2] - b[2]];
|
|
const d3 = ab[0] * bp[0] + ab[1] * bp[1] + ab[2] * bp[2];
|
|
const d4 = ac[0] * bp[0] + ac[1] * bp[1] + ac[2] * bp[2];
|
|
if (d3 >= 0 && d4 <= d3) return [b[0], b[1], b[2]];
|
|
|
|
const vc = d1 * d4 - d3 * d2;
|
|
if (vc <= 0 && d1 >= 0 && d3 <= 0) {
|
|
const along = d1 / (d1 - d3);
|
|
return [a[0] + ab[0] * along, a[1] + ab[1] * along, a[2] + ab[2] * along];
|
|
}
|
|
|
|
const cp: PartyNavigationPosition = [point[0] - c[0], point[1] - c[1], point[2] - c[2]];
|
|
const d5 = ab[0] * cp[0] + ab[1] * cp[1] + ab[2] * cp[2];
|
|
const d6 = ac[0] * cp[0] + ac[1] * cp[1] + ac[2] * cp[2];
|
|
if (d6 >= 0 && d5 <= d6) return [c[0], c[1], c[2]];
|
|
|
|
const vb = d5 * d2 - d1 * d6;
|
|
if (vb <= 0 && d2 >= 0 && d6 <= 0) {
|
|
const along = d2 / (d2 - d6);
|
|
return [a[0] + ac[0] * along, a[1] + ac[1] * along, a[2] + ac[2] * along];
|
|
}
|
|
|
|
const va = d3 * d6 - d5 * d4;
|
|
if (va <= 0 && d4 - d3 >= 0 && d5 - d6 >= 0) {
|
|
const along = (d4 - d3) / ((d4 - d3) + (d5 - d6));
|
|
return [
|
|
b[0] + (c[0] - b[0]) * along,
|
|
b[1] + (c[1] - b[1]) * along,
|
|
b[2] + (c[2] - b[2]) * along,
|
|
];
|
|
}
|
|
|
|
const denominator = 1 / (va + vb + vc);
|
|
const alongB = vb * denominator;
|
|
const alongC = vc * denominator;
|
|
return [
|
|
a[0] + ab[0] * alongB + ac[0] * alongC,
|
|
a[1] + ab[1] * alongB + ac[1] * alongC,
|
|
a[2] + ab[2] * alongB + ac[2] * alongC,
|
|
];
|
|
}
|
|
|
|
export function findNearestPartyNavigationSurface(
|
|
graph: PartyNavigationGraph,
|
|
point: PartyNavigationPoint,
|
|
maxDistance = Number.POSITIVE_INFINITY,
|
|
): PartyNavigationSurfaceMatch | null {
|
|
const triangles = graph.triangles;
|
|
if (!triangles?.length) {
|
|
const nearest = findNearestPartyNavigationNode(graph, point, maxDistance);
|
|
return nearest
|
|
? Object.freeze({
|
|
position: nearest.node.position,
|
|
triangleId: -1,
|
|
distance: nearest.distance,
|
|
})
|
|
: null;
|
|
}
|
|
const target = pointComponents(point);
|
|
const limit = Number.isFinite(maxDistance) ? Math.max(0, maxDistance) : Number.POSITIVE_INFINITY;
|
|
let nearestPosition: PartyNavigationPosition | null = null;
|
|
let nearestTriangleId = -1;
|
|
let nearestDistanceSquared = limit * limit;
|
|
|
|
for (let triangleId = 0; triangleId < triangles.length; triangleId += 1) {
|
|
const [a, b, c] = triangles[triangleId];
|
|
const candidate = closestPointOnTriangle(
|
|
target,
|
|
graph.nodes[a].position,
|
|
graph.nodes[b].position,
|
|
graph.nodes[c].position,
|
|
);
|
|
const candidateDistanceSquared = distanceSquared(target, candidate);
|
|
if (
|
|
candidateDistanceSquared < nearestDistanceSquared - DISTANCE_EPSILON
|
|
|| (Math.abs(candidateDistanceSquared - nearestDistanceSquared) <= DISTANCE_EPSILON
|
|
&& (nearestTriangleId < 0 || triangleId < nearestTriangleId))
|
|
) {
|
|
nearestPosition = candidate;
|
|
nearestTriangleId = triangleId;
|
|
nearestDistanceSquared = candidateDistanceSquared;
|
|
}
|
|
}
|
|
|
|
return nearestPosition
|
|
? Object.freeze({
|
|
position: Object.freeze([...nearestPosition]) as PartyNavigationPosition,
|
|
triangleId: nearestTriangleId,
|
|
distance: Math.sqrt(nearestDistanceSquared),
|
|
})
|
|
: null;
|
|
}
|
|
|
|
function pointToSegmentDistance(
|
|
point: PartyNavigationPosition,
|
|
start: PartyNavigationPosition,
|
|
end: PartyNavigationPosition,
|
|
): number {
|
|
return distance(point, closestPointOnSegment(point, start, end));
|
|
}
|
|
|
|
/** Removes duplicate and effectively-collinear points without cutting corners. */
|
|
export function simplifyPartyNavigationPath(
|
|
points: readonly PartyNavigationPoint[],
|
|
tolerance = PARTY_NAVIGATION_WELD_TOLERANCE,
|
|
): readonly PartyNavigationPosition[] {
|
|
const safeTolerance = Number.isFinite(tolerance) ? Math.max(0, tolerance) : 0;
|
|
const simplified: PartyNavigationPosition[] = [];
|
|
|
|
for (const rawPoint of points) {
|
|
const point = Object.freeze([...pointComponents(rawPoint)]) as PartyNavigationPosition;
|
|
if (simplified.length && distance(simplified[simplified.length - 1], point) <= safeTolerance) continue;
|
|
simplified.push(point);
|
|
|
|
while (simplified.length >= 3) {
|
|
const end = simplified[simplified.length - 1];
|
|
const middle = simplified[simplified.length - 2];
|
|
const start = simplified[simplified.length - 3];
|
|
if (pointToSegmentDistance(middle, start, end) > safeTolerance) break;
|
|
simplified.splice(simplified.length - 2, 1);
|
|
}
|
|
}
|
|
|
|
return Object.freeze(simplified);
|
|
}
|
|
|
|
function compareOpenEntries(left: OpenEntry, right: OpenEntry): number {
|
|
return left.f - right.f || left.h - right.h || left.id - right.id;
|
|
}
|
|
|
|
class OpenHeap {
|
|
private readonly entries: OpenEntry[] = [];
|
|
|
|
get length(): number {
|
|
return this.entries.length;
|
|
}
|
|
|
|
push(entry: OpenEntry): void {
|
|
const entries = this.entries;
|
|
entries.push(entry);
|
|
let index = entries.length - 1;
|
|
while (index > 0) {
|
|
const parent = Math.floor((index - 1) / 2);
|
|
if (compareOpenEntries(entries[parent], entry) <= 0) break;
|
|
entries[index] = entries[parent];
|
|
index = parent;
|
|
}
|
|
entries[index] = entry;
|
|
}
|
|
|
|
pop(): OpenEntry | null {
|
|
const entries = this.entries;
|
|
if (!entries.length) return null;
|
|
const first = entries[0];
|
|
const last = entries.pop()!;
|
|
if (entries.length) {
|
|
let index = 0;
|
|
while (true) {
|
|
const left = index * 2 + 1;
|
|
if (left >= entries.length) break;
|
|
const right = left + 1;
|
|
const bestChild = right < entries.length
|
|
&& compareOpenEntries(entries[right], entries[left]) < 0
|
|
? right
|
|
: left;
|
|
if (compareOpenEntries(entries[bestChild], last) >= 0) break;
|
|
entries[index] = entries[bestChild];
|
|
index = bestChild;
|
|
}
|
|
entries[index] = last;
|
|
}
|
|
return first;
|
|
}
|
|
}
|
|
|
|
function findPartyNavigationNodePath(
|
|
graph: PartyNavigationGraph,
|
|
startId: number,
|
|
endId: number,
|
|
): readonly number[] | null {
|
|
if (startId === endId) return Object.freeze([startId]);
|
|
const nodeCount = graph.nodes.length;
|
|
const cameFrom = new Int32Array(nodeCount);
|
|
cameFrom.fill(-1);
|
|
const scores = new Float64Array(nodeCount);
|
|
scores.fill(Number.POSITIVE_INFINITY);
|
|
scores[startId] = 0;
|
|
const closed = new Uint8Array(nodeCount);
|
|
const open = new OpenHeap();
|
|
const initialHeuristic = distance(graph.nodes[startId].position, graph.nodes[endId].position);
|
|
open.push({ id: startId, g: 0, h: initialHeuristic, f: initialHeuristic });
|
|
|
|
while (open.length) {
|
|
const current = open.pop()!;
|
|
if (closed[current.id] || current.g > scores[current.id] + DISTANCE_EPSILON) continue;
|
|
if (current.id === endId) break;
|
|
closed[current.id] = 1;
|
|
|
|
for (const edge of graph.nodes[current.id].neighbors) {
|
|
if (closed[edge.to]) continue;
|
|
const tentative = scores[current.id] + edge.cost;
|
|
if (tentative >= scores[edge.to] - DISTANCE_EPSILON) continue;
|
|
cameFrom[edge.to] = current.id;
|
|
scores[edge.to] = tentative;
|
|
const heuristic = distance(graph.nodes[edge.to].position, graph.nodes[endId].position);
|
|
open.push({ id: edge.to, g: tentative, h: heuristic, f: tentative + heuristic });
|
|
}
|
|
}
|
|
|
|
if (cameFrom[endId] < 0) return null;
|
|
const nodeIds = [endId];
|
|
let cursor = endId;
|
|
while (cursor !== startId && nodeIds.length <= nodeCount) {
|
|
cursor = cameFrom[cursor];
|
|
if (cursor < 0) return null;
|
|
nodeIds.push(cursor);
|
|
}
|
|
if (cursor !== startId) return null;
|
|
nodeIds.reverse();
|
|
return Object.freeze(nodeIds);
|
|
}
|
|
|
|
function findPartyNavigationTrianglePath(
|
|
triangleNodes: readonly PartyNavigationTriangleNode[],
|
|
startId: number,
|
|
endId: number,
|
|
blocked?: Uint8Array,
|
|
): readonly number[] | null {
|
|
if (blocked?.[startId] || blocked?.[endId]) return null;
|
|
if (startId === endId) return Object.freeze([startId]);
|
|
const nodeCount = triangleNodes.length;
|
|
const cameFrom = new Int32Array(nodeCount);
|
|
cameFrom.fill(-1);
|
|
const scores = new Float64Array(nodeCount);
|
|
scores.fill(Number.POSITIVE_INFINITY);
|
|
scores[startId] = 0;
|
|
const closed = new Uint8Array(nodeCount);
|
|
const open = new OpenHeap();
|
|
const initialHeuristic = distance(triangleNodes[startId].centroid, triangleNodes[endId].centroid);
|
|
open.push({ id: startId, g: 0, h: initialHeuristic, f: initialHeuristic });
|
|
|
|
while (open.length) {
|
|
const current = open.pop()!;
|
|
if (closed[current.id] || current.g > scores[current.id] + DISTANCE_EPSILON) continue;
|
|
if (current.id === endId) break;
|
|
closed[current.id] = 1;
|
|
|
|
for (const edge of triangleNodes[current.id].neighbors) {
|
|
if (blocked?.[edge.to] || closed[edge.to]) continue;
|
|
const tentative = scores[current.id] + edge.cost;
|
|
if (tentative >= scores[edge.to] - DISTANCE_EPSILON) continue;
|
|
cameFrom[edge.to] = current.id;
|
|
scores[edge.to] = tentative;
|
|
const heuristic = distance(triangleNodes[edge.to].centroid, triangleNodes[endId].centroid);
|
|
open.push({ id: edge.to, g: tentative, h: heuristic, f: tentative + heuristic });
|
|
}
|
|
}
|
|
|
|
if (cameFrom[endId] < 0) return null;
|
|
const triangleIds = [endId];
|
|
let cursor = endId;
|
|
while (cursor !== startId && triangleIds.length <= nodeCount) {
|
|
cursor = cameFrom[cursor];
|
|
if (cursor < 0) return null;
|
|
triangleIds.push(cursor);
|
|
}
|
|
if (cursor !== startId) return null;
|
|
triangleIds.reverse();
|
|
return Object.freeze(triangleIds);
|
|
}
|
|
|
|
/** Compatibility for hand-authored graphs that predate retained triangle data. */
|
|
function findPartyNavigationNodeSnappedPath(
|
|
graph: PartyNavigationGraph,
|
|
start: PartyNavigationPoint,
|
|
end: PartyNavigationPoint,
|
|
options: PartyNavigationPathOptions,
|
|
): readonly PartyNavigationPosition[] | null {
|
|
const maxSnapDistance = options.maxSnapDistance ?? Number.POSITIVE_INFINITY;
|
|
const startMatch = findNearestPartyNavigationNode(
|
|
graph,
|
|
start,
|
|
options.maxStartSnapDistance ?? maxSnapDistance,
|
|
);
|
|
const endMatch = findNearestPartyNavigationNode(
|
|
graph,
|
|
end,
|
|
options.maxEndSnapDistance ?? maxSnapDistance,
|
|
);
|
|
if (!startMatch || !endMatch) return null;
|
|
const nodeIds = findPartyNavigationNodePath(graph, startMatch.node.id, endMatch.node.id);
|
|
return nodeIds
|
|
? simplifyPartyNavigationPath(
|
|
nodeIds.map((id) => graph.nodes[id].position),
|
|
options.simplifyTolerance ?? graph.weldTolerance,
|
|
)
|
|
: null;
|
|
}
|
|
|
|
/**
|
|
* Projects both endpoints to the retained surface, runs deterministic A* over
|
|
* triangle centroids, and crosses each shared edge through its midpoint.
|
|
*/
|
|
export function findPartyNavigationPath(
|
|
graph: PartyNavigationGraph,
|
|
start: PartyNavigationPoint,
|
|
end: PartyNavigationPoint,
|
|
options: PartyNavigationPathOptions = {},
|
|
): readonly PartyNavigationPosition[] | null {
|
|
if (!graph.triangles?.length) {
|
|
return findPartyNavigationNodeSnappedPath(graph, start, end, options);
|
|
}
|
|
const maxSnapDistance = options.maxSnapDistance ?? Number.POSITIVE_INFINITY;
|
|
const startMatch = findNearestPartyNavigationSurface(
|
|
graph,
|
|
start,
|
|
options.maxStartSnapDistance ?? maxSnapDistance,
|
|
);
|
|
const endMatch = findNearestPartyNavigationSurface(
|
|
graph,
|
|
end,
|
|
options.maxEndSnapDistance ?? maxSnapDistance,
|
|
);
|
|
if (!startMatch || !endMatch) return null;
|
|
|
|
const triangleNodes = graph.triangleNodes?.length === graph.triangles.length
|
|
? graph.triangleNodes
|
|
: buildPartyNavigationTriangleNodes(
|
|
graph.triangles,
|
|
graph.nodes.map((node) => node.position),
|
|
);
|
|
const blocked = new Uint8Array(triangleNodes.length);
|
|
const maximumReroutes = Math.min(64, triangleNodes.length);
|
|
for (let reroute = 0; reroute <= maximumReroutes; reroute += 1) {
|
|
const triangleIds = findPartyNavigationTrianglePath(
|
|
triangleNodes,
|
|
startMatch.triangleId,
|
|
endMatch.triangleId,
|
|
blocked,
|
|
);
|
|
if (!triangleIds) return null;
|
|
|
|
const path: PartyNavigationPosition[] = [startMatch.position];
|
|
for (let index = 0; index < triangleIds.length - 1; index += 1) {
|
|
const from = triangleIds[index];
|
|
const to = triangleIds[index + 1];
|
|
const transition = triangleNodes[from].neighbors.find((edge) => edge.to === to);
|
|
if (!transition) return null;
|
|
const [a, b] = transition.portal;
|
|
const left = graph.nodes[a].position;
|
|
const right = graph.nodes[b].position;
|
|
path.push(Object.freeze([
|
|
(left[0] + right[0]) / 2,
|
|
(left[1] + right[1]) / 2,
|
|
(left[2] + right[2]) / 2,
|
|
]) as PartyNavigationPosition);
|
|
}
|
|
path.push(endMatch.position);
|
|
|
|
let rejectedSegment = -1;
|
|
if (options.routeSegmentAllowed) {
|
|
for (let index = 0; index < path.length - 1; index += 1) {
|
|
if (!options.routeSegmentAllowed(path[index], path[index + 1])) {
|
|
rejectedSegment = index;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
if (rejectedSegment < 0) return Object.freeze(path);
|
|
if (triangleIds.length <= 1 || reroute >= maximumReroutes) return null;
|
|
|
|
const blockedTriangleId = rejectedSegment === 0
|
|
? triangleIds[1]
|
|
: rejectedSegment === triangleIds.length - 1
|
|
? triangleIds[triangleIds.length - 2]
|
|
: triangleIds[rejectedSegment];
|
|
if (blocked[blockedTriangleId]) return null;
|
|
blocked[blockedTriangleId] = 1;
|
|
}
|
|
|
|
return null;
|
|
}
|