triangulate only

astronomy.js

exports.KM_PER_AU = 1.4959787069098932e+8;
exports.DEG2RAD = 0.017453292519943296;
exports.RAD2DEG = 57.295779513082321;
const EARTH_FLATTENING = 0.996647180302104;
const EARTH_EQUATORIAL_RADIUS_KM = 6378.1366;
const EARTH_POLAR_RADIUS_KM = EARTH_EQUATORIAL_RADIUS_KM * EARTH_FLATTENING;

function inverse_terra(ovec) {
    // Convert from AU to kilometers
    const x = ovec[0] * exports.KM_PER_AU;
    const y = ovec[1] * exports.KM_PER_AU;
    const z = ovec[2] * exports.KM_PER_AU;
    const p = Math.sqrt(x * x + y * y);
    let lon_deg, lat_deg, height_km;
    if (p < 1.0e-6) {
        // Special case: within 1 millimeter of a pole!
        // Use arbitrary longitude, and latitude determined by polarity of z.
        lon_deg = 0;
        lat_deg = (z > 0.0) ? +90 : -90;
        // Elevation is calculated directly from z.
        height_km = Math.abs(z) - EARTH_POLAR_RADIUS_KM;
    }
    else {
        const stlocl = Math.atan2(y, x);
        // Calculate exact longitude.
        lon_deg = (exports.RAD2DEG * stlocl);
        // Normalize longitude to the range (-180, +180].
        while (lon_deg <= -180)
            lon_deg += 360;
        while (lon_deg > +180)
            lon_deg -= 360;
        // Numerically solve for exact latitude, using Newton's Method.
        const F = EARTH_FLATTENING * EARTH_FLATTENING;
        // Start with initial latitude estimate, based on a spherical Earth.
        let lat = Math.atan2(z, p);
        let cos, sin, denom;
        for (;;) {
            // Calculate the error function W(lat).
            // We try to find the root of W, meaning where the error is 0.
            cos = Math.cos(lat);
            sin = Math.sin(lat);
            const factor = (F - 1) * EARTH_EQUATORIAL_RADIUS_KM;
            const cos2 = cos * cos;
            const sin2 = sin * sin;
            const radicand = cos2 + F * sin2;
            denom = Math.sqrt(radicand);
            const W = (factor * sin * cos) / denom - z * cos + p * sin;
            if (Math.abs(W) < 1.0e-12)
                break; // The error is now negligible
            // Error is still too large. Find the next estimate.
            // Calculate D = the derivative of W with respect to lat.
            const D = factor * ((cos2 - sin2) / denom - sin2 * cos2 * (F - 1) / (factor * radicand)) + z * sin + p * cos;
            lat -= W / D;
        }
        // We now have a solution for the latitude in radians.
        lat_deg = exports.RAD2DEG * lat;
        // Solve for exact height in meters.
        // There are two formulas I can use. Use whichever has the less risky denominator.
        const adjust = EARTH_EQUATORIAL_RADIUS_KM / denom;
        if (Math.abs(sin) > Math.abs(cos))
            height_km = z / sin - F * adjust;
        else
            height_km = p / cos - adjust;
    }
    return new Observer(lat_deg, lon_deg, 1000 * height_km);
}

function terra(observer) {
    const df = EARTH_FLATTENING;
    const df2 = df * df;

    const phi = observer.latitude * exports.DEG2RAD;
    const sinphi = Math.sin(phi);
    const cosphi = Math.cos(phi);
    const c = 1 / Math.sqrt(cosphi * cosphi + df2 * sinphi * sinphi);
    const s = df2 * c;
    const ht_km = observer.height / 1000;
    const ach = EARTH_EQUATORIAL_RADIUS_KM * c + ht_km;
    const ash = EARTH_EQUATORIAL_RADIUS_KM * s + ht_km;
    const stlocl = observer.longitude * exports.DEG2RAD;
    const sinst = Math.sin(stlocl);
    const cosst = Math.cos(stlocl);
    
    return [
        ach * cosphi * cosst / exports.KM_PER_AU,
        ach * cosphi * sinst / exports.KM_PER_AU,
        ash * sinphi / exports.KM_PER_AU
    ];
}


class Vector {
    constructor(x, y, z) {
        this.x = x;
        this.y = y;
        this.z = z;
    }

    Length() {
        return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
    }
}
exports.Vector = Vector;



class Spherical {
    constructor(lat, lon, dist) {
        this.lat = lat;
        this.lon = lon;
        this.dist = dist;
    }
}
exports.Spherical = Spherical;


class RotationMatrix {
    constructor(rot) {
        this.rot = rot;
    }
}

class Observer {
    constructor(latitude, longitude, height) {
        this.latitude = latitude;
        this.longitude = longitude;
        this.height = height;
    }
}
exports.Observer = Observer;


function VectorFromArray(av) {
    return new Vector(av[0], av[1], av[2]);
}

function ObserverVector(observer) {
    let ovec = terra(observer);
    return VectorFromArray(ovec);
}
exports.ObserverVector = ObserverVector;

function VectorObserver(vector) {
    let ovec = [vector.x, vector.y, vector.z];
    return inverse_terra(ovec);
}
exports.VectorObserver = VectorObserver;


function InverseRotation(rotation) {
    return new RotationMatrix([
        [rotation.rot[0][0], rotation.rot[1][0], rotation.rot[2][0]],
        [rotation.rot[0][1], rotation.rot[1][1], rotation.rot[2][1]],
        [rotation.rot[0][2], rotation.rot[1][2], rotation.rot[2][2]]
    ]);
}

function VectorFromSphere(sphere) {
    const radlat = sphere.lat * exports.DEG2RAD;
    const radlon = sphere.lon * exports.DEG2RAD;
    const rcoslat = sphere.dist * Math.cos(radlat);
    
    return new Vector(
        rcoslat * Math.cos(radlon),
        rcoslat * Math.sin(radlon),
        sphere.dist * Math.sin(radlat)
    );
}

function ToggleAzimuthDirection(az) {
    az = 360.0 - az;
    if (az >= 360.0)
        az -= 360.0;
    else if (az < 0.0)
        az += 360.0;
    return az;
}

function VectorFromHorizon(sphere) {
    const lon = ToggleAzimuthDirection(sphere.lon);
    const lat = sphere.lat + InverseRefraction(sphere.lat);
    const xsphere = new Spherical(lat, lon, sphere.dist);
    return VectorFromSphere(xsphere);
}
exports.VectorFromHorizon = VectorFromHorizon;

function InverseRefraction(bent_altitude) {
    if (bent_altitude < -90.0 || bent_altitude > +90.0)
        return 0.0; /* no attempt to correct an invalid altitude */
    /* Find the pre-adjusted altitude whose refraction correction leads to 'altitude'. */
    let altitude = bent_altitude;
    for (;;) {
        /* See how close we got. */
        let diff = altitude - bent_altitude;
        if (Math.abs(diff) < 1.0e-14)
            return altitude - bent_altitude;
        altitude -= diff;
    }
}

function RotateVector(rotation, vector) {
    return new Vector(
        rotation.rot[0][0] * vector.x + rotation.rot[1][0] * vector.y + rotation.rot[2][0] * vector.z,
        rotation.rot[0][1] * vector.x + rotation.rot[1][1] * vector.y + rotation.rot[2][1] * vector.z,
        rotation.rot[0][2] * vector.x + rotation.rot[1][2] * vector.y + rotation.rot[2][2] * vector.z
    );
}
exports.RotateVector = RotateVector;

function Rotation_EQD_HOR(observer) {
    const sinlat = Math.sin(observer.latitude * exports.DEG2RAD);
    const coslat = Math.cos(observer.latitude * exports.DEG2RAD);
    const sinlon = Math.sin(observer.longitude * exports.DEG2RAD);
    const coslon = Math.cos(observer.longitude * exports.DEG2RAD);
    const uz = [coslat * coslon, coslat * sinlon, sinlat];
    const un = [-sinlat * coslon, -sinlat * sinlon, coslat];
    const uw = [sinlon, -coslon, 0];

    return new RotationMatrix([
        [un[0], uw[0], uz[0]],
        [un[1], uw[1], uz[1]],
        [un[2], uw[2], uz[2]],
    ]);
}

function Rotation_HOR_EQD(observer) {
    const rot = Rotation_EQD_HOR(observer);
    return InverseRotation(rot);
}

exports.Rotation_HOR_EQD = Rotation_HOR_EQD;

triangulate.js

const Astronomy = require('./astronomy.js');

function ParseNumber(name, text) {
    const x = Number(text);
    if (!Number.isFinite(x)) {
        console.error(`ERROR: Not a valid numeric value for ${name}: "${text}"`);
        process.exit(1);
    }
    return x;
}


function DotProduct(a, b) {
    return a.x*b.x + a.y*b.y + a.z*b.z;
}


function AddScale(sa, va, sb, vb) {
    return new Astronomy.Vector(
        sa*va.x + sb*vb.x,
        sa*va.y + sb*vb.y,
        sa*va.z + sb*vb.z,
        va.t
    );
}


function DirectionVector(observer, altitude, azimuth) {
    // Convert horizontal angles to a horizontal unit vector.
    const hor = new Astronomy.Spherical(altitude, azimuth, 1.0);
    const hvec = Astronomy.VectorFromHorizon(hor);

    // Find the rotation matrix that converts horizontal vectors to equatorial vectors.
    const rot = Astronomy.Rotation_HOR_EQD(observer);

    // Rotate the horizontal (HOR) vector to an equator-of-date (EQD) vector.
    const evec = Astronomy.RotateVector(rot, hvec);

    return evec;
}


function Intersect(pos1, dir1, pos2, dir2) {
    const F = DotProduct(dir1, dir2);
    const amb = AddScale(+1, pos1, -1, pos2);    // amb = pos1 - pos2
    const E = DotProduct(dir1, amb);
    const G = DotProduct(dir2, amb);
    const denom = 1 - F*F;
    if (denom == 0.0) {
        console.error('ERROR: Cannot solve because directions are parallel.');
        process.exit(1);
    }

    const u = (F*G - E) / denom;
    const v = G + F*u;
    if (u < 0.0 || v < 0.0) {
        console.error('ERROR: Lines of sight do not converge.');
        process.exit(1);
    }

    const a = AddScale(1, pos1, u, dir1);     //  a = pos1 + u*dir1
    const b = AddScale(1, pos2, v, dir2);     //  b = pos2 + v*dir2
    const c = AddScale(0.5, a, 0.5, b);       //  c = (a+b)/2
    const miss = AddScale(+1, a, -1, b);      //  miss = a-b

    const dist = (Astronomy.KM_PER_AU * 1000 / 2) * miss.Length();   // error radius in meters
    const obs = Astronomy.VectorObserver(c, true);

    console.log(`Solution: lat = ${obs.latitude.toFixed(6)}, lon = ${obs.longitude.toFixed(6)}, elv = ${obs.height.toFixed(3)} meters; error = ${dist.toFixed(3)} meters`);
}


function Demo() {
    if (process.argv.length === 12) {
        // Validate and parse command line arguments.
        const lat1 = ParseNumber("lat1", process.argv[ 2]);
        const lon1 = ParseNumber("lon1", process.argv[ 3]);
        const elv1 = ParseNumber("elv1", process.argv[ 4]);
        const  az1 = ParseNumber("az1",  process.argv[ 5]);
        const alt1 = ParseNumber("alt1", process.argv[ 6]);
        const lat2 = ParseNumber("lat2", process.argv[ 7]);
        const lon2 = ParseNumber("lon2", process.argv[ 8]);
        const elv2 = ParseNumber("elv2", process.argv[ 9]);
        const  az2 = ParseNumber("az2",  process.argv[10]);
        const alt2 = ParseNumber("alt2", process.argv[11]);

        const obs1 = new Astronomy.Observer(lat1, lon1, elv1);
        const obs2 = new Astronomy.Observer(lat2, lon2, elv2);
        console.log({ obs1, obs2 });

        // Convert geographic coordinates of the observers to vectors.
        const pos1 = Astronomy.ObserverVector(obs1, true);
        const pos2 = Astronomy.ObserverVector(obs2, true);
        console.log({ pos1, pos2 });

        // Convert horizontal coordinates into unit direction vectors.
        const dir1 = DirectionVector(obs1, alt1, az1);
        const dir2 = DirectionVector(obs2, alt2, az2);
        console.log({ dir1, dir2 });

        // Find the closest point between the skew lines.
        Intersect(pos1, dir1, pos2, dir2);
        process.exit(0);
    }
}

Demo();