Loading...
Searching...
No Matches
Cartesian3.cpp
Go to the documentation of this file.
/**
* \file Cartesian3.cpp
* \brief Implementation for GeographicLib::Triaxial::Cartesian3 class
*
* Copyright (c) Charles Karney (2025) <karney@alum.mit.edu> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/Triaxial/Cartesian3.hpp>
namespace GeographicLib {
namespace Triaxial {
using namespace std;
Cartesian3::Cartesian3(const Ellipsoid3& t)
: _t(t)
, _axes{_t.a(), _t.b(), _t.c()}
, _axes2{Math::sq(_t.a()), Math::sq(_t.b()), Math::sq(_t.c())}
, _linecc2{(_t.a() - _t.c()) * (_t.a() + _t.c()),
(_t.b() - _t.c()) * (_t.b() + _t.c()), 0}
, _norm(0, 1)
, _uni(0, 1)
{}
Cartesian3::Cartesian3(real a, real b, real c)
: Cartesian3(Ellipsoid3(a, b, c))
{}
Cartesian3::Cartesian3(real b, real e2, real k2, real kp2)
: Cartesian3(Ellipsoid3(b, e2, k2, kp2))
{}
template<int n>
pair<Math::real, Math::real> Cartesian3::funp<n>::operator()(real p)
const {
real fp = 0, fv = -1, fcorr = 0;
for (int k = 0; k < 3; ++k) {
if (_r[k] == 0) continue;
real g = _r[k] / (p + _l[k]);
if constexpr (n == 2) g *= g;
real ga = round(g/_d) * _d, gb = g - ga;
fv = fv + ga; fcorr = fcorr + gb;
fp = fp - n * g / (p + _l[k]);
}
return pair<real, real>(fv + fcorr, fp);
}
Math::real Cartesian3::cartsolve(const function<pair<real, real>(real)>& f,
real p0, real pscale) {
// Solve
// f(p) = 0
// Initial guess is p0; pscale is a scale factor for p; scale factor for f
// = 1. This assumes that there's a single solution with p >= 0 and that
// for p > 0, f' < 0 and f'' > 0
const real eps = numeric_limits<real>::epsilon(),
tol = Math::sq(cbrt(eps)), tol2 = Math::sq(tol),
ptol = pscale * sqrt(eps);
real p = p0;
real od = -1;
for (int i = 0;
i < maxit_ ||
(throw_ && (throw GeographicLib::GeographicErr
("Convergence failure Cartesian3::cartsolve"), false));
++i) {
pair<real, real> fx = f(p);
real fv = fx.first, fp = fx.second;
// We're done if f(p) <= 0 on initial guess; this can happens when z = 0.
// However, since Newton converges from below, any negative f(p)
// indicates convergence.
if (!(fv > tol2)) break;
real d = -fv/fp; // d is positive
p = p + d;
// converged if fv <= 8*eps (after first iteration) or
// d <= max(eps, |p|) * tol and d <= od.
// N.B. d is always positive.
if ( (fv <= 8 * eps || d <= fmax(ptol, p) * tol) && d <= od )
// The condition d <= od means that this won't trip on the first
// iteration
break;
od = d;
}
return p;
}
Math::real Cartesian3::cubic(vec3 R2) const {
// Solve sum(R2[i]/(z + lineq2[i]), i,0,2) - 1 = 0 with lineq2[2] = 0.
// This has three real roots with just one satisifying q >= 0.
// Express as a cubic equation z^3 + a*z^2 + b*z + c = 0.
real c = - _linecc2[0]*_linecc2[1] * R2[2],
b = _linecc2[0]*_linecc2[1]
- (_linecc2[1] * R2[0] + _linecc2[0] * R2[1] +
(_linecc2[0] + _linecc2[1]) * R2[2]),
a = _linecc2[0] + _linecc2[1] - (R2[0] + R2[1] + R2[2]);
bool recip = b > 0;
if (recip) {
// If b positive there a cancellation in p = (3*b - a^2) / 3, so
// transform to a polynomial in 1/t. The resulting coefficients are
real ax = b/c, bx = a/c, cx = 1/c;
a = ax; b = bx; c = cx;
}
// Reduce cubic to w^3 + p*w + q = 0, where z = w - a/3.
// See https://dlmf.nist.gov/1.11#iii
real p = (3*b - Math::sq(a)) / 3,
q = (2*a*Math::sq(a) - 9*a*b + 27*c) / 27;
// Now switch to https://dlmf.nist.gov/4.43
// We have 3 real roots, so 4*p^3 + 27*q^2 <= 0
real A = sqrt(fmax(real(0), -4*p/3)),
alp = atan2(q, sqrt(fmax(real(0),
-(4*p*Math::sq(p)/27 + Math::sq(q)))))/3;
// alp is in [-pi/3, pi/3]
// z = A*sin(alp + 2*pi/3 * k) - a/3 for k = -1, 0, 1
// for the single positive solution we pick k = 1 which gives the
// algebraically largest result
real t = A/2 * (cos(alp) * sqrt(real(3)) - sin(alp)) - a/3;
return recip ? 1/t : t;
}
void Cartesian3::carttoellip(vec3 R, Angle& bet, Angle& omg, real& H) const {
// tol2 = eps^(4/3)
real tol2 = Math::sq(Math::sq(cbrt(numeric_limits<real>::epsilon())));
vec3 R2 = {Math::sq(R[0]), Math::sq(R[1]), Math::sq(R[2])};
real qmax = R2[0] + R2[1] + R2[2],
qmin = fmax(fmax(R2[2], R2[1] + R2[2] - _linecc2[1]),
R2[0] + R2[1] + R2[2] - _linecc2[0]),
q = qmin;
do { // Executed once (provides the ability to break)
const funp<1> f(R2, _linecc2);
pair<real, real> fx = f(q);
if (!( fx.first > tol2 ))
break; // negative means converged
q = fmax(qmin, fmin(qmax, cubic(R2)));
fx = f(q);
if (!( fabs(fx.first) > tol2 ))
break; // test abs(fv) here
q = fmax(qmin, q - fx.first/fx.second);
q = cartsolve(f, q, Math::sq(b()));
} while (false);
vec3 axes = {sqrt(_linecc2[0] + q), sqrt(_linecc2[1] + q), sqrt(q)};
_t.cart2toellipint(R, bet, omg, axes);
H = axes[2] - c();
}
void Cartesian3::elliptocart(Angle bet, Angle omg, real H, vec3& R) const {
vec3 ax;
real shift = H * (2*c() + H);
for (int k = 0; k < 2; ++k)
ax[k] = sqrt(_axes2[k] + shift);
ax[2] = c() + H;
real tx = hypot(_t.k() * bet.c(), _t.kp()),
tz = hypot(_t.k(), _t.kp() * omg.s());
R = { ax[0] * omg.c() * tx,
ax[1] * bet.c() * omg.s(),
ax[2] * bet.s() * tz };
}
template<int n>
void Cartesian3::cart2togeneric(vec3 R, ang& phi, ang& lam, bool alt) const {
static_assert(n >= 0 && n <= 2, "Bad coordinate conversion");
if constexpr (n == 2) {
R[0] /= _axes2[0];
R[1] /= _axes2[1];
R[2] /= _axes2[2];
} else if constexpr (n == 1) {
R[0] /= _axes[0];
R[1] /= _axes[1];
R[2] /= _axes[2];
} // else n == 0, R is unchanged
roty(R, alt ? 1 : 0);
// nr = [-R[2], R[1], R[0]]
phi = ang(R[2], hypot(R[0], R[1]));
lam = ang(R[1], R[0]); // ang{0, 0} -> 0
}
template<int n>
void Cartesian3::generictocart2(ang phi, ang lam, vec3& R, bool alt) const {
static_assert(n >= 0 && n <= 2, "Bad coordinate conversion");
R = {phi.c() * lam.c(), phi.c() * lam.s(), phi.s()};
roty(R, alt ? -1 : 0);
if constexpr (n == 2) {
R[0] *= _axes2[0];
R[1] *= _axes2[1];
R[2] *= _axes2[2];
} else if constexpr (n == 1) {
R[0] *= _axes[0];
R[1] *= _axes[1];
R[2] *= _axes[2];
} // else n == 0, R is unchanged
if constexpr (n != 1) {
real d = Math::hypot3(R[0] / _axes[0], R[1] / _axes[1], R[2] / _axes[2]);
R[0] /= d; R[1] /= d; R[2] /= d;
} // else n == 1, d = 1 and R is already on the surface of the ellipsoid
}
template<int n>
Angle Cartesian3::meridianplane(ang lam, bool alt) const {
if constexpr (n == 2)
return lam.modang(_axes2[alt ? 2 : 0]/_axes2[1]);
else if constexpr (n == 1)
return lam.modang(_axes[alt ? 2 : 0]/_axes[1]);
else { // n == 0
(void) alt; // Visual Studio 15 complains about used alt
return lam;
}
}
void Cartesian3::cardinaldir(vec3 R, ang merid, vec3& N, vec3& E,
bool alt) const {
roty(R, alt ? 1 : 0);
int i0 = alt ? 2 : 0, i1 = 1, i2 = alt ? 0 : 2;
vec3 up = {R[0] / _axes2[i0], R[1] / _axes2[i1],
R[2] / _axes2[i2]};
Ellipsoid3::normvec(up);
N = { -R[0]*R[2] / _axes2[i2], -R[1]*R[2] / _axes2[i2],
Math::sq(R[0]) / _axes2[i0] + Math::sq(R[1]) / _axes2[i1]};
if (R[0] == 0 && R[1] == 0) {
real s = copysign(real(1), -R[2]);
N = {s*merid.c(), s*merid.s(), 0};
} else
Ellipsoid3::normvec(N);
// E = N x up
E = { N[1]*up[2] - N[2]*up[1], N[2]*up[0] - N[0]*up[2],
N[0]*up[1] - N[1]*up[0]};
roty(E, alt ? -1 : 0);
roty(N, alt ? -1 : 0);
}
template<int n>
void Cartesian3::cart2togeneric(vec3 R, vec3 V,
ang& phi, ang& lam, ang& zet,
bool alt) const {
cart2togeneric<n>(R, phi, lam, alt);
vec3 N, E;
cardinaldir(R, meridianplane<n>(lam, alt), N, E, alt);
zet = ang(V[0]*E[0] + V[1]*E[1] + V[2]*E[2],
V[0]*N[0] + V[1]*N[1] + V[2]*N[2]);
}
template<int n>
void Cartesian3::generictocart2(ang phi, ang lam, ang zet,
vec3& R, vec3&V, bool alt) const {
generictocart2<n>(phi, lam, R, alt);
vec3 N, E;
cardinaldir(R, meridianplane<n>(lam, alt), N, E, alt);
V = {zet.c() * N[0] + zet.s() * E[0],
zet.c() * N[1] + zet.s() * E[1],
zet.c() * N[2] + zet.s() * E[2]};
}
void Cartesian3::cart2toany(vec3 R,
coord coordout, Angle& lat, Angle& lon) const {
bool alt = coordout > ELLIPSOIDAL;
switch (coordout) {
case GEODETIC:
case GEODETIC_X:
cart2togeneric<2>(R, lat, lon, alt); break;
case PARAMETRIC:
case PARAMETRIC_X:
cart2togeneric<1>(R, lat, lon, alt); break;
case GEOCENTRIC:
case GEOCENTRIC_X:
cart2togeneric<0>(R, lat, lon, alt); break;
case ELLIPSOIDAL:
_t.cart2toellip(R, lat, lon); break;
default:
throw GeographicErr("Bad Cartesian3::coord value " + to_string(coordout));
}
}
void Cartesian3::anytocart2(coord coordin, Angle lat, Angle lon,
vec3& R) const {
bool alt = coordin > ELLIPSOIDAL;
switch (coordin) {
case GEODETIC:
case GEODETIC_X:
generictocart2<2>(lat, lon, R, alt); break;
case PARAMETRIC:
case PARAMETRIC_X:
generictocart2<1>(lat, lon, R, alt); break;
case GEOCENTRIC:
case GEOCENTRIC_X:
generictocart2<0>(lat, lon, R, alt); break;
case ELLIPSOIDAL:
_t.elliptocart2(lat, lon, R); break;
default:
throw GeographicErr("Bad Cartesian3::coord value " + to_string(coordin));
}
}
void Cartesian3::anytoany(coord coordin, Angle lat1, Angle lon1,
coord coordout, Angle& lat2, Angle& lon2) const {
vec3 R;
anytocart2(coordin, lat1, lon1, R);
cart2toany(R, coordout, lat2, lon2);
}
void Cartesian3::cart2toany(vec3 R, vec3 V, coord coordout,
Angle& lat, Angle& lon, Angle& azi) const {
bool alt = coordout > ELLIPSOIDAL;
switch (coordout) {
case GEODETIC:
case GEODETIC_X:
cart2togeneric<2>(R, V, lat, lon, azi, alt); break;
case PARAMETRIC:
case PARAMETRIC_X:
cart2togeneric<1>(R, V, lat, lon, azi, alt); break;
case GEOCENTRIC:
case GEOCENTRIC_X:
cart2togeneric<0>(R, V, lat, lon, azi, alt); break;
case ELLIPSOIDAL:
_t.cart2toellip(R, V, lat, lon, azi); break;
default:
throw GeographicErr("Bad Cartesian3::coord value " + to_string(coordout));
}
}
void Cartesian3::anytocart2(coord coordin, Angle lat, Angle lon, Angle azi,
vec3& R, vec3& V) const {
bool alt = coordin > ELLIPSOIDAL;
switch (coordin) {
case GEODETIC:
case GEODETIC_X:
generictocart2<2>(lat, lon, azi, R, V, alt); break;
case PARAMETRIC:
case PARAMETRIC_X:
generictocart2<1>(lat, lon, azi, R, V, alt); break;
case GEOCENTRIC:
case GEOCENTRIC_X:
generictocart2<0>(lat, lon, azi, R, V, alt); break;
case ELLIPSOIDAL:
_t.elliptocart2(lat, lon, azi, R, V); break;
default:
throw GeographicErr("Bad Cartesian3::coord value " + to_string(coordin));
}
}
void Cartesian3::carttoany(vec3 R, coord coordout,
Angle& lat, Angle& lon, real& h) const {
switch (coordout) {
case ELLIPSOIDAL: carttoellip(R, lat, lon, h); break;
default:
vec3 R2;
carttocart2(R, R2, h);
cart2toany(R2, coordout, lat, lon);
}
}
void Cartesian3::anytocart(coord coordin, Angle lat, Angle lon, real h,
vec3& R) const {
switch (coordin) {
case ELLIPSOIDAL: elliptocart(lat, lon, h, R); break;
default:
vec3 R2;
anytocart2(coordin, lat, lon, R2);
cart2tocart(R2, h, R);
}
}
void Cartesian3::cart2tocart(vec3 R2, real h, vec3& R) const {
vec3 Rn = {R2[0] / _axes2[0], R2[1] / _axes2[1], R2[2] / _axes2[2]};
real d = h / Math::hypot3(Rn[0], Rn[1], Rn[2]);
R = R2;
for (int k = 0; k < 3; ++k)
R[k] += Rn[k] * d;
}
void Cartesian3::carttocart2(vec3 R, vec3& R2, real& h) const {
const real eps = numeric_limits<real>::epsilon(), ztol = b() * eps/8;
for (int k = 0; k < 3; ++k)
if (fabs(R[k]) <= ztol) R[k] = copysign(real(0), R[k]);
vec3 s = {R[0] * _axes[0], R[1] * _axes[1], R[2] * _axes[2]};
real p = fmax(fmax(fabs(s[2]), hypot(s[1], s[2]) - _linecc2[1]),
Math::hypot3(s[0], s[1], s[2]) - _linecc2[0]);
const funp<2> f(s, _linecc2);
p = cartsolve(f, p, Math::sq(b()));
R2 = R;
for (int k = 0; k < 3; ++k)
R2[k] *= _axes2[k] / (p + _linecc2[k]);
// Deal with case p == 0 (when R2[2] is indeterminate).
if (p == 0) {
if (_linecc2[0] == 0) // sphere
R2[0] = R[0];
if (_linecc2[1] == 0) // sphere or prolate
R2[1] = R[1];
R2[2] = copysign(_axes[2], R[2]) *
sqrt(1 - Math::sq(R2[0]/_axes[0]) - Math::sq(R2[1]/_axes[1]));
}
// Easily shown that U.(R-R2) = (p - c^2) * U.U
// => h = Uhat.(R-R2) = (p - c^2) * |U|
h = (p - _axes2[2]) * Math::hypot3(R2[0] / _axes2[0],
R2[1] / _axes2[1],
R2[2] / _axes2[2]);
}
} // namespace Triaxial
} // namespace GeographicLib
Cartesian3.hpp
Header for GeographicLib::Triaxial::Cartesian3 class.
ang
GeographicLib::Angle ang
Definition Geod3Solve.cpp:26
real
GeographicLib::Math::real real
Definition Geod3Solve.cpp:25
GeographicLib::AngleT::c
T c() const
Definition Angle.hpp:153
GeographicLib::AngleT::s
T s() const
Definition Angle.hpp:149
GeographicLib::AngleT::modang
AngleT modang(T m) const
Definition Angle.hpp:711
GeographicLib::GeographicErr
Exception handling for GeographicLib.
Definition Constants.hpp:344
GeographicLib::Math
Mathematical functions needed by GeographicLib.
Definition Math.hpp:82
GeographicLib::Math::sq
static T sq(T x)
Definition Math.hpp:209
GeographicLib::Math::hypot3
static T hypot3(T x, T y, T z)
Definition Math.cpp:285
GeographicLib::Math::real
double real
Definition Math.hpp:115
GeographicLib::Triaxial::Cartesian3::anytoany
void anytoany(coord coordin, Angle lat1, Angle lon1, coord coordout, Angle &lat2, Angle &lon2) const
Definition Cartesian3.cpp:293
GeographicLib::Triaxial::Cartesian3::Cartesian3
Cartesian3(const Ellipsoid3 &t)
Definition Cartesian3.cpp:17
GeographicLib::Triaxial::Cartesian3::cart2tocart
void cart2tocart(vec3 R2, real h, vec3 &R) const
Definition Cartesian3.cpp:362
GeographicLib::Triaxial::Cartesian3::anytocart2
void anytocart2(coord coordin, Angle lat, Angle lon, vec3 &R) const
Definition Cartesian3.cpp:273
GeographicLib::Triaxial::Cartesian3::vec3
Ellipsoid3::vec3 vec3
Definition Cartesian3.hpp:107
GeographicLib::Triaxial::Cartesian3::coord
coord
Definition Cartesian3.hpp:192
GeographicLib::Triaxial::Cartesian3::ELLIPSOIDAL
@ ELLIPSOIDAL
Definition Cartesian3.hpp:212
GeographicLib::Triaxial::Cartesian3::GEODETIC_X
@ GEODETIC_X
Definition Cartesian3.hpp:217
GeographicLib::Triaxial::Cartesian3::GEOCENTRIC
@ GEOCENTRIC
Definition Cartesian3.hpp:207
GeographicLib::Triaxial::Cartesian3::GEODETIC
@ GEODETIC
Definition Cartesian3.hpp:197
GeographicLib::Triaxial::Cartesian3::PARAMETRIC_X
@ PARAMETRIC_X
Definition Cartesian3.hpp:222
GeographicLib::Triaxial::Cartesian3::PARAMETRIC
@ PARAMETRIC
Definition Cartesian3.hpp:202
GeographicLib::Triaxial::Cartesian3::GEOCENTRIC_X
@ GEOCENTRIC_X
Definition Cartesian3.hpp:227
GeographicLib::Triaxial::Cartesian3::carttoany
void carttoany(vec3 R, coord coordout, Angle &lat, Angle &lon, real &h) const
Definition Cartesian3.cpp:340
GeographicLib::Triaxial::Cartesian3::carttocart2
void carttocart2(vec3 R, vec3 &R2, real &h) const
Definition Cartesian3.cpp:370
GeographicLib::Triaxial::Cartesian3::t
const Ellipsoid3 & t() const
Definition Cartesian3.hpp:550
GeographicLib::Triaxial::Cartesian3::anytocart
void anytocart(coord coordin, Angle lat, Angle lon, real h, vec3 &R) const
Definition Cartesian3.cpp:351
GeographicLib::Triaxial::Cartesian3::cart2toany
void cart2toany(vec3 R, coord coordout, Angle &lat, Angle &lon) const
Definition Cartesian3.cpp:253
GeographicLib::Triaxial::Ellipsoid3
A triaxial ellipsoid.
Definition Ellipsoid3.hpp:89
GeographicLib::Triaxial
Namespace for operations on triaxial ellipsoids.
Definition Cartesian3.cpp:13
GeographicLib
Namespace for GeographicLib.
Definition Accumulator.cpp:12
GeographicLib::Angle
AngleT< Math::real > Angle
Definition Angle.hpp:760
std
Definition NearestNeighbor.hpp:806