Basic functions needed to compute along- and cross-track error, using acfutils

nav_utils.c

//===--------------------------------------------------------------------------------------------===
// nav_utils.c - navigation utilities using acfutils
//
// Created by Amy Parent <[email protected]>
// Copyright (c) 2022 Amy Parent
// Licensed under the MIT License
//===--------------------------------------------------------------------------------------------===
#include <acfutils/geom.h>
#include <acfutils/assert.h>

// Returns a normalised version of an ECEF vector (unit vector)
vect3_t geo2ecef_norm(geo_pos2_t pos) {
    if(IS_NULL_GEO_POS2(pos)) return NULL_VECT3;
    return vect3_unit(geo2ecef_mtr(GEO2_TO_GEO3(pos, 0), &wgs84), NULL);
}

// Returns the local-horizontal bearing vector for a given position and compass bearing
vect3_t geo_gc_brg(geo_pos2_t from, double brg) {
    
    static const vect3_t up = VECT3(0, 0, 1);
    vect3_t norm = geo2ecef_norm(from);
    vect3_t east = vect3_unit(vect3_xprod(up, norm), NULL);
    vect3_t north = vect3_unit(vect3_xprod(norm, east), NULL);
    
    double angle = DEG2RAD(brg-90.0);
    return vect3_add(vect3_scmul(north, cos(angle)), vect3_scmul(east, sin(angle)));
}

// Calculates the normal vector to a great circle trajectory in ECEF
vect3_t ecef_gc(vect3_t from, vect3_t to) {
    return vect3_unit(vect3_xprod(from, to), NULL);
}

// Returns the distance along the track define by an end point and a course into that point
double gc_distance_along(geo_pos2_t to, double brg, geo_pos2_t pos) {
    VERIFY(IS_VALID_GEO_POS2(to));
    VERIFY(IS_VALID_GEO_POS2(pos));
    if(isnan(brg)) return gc_distance(pos, to);
    
    vect3_t to_ecef = geo2ecef_norm(to);
    vect3_t pos_ecef = geo2ecef_norm(pos);
    
    vect3_t gc = geo_gc_brg(to, brg);
    vect3_t along_pt = vect3_unit(vect3_xprod(vect3_xprod(gc, pos_ecef), gc), NULL);
    return EARTH_MSL * acos(vect3_dotprod(along_pt, to_ecef));
}

// Returns the cross track distance from a great circle defined by its normal vector
double ecef_xtk(vect3_t gc, vect3_t ppos) {
    // vect3_t abeam = vect3_xprod(gc, vect3_xprod(gc, ppos));
    return -EARTH_MSL * (acos(vect3_dotprod(gc, ppos)) - M_PI/2.0);
}


// Returns the cross-track distance of [pos] from a great circle [from -> to]
double gc_xtk_between(geo_pos2_t from, geo_pos2_t to, geo_pos2_t pos) {
    if(IS_NULL_GEO_POS2(from) || IS_NULL_GEO_POS2(to) || IS_NULL_GEO_POS2(pos)) return NAN;
    
    vect3_t fr_ecef = geo2ecef_norm(from);
    vect3_t to_ecef = geo2ecef_norm(to);
    vect3_t pos_ecef = geo2ecef_norm(pos);
    
    vect3_t gc = ecef_gc(fr_ecef, to_ecef);
    return ecef_xtk(gc, pos_ecef);
}

// Returns the cross-track distance of [pos] from a great circle defined by an end point and a track.
double gc_xtk_trk(geo_pos2_t to, double trk, geo_pos2_t pos) {
    if(IS_NULL_GEO_POS2(to) || IS_NULL_GEO_POS2(pos) || isnan(trk)) return NAN;

    vect3_t pos_ecef = geo2ecef_norm(pos);
    vect3_t gc = geo_gc_brg(to, trk);
    return ecef_xtk(gc, pos_ecef);
}

// Same as gc_distance_along, but using local flat plane projection
double lh_distance_along(geo_pos2_t to, double track, geo_pos2_t loc) {
    if(IS_NULL_GEO_POS2(to) || IS_NULL_GEO_POS2(loc)) return NAN;
    fpp_t fpp = gnomo_fpp_init(to, 0, &wgs84, B_FALSE);
    vect2_t proj = geo2fpp(loc, &fpp);
    
    double brg = dir2hdg(vect2_sub(VECT2(0, 0), proj));
    return fabs(vect2_abs(proj) * cos(DEG2RAD(brg-track)));
}

// Same as gc_xtk_to, but using local flat plane projection
double lh_xtk_to(geo_pos2_t to, double track, geo_pos2_t loc) {
    if(IS_NULL_GEO_POS2(to) || IS_NULL_GEO_POS2(loc)) return NAN;
    fpp_t fpp = stereo_fpp_init(to, 0, &wgs84, B_FALSE);
    vect2_t proj = geo2fpp(loc, &fpp);
    
    double brg = dir2hdg(vect2_sub(VECT2(0, 0), proj));
    return vect2_abs(proj) * sin(DEG2RAD(brg-track));
}

// Same as gc_xtk_from, but using local flat plane projection
double lh_xtk_from(geo_pos2_t from, double track, geo_pos2_t loc) {
    if(IS_NULL_GEO_POS2(from) || IS_NULL_GEO_POS2(loc)) return NAN;
    fpp_t fpp = gnomo_fpp_init(from, 0, &wgs84, B_FALSE);
    vect2_t proj = geo2fpp(loc, &fpp);
    
    double brg = dir2hdg(vect2_sub(VECT2(0, 0), proj));
    return vect2_abs(proj) * sin(DEG2RAD(brg-track));
}