diff --git a/docs/CURRENT-STATE.md b/docs/CURRENT-STATE.md index df8bf6d1e..96daeb09a 100644 --- a/docs/CURRENT-STATE.md +++ b/docs/CURRENT-STATE.md @@ -6008,6 +6008,12 @@ in Node, plus camera traces recorded from Cesium's own `Camera` into checked against `goldens/visual/` on the `/dev/conformance/` page. See [RENDERER-CONTRACT.md](RENDERER-CONTRACT.md). +`src/geo/` (export `./geo`) is the renderer-neutral maths: WGS84, ECEF and +geodetic conversions, surface normals and east-north-up frames, as plain +`{x, y, z}` objects. It follows Cesium's formulas step for step, and +`geo.test.mjs` checks it against Cesium. The stub renderer's maths is built +on it. + ## Operational Notes - **Earthquake discs are STATIC geometry.** Every quake is a `CLAMP_TO_GROUND` diff --git a/package.json b/package.json index aecb52ce5..d81398cce 100644 --- a/package.json +++ b/package.json @@ -295,6 +295,7 @@ "./tools/services": "./src/tools/services.js", "./view": "./src/view/index.js", "./agent/world-packet": "./src/agent/worldPacket.js", + "./geo": "./src/geo/index.js", "./render/contract": "./src/render/contract/index.js", "./render/stub": "./src/render/stub/index.js", "./render/conformance": "./src/render/conformance/index.js" diff --git a/scripts/package-boundaries.json b/scripts/package-boundaries.json index 73fb261b5..5a1b4e55f 100644 --- a/scripts/package-boundaries.json +++ b/scripts/package-boundaries.json @@ -5127,6 +5127,16 @@ "modules": ["src/queries/heights.js"], "external": ["cesium"] }, + "geo": { + "exports": ["./geo"], + "modules": [ + "src/geo/index.js", + "src/geo/vector.js", + "src/geo/ellipsoid.js", + "src/geo/frames.js" + ], + "external": [] + }, "render": { "exports": ["./render/contract", "./render/stub", "./render/conformance"], "modules": [ @@ -5140,6 +5150,10 @@ "src/render/conformance/expect.js", "src/render/conformance/checks/ports.js", "src/render/stub/math.js", + "src/geo/index.js", + "src/geo/vector.js", + "src/geo/ellipsoid.js", + "src/geo/frames.js", "src/render/stub/motion.js", "src/render/stub/camera.js", "src/render/stub/geodesic.js", diff --git a/src/geo/ellipsoid.js b/src/geo/ellipsoid.js new file mode 100644 index 000000000..45d67da0d --- /dev/null +++ b/src/geo/ellipsoid.js @@ -0,0 +1,148 @@ +/** + * @module geo/ellipsoid + * @description The WGS84 ellipsoid: geodetic and Earth-centred, Earth-fixed + * (ECEF) conversions, surface normals and projection onto the surface. The + * formulas follow the default renderer's step for step, so they agree with + * it to floating-point noise. + */ +import { + add, + clone, + dot, + magnitude, + normalize, + scale, + sub, + toDegrees, + toRadians, + vec, +} from './vector.js'; + +export const WGS84 = Object.freeze({ + radii: Object.freeze({ x: 6378137.0, y: 6378137.0, z: 6356752.3142451793 }), + radiiSquared: Object.freeze({ + x: 6378137.0 * 6378137.0, + y: 6378137.0 * 6378137.0, + z: 6356752.3142451793 * 6356752.3142451793, + }), + oneOverRadii: Object.freeze({ + x: 1 / 6378137.0, + y: 1 / 6378137.0, + z: 1 / 6356752.3142451793, + }), + oneOverRadiiSquared: Object.freeze({ + x: 1 / (6378137.0 * 6378137.0), + y: 1 / (6378137.0 * 6378137.0), + z: 1 / (6356752.3142451793 * 6356752.3142451793), + }), +}); + +const EPSILON12 = 1e-12; +const CENTER_TOLERANCE_SQUARED = 0.1; + +export function geodeticSurfaceNormal(p) { + const o = WGS84.oneOverRadiiSquared; + return normalize(vec(p.x * o.x, p.y * o.y, p.z * o.z)); +} + +function geodeticSurfaceNormalCartographic(lon, lat) { + const cosLat = Math.cos(lat); + return normalize( + vec(cosLat * Math.cos(lon), cosLat * Math.sin(lon), Math.sin(lat)), + ); +} + +/** Geodetic radians and metres to ECEF. */ +export function fromRadians(lon, lat, height = 0) { + const n = geodeticSurfaceNormalCartographic(lon, lat); + const r = WGS84.radiiSquared; + let k = vec(r.x * n.x, r.y * n.y, r.z * n.z); + const gamma = Math.sqrt(dot(n, k)); + k = scale(k, 1 / gamma); + return add(k, scale(n, height)); +} + +/** Geodetic degrees and metres to ECEF. */ +export const fromDegrees = (lon, lat, height = 0) => + fromRadians(toRadians(lon), toRadians(lat), height); + +/** A position in either contract form, as ECEF. */ +export function toEcef(position) { + if ('lon' in position && 'lat' in position) + return fromDegrees(position.lon, position.lat, position.height ?? 0); + return clone(position); +} + +/** The point on the ellipsoid along the geodetic normal, or null at the centre. */ +export function scaleToGeodeticSurface(p) { + const { x: positionX, y: positionY, z: positionZ } = p; + const oneOverRadiiX = WGS84.oneOverRadii.x; + const oneOverRadiiY = WGS84.oneOverRadii.y; + const oneOverRadiiZ = WGS84.oneOverRadii.z; + const x2 = positionX * positionX * oneOverRadiiX * oneOverRadiiX; + const y2 = positionY * positionY * oneOverRadiiY * oneOverRadiiY; + const z2 = positionZ * positionZ * oneOverRadiiZ * oneOverRadiiZ; + const squaredNorm = x2 + y2 + z2; + const ratio = Math.sqrt(1.0 / squaredNorm); + const intersection = scale(p, ratio); + if (squaredNorm < CENTER_TOLERANCE_SQUARED) + return Number.isFinite(ratio) ? intersection : null; + + const oneOverRadiiSquaredX = WGS84.oneOverRadiiSquared.x; + const oneOverRadiiSquaredY = WGS84.oneOverRadiiSquared.y; + const oneOverRadiiSquaredZ = WGS84.oneOverRadiiSquared.z; + const gradient = vec( + intersection.x * oneOverRadiiSquaredX * 2.0, + intersection.y * oneOverRadiiSquaredY * 2.0, + intersection.z * oneOverRadiiSquaredZ * 2.0, + ); + let lambda = ((1.0 - ratio) * magnitude(p)) / (0.5 * magnitude(gradient)); + let correction = 0.0; + let func, denominator, xMultiplier, yMultiplier, zMultiplier; + let xMultiplier2, yMultiplier2, zMultiplier2; + let xMultiplier3, yMultiplier3, zMultiplier3; + do { + lambda -= correction; + xMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredX); + yMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredY); + zMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredZ); + xMultiplier2 = xMultiplier * xMultiplier; + yMultiplier2 = yMultiplier * yMultiplier; + zMultiplier2 = zMultiplier * zMultiplier; + xMultiplier3 = xMultiplier2 * xMultiplier; + yMultiplier3 = yMultiplier2 * yMultiplier; + zMultiplier3 = zMultiplier2 * zMultiplier; + func = x2 * xMultiplier2 + y2 * yMultiplier2 + z2 * zMultiplier2 - 1.0; + denominator = + x2 * xMultiplier3 * oneOverRadiiSquaredX + + y2 * yMultiplier3 * oneOverRadiiSquaredY + + z2 * zMultiplier3 * oneOverRadiiSquaredZ; + const derivative = -2.0 * denominator; + correction = func / derivative; + } while (Math.abs(func) > EPSILON12); + return vec( + positionX * xMultiplier, + positionY * yMultiplier, + positionZ * zMultiplier, + ); +} + +/** ECEF to geodetic `{lon, lat}` radians and `height` metres, or null at the centre. */ +export function toCartographicRadians(p) { + const surface = scaleToGeodeticSurface(p); + if (!surface) return null; + const n = geodeticSurfaceNormal(surface); + const h = sub(p, surface); + return { + lon: Math.atan2(n.y, n.x), + lat: Math.asin(n.z), + height: Math.sign(dot(h, p)) * magnitude(h), + }; +} + +/** ECEF to `{lon, lat}` degrees and `height` metres. */ +export function toLonLatH(p) { + const c = toCartographicRadians(p); + if (!c) return null; + return { lon: toDegrees(c.lon), lat: toDegrees(c.lat), height: c.height }; +} diff --git a/src/geo/frames.js b/src/geo/frames.js new file mode 100644 index 000000000..f0a94829b --- /dev/null +++ b/src/geo/frames.js @@ -0,0 +1,92 @@ +/** + * @module geo/frames + * @description Rigid frames (unit axes and an origin, local to ECEF), the + * east-north-up frame at a point, and rotation about an axis. + */ +import { geodeticSurfaceNormal } from './ellipsoid.js'; +import { + EPSILON14, + add, + clone, + cross, + dot, + equalsEpsilon, + normalize, + sub, + vec, +} from './vector.js'; + +/** + * A rigid frame: unit axes `x`, `y`, `z` (columns) and an `origin`, mapping + * local coordinates to ECEF. + * @typedef {{x: object, y: object, z: object, origin: object}} Frame + */ + +export const IDENTITY = Object.freeze({ + x: Object.freeze(vec(1, 0, 0)), + y: Object.freeze(vec(0, 1, 0)), + z: Object.freeze(vec(0, 0, 1)), + origin: Object.freeze(vec(0, 0, 0)), +}); + +export const isIdentity = (frame) => + frame === IDENTITY || + (frame.x.x === 1 && + frame.y.y === 1 && + frame.z.z === 1 && + frame.x.y === 0 && + frame.x.z === 0 && + frame.y.x === 0 && + frame.y.z === 0 && + frame.z.x === 0 && + frame.z.y === 0 && + frame.origin.x === 0 && + frame.origin.y === 0 && + frame.origin.z === 0); + +/** East-north-up at `origin`. */ +export function eastNorthUp(origin) { + if ( + equalsEpsilon(origin.x, 0, EPSILON14) && + equalsEpsilon(origin.y, 0, EPSILON14) + ) { + // At a pole: east is +y, north points away from the pole. + const sign = Math.sign(origin.z) || 1; + return { + x: vec(0, 1, 0), + y: vec(-sign, 0, 0), + z: vec(0, 0, sign), + origin: clone(origin), + }; + } + const up = geodeticSurfaceNormal(origin); + const east = normalize(vec(-origin.y, origin.x, 0)); + const north = cross(up, east); + return { x: east, y: north, z: up, origin: clone(origin) }; +} + +export const frameVector = (frame, v) => + vec( + frame.x.x * v.x + frame.y.x * v.y + frame.z.x * v.z, + frame.x.y * v.x + frame.y.y * v.y + frame.z.y * v.z, + frame.x.z * v.x + frame.y.z * v.y + frame.z.z * v.z, + ); +export const framePoint = (frame, p) => + add(frameVector(frame, p), frame.origin); +export const inverseFrameVector = (frame, v) => + vec(dot(frame.x, v), dot(frame.y, v), dot(frame.z, v)); +export const inverseFramePoint = (frame, p) => + inverseFrameVector(frame, sub(p, frame.origin)); + +/** Rotate `v` about unit `axis` by `angle` (right-handed). */ +export function rotate(v, axis, angle) { + const c = Math.cos(angle); + const s = Math.sin(angle); + const d = dot(axis, v); + const k = cross(axis, v); + return vec( + v.x * c + k.x * s + axis.x * d * (1 - c), + v.y * c + k.y * s + axis.y * d * (1 - c), + v.z * c + k.z * s + axis.z * d * (1 - c), + ); +} diff --git a/src/geo/geo.test.mjs b/src/geo/geo.test.mjs new file mode 100644 index 000000000..4085918ce --- /dev/null +++ b/src/geo/geo.test.mjs @@ -0,0 +1,121 @@ +import assert from 'node:assert/strict'; +import test from 'node:test'; +import * as Cesium from 'cesium'; +import { + eastNorthUp, + fromDegrees, + geodeticSurfaceNormal, + scaleToGeodeticSurface, + toCartographicRadians, + toLonLatH, +} from './index.js'; + +/** A seeded generator, so a failure names a reproducible point. */ +function random(seed) { + let s = seed >>> 0; + return () => { + s = (s * 1664525 + 1013904223) >>> 0; + return s / 2 ** 32; + }; +} + +function points(count = 500) { + const next = random(20261105); + const out = [ + { lon: 0, lat: 90, height: 0 }, + { lon: 0, lat: -90, height: 1000 }, + { lon: 180, lat: 0, height: -400 }, + { lon: -97.7431, lat: 30.2672, height: 150 }, + ]; + for (let i = 0; i < count; i++) + out.push({ + lon: next() * 360 - 180, + lat: next() * 180 - 90, + height: next() * 4e7 - 1e4, + }); + return out; +} + +const close = (actual, expected, tolerance, message) => + assert.ok( + Math.abs(actual - expected) <= tolerance, + `${message}: ${actual} vs ${expected} (± ${tolerance})`, + ); +const closeVec = (a, b, tolerance, message) => { + close(a.x, b.x, tolerance, `${message} x`); + close(a.y, b.y, tolerance, `${message} y`); + close(a.z, b.z, tolerance, `${message} z`); +}; + +test('geodetic to ECEF matches the default renderer', () => { + for (const p of points()) { + const ours = fromDegrees(p.lon, p.lat, p.height); + const theirs = Cesium.Cartesian3.fromDegrees(p.lon, p.lat, p.height); + closeVec(ours, theirs, 1e-6, `${p.lon},${p.lat},${p.height}`); + } +}); + +test('ECEF to geodetic matches the default renderer', () => { + for (const p of points()) { + const ecef = Cesium.Cartesian3.fromDegrees(p.lon, p.lat, p.height); + const ours = toCartographicRadians(ecef); + const theirs = Cesium.Cartographic.fromCartesian(ecef); + const where = `${p.lon},${p.lat},${p.height}`; + // Longitude is undefined at a pole. + if (Math.abs(p.lat) < 89.999) + close(ours.lon, theirs.longitude, 1e-12, `${where} lon`); + close(ours.lat, theirs.latitude, 1e-12, `${where} lat`); + close(ours.height, theirs.height, 1e-6, `${where} height`); + } + assert.equal(toCartographicRadians({ x: 0, y: 0, z: 0 }), null); + assert.equal( + Cesium.Cartographic.fromCartesian(Cesium.Cartesian3.ZERO), + undefined, + ); + const deg = toLonLatH(fromDegrees(-97.7431, 30.2672, 150)); + close(deg.lon, -97.7431, 1e-9, 'degrees lon'); + close(deg.lat, 30.2672, 1e-9, 'degrees lat'); + close(deg.height, 150, 1e-6, 'degrees height'); +}); + +test('surface normals and projection match the default renderer', () => { + for (const p of points()) { + const ecef = Cesium.Cartesian3.fromDegrees(p.lon, p.lat, p.height); + const where = `${p.lon},${p.lat},${p.height}`; + closeVec( + geodeticSurfaceNormal(ecef), + Cesium.Ellipsoid.WGS84.geodeticSurfaceNormal(ecef), + 1e-15, + `${where} normal`, + ); + closeVec( + scaleToGeodeticSurface(ecef), + Cesium.Ellipsoid.WGS84.scaleToGeodeticSurface(ecef), + 1e-6, + `${where} surface`, + ); + } +}); + +test('east-north-up frames match the default renderer', () => { + const column = (m, i) => ({ x: m[i * 4], y: m[i * 4 + 1], z: m[i * 4 + 2] }); + const origins = [ + ...points(200).map((p) => + Cesium.Cartesian3.fromDegrees(p.lon, p.lat, p.height), + ), + // Exactly on the axis, where east is chosen by convention. + new Cesium.Cartesian3(0, 0, 6356752.3142451793), + new Cesium.Cartesian3(0, 0, -7000000), + ]; + for (const origin of origins) { + const m = Cesium.Matrix4.toArray( + Cesium.Transforms.eastNorthUpToFixedFrame(origin), + ); + const ours = eastNorthUp(origin); + const where = `${origin.x},${origin.y},${origin.z}`; + closeVec(ours.x, column(m, 0), 1e-12, `${where} east`); + closeVec(ours.y, column(m, 1), 1e-12, `${where} north`); + closeVec(ours.z, column(m, 2), 1e-12, `${where} up`); + closeVec(ours.origin, column(m, 3), 0, `${where} origin`); + } +}); diff --git a/src/geo/index.js b/src/geo/index.js new file mode 100644 index 000000000..6eec330aa --- /dev/null +++ b/src/geo/index.js @@ -0,0 +1,10 @@ +/** + * @module geo + * @description Renderer-neutral geodesy and vector maths: WGS84, ECEF and + * geodetic conversions, east-north-up frames, quaternions, and the sun and + * moon. Plain numbers and `{x, y, z}` objects only, so code that needs maths + * but not a renderer does not import one. + */ +export * from './vector.js'; +export * from './ellipsoid.js'; +export * from './frames.js'; diff --git a/src/geo/vector.js b/src/geo/vector.js new file mode 100644 index 000000000..ad4ef8963 --- /dev/null +++ b/src/geo/vector.js @@ -0,0 +1,45 @@ +/** + * @module geo/vector + * @description Three-vectors as plain `{x, y, z}` objects, and the angle + * helpers the rest of `geo` uses. No engine. + */ + +export const TWO_PI = Math.PI * 2; +export const PI_OVER_TWO = Math.PI / 2; +export const EPSILON14 = 1e-14; + +export const vec = (x = 0, y = 0, z = 0) => ({ x, y, z }); +export const clone = (a) => ({ x: a.x, y: a.y, z: a.z }); +export const add = (a, b) => vec(a.x + b.x, a.y + b.y, a.z + b.z); +export const sub = (a, b) => vec(a.x - b.x, a.y - b.y, a.z - b.z); +export const scale = (a, s) => vec(a.x * s, a.y * s, a.z * s); +export const negate = (a) => vec(-a.x, -a.y, -a.z); +export const dot = (a, b) => a.x * b.x + a.y * b.y + a.z * b.z; +export const cross = (a, b) => + vec(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); +export const magnitude = (a) => Math.sqrt(dot(a, a)); +export const distance = (a, b) => magnitude(sub(a, b)); +export function normalize(a) { + const m = magnitude(a); + return vec(a.x / m, a.y / m, a.z / m); +} + +export const toRadians = (degrees) => (degrees * Math.PI) / 180; +export const toDegrees = (radians) => (radians * 180) / Math.PI; +export const clamp = (v, lo, hi) => Math.min(hi, Math.max(lo, v)); +export const acosClamped = (v) => Math.acos(clamp(v, -1, 1)); +export const equalsEpsilon = (a, b, eps) => Math.abs(a - b) <= eps; + +/** An angle in [0, 2π). */ +export function zeroToTwoPi(angle) { + if (angle >= 0 && angle <= TWO_PI) return angle; + const mod = ((angle % TWO_PI) + TWO_PI) % TWO_PI; + if (Math.abs(mod) < EPSILON14 && Math.abs(angle) > EPSILON14) return TWO_PI; + return mod; +} + +/** An angle in [-π, π]. */ +export function negativePiToPi(angle) { + if (angle >= -Math.PI && angle <= Math.PI) return angle; + return zeroToTwoPi(angle + Math.PI) - Math.PI; +} diff --git a/src/render/stub/math.js b/src/render/stub/math.js index 391f1b0cf..4fa6665b1 100644 --- a/src/render/stub/math.js +++ b/src/render/stub/math.js @@ -1,264 +1,29 @@ /** * @module render/stub/math - * @description The geodesy and vector maths the stub renderer poses its - * camera with: WGS84, east-north-up frames, rigid transforms and the camera - * angle conventions of the contract (heading clockwise from north, pitch - * negative looking down). Plain objects and numbers, no engine. + * @description The maths the stub renderer poses its camera with: the shared + * geodesy (`src/geo`) and the camera angle conventions of the contract + * (heading clockwise from north, pitch negative looking down). * * The formulas follow the default renderer's, step for step, so the stub's * poses match its reference traces to floating-point noise. */ +import { + PI_OVER_TWO, + TWO_PI, + acosClamped, + clamp, + equalsEpsilon, + negate, + rotate, + scale, + vec, + zeroToTwoPi, +} from '../../geo/index.js'; -export const WGS84 = Object.freeze({ - radii: Object.freeze({ x: 6378137.0, y: 6378137.0, z: 6356752.3142451793 }), - radiiSquared: Object.freeze({ - x: 6378137.0 * 6378137.0, - y: 6378137.0 * 6378137.0, - z: 6356752.3142451793 * 6356752.3142451793, - }), - oneOverRadii: Object.freeze({ - x: 1 / 6378137.0, - y: 1 / 6378137.0, - z: 1 / 6356752.3142451793, - }), - oneOverRadiiSquared: Object.freeze({ - x: 1 / (6378137.0 * 6378137.0), - y: 1 / (6378137.0 * 6378137.0), - z: 1 / (6356752.3142451793 * 6356752.3142451793), - }), -}); +export * from '../../geo/index.js'; -export const TWO_PI = Math.PI * 2; -export const PI_OVER_TWO = Math.PI / 2; const EPSILON3 = 1e-3; const EPSILON10 = 1e-10; -const EPSILON12 = 1e-12; -const EPSILON14 = 1e-14; -const CENTER_TOLERANCE_SQUARED = 0.1; - -export const vec = (x = 0, y = 0, z = 0) => ({ x, y, z }); -export const clone = (a) => ({ x: a.x, y: a.y, z: a.z }); -export const add = (a, b) => vec(a.x + b.x, a.y + b.y, a.z + b.z); -export const sub = (a, b) => vec(a.x - b.x, a.y - b.y, a.z - b.z); -export const scale = (a, s) => vec(a.x * s, a.y * s, a.z * s); -export const negate = (a) => vec(-a.x, -a.y, -a.z); -export const dot = (a, b) => a.x * b.x + a.y * b.y + a.z * b.z; -export const cross = (a, b) => - vec(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); -export const magnitude = (a) => Math.sqrt(dot(a, a)); -export const distance = (a, b) => magnitude(sub(a, b)); -export function normalize(a) { - const m = magnitude(a); - return vec(a.x / m, a.y / m, a.z / m); -} - -export const toRadians = (degrees) => (degrees * Math.PI) / 180; -export const toDegrees = (radians) => (radians * 180) / Math.PI; -export const clamp = (v, lo, hi) => Math.min(hi, Math.max(lo, v)); -export const acosClamped = (v) => Math.acos(clamp(v, -1, 1)); -export const equalsEpsilon = (a, b, eps) => Math.abs(a - b) <= eps; - -/** An angle in [0, 2π). */ -export function zeroToTwoPi(angle) { - if (angle >= 0 && angle <= TWO_PI) return angle; - const mod = ((angle % TWO_PI) + TWO_PI) % TWO_PI; - if (Math.abs(mod) < EPSILON14 && Math.abs(angle) > EPSILON14) return TWO_PI; - return mod; -} - -/** An angle in [-π, π]. */ -export function negativePiToPi(angle) { - if (angle >= -Math.PI && angle <= Math.PI) return angle; - return zeroToTwoPi(angle + Math.PI) - Math.PI; -} - -// --- Geodesy ----------------------------------------------------------------- - -export function geodeticSurfaceNormal(p) { - const o = WGS84.oneOverRadiiSquared; - return normalize(vec(p.x * o.x, p.y * o.y, p.z * o.z)); -} - -function geodeticSurfaceNormalCartographic(lon, lat) { - const cosLat = Math.cos(lat); - return normalize( - vec(cosLat * Math.cos(lon), cosLat * Math.sin(lon), Math.sin(lat)), - ); -} - -/** Geodetic radians and metres to ECEF. */ -export function fromRadians(lon, lat, height = 0) { - const n = geodeticSurfaceNormalCartographic(lon, lat); - const r = WGS84.radiiSquared; - let k = vec(r.x * n.x, r.y * n.y, r.z * n.z); - const gamma = Math.sqrt(dot(n, k)); - k = scale(k, 1 / gamma); - return add(k, scale(n, height)); -} - -/** Geodetic degrees and metres to ECEF. */ -export const fromDegrees = (lon, lat, height = 0) => - fromRadians(toRadians(lon), toRadians(lat), height); - -/** A position in either contract form, as ECEF. */ -export function toEcef(position) { - if ('lon' in position && 'lat' in position) - return fromDegrees(position.lon, position.lat, position.height ?? 0); - return clone(position); -} - -/** The point on the ellipsoid along the geodetic normal, or null at the centre. */ -export function scaleToGeodeticSurface(p) { - const { x: positionX, y: positionY, z: positionZ } = p; - const oneOverRadiiX = WGS84.oneOverRadii.x; - const oneOverRadiiY = WGS84.oneOverRadii.y; - const oneOverRadiiZ = WGS84.oneOverRadii.z; - const x2 = positionX * positionX * oneOverRadiiX * oneOverRadiiX; - const y2 = positionY * positionY * oneOverRadiiY * oneOverRadiiY; - const z2 = positionZ * positionZ * oneOverRadiiZ * oneOverRadiiZ; - const squaredNorm = x2 + y2 + z2; - const ratio = Math.sqrt(1.0 / squaredNorm); - const intersection = scale(p, ratio); - if (squaredNorm < CENTER_TOLERANCE_SQUARED) - return Number.isFinite(ratio) ? intersection : null; - - const oneOverRadiiSquaredX = WGS84.oneOverRadiiSquared.x; - const oneOverRadiiSquaredY = WGS84.oneOverRadiiSquared.y; - const oneOverRadiiSquaredZ = WGS84.oneOverRadiiSquared.z; - const gradient = vec( - intersection.x * oneOverRadiiSquaredX * 2.0, - intersection.y * oneOverRadiiSquaredY * 2.0, - intersection.z * oneOverRadiiSquaredZ * 2.0, - ); - let lambda = ((1.0 - ratio) * magnitude(p)) / (0.5 * magnitude(gradient)); - let correction = 0.0; - let func, denominator, xMultiplier, yMultiplier, zMultiplier; - let xMultiplier2, yMultiplier2, zMultiplier2; - let xMultiplier3, yMultiplier3, zMultiplier3; - do { - lambda -= correction; - xMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredX); - yMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredY); - zMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredZ); - xMultiplier2 = xMultiplier * xMultiplier; - yMultiplier2 = yMultiplier * yMultiplier; - zMultiplier2 = zMultiplier * zMultiplier; - xMultiplier3 = xMultiplier2 * xMultiplier; - yMultiplier3 = yMultiplier2 * yMultiplier; - zMultiplier3 = zMultiplier2 * zMultiplier; - func = x2 * xMultiplier2 + y2 * yMultiplier2 + z2 * zMultiplier2 - 1.0; - denominator = - x2 * xMultiplier3 * oneOverRadiiSquaredX + - y2 * yMultiplier3 * oneOverRadiiSquaredY + - z2 * zMultiplier3 * oneOverRadiiSquaredZ; - const derivative = -2.0 * denominator; - correction = func / derivative; - } while (Math.abs(func) > EPSILON12); - return vec( - positionX * xMultiplier, - positionY * yMultiplier, - positionZ * zMultiplier, - ); -} - -/** ECEF to geodetic `{lon, lat}` radians and `height` metres, or null at the centre. */ -export function toCartographicRadians(p) { - const surface = scaleToGeodeticSurface(p); - if (!surface) return null; - const n = geodeticSurfaceNormal(surface); - const h = sub(p, surface); - return { - lon: Math.atan2(n.y, n.x), - lat: Math.asin(n.z), - height: Math.sign(dot(h, p)) * magnitude(h), - }; -} - -/** ECEF to `{lon, lat}` degrees and `height` metres. */ -export function toLonLatH(p) { - const c = toCartographicRadians(p); - if (!c) return null; - return { lon: toDegrees(c.lon), lat: toDegrees(c.lat), height: c.height }; -} - -// --- Rigid frames ------------------------------------------------------------ - -/** - * A rigid frame: unit axes `x`, `y`, `z` (columns) and an `origin`, mapping - * local coordinates to ECEF. - * @typedef {{x: object, y: object, z: object, origin: object}} Frame - */ - -export const IDENTITY = Object.freeze({ - x: Object.freeze(vec(1, 0, 0)), - y: Object.freeze(vec(0, 1, 0)), - z: Object.freeze(vec(0, 0, 1)), - origin: Object.freeze(vec(0, 0, 0)), -}); - -export const isIdentity = (frame) => - frame === IDENTITY || - (frame.x.x === 1 && - frame.y.y === 1 && - frame.z.z === 1 && - frame.x.y === 0 && - frame.x.z === 0 && - frame.y.x === 0 && - frame.y.z === 0 && - frame.z.x === 0 && - frame.z.y === 0 && - frame.origin.x === 0 && - frame.origin.y === 0 && - frame.origin.z === 0); - -/** East-north-up at `origin`. */ -export function eastNorthUp(origin) { - if ( - equalsEpsilon(origin.x, 0, EPSILON14) && - equalsEpsilon(origin.y, 0, EPSILON14) - ) { - // At a pole: east is +y, north points away from the pole. - const sign = Math.sign(origin.z) || 1; - return { - x: vec(0, 1, 0), - y: vec(-sign, 0, 0), - z: vec(0, 0, sign), - origin: clone(origin), - }; - } - const up = geodeticSurfaceNormal(origin); - const east = normalize(vec(-origin.y, origin.x, 0)); - const north = cross(up, east); - return { x: east, y: north, z: up, origin: clone(origin) }; -} - -export const frameVector = (frame, v) => - vec( - frame.x.x * v.x + frame.y.x * v.y + frame.z.x * v.z, - frame.x.y * v.x + frame.y.y * v.y + frame.z.y * v.z, - frame.x.z * v.x + frame.y.z * v.y + frame.z.z * v.z, - ); -export const framePoint = (frame, p) => - add(frameVector(frame, p), frame.origin); -export const inverseFrameVector = (frame, v) => - vec(dot(frame.x, v), dot(frame.y, v), dot(frame.z, v)); -export const inverseFramePoint = (frame, p) => - inverseFrameVector(frame, sub(p, frame.origin)); - -// --- Rotations --------------------------------------------------------------- - -/** Rotate `v` about unit `axis` by `angle` (right-handed). */ -export function rotate(v, axis, angle) { - const c = Math.cos(angle); - const s = Math.sin(angle); - const d = dot(axis, v); - const k = cross(axis, v); - return vec( - v.x * c + k.x * s + axis.x * d * (1 - c), - v.y * c + k.y * s + axis.y * d * (1 - c), - v.z * c + k.z * s + axis.z * d * (1 - c), - ); -} /** * The rotation the contract's heading, pitch and roll describe in a local