satdump/src-core/common/geodetic/euler_raytrace.cpp

#include "euler_raytrace.h"
#include "wgs84.h"

extern "C"
{
#include "libs/predict/unsorted.h"
}

#define EPSILON 2.2204460492503131e-016

#define JULIAN_TIME_DIFF 2444238.5

namespace geodetic
{
    namespace raytrace_to_earth_namespace
    {
        struct vector
        {
            double x = 0;
            double y = 0;
            double z = 0;

            vector() {}
            vector(double x, double y, double z) : x(x), y(y), z(z) {}

            double distance_square()
            {
                return x * x + y * y + z * z;
            }

            double magnitude()
            {
                return sqrt(x * x + y * y + z * z);
            }

            vector &normalize()
            {
                double mag = magnitude();

                if (mag <= 0)
                    return *this;

                x /= mag;
                y /= mag;
                z /= mag;

                return *this;
            }
        };

        inline vector vectorCross(vector a, vector b)
        {
            vector c;
            c.x = a.y * b.z - a.z * b.y;
            c.y = a.z * b.x - a.x * b.z;
            c.z = a.x * b.y - a.y * b.x;
            return c;
        }

        inline vector rotate_vector_a_around_b(vector a, vector b, double theta)
        {
            double _const = (a.x * b.x + a.y * b.y + a.z * b.z) / (b.x * b.x + b.y * b.y + b.z * b.z);

            vector a_par_b;
            vector a_ortho_b;

            a_par_b.x = _const * b.x;
            a_par_b.y = _const * b.y;
            a_par_b.z = _const * b.z;
            a_ortho_b.x = a.x - a_par_b.x;
            a_ortho_b.y = a.y - a_par_b.y;
            a_ortho_b.z = a.z - a_par_b.z;

            vector w;

            w.x = b.y * a_ortho_b.z - b.z * a_ortho_b.y;
            w.y = b.z * a_ortho_b.x - b.x * a_ortho_b.z;
            w.z = b.x * a_ortho_b.y - b.y * a_ortho_b.x;

            double rnorm_ortho = sqrt(a_ortho_b.x * a_ortho_b.x + a_ortho_b.y * a_ortho_b.y + a_ortho_b.z * a_ortho_b.z);

            double x1 = cos(theta) / rnorm_ortho;
            double x2 = sin(theta) / sqrt(w.x * w.x + w.y * w.y + w.z * w.z);

            vector a_ortho_b_theta;

            a_ortho_b_theta.x = rnorm_ortho * (x1 * a_ortho_b.x + x2 * w.x);
            a_ortho_b_theta.y = rnorm_ortho * (x1 * a_ortho_b.y + x2 * w.y);
            a_ortho_b_theta.z = rnorm_ortho * (x1 * a_ortho_b.z + x2 * w.z);

            vector c;

            c.x = a_ortho_b_theta.x + a_par_b.x;
            c.y = a_ortho_b_theta.y + a_par_b.y;
            c.z = a_ortho_b_theta.z + a_par_b.z;

            return c;
        }
    };

    int raytrace_to_earth(double time, const double position_in[3], const double velocity_in[3], geodetic::euler_coords_t pointing, geodetic::geodetic_coords_t &earth_point)
    {
        // Ensure all inputs are in radians
        // position_geo.toRads();
        pointing.toRads();

        // Geodetic coordinates to vector
        raytrace_to_earth_namespace::vector position;
        position.x = position_in[0];
        position.y = position_in[1];
        position.z = position_in[2];

        // Get vector to Nadir
        raytrace_to_earth_namespace::vector nadir_vector;
        {
            geodetic_t out;
            double obspos[3];
            double obs_vel[3];
            Calculate_LatLonAlt(time, position_in, &out);
            out.alt = 0;
            Calculate_User_PosVel(time + JULIAN_TIME_DIFF, &out, obspos, obs_vel);
            // logger->critical("%f-%f %f-%f %f-%f",
            //                  obspos[0], position_in[0],
            //                  obspos[1], position_in[1],
            //                  obspos[2], position_in[2]);
            nadir_vector.x = obspos[0] - position_in[0];
            nadir_vector.y = obspos[1] - position_in[1];
            nadir_vector.z = obspos[2] - position_in[2];
        }

        // Satellite velocity vector
        raytrace_to_earth_namespace::vector velocity_vector;
        velocity_vector.x = velocity_in[0];
        velocity_vector.y = velocity_in[1];
        velocity_vector.z = velocity_in[2];

        // Rotate to apply yaw
        velocity_vector =
            raytrace_to_earth_namespace::rotate_vector_a_around_b(velocity_vector, nadir_vector, pointing.yaw);

        // Compute a vector orthogonal to the velocity vector
        raytrace_to_earth_namespace::vector velocity_vector_90 =
            raytrace_to_earth_namespace::rotate_vector_a_around_b(velocity_vector, nadir_vector, 90 * DEG_TO_RAD);

        // Apply rotations. First, roll
        raytrace_to_earth_namespace::vector pointing_vector =
            raytrace_to_earth_namespace::rotate_vector_a_around_b(nadir_vector, velocity_vector, pointing.roll);

        // Apply rotations. Then, pitch
        pointing_vector =
            raytrace_to_earth_namespace::rotate_vector_a_around_b(pointing_vector, velocity_vector_90, pointing.pitch);

        double u = pointing_vector.x;
        double v = pointing_vector.y;
        double w = pointing_vector.z;

        // WGS84 ellipsoid
        const double &a = geodetic::WGS84::a;
        const double &b = geodetic::WGS84::a;
        const double &c = geodetic::WGS84::b;

        double value = -pow(a, 2) * pow(b, 2) * w * position.z - pow(a, 2) * pow(c, 2) * v * position.y - pow(b, 2) * pow(c, 2) * u * position.x;
        double radical = pow(a, 2) * pow(b, 2) * pow(w, 2) + pow(a, 2) * pow(c, 2) * pow(v, 2) - pow(a, 2) * pow(v, 2) * pow(position.z, 2) +
                         2 * pow(a, 2) * v * w * position.y * position.z - pow(a, 2) * pow(w, 2) * pow(position.y, 2) + pow(b, 2) * pow(c, 2) * pow(u, 2) -
                         pow(b, 2) * pow(u, 2) * pow(position.z, 2) + 2 * pow(b, 2) * u * w * position.x * position.z - pow(b, 2) * pow(w, 2) * pow(position.x, 2) -
                         pow(c, 2) * pow(u, 2) * pow(position.y, 2) + 2 * pow(c, 2) * u * v * position.x * position.y - pow(c, 2) * pow(v, 2) * pow(position.x, 2);
        double magnitude = pow(a, 2) * pow(b, 2) * pow(w, 2) + pow(a, 2) * pow(c, 2) * pow(v, 2) + pow(b, 2) * pow(c, 2) * pow(u, 2);

        if (radical < 0)
            return 1;

        double d = (value - a * b * c * sqrt(radical)) / magnitude;

        if (d < 0)
            return 1;

        position.x += d * u;
        position.y += d * v;
        position.z += d * w;

        // To geodetic
        // xyz2lla(position, earth_point);
        const double position_final[3] = {position.x, position.y, position.z};
        geodetic_t out;
        Calculate_LatLonAlt(time, position_final, &out);

        earth_point = geodetic::geodetic_coords_t(out.lat, out.lon, out.alt / 1000.0, true).toDegs();

        if (earth_point.lon > 180.0)
            earth_point.lon -= 360.0;

        if (std::isnan(earth_point.lat) || std::isnan(earth_point.lon))
            return 1;

        return 0;
    }
};