mirror of
https://github.com/SatDump/SatDump
synced 2026-08-13 17:47:30 -04:00
137 lines
No EOL
3.7 KiB
C++
137 lines
No EOL
3.7 KiB
C++
#include "geos.h"
|
|
|
|
namespace proj
|
|
{
|
|
namespace
|
|
{
|
|
struct projection_geos_t
|
|
{
|
|
double h;
|
|
double radius_p;
|
|
double radius_p2;
|
|
double radius_p_inv2;
|
|
double radius_g;
|
|
double radius_g_1;
|
|
double C;
|
|
int flip_axis;
|
|
};
|
|
}
|
|
|
|
bool projection_geos_setup(projection_t *proj, double altitude, bool sweep_x)
|
|
{
|
|
projection_geos_t *ptr = (projection_geos_t *)malloc(sizeof(projection_geos_t));
|
|
if (ptr == nullptr)
|
|
return true;
|
|
proj->proj_dat = ptr;
|
|
|
|
if (altitude <= 0)
|
|
return true;
|
|
|
|
ptr->h = altitude;
|
|
ptr->flip_axis = sweep_x;
|
|
|
|
ptr->radius_g_1 = ptr->h / proj->a;
|
|
if (ptr->radius_g_1 <= 0 || ptr->radius_g_1 > 1e10)
|
|
{
|
|
return true;
|
|
}
|
|
ptr->radius_g = 1. + ptr->radius_g_1;
|
|
ptr->C = ptr->radius_g * ptr->radius_g - 1.0;
|
|
if (proj->es != 0.0)
|
|
{
|
|
ptr->radius_p = sqrt(proj->one_es);
|
|
ptr->radius_p2 = proj->one_es;
|
|
ptr->radius_p_inv2 = proj->rone_es;
|
|
}
|
|
else
|
|
{
|
|
ptr->radius_p = ptr->radius_p2 = ptr->radius_p_inv2 = 1.0;
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
bool projection_geos_fwd(const projection_t *proj, double lam, double phi, double *x, double *y)
|
|
{
|
|
projection_geos_t *ptr = (projection_geos_t *)proj->proj_dat;
|
|
|
|
double r, Vx, Vy, Vz, tmp;
|
|
|
|
/* Calculation of geocentric latitude. */
|
|
phi = atan(ptr->radius_p2 * tan(phi));
|
|
|
|
/* Calculation of the three components of the vector from satellite to
|
|
** position on earth surface (long,lat).*/
|
|
r = (ptr->radius_p) / hypot(ptr->radius_p * cos(phi), sin(phi));
|
|
Vx = r * cos(lam) * cos(phi);
|
|
Vy = r * sin(lam) * cos(phi);
|
|
Vz = r * sin(phi);
|
|
|
|
/* Check visibility. */
|
|
if (((ptr->radius_g - Vx) * Vx - Vy * Vy - Vz * Vz * ptr->radius_p_inv2) < 0.)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
/* Calculation based on view angles from satellite. */
|
|
tmp = ptr->radius_g - Vx;
|
|
|
|
if (ptr->flip_axis)
|
|
{
|
|
*x = ptr->radius_g_1 * atan(Vy / hypot(Vz, tmp));
|
|
*y = ptr->radius_g_1 * atan(Vz / tmp);
|
|
}
|
|
else
|
|
{
|
|
*x = ptr->radius_g_1 * atan(Vy / tmp);
|
|
*y = ptr->radius_g_1 * atan(Vz / hypot(Vy, tmp));
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
bool projection_geos_inv(const projection_t *proj, double x, double y, double *lam, double *phi)
|
|
{
|
|
projection_geos_t *ptr = (projection_geos_t *)proj->proj_dat;
|
|
|
|
double Vx, Vy, Vz, a, b, k;
|
|
|
|
/* Setting three components of vector from satellite to position.*/
|
|
Vx = -1.0;
|
|
|
|
if (ptr->flip_axis)
|
|
{
|
|
Vz = tan(y / ptr->radius_g_1);
|
|
Vy = tan(x / ptr->radius_g_1) * hypot(1.0, Vz);
|
|
}
|
|
else
|
|
{
|
|
Vy = tan(x / ptr->radius_g_1);
|
|
Vz = tan(y / ptr->radius_g_1) * hypot(1.0, Vy);
|
|
}
|
|
|
|
/* Calculation of terms in cubic equation and determinant.*/
|
|
a = Vz / ptr->radius_p;
|
|
a = Vy * Vy + a * a + Vx * Vx;
|
|
b = 2 * ptr->radius_g * Vx;
|
|
const double det = (b * b) - 4 * a * ptr->C;
|
|
if (det < 0.)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
/* Calculation of three components of vector from satellite to position.*/
|
|
k = (-b - sqrt(det)) / (2. * a);
|
|
Vx = ptr->radius_g + k * Vx;
|
|
Vy *= k;
|
|
Vz *= k;
|
|
|
|
/* Calculation of longitude and latitude.*/
|
|
*lam = atan2(Vy, Vx);
|
|
*phi = atan(Vz * cos(*lam) / Vx);
|
|
*phi = atan(ptr->radius_p_inv2 * tan(*phi));
|
|
|
|
return false;
|
|
}
|
|
} |