mirror of
https://github.com/SatDump/SatDump
synced 2026-08-13 17:47:30 -04:00
284 lines
No EOL
8.5 KiB
C++
284 lines
No EOL
8.5 KiB
C++
#include "stereo.h"
|
|
#include <cmath>
|
|
#include "common/geodetic/wgs84.h"
|
|
#ifndef M_PI_2
|
|
#define M_PI_2 1.57079632679489661923 /* pi/2 */
|
|
#endif
|
|
#ifndef M_PI_4
|
|
#define M_PI_4 0.78539816339744830962 /* pi/4 */
|
|
#endif
|
|
|
|
/*
|
|
** This file was adapted and simplified from libproj, and the below
|
|
** notice kept as credits.
|
|
**
|
|
** libproj -- library of cartographic projections
|
|
**
|
|
** Copyright (c) 2004 Gerald I. Evenden
|
|
** Copyright (c) 2012 Martin Raspaud
|
|
**
|
|
** See also (section 4.4.3.2):
|
|
** https://www.cgms-info.org/documents/pdf_cgms_03.pdf
|
|
**
|
|
** Permission is hereby granted, free of charge, to any person obtaining
|
|
** a copy of this software and associated documentation files (the
|
|
** "Software"), to deal in the Software without restriction, including
|
|
** without limitation the rights to use, copy, modify, merge, publish,
|
|
** distribute, sublicense, and/or sell copies of the Software, and to
|
|
** permit persons to whom the Software is furnished to do so, subject to
|
|
** the following conditions:
|
|
**
|
|
** The above copyright notice and this permission notice shall be
|
|
** included in all copies or substantial portions of the Software.
|
|
**
|
|
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
|
** EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
|
|
** MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
|
|
** IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
|
|
** CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
|
|
** TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
|
** SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
|
*/
|
|
|
|
#define EPS10 1.e-10
|
|
|
|
namespace geodetic
|
|
{
|
|
namespace projection
|
|
{
|
|
double pj_tsfn(double phi, double sinphi, double e)
|
|
{
|
|
double cosphi = cos(phi);
|
|
return exp(e * atanh(e * sinphi)) * (sinphi > 0 ? cosphi / (1 + sinphi) : (1 - sinphi) / cosphi);
|
|
}
|
|
|
|
static double ssfn_(double phit, double sinphi, double eccen)
|
|
{
|
|
sinphi *= eccen;
|
|
return (tan(.5 * (M_PI_2 + phit)) * pow((1. - sinphi) / (1. + sinphi), .5 * eccen));
|
|
}
|
|
|
|
int StereoProjection::init(double latitude, double longitude)
|
|
{
|
|
lon_0 = longitude; // The projection's longitude
|
|
|
|
// Constants, WGS84
|
|
e = WGS84::e;
|
|
phi0 = latitude * 0.01745329;
|
|
a = WGS84::a * 1000;
|
|
es = WGS84::es;
|
|
one_es = WGS84::one_es;
|
|
|
|
if (es == 0.0)
|
|
{
|
|
// Illegal
|
|
return 1;
|
|
}
|
|
|
|
k0 = .994;
|
|
//x0 = 2000000.;
|
|
//y0 = 2000000.;
|
|
phits = M_PI_2;
|
|
lam0 = 0.;
|
|
|
|
// Setup
|
|
double t;
|
|
|
|
if (fabs((t = fabs(phi0)) - M_PI_2) < EPS10)
|
|
mode = phi0 < 0. ? S_POLE : N_POLE;
|
|
else
|
|
mode = t > EPS10 ? OBLIQ : EQUIT;
|
|
phits = fabs(phits);
|
|
|
|
if (es != 0.0)
|
|
{
|
|
double X;
|
|
|
|
switch (mode)
|
|
{
|
|
case N_POLE:
|
|
case S_POLE:
|
|
if (fabs(phits - M_PI_2) < EPS10)
|
|
akm1 = 2. * k0 /
|
|
sqrt(pow(1 + e, 1 + e) * pow(1 - e, 1 - e));
|
|
else
|
|
{
|
|
t = sin(phits);
|
|
akm1 = cos(phits) / pj_tsfn(phits, t, e);
|
|
t *= e;
|
|
akm1 /= sqrt(1. - t * t);
|
|
}
|
|
break;
|
|
case EQUIT:
|
|
case OBLIQ:
|
|
t = sin(phi0);
|
|
X = 2. * atan(ssfn_(phi0, t, e)) - M_PI_2;
|
|
t *= e;
|
|
akm1 = 2. * k0 * cos(phi0) / sqrt(1. - t * t);
|
|
sinX1 = sin(X);
|
|
cosX1 = cos(X);
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
switch (mode)
|
|
{
|
|
case OBLIQ:
|
|
sinX1 = sin(phi0);
|
|
cosX1 = cos(phi0);
|
|
/*-fallthrough*/
|
|
case EQUIT:
|
|
akm1 = 2. * k0;
|
|
break;
|
|
case S_POLE:
|
|
case N_POLE:
|
|
akm1 = fabs(phits - M_PI_2) >= EPS10 ? cos(phits) / tan(M_PI_4 - .5 * phits) : 2. * k0;
|
|
break;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int StereoProjection::forward(double lon, double lat, double &x, double &y)
|
|
{
|
|
x = y = 0; // Safety
|
|
|
|
// Shift longitudes
|
|
lon -= lon_0;
|
|
if (lon < -180)
|
|
lon = lon + 360;
|
|
if (lon > 180)
|
|
lon = lon - 360;
|
|
|
|
// To radians
|
|
double phi = lat * 0.01745329, lam = lon * 0.01745329;
|
|
|
|
double coslam, sinlam, sinX = 0.0, cosX = 0.0, A = 0.0, sinphi;
|
|
|
|
coslam = cos(lam);
|
|
sinlam = sin(lam);
|
|
sinphi = sin(phi);
|
|
if (mode == OBLIQ || mode == EQUIT)
|
|
{
|
|
const double X = 2. * atan(ssfn_(phi, sinphi, e)) - M_PI_2;
|
|
sinX = sin(X);
|
|
cosX = cos(X);
|
|
}
|
|
|
|
switch (mode)
|
|
{
|
|
case OBLIQ:
|
|
{
|
|
const double denom = cosX1 * (1. + sinX1 * sinX + cosX1 * cosX * coslam);
|
|
if (denom == 0)
|
|
{
|
|
// Illegal
|
|
return 1;
|
|
}
|
|
A = akm1 / denom;
|
|
y = A * (cosX1 * sinX - sinX1 * cosX * coslam);
|
|
x = A * cosX;
|
|
break;
|
|
}
|
|
|
|
case EQUIT:
|
|
/* avoid zero division */
|
|
if (1. + cosX * coslam == 0.0)
|
|
{
|
|
y = HUGE_VAL;
|
|
}
|
|
else
|
|
{
|
|
A = akm1 / (1. + cosX * coslam);
|
|
y = A * sinX;
|
|
}
|
|
x = A * cosX;
|
|
break;
|
|
|
|
case S_POLE:
|
|
phi = -phi;
|
|
coslam = -coslam;
|
|
sinphi = -sinphi;
|
|
/*-fallthrough*/
|
|
case N_POLE:
|
|
if (fabs(phi - M_PI_2) < 1e-15)
|
|
x = 0;
|
|
else
|
|
x = akm1 * pj_tsfn(phi, sinphi, e);
|
|
y = -x * coslam;
|
|
break;
|
|
}
|
|
|
|
x = x * sinlam;
|
|
return 0;
|
|
}
|
|
|
|
int StereoProjection::inverse(double x, double y, double &lon, double &lat)
|
|
{
|
|
lon = lat = 0.0;
|
|
double phi = 0, lam = 0;
|
|
|
|
double cosphi, sinphi, tp = 0.0, phi_l = 0.0, rho, halfe = 0.0, halfpi = 0.0;
|
|
|
|
rho = hypot(x, y);
|
|
|
|
switch (mode)
|
|
{
|
|
case OBLIQ:
|
|
case EQUIT:
|
|
tp = 2. * atan2(rho * cosX1, akm1);
|
|
cosphi = cos(tp);
|
|
sinphi = sin(tp);
|
|
if (rho == 0.0)
|
|
phi_l = asin(cosphi * sinX1);
|
|
else
|
|
phi_l = asin(cosphi * sinX1 + (y * sinphi * cosX1 / rho));
|
|
|
|
tp = tan(.5 * (M_PI_2 + phi_l));
|
|
x *= sinphi;
|
|
y = rho * cosX1 * cosphi - y * sinX1 * sinphi;
|
|
halfpi = M_PI_2;
|
|
halfe = .5 * e;
|
|
break;
|
|
case N_POLE:
|
|
y = -y;
|
|
/*-fallthrough*/
|
|
case S_POLE:
|
|
tp = -rho / akm1;
|
|
phi_l = M_PI_2 - 2. * atan(tp);
|
|
halfpi = -M_PI_2;
|
|
halfe = -.5 * e;
|
|
break;
|
|
}
|
|
|
|
for (int i = 8; i > 0; --i)
|
|
{
|
|
sinphi = e * sin(phi_l);
|
|
phi = 2. * atan(tp * pow((1. + sinphi) / (1. - sinphi), halfe)) - halfpi;
|
|
if (fabs(phi_l - phi) < 1.e-10)
|
|
{
|
|
if (mode == S_POLE)
|
|
phi = -phi;
|
|
lam = (x == 0. && y == 0.) ? 0. : atan2(x, y);
|
|
|
|
// To degs
|
|
lat = phi * 57.29578;
|
|
lon = lam * 57.29578;
|
|
|
|
// Shift longitudes back to reference 0
|
|
lon += lon_0;
|
|
if (lon < -180)
|
|
lon = lon + 360;
|
|
if (lon > 180)
|
|
lon = lon - 360;
|
|
|
|
return 0;
|
|
}
|
|
phi_l = phi;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
};
|
|
}; |