56 lines
3.0 KiB
JavaScript
56 lines
3.0 KiB
JavaScript
import { Quaternion, Vector3 } from "three";
|
|
|
|
export function sourceNavigationAnchors(recipe, placement) {
|
|
const policy = recipe.navigation?.sourceAnchorComponents;
|
|
if (!policy) return [];
|
|
if (policy.kind !== "native-source-anchor-components-v1"
|
|
|| recipe.navigation.includeDetourTopology !== true) {
|
|
throw new Error("Source anchor components require the supported policy and Detour topology.");
|
|
}
|
|
const finiteVector = (values, length) => Array.isArray(values)
|
|
&& values.length === length && values.every(Number.isFinite);
|
|
if (!finiteVector(placement.translation, 3) || !finiteVector(placement.rotation, 4)
|
|
|| !finiteVector(placement.scale, 3) || placement.scale.some(value => value === 0)
|
|
|| !Number.isFinite(recipe.coordinateSystem.metersPerSourceUnit)
|
|
|| recipe.coordinateSystem.metersPerSourceUnit <= 0) {
|
|
throw new Error("Source navigation anchors require a finite invertible primary WDB placement.");
|
|
}
|
|
const transform = sourcePosition => {
|
|
if (!finiteVector(sourcePosition, 3)) throw new Error("Source navigation anchor requires finite XYZ coordinates.");
|
|
const point = new Vector3(...sourcePosition).sub(new Vector3(...placement.translation))
|
|
.applyQuaternion(new Quaternion(...placement.rotation).invert())
|
|
.divide(new Vector3(...placement.scale));
|
|
const meters = recipe.coordinateSystem.metersPerSourceUnit;
|
|
return [point.x * meters, point.y * meters, -point.z * meters];
|
|
};
|
|
if (!Array.isArray(recipe.source.sourceAnchorSupport?.points)
|
|
|| !Number.isFinite(policy.maximumProjectionDistance)
|
|
|| policy.maximumProjectionDistance <= 0
|
|
|| !Array.isArray(policy.requiredSourcePoints) || !policy.requiredSourcePoints.length) {
|
|
throw new Error("Source navigation selection needs native anchor points, a projection limit and required source guards.");
|
|
}
|
|
const anchors = recipe.source.sourceAnchorSupport.points.map((sourcePosition, index) => ({
|
|
id: `native-source-anchor-${index}`, kind: "native-source-anchor",
|
|
sourcePosition: [...sourcePosition], position: transform(sourcePosition),
|
|
maximumDistance: policy.maximumProjectionDistance,
|
|
}));
|
|
for (const guard of policy.requiredSourcePoints) {
|
|
if (typeof guard.id !== "string" || !guard.id.trim() || !guard.evidence
|
|
|| !Number.isFinite(guard.maximumDistance) || guard.maximumDistance <= 0) {
|
|
throw new Error("Required source navigation guards need an ID, evidence and a projection limit.");
|
|
}
|
|
if (guard.kind === "source-entry"
|
|
&& JSON.stringify(guard.sourcePosition) !== JSON.stringify(recipe.source.entryCoordinates?.position)) {
|
|
throw new Error("The navigation entry guard must match the authoritative source entry.");
|
|
}
|
|
anchors.push({
|
|
...guard, sourcePosition: [...guard.sourcePosition],
|
|
position: transform(guard.sourcePosition), required: true,
|
|
});
|
|
}
|
|
if (new Set(anchors.map(anchor => anchor.id)).size !== anchors.length) {
|
|
throw new Error("Source navigation anchor IDs must be unique.");
|
|
}
|
|
return anchors;
|
|
}
|