pi/openvins_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

gps fusion implemented

Browse Source
This commit is contained in:
admin1
2022-08-28 22:25:47 +03:00
parent b8ee6672d1
commit 979c7a2250
27 changed files with 132151 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
global_fusion/Thirdparty/GeographicLib/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,36 @@
project (GeographicLib)
# Version information
set (PROJECT_VERSION_MAJOR 1)
set (PROJECT_VERSION_MINOR 49)
set (PROJECT_VERSION_PATCH 0)
set (PROJECT_VERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}")
if (PROJECT_VERSION_PATCH GREATER 0)
set (PROJECT_VERSION "${PROJECT_VERSION}.${PROJECT_VERSION_PATCH}")
endif ()
# The library version tracks the numbering given by libtool in the
# autoconf set up.
set (LIBVERSION_API 17)
set (LIBVERSION_BUILD 17.1.2)
string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
cmake_minimum_required (VERSION 2.8.4) # This version was released 2011-02-16
# (7) Set the default "real" precision. This should probably be left
# at 2 (double).
set (GEOGRAPHICLIB_PRECISION 2 CACHE STRING
"Precision: 1 = float, 2 = double, 3 = extended, 4 = quadruple, 5 = variable")
set_property (CACHE GEOGRAPHICLIB_PRECISION PROPERTY STRINGS 1 2 3 4 5)
set (LIBNAME Geographic)
include_directories(
./include/
)
add_library(libGeographiccc src/LocalCartesian.cpp
src/Geocentric.cpp
src/Math.cpp)

12
global_fusion/Thirdparty/GeographicLib/include/Config.h vendored Normal file
View File

@@ -0,0 +1,12 @@
// This will be overwritten by ./configure
#define GEOGRAPHICLIB_VERSION_STRING "1.49"
#define GEOGRAPHICLIB_VERSION_MAJOR 1
#define GEOGRAPHICLIB_VERSION_MINOR 49
#define GEOGRAPHICLIB_VERSION_PATCH 0
// Undefine HAVE_LONG_DOUBLE if this type is unknown to the compiler
#define GEOGRAPHICLIB_HAVE_LONG_DOUBLE 1
// Define WORDS_BIGENDIAN to be 1 if your machine is big endian
/* #undef WORDS_BIGENDIAN */

403
global_fusion/Thirdparty/GeographicLib/include/Constants.hpp vendored Normal file
View File

@@ -0,0 +1,403 @@
/**
* \file Constants.hpp
* \brief Header for GeographicLib::Constants class
*
* Copyright (c) Charles Karney (2008-2016) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#if !defined(GEOGRAPHICLIB_CONSTANTS_HPP)
#define GEOGRAPHICLIB_CONSTANTS_HPP 1
#include "Config.h"
/**
* @relates GeographicLib::Constants
* Pack the version components into a single integer. Users should not rely on
* this particular packing of the components of the version number; see the
* documentation for GEOGRAPHICLIB_VERSION, below.
**********************************************************************/
#define GEOGRAPHICLIB_VERSION_NUM(a,b,c) ((((a) * 10000 + (b)) * 100) + (c))
/**
* @relates GeographicLib::Constants
* The version of GeographicLib as a single integer, packed as MMmmmmpp where
* MM is the major version, mmmm is the minor version, and pp is the patch
* level. Users should not rely on this particular packing of the components
* of the version number. Instead they should use a test such as \code
#if GEOGRAPHICLIB_VERSION >= GEOGRAPHICLIB_VERSION_NUM(1,37,0)
...
#endif
* \endcode
**********************************************************************/
#define GEOGRAPHICLIB_VERSION \
GEOGRAPHICLIB_VERSION_NUM(GEOGRAPHICLIB_VERSION_MAJOR, \
GEOGRAPHICLIB_VERSION_MINOR, \
GEOGRAPHICLIB_VERSION_PATCH)
/**
* @relates GeographicLib::Constants
* Is the C++11 static_assert available?
**********************************************************************/
#if !defined(GEOGRAPHICLIB_HAS_STATIC_ASSERT)
# if __cplusplus >= 201103 || defined(__GXX_EXPERIMENTAL_CXX0X__)
# define GEOGRAPHICLIB_HAS_STATIC_ASSERT 1
# elif defined(_MSC_VER) && _MSC_VER >= 1600
// For reference, here is a table of Visual Studio and _MSC_VER
// correspondences:
//
// _MSC_VER Visual Studio
// 1100 vc5
// 1200 vc6
// 1300 vc7
// 1310 vc7.1 (2003)
// 1400 vc8 (2005)
// 1500 vc9 (2008)
// 1600 vc10 (2010)
// 1700 vc11 (2012)
// 1800 vc12 (2013)
// 1900 vc14 (2015)
// 1910+ vc15 (2017)
# define GEOGRAPHICLIB_HAS_STATIC_ASSERT 1
# else
# define GEOGRAPHICLIB_HAS_STATIC_ASSERT 0
# endif
#endif
/**
* @relates GeographicLib::Constants
* A compile-time assert. Use C++11 static_assert, if available.
**********************************************************************/
#if !defined(GEOGRAPHICLIB_STATIC_ASSERT)
# if GEOGRAPHICLIB_HAS_STATIC_ASSERT
# define GEOGRAPHICLIB_STATIC_ASSERT static_assert
# else
# define GEOGRAPHICLIB_STATIC_ASSERT(cond,reason) \
{ enum{ GEOGRAPHICLIB_STATIC_ASSERT_ENUM = 1/int(cond) }; }
# endif
#endif
#if defined(_MSC_VER) && defined(GEOGRAPHICLIB_SHARED_LIB) && \
GEOGRAPHICLIB_SHARED_LIB
# if GEOGRAPHICLIB_SHARED_LIB > 1
# error GEOGRAPHICLIB_SHARED_LIB must be 0 or 1
# elif defined(GeographicLib_EXPORTS)
# define GEOGRAPHICLIB_EXPORT __declspec(dllexport)
# else
# define GEOGRAPHICLIB_EXPORT __declspec(dllimport)
# endif
#else
# define GEOGRAPHICLIB_EXPORT
#endif
// Use GEOGRAPHICLIB_DEPRECATED to mark functions, types or variables as
// deprecated. Code inspired by Apache Subversion's svn_types.h file (via
// MPFR).
#if defined(__GNUC__)
# if __GNUC__ > 4
# define GEOGRAPHICLIB_DEPRECATED(msg) __attribute__((deprecated(msg)))
# else
# define GEOGRAPHICLIB_DEPRECATED(msg) __attribute__((deprecated))
# endif
#elif defined(_MSC_VER) && _MSC_VER >= 1300
# define GEOGRAPHICLIB_DEPRECATED(msg) __declspec(deprecated(msg))
#else
# define GEOGRAPHICLIB_DEPRECATED(msg)
#endif
#include <stdexcept>
#include <string>
#include "Math.hpp"
/**
* \brief Namespace for %GeographicLib
*
* All of %GeographicLib is defined within the GeographicLib namespace. In
* addition all the header files are included via %GeographicLib/Class.hpp.
* This minimizes the likelihood of conflicts with other packages.
**********************************************************************/
namespace GeographicLib {
/**
* \brief %Constants needed by %GeographicLib
*
* Define constants specifying the WGS84 ellipsoid, the UTM and UPS
* projections, and various unit conversions.
*
* Example of use:
* \include example-Constants.cpp
**********************************************************************/
class GEOGRAPHICLIB_EXPORT Constants {
private:
typedef Math::real real;
Constants(); // Disable constructor
public:
/**
* A synonym for Math::degree<real>().
**********************************************************************/
static Math::real degree() { return Math::degree(); }
/**
* @return the number of radians in an arcminute.
**********************************************************************/
static Math::real arcminute()
{ return Math::degree() / 60; }
/**
* @return the number of radians in an arcsecond.
**********************************************************************/
static Math::real arcsecond()
{ return Math::degree() / 3600; }
/** \name Ellipsoid parameters
**********************************************************************/
///@{
/**
* @tparam T the type of the returned value.
* @return the equatorial radius of WGS84 ellipsoid (6378137 m).
**********************************************************************/
template<typename T> static T WGS84_a()
{ return 6378137 * meter<T>(); }
/**
* A synonym for WGS84_a<real>().
**********************************************************************/
static Math::real WGS84_a() { return WGS84_a<real>(); }
/**
* @tparam T the type of the returned value.
* @return the flattening of WGS84 ellipsoid (1/298.257223563).
**********************************************************************/
template<typename T> static T WGS84_f() {
// Evaluating this as 1000000000 / T(298257223563LL) reduces the
// round-off error by about 10%. However, expressing the flattening as
// 1/298.257223563 is well ingrained.
return 1 / ( T(298257223563LL) / 1000000000 );
}
/**
* A synonym for WGS84_f<real>().
**********************************************************************/
static Math::real WGS84_f() { return WGS84_f<real>(); }
/**
* @tparam T the type of the returned value.
* @return the gravitational constant of the WGS84 ellipsoid, \e GM, in
* m<sup>3</sup> s<sup>&minus;2</sup>.
**********************************************************************/
template<typename T> static T WGS84_GM()
{ return T(3986004) * 100000000 + 41800000; }
/**
* A synonym for WGS84_GM<real>().
**********************************************************************/
static Math::real WGS84_GM() { return WGS84_GM<real>(); }
/**
* @tparam T the type of the returned value.
* @return the angular velocity of the WGS84 ellipsoid, &omega;, in rad
* s<sup>&minus;1</sup>.
**********************************************************************/
template<typename T> static T WGS84_omega()
{ return 7292115 / (T(1000000) * 100000); }
/**
* A synonym for WGS84_omega<real>().
**********************************************************************/
static Math::real WGS84_omega() { return WGS84_omega<real>(); }
/**
* @tparam T the type of the returned value.
* @return the equatorial radius of GRS80 ellipsoid, \e a, in m.
**********************************************************************/
template<typename T> static T GRS80_a()
{ return 6378137 * meter<T>(); }
/**
* A synonym for GRS80_a<real>().
**********************************************************************/
static Math::real GRS80_a() { return GRS80_a<real>(); }
/**
* @tparam T the type of the returned value.
* @return the gravitational constant of the GRS80 ellipsoid, \e GM, in
* m<sup>3</sup> s<sup>&minus;2</sup>.
**********************************************************************/
template<typename T> static T GRS80_GM()
{ return T(3986005) * 100000000; }
/**
* A synonym for GRS80_GM<real>().
**********************************************************************/
static Math::real GRS80_GM() { return GRS80_GM<real>(); }
/**
* @tparam T the type of the returned value.
* @return the angular velocity of the GRS80 ellipsoid, &omega;, in rad
* s<sup>&minus;1</sup>.
*
* This is about 2 &pi; 366.25 / (365.25 &times; 24 &times; 3600) rad
* s<sup>&minus;1</sup>. 365.25 is the number of days in a Julian year and
* 365.35/366.25 converts from solar days to sidereal days. Using the
* number of days in a Gregorian year (365.2425) results in a worse
* approximation (because the Gregorian year includes the precession of the
* earth's axis).
**********************************************************************/
template<typename T> static T GRS80_omega()
{ return 7292115 / (T(1000000) * 100000); }
/**
* A synonym for GRS80_omega<real>().
**********************************************************************/
static Math::real GRS80_omega() { return GRS80_omega<real>(); }
/**
* @tparam T the type of the returned value.
* @return the dynamical form factor of the GRS80 ellipsoid,
* <i>J</i><sub>2</sub>.
**********************************************************************/
template<typename T> static T GRS80_J2()
{ return T(108263) / 100000000; }
/**
* A synonym for GRS80_J2<real>().
**********************************************************************/
static Math::real GRS80_J2() { return GRS80_J2<real>(); }
/**
* @tparam T the type of the returned value.
* @return the central scale factor for UTM (0.9996).
**********************************************************************/
template<typename T> static T UTM_k0()
{return T(9996) / 10000; }
/**
* A synonym for UTM_k0<real>().
**********************************************************************/
static Math::real UTM_k0() { return UTM_k0<real>(); }
/**
* @tparam T the type of the returned value.
* @return the central scale factor for UPS (0.994).
**********************************************************************/
template<typename T> static T UPS_k0()
{ return T(994) / 1000; }
/**
* A synonym for UPS_k0<real>().
**********************************************************************/
static Math::real UPS_k0() { return UPS_k0<real>(); }
///@}
/** \name SI units
**********************************************************************/
///@{
/**
* @tparam T the type of the returned value.
* @return the number of meters in a meter.
*
* This is unity, but this lets the internal system of units be changed if
* necessary.
**********************************************************************/
template<typename T> static T meter() { return T(1); }
/**
* A synonym for meter<real>().
**********************************************************************/
static Math::real meter() { return meter<real>(); }
/**
* @return the number of meters in a kilometer.
**********************************************************************/
static Math::real kilometer()
{ return 1000 * meter<real>(); }
/**
* @return the number of meters in a nautical mile (approximately 1 arc
* minute)
**********************************************************************/
static Math::real nauticalmile()
{ return 1852 * meter<real>(); }
/**
* @tparam T the type of the returned value.
* @return the number of square meters in a square meter.
*
* This is unity, but this lets the internal system of units be changed if
* necessary.
**********************************************************************/
template<typename T> static T square_meter()
{ return meter<real>() * meter<real>(); }
/**
* A synonym for square_meter<real>().
**********************************************************************/
static Math::real square_meter()
{ return square_meter<real>(); }
/**
* @return the number of square meters in a hectare.
**********************************************************************/
static Math::real hectare()
{ return 10000 * square_meter<real>(); }
/**
* @return the number of square meters in a square kilometer.
**********************************************************************/
static Math::real square_kilometer()
{ return kilometer() * kilometer(); }
/**
* @return the number of square meters in a square nautical mile.
**********************************************************************/
static Math::real square_nauticalmile()
{ return nauticalmile() * nauticalmile(); }
///@}
/** \name Anachronistic British units
**********************************************************************/
///@{
/**
* @return the number of meters in an international foot.
**********************************************************************/
static Math::real foot()
{ return real(254 * 12) / 10000 * meter<real>(); }
/**
* @return the number of meters in a yard.
**********************************************************************/
static Math::real yard() { return 3 * foot(); }
/**
* @return the number of meters in a fathom.
**********************************************************************/
static Math::real fathom() { return 2 * yard(); }
/**
* @return the number of meters in a chain.
**********************************************************************/
static Math::real chain() { return 22 * yard(); }
/**
* @return the number of meters in a furlong.
**********************************************************************/
static Math::real furlong() { return 10 * chain(); }
/**
* @return the number of meters in a statute mile.
**********************************************************************/
static Math::real mile() { return 8 * furlong(); }
/**
* @return the number of square meters in an acre.
**********************************************************************/
static Math::real acre() { return chain() * furlong(); }
/**
* @return the number of square meters in a square statute mile.
**********************************************************************/
static Math::real square_mile() { return mile() * mile(); }
///@}
/** \name Anachronistic US units
**********************************************************************/
///@{
/**
* @return the number of meters in a US survey foot.
**********************************************************************/
static Math::real surveyfoot()
{ return real(1200) / 3937 * meter<real>(); }
///@}
};
/**
* \brief Exception handling for %GeographicLib
*
* A class to handle exceptions. It's derived from std::runtime_error so it
* can be caught by the usual catch clauses.
*
* Example of use:
* \include example-GeographicErr.cpp
**********************************************************************/
class GeographicErr : public std::runtime_error {
public:
/**
* Constructor
*
* @param[in] msg a string message, which is accessible in the catch
* clause via what().
**********************************************************************/
GeographicErr(const std::string& msg) : std::runtime_error(msg) {}
};
} // namespace GeographicLib
#endif // GEOGRAPHICLIB_CONSTANTS_HPP

267
global_fusion/Thirdparty/GeographicLib/include/Geocentric.hpp vendored Normal file
View File

@@ -0,0 +1,267 @@
/**
* \file Geocentric.hpp
* \brief Header for GeographicLib::Geocentric class
*
* Copyright (c) Charles Karney (2008-2016) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#if !defined(GEOGRAPHICLIB_GEOCENTRIC_HPP)
#define GEOGRAPHICLIB_GEOCENTRIC_HPP 1
#include <vector>
#include "Constants.hpp"
namespace GeographicLib {
/**
* \brief %Geocentric coordinates
*
* Convert between geodetic coordinates latitude = \e lat, longitude = \e
* lon, height = \e h (measured vertically from the surface of the ellipsoid)
* to geocentric coordinates (\e X, \e Y, \e Z). The origin of geocentric
* coordinates is at the center of the earth. The \e Z axis goes thru the
* north pole, \e lat = 90&deg;. The \e X axis goes thru \e lat = 0,
* \e lon = 0. %Geocentric coordinates are also known as earth centered,
* earth fixed (ECEF) coordinates.
*
* The conversion from geographic to geocentric coordinates is
* straightforward. For the reverse transformation we use
* - H. Vermeille,
* <a href="https://doi.org/10.1007/s00190-002-0273-6"> Direct
* transformation from geocentric coordinates to geodetic coordinates</a>,
* J. Geodesy 76, 451--454 (2002).
* .
* Several changes have been made to ensure that the method returns accurate
* results for all finite inputs (even if \e h is infinite). The changes are
* described in Appendix B of
* - C. F. F. Karney,
* <a href="https://arxiv.org/abs/1102.1215v1">Geodesics
* on an ellipsoid of revolution</a>,
* Feb. 2011;
* preprint
* <a href="https://arxiv.org/abs/1102.1215v1">arxiv:1102.1215v1</a>.
* .
* Vermeille similarly updated his method in
* - H. Vermeille,
* <a href="https://doi.org/10.1007/s00190-010-0419-x">
* An analytical method to transform geocentric into
* geodetic coordinates</a>, J. Geodesy 85, 105--117 (2011).
* .
* See \ref geocentric for more information.
*
* The errors in these routines are close to round-off. Specifically, for
* points within 5000 km of the surface of the ellipsoid (either inside or
* outside the ellipsoid), the error is bounded by 7 nm (7 nanometers) for
* the WGS84 ellipsoid. See \ref geocentric for further information on the
* errors.
*
* Example of use:
* \include example-Geocentric.cpp
*
* <a href="CartConvert.1.html">CartConvert</a> is a command-line utility
* providing access to the functionality of Geocentric and LocalCartesian.
**********************************************************************/
class GEOGRAPHICLIB_EXPORT Geocentric {
private:
typedef Math::real real;
friend class LocalCartesian;
friend class MagneticCircle; // MagneticCircle uses Rotation
friend class MagneticModel; // MagneticModel uses IntForward
friend class GravityCircle; // GravityCircle uses Rotation
friend class GravityModel; // GravityModel uses IntForward
friend class NormalGravity; // NormalGravity uses IntForward
static const size_t dim_ = 3;
static const size_t dim2_ = dim_ * dim_;
real _a, _f, _e2, _e2m, _e2a, _e4a, _maxrad;
static void Rotation(real sphi, real cphi, real slam, real clam,
real M[dim2_]);
static void Rotate(real M[dim2_], real x, real y, real z,
real& X, real& Y, real& Z) {
// Perform [X,Y,Z]^t = M.[x,y,z]^t
// (typically local cartesian to geocentric)
X = M[0] * x + M[1] * y + M[2] * z;
Y = M[3] * x + M[4] * y + M[5] * z;
Z = M[6] * x + M[7] * y + M[8] * z;
}
static void Unrotate(real M[dim2_], real X, real Y, real Z,
real& x, real& y, real& z) {
// Perform [x,y,z]^t = M^t.[X,Y,Z]^t
// (typically geocentric to local cartesian)
x = M[0] * X + M[3] * Y + M[6] * Z;
y = M[1] * X + M[4] * Y + M[7] * Z;
z = M[2] * X + M[5] * Y + M[8] * Z;
}
void IntForward(real lat, real lon, real h, real& X, real& Y, real& Z,
real M[dim2_]) const;
void IntReverse(real X, real Y, real Z, real& lat, real& lon, real& h,
real M[dim2_]) const;
public:
/**
* Constructor for a ellipsoid with
*
* @param[in] a equatorial radius (meters).
* @param[in] f flattening of ellipsoid. Setting \e f = 0 gives a sphere.
* Negative \e f gives a prolate ellipsoid.
* @exception GeographicErr if \e a or (1 &minus; \e f) \e a is not
* positive.
**********************************************************************/
Geocentric(real a, real f);
/**
* A default constructor (for use by NormalGravity).
**********************************************************************/
Geocentric() : _a(-1) {}
/**
* Convert from geodetic to geocentric coordinates.
*
* @param[in] lat latitude of point (degrees).
* @param[in] lon longitude of point (degrees).
* @param[in] h height of point above the ellipsoid (meters).
* @param[out] X geocentric coordinate (meters).
* @param[out] Y geocentric coordinate (meters).
* @param[out] Z geocentric coordinate (meters).
*
* \e lat should be in the range [&minus;90&deg;, 90&deg;].
**********************************************************************/
void Forward(real lat, real lon, real h, real& X, real& Y, real& Z)
const {
if (Init())
IntForward(lat, lon, h, X, Y, Z, NULL);
}
/**
* Convert from geodetic to geocentric coordinates and return rotation
* matrix.
*
* @param[in] lat latitude of point (degrees).
* @param[in] lon longitude of point (degrees).
* @param[in] h height of point above the ellipsoid (meters).
* @param[out] X geocentric coordinate (meters).
* @param[out] Y geocentric coordinate (meters).
* @param[out] Z geocentric coordinate (meters).
* @param[out] M if the length of the vector is 9, fill with the rotation
* matrix in row-major order.
*
* Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can
* express \e v as \e column vectors in one of two ways
* - in east, north, up coordinates (where the components are relative to a
* local coordinate system at (\e lat, \e lon, \e h)); call this
* representation \e v1.
* - in geocentric \e X, \e Y, \e Z coordinates; call this representation
* \e v0.
* .
* Then we have \e v0 = \e M &sdot; \e v1.
**********************************************************************/
void Forward(real lat, real lon, real h, real& X, real& Y, real& Z,
std::vector<real>& M)
const {
if (!Init())
return;
if (M.end() == M.begin() + dim2_) {
real t[dim2_];
IntForward(lat, lon, h, X, Y, Z, t);
std::copy(t, t + dim2_, M.begin());
} else
IntForward(lat, lon, h, X, Y, Z, NULL);
}
/**
* Convert from geocentric to geodetic to coordinates.
*
* @param[in] X geocentric coordinate (meters).
* @param[in] Y geocentric coordinate (meters).
* @param[in] Z geocentric coordinate (meters).
* @param[out] lat latitude of point (degrees).
* @param[out] lon longitude of point (degrees).
* @param[out] h height of point above the ellipsoid (meters).
*
* In general there are multiple solutions and the result which maximizes
* \e h is returned. If there are still multiple solutions with different
* latitudes (applies only if \e Z = 0), then the solution with \e lat > 0
* is returned. If there are still multiple solutions with different
* longitudes (applies only if \e X = \e Y = 0) then \e lon = 0 is
* returned. The value of \e h returned satisfies \e h &ge; &minus; \e a
* (1 &minus; <i>e</i><sup>2</sup>) / sqrt(1 &minus; <i>e</i><sup>2</sup>
* sin<sup>2</sup>\e lat). The value of \e lon returned is in the range
* [&minus;180&deg;, 180&deg;].
**********************************************************************/
void Reverse(real X, real Y, real Z, real& lat, real& lon, real& h)
const {
if (Init())
IntReverse(X, Y, Z, lat, lon, h, NULL);
}
/**
* Convert from geocentric to geodetic to coordinates.
*
* @param[in] X geocentric coordinate (meters).
* @param[in] Y geocentric coordinate (meters).
* @param[in] Z geocentric coordinate (meters).
* @param[out] lat latitude of point (degrees).
* @param[out] lon longitude of point (degrees).
* @param[out] h height of point above the ellipsoid (meters).
* @param[out] M if the length of the vector is 9, fill with the rotation
* matrix in row-major order.
*
* Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can
* express \e v as \e column vectors in one of two ways
* - in east, north, up coordinates (where the components are relative to a
* local coordinate system at (\e lat, \e lon, \e h)); call this
* representation \e v1.
* - in geocentric \e X, \e Y, \e Z coordinates; call this representation
* \e v0.
* .
* Then we have \e v1 = <i>M</i><sup>T</sup> &sdot; \e v0, where
* <i>M</i><sup>T</sup> is the transpose of \e M.
**********************************************************************/
void Reverse(real X, real Y, real Z, real& lat, real& lon, real& h,
std::vector<real>& M)
const {
if (!Init())
return;
if (M.end() == M.begin() + dim2_) {
real t[dim2_];
IntReverse(X, Y, Z, lat, lon, h, t);
std::copy(t, t + dim2_, M.begin());
} else
IntReverse(X, Y, Z, lat, lon, h, NULL);
}
/** \name Inspector functions
**********************************************************************/
///@{
/**
* @return true if the object has been initialized.
**********************************************************************/
bool Init() const { return _a > 0; }
/**
* @return \e a the equatorial radius of the ellipsoid (meters). This is
* the value used in the constructor.
**********************************************************************/
Math::real MajorRadius() const
{ return Init() ? _a : Math::NaN(); }
/**
* @return \e f the flattening of the ellipsoid. This is the
* value used in the constructor.
**********************************************************************/
Math::real Flattening() const
{ return Init() ? _f : Math::NaN(); }
///@}
/**
* A global instantiation of Geocentric with the parameters for the WGS84
* ellipsoid.
**********************************************************************/
static const Geocentric& WGS84();
};
} // namespace GeographicLib
#endif // GEOGRAPHICLIB_GEOCENTRIC_HPP

236
global_fusion/Thirdparty/GeographicLib/include/LocalCartesian.hpp vendored Normal file
View File

@@ -0,0 +1,236 @@
/**
* \file LocalCartesian.hpp
* \brief Header for GeographicLib::LocalCartesian class
*
* Copyright (c) Charles Karney (2008-2016) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#if !defined(GEOGRAPHICLIB_LOCALCARTESIAN_HPP)
#define GEOGRAPHICLIB_LOCALCARTESIAN_HPP 1
#include "Geocentric.hpp"
#include "Constants.hpp"
namespace GeographicLib {
/**
* \brief Local cartesian coordinates
*
* Convert between geodetic coordinates latitude = \e lat, longitude = \e
* lon, height = \e h (measured vertically from the surface of the ellipsoid)
* to local cartesian coordinates (\e x, \e y, \e z). The origin of local
* cartesian coordinate system is at \e lat = \e lat0, \e lon = \e lon0, \e h
* = \e h0. The \e z axis is normal to the ellipsoid; the \e y axis points
* due north. The plane \e z = - \e h0 is tangent to the ellipsoid.
*
* The conversions all take place via geocentric coordinates using a
* Geocentric object (by default Geocentric::WGS84()).
*
* Example of use:
* \include example-LocalCartesian.cpp
*
* <a href="CartConvert.1.html">CartConvert</a> is a command-line utility
* providing access to the functionality of Geocentric and LocalCartesian.
**********************************************************************/
class GEOGRAPHICLIB_EXPORT LocalCartesian {
private:
typedef Math::real real;
static const size_t dim_ = 3;
static const size_t dim2_ = dim_ * dim_;
Geocentric _earth;
real _lat0, _lon0, _h0;
real _x0, _y0, _z0, _r[dim2_];
void IntForward(real lat, real lon, real h, real& x, real& y, real& z,
real M[dim2_]) const;
void IntReverse(real x, real y, real z, real& lat, real& lon, real& h,
real M[dim2_]) const;
void MatrixMultiply(real M[dim2_]) const;
public:
/**
* Constructor setting the origin.
*
* @param[in] lat0 latitude at origin (degrees).
* @param[in] lon0 longitude at origin (degrees).
* @param[in] h0 height above ellipsoid at origin (meters); default 0.
* @param[in] earth Geocentric object for the transformation; default
* Geocentric::WGS84().
*
* \e lat0 should be in the range [&minus;90&deg;, 90&deg;].
**********************************************************************/
LocalCartesian(real lat0, real lon0, real h0 = 0,
const Geocentric& earth = Geocentric::WGS84())
: _earth(earth)
{ Reset(lat0, lon0, h0); }
/**
* Default constructor.
*
* @param[in] earth Geocentric object for the transformation; default
* Geocentric::WGS84().
*
* Sets \e lat0 = 0, \e lon0 = 0, \e h0 = 0.
**********************************************************************/
explicit LocalCartesian(const Geocentric& earth = Geocentric::WGS84())
: _earth(earth)
{ Reset(real(0), real(0), real(0)); }
/**
* Reset the origin.
*
* @param[in] lat0 latitude at origin (degrees).
* @param[in] lon0 longitude at origin (degrees).
* @param[in] h0 height above ellipsoid at origin (meters); default 0.
*
* \e lat0 should be in the range [&minus;90&deg;, 90&deg;].
**********************************************************************/
void Reset(real lat0, real lon0, real h0 = 0);
/**
* Convert from geodetic to local cartesian coordinates.
*
* @param[in] lat latitude of point (degrees).
* @param[in] lon longitude of point (degrees).
* @param[in] h height of point above the ellipsoid (meters).
* @param[out] x local cartesian coordinate (meters).
* @param[out] y local cartesian coordinate (meters).
* @param[out] z local cartesian coordinate (meters).
*
* \e lat should be in the range [&minus;90&deg;, 90&deg;].
**********************************************************************/
void Forward(real lat, real lon, real h, real& x, real& y, real& z)
const {
IntForward(lat, lon, h, x, y, z, NULL);
}
/**
* Convert from geodetic to local cartesian coordinates and return rotation
* matrix.
*
* @param[in] lat latitude of point (degrees).
* @param[in] lon longitude of point (degrees).
* @param[in] h height of point above the ellipsoid (meters).
* @param[out] x local cartesian coordinate (meters).
* @param[out] y local cartesian coordinate (meters).
* @param[out] z local cartesian coordinate (meters).
* @param[out] M if the length of the vector is 9, fill with the rotation
* matrix in row-major order.
*
* \e lat should be in the range [&minus;90&deg;, 90&deg;].
*
* Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can
* express \e v as \e column vectors in one of two ways
* - in east, north, up coordinates (where the components are relative to a
* local coordinate system at (\e lat, \e lon, \e h)); call this
* representation \e v1.
* - in \e x, \e y, \e z coordinates (where the components are relative to
* the local coordinate system at (\e lat0, \e lon0, \e h0)); call this
* representation \e v0.
* .
* Then we have \e v0 = \e M &sdot; \e v1.
**********************************************************************/
void Forward(real lat, real lon, real h, real& x, real& y, real& z,
std::vector<real>& M)
const {
if (M.end() == M.begin() + dim2_) {
real t[dim2_];
IntForward(lat, lon, h, x, y, z, t);
std::copy(t, t + dim2_, M.begin());
} else
IntForward(lat, lon, h, x, y, z, NULL);
}
/**
* Convert from local cartesian to geodetic coordinates.
*
* @param[in] x local cartesian coordinate (meters).
* @param[in] y local cartesian coordinate (meters).
* @param[in] z local cartesian coordinate (meters).
* @param[out] lat latitude of point (degrees).
* @param[out] lon longitude of point (degrees).
* @param[out] h height of point above the ellipsoid (meters).
*
* The value of \e lon returned is in the range [&minus;180&deg;,
* 180&deg;].
**********************************************************************/
void Reverse(real x, real y, real z, real& lat, real& lon, real& h)
const {
IntReverse(x, y, z, lat, lon, h, NULL);
}
/**
* Convert from local cartesian to geodetic coordinates and return rotation
* matrix.
*
* @param[in] x local cartesian coordinate (meters).
* @param[in] y local cartesian coordinate (meters).
* @param[in] z local cartesian coordinate (meters).
* @param[out] lat latitude of point (degrees).
* @param[out] lon longitude of point (degrees).
* @param[out] h height of point above the ellipsoid (meters).
* @param[out] M if the length of the vector is 9, fill with the rotation
* matrix in row-major order.
*
* Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can
* express \e v as \e column vectors in one of two ways
* - in east, north, up coordinates (where the components are relative to a
* local coordinate system at (\e lat, \e lon, \e h)); call this
* representation \e v1.
* - in \e x, \e y, \e z coordinates (where the components are relative to
* the local coordinate system at (\e lat0, \e lon0, \e h0)); call this
* representation \e v0.
* .
* Then we have \e v1 = <i>M</i><sup>T</sup> &sdot; \e v0, where
* <i>M</i><sup>T</sup> is the transpose of \e M.
**********************************************************************/
void Reverse(real x, real y, real z, real& lat, real& lon, real& h,
std::vector<real>& M)
const {
if (M.end() == M.begin() + dim2_) {
real t[dim2_];
IntReverse(x, y, z, lat, lon, h, t);
std::copy(t, t + dim2_, M.begin());
} else
IntReverse(x, y, z, lat, lon, h, NULL);
}
/** \name Inspector functions
**********************************************************************/
///@{
/**
* @return latitude of the origin (degrees).
**********************************************************************/
Math::real LatitudeOrigin() const { return _lat0; }
/**
* @return longitude of the origin (degrees).
**********************************************************************/
Math::real LongitudeOrigin() const { return _lon0; }
/**
* @return height of the origin (meters).
**********************************************************************/
Math::real HeightOrigin() const { return _h0; }
/**
* @return \e a the equatorial radius of the ellipsoid (meters). This is
* the value of \e a inherited from the Geocentric object used in the
* constructor.
**********************************************************************/
Math::real MajorRadius() const { return _earth.MajorRadius(); }
/**
* @return \e f the flattening of the ellipsoid. This is the value
* inherited from the Geocentric object used in the constructor.
**********************************************************************/
Math::real Flattening() const { return _earth.Flattening(); }
///@}
};
} // namespace GeographicLib
#endif // GEOGRAPHICLIB_LOCALCARTESIAN_HPP

945
global_fusion/Thirdparty/GeographicLib/include/Math.hpp vendored Normal file
View File

@@ -0,0 +1,945 @@
/**
* \file Math.hpp
* \brief Header for GeographicLib::Math class
*
* Copyright (c) Charles Karney (2008-2017) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
// Constants.hpp includes Math.hpp. Place this include outside Math.hpp's
// include guard to enforce this ordering.
#include "Constants.hpp"
#if !defined(GEOGRAPHICLIB_MATH_HPP)
#define GEOGRAPHICLIB_MATH_HPP 1
/**
* Are C++11 math functions available?
**********************************************************************/
#if !defined(GEOGRAPHICLIB_CXX11_MATH)
// Recent versions of g++ -std=c++11 (4.7 and later?) set __cplusplus to 201103
// and support the new C++11 mathematical functions, std::atanh, etc. However
// the Android toolchain, which uses g++ -std=c++11 (4.8 as of 2014-03-11,
// according to Pullan Lu), does not support std::atanh. Android toolchains
// might define __ANDROID__ or ANDROID; so need to check both. With OSX the
// version is GNUC version 4.2 and __cplusplus is set to 201103, so remove the
// version check on GNUC.
# if defined(__GNUC__) && __cplusplus >= 201103 && \
!(defined(__ANDROID__) || defined(ANDROID) || defined(__CYGWIN__))
# define GEOGRAPHICLIB_CXX11_MATH 1
// Visual C++ 12 supports these functions
# elif defined(_MSC_VER) && _MSC_VER >= 1800
# define GEOGRAPHICLIB_CXX11_MATH 1
# else
# define GEOGRAPHICLIB_CXX11_MATH 0
# endif
#endif
#if !defined(GEOGRAPHICLIB_WORDS_BIGENDIAN)
# define GEOGRAPHICLIB_WORDS_BIGENDIAN 0
#endif
#if !defined(GEOGRAPHICLIB_HAVE_LONG_DOUBLE)
# define GEOGRAPHICLIB_HAVE_LONG_DOUBLE 0
#endif
#if !defined(GEOGRAPHICLIB_PRECISION)
/**
* The precision of floating point numbers used in %GeographicLib. 1 means
* float (single precision); 2 (the default) means double; 3 means long double;
* 4 is reserved for quadruple precision. Nearly all the testing has been
* carried out with doubles and that's the recommended configuration. In order
* for long double to be used, GEOGRAPHICLIB_HAVE_LONG_DOUBLE needs to be
* defined. Note that with Microsoft Visual Studio, long double is the same as
* double.
**********************************************************************/
# define GEOGRAPHICLIB_PRECISION 2
#endif
#include <cmath>
#include <algorithm>
#include <limits>
#if GEOGRAPHICLIB_PRECISION == 4
#include <boost/version.hpp>
#if BOOST_VERSION >= 105600
#include <boost/cstdfloat.hpp>
#endif
#include <boost/multiprecision/float128.hpp>
#include <boost/math/special_functions.hpp>
__float128 fmaq(__float128, __float128, __float128);
#elif GEOGRAPHICLIB_PRECISION == 5
#include <mpreal.h>
#endif
#if GEOGRAPHICLIB_PRECISION > 3
// volatile keyword makes no sense for multiprec types
#define GEOGRAPHICLIB_VOLATILE
// Signal a convergence failure with multiprec types by throwing an exception
// at loop exit.
#define GEOGRAPHICLIB_PANIC \
(throw GeographicLib::GeographicErr("Convergence failure"), false)
#else
#define GEOGRAPHICLIB_VOLATILE volatile
// Ignore convergence failures with standard floating points types by allowing
// loop to exit cleanly.
#define GEOGRAPHICLIB_PANIC false
#endif
namespace GeographicLib {
/**
* \brief Mathematical functions needed by %GeographicLib
*
* Define mathematical functions in order to localize system dependencies and
* to provide generic versions of the functions. In addition define a real
* type to be used by %GeographicLib.
*
* Example of use:
* \include example-Math.cpp
**********************************************************************/
class GEOGRAPHICLIB_EXPORT Math {
private:
void dummy() {
GEOGRAPHICLIB_STATIC_ASSERT(GEOGRAPHICLIB_PRECISION >= 1 &&
GEOGRAPHICLIB_PRECISION <= 5,
"Bad value of precision");
}
Math(); // Disable constructor
public:
#if GEOGRAPHICLIB_HAVE_LONG_DOUBLE
/**
* The extended precision type for real numbers, used for some testing.
* This is long double on computers with this type; otherwise it is double.
**********************************************************************/
typedef long double extended;
#else
typedef double extended;
#endif
#if GEOGRAPHICLIB_PRECISION == 2
/**
* The real type for %GeographicLib. Nearly all the testing has been done
* with \e real = double. However, the algorithms should also work with
* float and long double (where available). (<b>CAUTION</b>: reasonable
* accuracy typically cannot be obtained using floats.)
**********************************************************************/
typedef double real;
#elif GEOGRAPHICLIB_PRECISION == 1
typedef float real;
#elif GEOGRAPHICLIB_PRECISION == 3
typedef extended real;
#elif GEOGRAPHICLIB_PRECISION == 4
typedef boost::multiprecision::float128 real;
#elif GEOGRAPHICLIB_PRECISION == 5
typedef mpfr::mpreal real;
#else
typedef double real;
#endif
/**
* @return the number of bits of precision in a real number.
**********************************************************************/
static int digits() {
#if GEOGRAPHICLIB_PRECISION != 5
return std::numeric_limits<real>::digits;
#else
return std::numeric_limits<real>::digits();
#endif
}
/**
* Set the binary precision of a real number.
*
* @param[in] ndigits the number of bits of precision.
* @return the resulting number of bits of precision.
*
* This only has an effect when GEOGRAPHICLIB_PRECISION = 5. See also
* Utility::set_digits for caveats about when this routine should be
* called.
**********************************************************************/
static int set_digits(int ndigits) {
#if GEOGRAPHICLIB_PRECISION != 5
(void)ndigits;
#else
mpfr::mpreal::set_default_prec(ndigits >= 2 ? ndigits : 2);
#endif
return digits();
}
/**
* @return the number of decimal digits of precision in a real number.
**********************************************************************/
static int digits10() {
#if GEOGRAPHICLIB_PRECISION != 5
return std::numeric_limits<real>::digits10;
#else
return std::numeric_limits<real>::digits10();
#endif
}
/**
* Number of additional decimal digits of precision for real relative to
* double (0 for float).
**********************************************************************/
static int extra_digits() {
return
digits10() > std::numeric_limits<double>::digits10 ?
digits10() - std::numeric_limits<double>::digits10 : 0;
}
/**
* true if the machine is big-endian.
**********************************************************************/
static const bool bigendian = GEOGRAPHICLIB_WORDS_BIGENDIAN;
/**
* @tparam T the type of the returned value.
* @return &pi;.
**********************************************************************/
template<typename T> static T pi() {
using std::atan2;
static const T pi = atan2(T(0), T(-1));
return pi;
}
/**
* A synonym for pi<real>().
**********************************************************************/
static real pi() { return pi<real>(); }
/**
* @tparam T the type of the returned value.
* @return the number of radians in a degree.
**********************************************************************/
template<typename T> static T degree() {
static const T degree = pi<T>() / 180;
return degree;
}
/**
* A synonym for degree<real>().
**********************************************************************/
static real degree() { return degree<real>(); }
/**
* Square a number.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return <i>x</i><sup>2</sup>.
**********************************************************************/
template<typename T> static T sq(T x)
{ return x * x; }
/**
* The hypotenuse function avoiding underflow and overflow.
*
* @tparam T the type of the arguments and the returned value.
* @param[in] x
* @param[in] y
* @return sqrt(<i>x</i><sup>2</sup> + <i>y</i><sup>2</sup>).
**********************************************************************/
template<typename T> static T hypot(T x, T y) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::hypot; return hypot(x, y);
#else
using std::abs; using std::sqrt;
x = abs(x); y = abs(y);
if (x < y) std::swap(x, y); // Now x >= y >= 0
y /= (x ? x : 1);
return x * sqrt(1 + y * y);
// For an alternative (square-root free) method see
// C. Moler and D. Morrision (1983) https://doi.org/10.1147/rd.276.0577
// and A. A. Dubrulle (1983) https://doi.org/10.1147/rd.276.0582
#endif
}
/**
* exp(\e x) &minus; 1 accurate near \e x = 0.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return exp(\e x) &minus; 1.
**********************************************************************/
template<typename T> static T expm1(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::expm1; return expm1(x);
#else
using std::exp; using std::abs; using std::log;
GEOGRAPHICLIB_VOLATILE T
y = exp(x),
z = y - 1;
// The reasoning here is similar to that for log1p. The expression
// mathematically reduces to exp(x) - 1, and the factor z/log(y) = (y -
// 1)/log(y) is a slowly varying quantity near y = 1 and is accurately
// computed.
return abs(x) > 1 ? z : (z == 0 ? x : x * z / log(y));
#endif
}
/**
* log(1 + \e x) accurate near \e x = 0.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return log(1 + \e x).
**********************************************************************/
template<typename T> static T log1p(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::log1p; return log1p(x);
#else
using std::log;
GEOGRAPHICLIB_VOLATILE T
y = 1 + x,
z = y - 1;
// Here's the explanation for this magic: y = 1 + z, exactly, and z
// approx x, thus log(y)/z (which is nearly constant near z = 0) returns
// a good approximation to the true log(1 + x)/x. The multiplication x *
// (log(y)/z) introduces little additional error.
return z == 0 ? x : x * log(y) / z;
#endif
}
/**
* The inverse hyperbolic sine function.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return asinh(\e x).
**********************************************************************/
template<typename T> static T asinh(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::asinh; return asinh(x);
#else
using std::abs; T y = abs(x); // Enforce odd parity
y = log1p(y * (1 + y/(hypot(T(1), y) + 1)));
return x < 0 ? -y : y;
#endif
}
/**
* The inverse hyperbolic tangent function.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return atanh(\e x).
**********************************************************************/
template<typename T> static T atanh(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::atanh; return atanh(x);
#else
using std::abs; T y = abs(x); // Enforce odd parity
y = log1p(2 * y/(1 - y))/2;
return x < 0 ? -y : y;
#endif
}
/**
* The cube root function.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return the real cube root of \e x.
**********************************************************************/
template<typename T> static T cbrt(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::cbrt; return cbrt(x);
#else
using std::abs; using std::pow;
T y = pow(abs(x), 1/T(3)); // Return the real cube root
return x < 0 ? -y : y;
#endif
}
/**
* Fused multiply and add.
*
* @tparam T the type of the arguments and the returned value.
* @param[in] x
* @param[in] y
* @param[in] z
* @return <i>xy</i> + <i>z</i>, correctly rounded (on those platforms with
* support for the <code>fma</code> instruction).
*
* On platforms without the <code>fma</code> instruction, no attempt is
* made to improve on the result of a rounded multiplication followed by a
* rounded addition.
**********************************************************************/
template<typename T> static T fma(T x, T y, T z) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::fma; return fma(x, y, z);
#else
return x * y + z;
#endif
}
/**
* Normalize a two-vector.
*
* @tparam T the type of the argument and the returned value.
* @param[in,out] x on output set to <i>x</i>/hypot(<i>x</i>, <i>y</i>).
* @param[in,out] y on output set to <i>y</i>/hypot(<i>x</i>, <i>y</i>).
**********************************************************************/
template<typename T> static void norm(T& x, T& y)
{ T h = hypot(x, y); x /= h; y /= h; }
/**
* The error-free sum of two numbers.
*
* @tparam T the type of the argument and the returned value.
* @param[in] u
* @param[in] v
* @param[out] t the exact error given by (\e u + \e v) - \e s.
* @return \e s = round(\e u + \e v).
*
* See D. E. Knuth, TAOCP, Vol 2, 4.2.2, Theorem B. (Note that \e t can be
* the same as one of the first two arguments.)
**********************************************************************/
template<typename T> static T sum(T u, T v, T& t) {
GEOGRAPHICLIB_VOLATILE T s = u + v;
GEOGRAPHICLIB_VOLATILE T up = s - v;
GEOGRAPHICLIB_VOLATILE T vpp = s - up;
up -= u;
vpp -= v;
t = -(up + vpp);
// u + v = s + t
// = round(u + v) + t
return s;
}
/**
* Evaluate a polynomial.
*
* @tparam T the type of the arguments and returned value.
* @param[in] N the order of the polynomial.
* @param[in] p the coefficient array (of size \e N + 1).
* @param[in] x the variable.
* @return the value of the polynomial.
*
* Evaluate <i>y</i> = &sum;<sub><i>n</i>=0..<i>N</i></sub>
* <i>p</i><sub><i>n</i></sub> <i>x</i><sup><i>N</i>&minus;<i>n</i></sup>.
* Return 0 if \e N &lt; 0. Return <i>p</i><sub>0</sub>, if \e N = 0 (even
* if \e x is infinite or a nan). The evaluation uses Horner's method.
**********************************************************************/
template<typename T> static T polyval(int N, const T p[], T x)
// This used to employ Math::fma; but that's too slow and it seemed not to
// improve the accuracy noticeably. This might change when there's direct
// hardware support for fma.
{ T y = N < 0 ? 0 : *p++; while (--N >= 0) y = y * x + *p++; return y; }
/**
* Normalize an angle.
*
* @tparam T the type of the argument and returned value.
* @param[in] x the angle in degrees.
* @return the angle reduced to the range([&minus;180&deg;, 180&deg;].
*
* The range of \e x is unrestricted.
**********************************************************************/
template<typename T> static T AngNormalize(T x) {
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION != 4
using std::remainder;
x = remainder(x, T(360)); return x != -180 ? x : 180;
#else
using std::fmod;
T y = fmod(x, T(360));
#if defined(_MSC_VER) && _MSC_VER < 1900
// Before version 14 (2015), Visual Studio had problems dealing
// with -0.0. Specifically
// VC 10,11,12 and 32-bit compile: fmod(-0.0, 360.0) -> +0.0
// sincosd has a similar fix.
// python 2.7 on Windows 32-bit machines has the same problem.
if (x == 0) y = x;
#endif
return y <= -180 ? y + 360 : (y <= 180 ? y : y - 360);
#endif
}
/**
* Normalize a latitude.
*
* @tparam T the type of the argument and returned value.
* @param[in] x the angle in degrees.
* @return x if it is in the range [&minus;90&deg;, 90&deg;], otherwise
* return NaN.
**********************************************************************/
template<typename T> static T LatFix(T x)
{ using std::abs; return abs(x) > 90 ? NaN<T>() : x; }
/**
* The exact difference of two angles reduced to
* (&minus;180&deg;, 180&deg;].
*
* @tparam T the type of the arguments and returned value.
* @param[in] x the first angle in degrees.
* @param[in] y the second angle in degrees.
* @param[out] e the error term in degrees.
* @return \e d, the truncated value of \e y &minus; \e x.
*
* This computes \e z = \e y &minus; \e x exactly, reduced to
* (&minus;180&deg;, 180&deg;]; and then sets \e z = \e d + \e e where \e d
* is the nearest representable number to \e z and \e e is the truncation
* error. If \e d = &minus;180, then \e e &gt; 0; If \e d = 180, then \e e
* &le; 0.
**********************************************************************/
template<typename T> static T AngDiff(T x, T y, T& e) {
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION != 4
using std::remainder;
T t, d = AngNormalize(sum(remainder(-x, T(360)),
remainder( y, T(360)), t));
#else
T t, d = AngNormalize(sum(AngNormalize(-x), AngNormalize(y), t));
#endif
// Here y - x = d + t (mod 360), exactly, where d is in (-180,180] and
// abs(t) <= eps (eps = 2^-45 for doubles). The only case where the
// addition of t takes the result outside the range (-180,180] is d = 180
// and t > 0. The case, d = -180 + eps, t = -eps, can't happen, since
// sum would have returned the exact result in such a case (i.e., given t
// = 0).
return sum(d == 180 && t > 0 ? -180 : d, t, e);
}
/**
* Difference of two angles reduced to [&minus;180&deg;, 180&deg;]
*
* @tparam T the type of the arguments and returned value.
* @param[in] x the first angle in degrees.
* @param[in] y the second angle in degrees.
* @return \e y &minus; \e x, reduced to the range [&minus;180&deg;,
* 180&deg;].
*
* The result is equivalent to computing the difference exactly, reducing
* it to (&minus;180&deg;, 180&deg;] and rounding the result. Note that
* this prescription allows &minus;180&deg; to be returned (e.g., if \e x
* is tiny and negative and \e y = 180&deg;).
**********************************************************************/
template<typename T> static T AngDiff(T x, T y)
{ T e; return AngDiff(x, y, e); }
/**
* Coarsen a value close to zero.
*
* @tparam T the type of the argument and returned value.
* @param[in] x
* @return the coarsened value.
*
* The makes the smallest gap in \e x = 1/16 - nextafter(1/16, 0) =
* 1/2<sup>57</sup> for reals = 0.7 pm on the earth if \e x is an angle in
* degrees. (This is about 1000 times more resolution than we get with
* angles around 90&deg;.) We use this to avoid having to deal with near
* singular cases when \e x is non-zero but tiny (e.g.,
* 10<sup>&minus;200</sup>). This converts -0 to +0; however tiny negative
* numbers get converted to -0.
**********************************************************************/
template<typename T> static T AngRound(T x) {
using std::abs;
static const T z = 1/T(16);
if (x == 0) return 0;
GEOGRAPHICLIB_VOLATILE T y = abs(x);
// The compiler mustn't "simplify" z - (z - y) to y
y = y < z ? z - (z - y) : y;
return x < 0 ? -y : y;
}
/**
* Evaluate the sine and cosine function with the argument in degrees
*
* @tparam T the type of the arguments.
* @param[in] x in degrees.
* @param[out] sinx sin(<i>x</i>).
* @param[out] cosx cos(<i>x</i>).
*
* The results obey exactly the elementary properties of the trigonometric
* functions, e.g., sin 9&deg; = cos 81&deg; = &minus; sin 123456789&deg;.
* If x = &minus;0, then \e sinx = &minus;0; this is the only case where
* &minus;0 is returned.
**********************************************************************/
template<typename T> static void sincosd(T x, T& sinx, T& cosx) {
// In order to minimize round-off errors, this function exactly reduces
// the argument to the range [-45, 45] before converting it to radians.
using std::sin; using std::cos;
T r; int q;
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \
!defined(__GNUC__)
// Disable for gcc because of bug in glibc version < 2.22, see
// https://sourceware.org/bugzilla/show_bug.cgi?id=17569
// Once this fix is widely deployed, should insert a runtime test for the
// glibc version number. For example
// #include <gnu/libc-version.h>
// std::string version(gnu_get_libc_version()); => "2.22"
using std::remquo;
r = remquo(x, T(90), &q);
#else
using std::fmod; using std::floor;
r = fmod(x, T(360));
q = int(floor(r / 90 + T(0.5)));
r -= 90 * q;
#endif
// now abs(r) <= 45
r *= degree();
// Possibly could call the gnu extension sincos
T s = sin(r), c = cos(r);
#if defined(_MSC_VER) && _MSC_VER < 1900
// Before version 14 (2015), Visual Studio had problems dealing
// with -0.0. Specifically
// VC 10,11,12 and 32-bit compile: fmod(-0.0, 360.0) -> +0.0
// VC 12 and 64-bit compile: sin(-0.0) -> +0.0
// AngNormalize has a similar fix.
// python 2.7 on Windows 32-bit machines has the same problem.
if (x == 0) s = x;
#endif
switch (unsigned(q) & 3U) {
case 0U: sinx = s; cosx = c; break;
case 1U: sinx = c; cosx = -s; break;
case 2U: sinx = -s; cosx = -c; break;
default: sinx = -c; cosx = s; break; // case 3U
}
// Set sign of 0 results. -0 only produced for sin(-0)
if (x != 0) { sinx += T(0); cosx += T(0); }
}
/**
* Evaluate the sine function with the argument in degrees
*
* @tparam T the type of the argument and the returned value.
* @param[in] x in degrees.
* @return sin(<i>x</i>).
**********************************************************************/
template<typename T> static T sind(T x) {
// See sincosd
using std::sin; using std::cos;
T r; int q;
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \
!defined(__GNUC__)
using std::remquo;
r = remquo(x, T(90), &q);
#else
using std::fmod; using std::floor;
r = fmod(x, T(360));
q = int(floor(r / 90 + T(0.5)));
r -= 90 * q;
#endif
// now abs(r) <= 45
r *= degree();
unsigned p = unsigned(q);
r = p & 1U ? cos(r) : sin(r);
if (p & 2U) r = -r;
if (x != 0) r += T(0);
return r;
}
/**
* Evaluate the cosine function with the argument in degrees
*
* @tparam T the type of the argument and the returned value.
* @param[in] x in degrees.
* @return cos(<i>x</i>).
**********************************************************************/
template<typename T> static T cosd(T x) {
// See sincosd
using std::sin; using std::cos;
T r; int q;
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \
!defined(__GNUC__)
using std::remquo;
r = remquo(x, T(90), &q);
#else
using std::fmod; using std::floor;
r = fmod(x, T(360));
q = int(floor(r / 90 + T(0.5)));
r -= 90 * q;
#endif
// now abs(r) <= 45
r *= degree();
unsigned p = unsigned(q + 1);
r = p & 1U ? cos(r) : sin(r);
if (p & 2U) r = -r;
return T(0) + r;
}
/**
* Evaluate the tangent function with the argument in degrees
*
* @tparam T the type of the argument and the returned value.
* @param[in] x in degrees.
* @return tan(<i>x</i>).
*
* If \e x = &plusmn;90&deg;, then a suitably large (but finite) value is
* returned.
**********************************************************************/
template<typename T> static T tand(T x) {
static const T overflow = 1 / sq(std::numeric_limits<T>::epsilon());
T s, c;
sincosd(x, s, c);
return c != 0 ? s / c : (s < 0 ? -overflow : overflow);
}
/**
* Evaluate the atan2 function with the result in degrees
*
* @tparam T the type of the arguments and the returned value.
* @param[in] y
* @param[in] x
* @return atan2(<i>y</i>, <i>x</i>) in degrees.
*
* The result is in the range (&minus;180&deg; 180&deg;]. N.B.,
* atan2d(&plusmn;0, &minus;1) = +180&deg;; atan2d(&minus;&epsilon;,
* &minus;1) = &minus;180&deg;, for &epsilon; positive and tiny;
* atan2d(&plusmn;0, +1) = &plusmn;0&deg;.
**********************************************************************/
template<typename T> static T atan2d(T y, T x) {
// In order to minimize round-off errors, this function rearranges the
// arguments so that result of atan2 is in the range [-pi/4, pi/4] before
// converting it to degrees and mapping the result to the correct
// quadrant.
using std::atan2; using std::abs;
int q = 0;
if (abs(y) > abs(x)) { std::swap(x, y); q = 2; }
if (x < 0) { x = -x; ++q; }
// here x >= 0 and x >= abs(y), so angle is in [-pi/4, pi/4]
T ang = atan2(y, x) / degree();
switch (q) {
// Note that atan2d(-0.0, 1.0) will return -0. However, we expect that
// atan2d will not be called with y = -0. If need be, include
//
// case 0: ang = 0 + ang; break;
//
// and handle mpfr as in AngRound.
case 1: ang = (y >= 0 ? 180 : -180) - ang; break;
case 2: ang = 90 - ang; break;
case 3: ang = -90 + ang; break;
}
return ang;
}
/**
* Evaluate the atan function with the result in degrees
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return atan(<i>x</i>) in degrees.
**********************************************************************/
template<typename T> static T atand(T x)
{ return atan2d(x, T(1)); }
/**
* Evaluate <i>e</i> atanh(<i>e x</i>)
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @param[in] es the signed eccentricity = sign(<i>e</i><sup>2</sup>)
* sqrt(|<i>e</i><sup>2</sup>|)
* @return <i>e</i> atanh(<i>e x</i>)
*
* If <i>e</i><sup>2</sup> is negative (<i>e</i> is imaginary), the
* expression is evaluated in terms of atan.
**********************************************************************/
template<typename T> static T eatanhe(T x, T es);
/**
* Copy the sign.
*
* @tparam T the type of the argument.
* @param[in] x gives the magitude of the result.
* @param[in] y gives the sign of the result.
* @return value with the magnitude of \e x and with the sign of \e y.
*
* This routine correctly handles the case \e y = &minus;0, returning
* &minus|<i>x</i>|.
**********************************************************************/
template<typename T> static T copysign(T x, T y) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::copysign; return copysign(x, y);
#else
using std::abs;
// NaN counts as positive
return abs(x) * (y < 0 || (y == 0 && 1/y < 0) ? -1 : 1);
#endif
}
/**
* tan&chi; in terms of tan&phi;
*
* @tparam T the type of the argument and the returned value.
* @param[in] tau &tau; = tan&phi;
* @param[in] es the signed eccentricity = sign(<i>e</i><sup>2</sup>)
* sqrt(|<i>e</i><sup>2</sup>|)
* @return &tau;&prime; = tan&chi;
*
* See Eqs. (7--9) of
* C. F. F. Karney,
* <a href="https://doi.org/10.1007/s00190-011-0445-3">
* Transverse Mercator with an accuracy of a few nanometers,</a>
* J. Geodesy 85(8), 475--485 (Aug. 2011)
* (preprint
* <a href="https://arxiv.org/abs/1002.1417">arXiv:1002.1417</a>).
**********************************************************************/
template<typename T> static T taupf(T tau, T es);
/**
* tan&phi; in terms of tan&chi;
*
* @tparam T the type of the argument and the returned value.
* @param[in] taup &tau;&prime; = tan&chi;
* @param[in] es the signed eccentricity = sign(<i>e</i><sup>2</sup>)
* sqrt(|<i>e</i><sup>2</sup>|)
* @return &tau; = tan&phi;
*
* See Eqs. (19--21) of
* C. F. F. Karney,
* <a href="https://doi.org/10.1007/s00190-011-0445-3">
* Transverse Mercator with an accuracy of a few nanometers,</a>
* J. Geodesy 85(8), 475--485 (Aug. 2011)
* (preprint
* <a href="https://arxiv.org/abs/1002.1417">arXiv:1002.1417</a>).
**********************************************************************/
template<typename T> static T tauf(T taup, T es);
/**
* Test for finiteness.
*
* @tparam T the type of the argument.
* @param[in] x
* @return true if number is finite, false if NaN or infinite.
**********************************************************************/
template<typename T> static bool isfinite(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::isfinite; return isfinite(x);
#else
using std::abs;
#if defined(_MSC_VER)
return abs(x) <= (std::numeric_limits<T>::max)();
#else
// There's a problem using MPFR C++ 3.6.3 and g++ -std=c++14 (reported on
// 2015-05-04) with the parens around std::numeric_limits<T>::max. Of
// course, these parens are only needed to deal with Windows stupidly
// defining max as a macro. So don't insert the parens on non-Windows
// platforms.
return abs(x) <= std::numeric_limits<T>::max();
#endif
#endif
}
/**
* The NaN (not a number)
*
* @tparam T the type of the returned value.
* @return NaN if available, otherwise return the max real of type T.
**********************************************************************/
template<typename T> static T NaN() {
#if defined(_MSC_VER)
return std::numeric_limits<T>::has_quiet_NaN ?
std::numeric_limits<T>::quiet_NaN() :
(std::numeric_limits<T>::max)();
#else
return std::numeric_limits<T>::has_quiet_NaN ?
std::numeric_limits<T>::quiet_NaN() :
std::numeric_limits<T>::max();
#endif
}
/**
* A synonym for NaN<real>().
**********************************************************************/
static real NaN() { return NaN<real>(); }
/**
* Test for NaN.
*
* @tparam T the type of the argument.
* @param[in] x
* @return true if argument is a NaN.
**********************************************************************/
template<typename T> static bool isnan(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::isnan; return isnan(x);
#else
return x != x;
#endif
}
/**
* Infinity
*
* @tparam T the type of the returned value.
* @return infinity if available, otherwise return the max real.
**********************************************************************/
template<typename T> static T infinity() {
#if defined(_MSC_VER)
return std::numeric_limits<T>::has_infinity ?
std::numeric_limits<T>::infinity() :
(std::numeric_limits<T>::max)();
#else
return std::numeric_limits<T>::has_infinity ?
std::numeric_limits<T>::infinity() :
std::numeric_limits<T>::max();
#endif
}
/**
* A synonym for infinity<real>().
**********************************************************************/
static real infinity() { return infinity<real>(); }
/**
* Swap the bytes of a quantity
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return x with its bytes swapped.
**********************************************************************/
template<typename T> static T swab(T x) {
union {
T r;
unsigned char c[sizeof(T)];
} b;
b.r = x;
for (int i = sizeof(T)/2; i--; )
std::swap(b.c[i], b.c[sizeof(T) - 1 - i]);
return b.r;
}
#if GEOGRAPHICLIB_PRECISION == 4
typedef boost::math::policies::policy
< boost::math::policies::domain_error
<boost::math::policies::errno_on_error>,
boost::math::policies::pole_error
<boost::math::policies::errno_on_error>,
boost::math::policies::overflow_error
<boost::math::policies::errno_on_error>,
boost::math::policies::evaluation_error
<boost::math::policies::errno_on_error> >
boost_special_functions_policy;
static real hypot(real x, real y)
{ return boost::math::hypot(x, y, boost_special_functions_policy()); }
static real expm1(real x)
{ return boost::math::expm1(x, boost_special_functions_policy()); }
static real log1p(real x)
{ return boost::math::log1p(x, boost_special_functions_policy()); }
static real asinh(real x)
{ return boost::math::asinh(x, boost_special_functions_policy()); }
static real atanh(real x)
{ return boost::math::atanh(x, boost_special_functions_policy()); }
static real cbrt(real x)
{ return boost::math::cbrt(x, boost_special_functions_policy()); }
static real fma(real x, real y, real z)
{ return fmaq(__float128(x), __float128(y), __float128(z)); }
static real copysign(real x, real y)
{ return boost::math::copysign(x, y); }
static bool isnan(real x) { return boost::math::isnan(x); }
static bool isfinite(real x) { return boost::math::isfinite(x); }
#endif
};
} // namespace GeographicLib
#endif // GEOGRAPHICLIB_MATH_HPP

172
global_fusion/Thirdparty/GeographicLib/src/Geocentric.cpp vendored Normal file
View File

@@ -0,0 +1,172 @@
/**
* \file Geocentric.cpp
* \brief Implementation for GeographicLib::Geocentric class
*
* Copyright (c) Charles Karney (2008-2017) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include "Geocentric.hpp"
namespace GeographicLib {
using namespace std;
Geocentric::Geocentric(real a, real f)
: _a(a)
, _f(f)
, _e2(_f * (2 - _f))
, _e2m(Math::sq(1 - _f)) // 1 - _e2
, _e2a(abs(_e2))
, _e4a(Math::sq(_e2))
, _maxrad(2 * _a / numeric_limits<real>::epsilon())
{
if (!(Math::isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(Math::isfinite(_f) && _f < 1))
throw GeographicErr("Polar semi-axis is not positive");
}
const Geocentric& Geocentric::WGS84() {
static const Geocentric wgs84(Constants::WGS84_a(), Constants::WGS84_f());
return wgs84;
}
void Geocentric::IntForward(real lat, real lon, real h,
real& X, real& Y, real& Z,
real M[dim2_]) const {
real sphi, cphi, slam, clam;
Math::sincosd(Math::LatFix(lat), sphi, cphi);
Math::sincosd(lon, slam, clam);
real n = _a/sqrt(1 - _e2 * Math::sq(sphi));
Z = (_e2m * n + h) * sphi;
X = (n + h) * cphi;
Y = X * slam;
X *= clam;
if (M)
Rotation(sphi, cphi, slam, clam, M);
}
void Geocentric::IntReverse(real X, real Y, real Z,
real& lat, real& lon, real& h,
real M[dim2_]) const {
real
R = Math::hypot(X, Y),
slam = R != 0 ? Y / R : 0,
clam = R != 0 ? X / R : 1;
h = Math::hypot(R, Z); // Distance to center of earth
real sphi, cphi;
if (h > _maxrad) {
// We really far away (> 12 million light years); treat the earth as a
// point and h, above, is an acceptable approximation to the height.
// This avoids overflow, e.g., in the computation of disc below. It's
// possible that h has overflowed to inf; but that's OK.
//
// Treat the case X, Y finite, but R overflows to +inf by scaling by 2.
R = Math::hypot(X/2, Y/2);
slam = R != 0 ? (Y/2) / R : 0;
clam = R != 0 ? (X/2) / R : 1;
real H = Math::hypot(Z/2, R);
sphi = (Z/2) / H;
cphi = R / H;
} else if (_e4a == 0) {
// Treat the spherical case. Dealing with underflow in the general case
// with _e2 = 0 is difficult. Origin maps to N pole same as with
// ellipsoid.
real H = Math::hypot(h == 0 ? 1 : Z, R);
sphi = (h == 0 ? 1 : Z) / H;
cphi = R / H;
h -= _a;
} else {
// Treat prolate spheroids by swapping R and Z here and by switching
// the arguments to phi = atan2(...) at the end.
real
p = Math::sq(R / _a),
q = _e2m * Math::sq(Z / _a),
r = (p + q - _e4a) / 6;
if (_f < 0) swap(p, q);
if ( !(_e4a * q == 0 && r <= 0) ) {
real
// Avoid possible division by zero when r = 0 by multiplying
// equations for s and t by r^3 and r, resp.
S = _e4a * p * q / 4, // S = r^3 * s
r2 = Math::sq(r),
r3 = r * r2,
disc = S * (2 * r3 + S);
real u = r;
if (disc >= 0) {
real T3 = S + r3;
// Pick the sign on the sqrt to maximize abs(T3). This minimizes
// loss of precision due to cancellation. The result is unchanged
// because of the way the T is used in definition of u.
T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3
// N.B. cbrt always returns the real root. cbrt(-8) = -2.
real T = Math::cbrt(T3); // T = r * t
// T can be zero; but then r2 / T -> 0.
u += T + (T != 0 ? r2 / T : 0);
} else {
// T is complex, but the way u is defined the result is real.
real ang = atan2(sqrt(-disc), -(S + r3));
// There are three possible cube roots. We choose the root which
// avoids cancellation. Note that disc < 0 implies that r < 0.
u += 2 * r * cos(ang / 3);
}
real
v = sqrt(Math::sq(u) + _e4a * q), // guaranteed positive
// Avoid loss of accuracy when u < 0. Underflow doesn't occur in
// e4 * q / (v - u) because u ~ e^4 when q is small and u < 0.
uv = u < 0 ? _e4a * q / (v - u) : u + v, // u+v, guaranteed positive
// Need to guard against w going negative due to roundoff in uv - q.
w = max(real(0), _e2a * (uv - q) / (2 * v)),
// Rearrange expression for k to avoid loss of accuracy due to
// subtraction. Division by 0 not possible because uv > 0, w >= 0.
k = uv / (sqrt(uv + Math::sq(w)) + w),
k1 = _f >= 0 ? k : k - _e2,
k2 = _f >= 0 ? k + _e2 : k,
d = k1 * R / k2,
H = Math::hypot(Z/k1, R/k2);
sphi = (Z/k1) / H;
cphi = (R/k2) / H;
h = (1 - _e2m/k1) * Math::hypot(d, Z);
} else { // e4 * q == 0 && r <= 0
// This leads to k = 0 (oblate, equatorial plane) and k + e^2 = 0
// (prolate, rotation axis) and the generation of 0/0 in the general
// formulas for phi and h. using the general formula and division by 0
// in formula for h. So handle this case by taking the limits:
// f > 0: z -> 0, k -> e2 * sqrt(q)/sqrt(e4 - p)
// f < 0: R -> 0, k + e2 -> - e2 * sqrt(q)/sqrt(e4 - p)
real
zz = sqrt((_f >= 0 ? _e4a - p : p) / _e2m),
xx = sqrt( _f < 0 ? _e4a - p : p ),
H = Math::hypot(zz, xx);
sphi = zz / H;
cphi = xx / H;
if (Z < 0) sphi = -sphi; // for tiny negative Z (not for prolate)
h = - _a * (_f >= 0 ? _e2m : 1) * H / _e2a;
}
}
lat = Math::atan2d(sphi, cphi);
lon = Math::atan2d(slam, clam);
if (M)
Rotation(sphi, cphi, slam, clam, M);
}
void Geocentric::Rotation(real sphi, real cphi, real slam, real clam,
real M[dim2_]) {
// This rotation matrix is given by the following quaternion operations
// qrot(lam, [0,0,1]) * qrot(phi, [0,-1,0]) * [1,1,1,1]/2
// or
// qrot(pi/2 + lam, [0,0,1]) * qrot(-pi/2 + phi , [-1,0,0])
// where
// qrot(t,v) = [cos(t/2), sin(t/2)*v[1], sin(t/2)*v[2], sin(t/2)*v[3]]
// Local X axis (east) in geocentric coords
M[0] = -slam; M[3] = clam; M[6] = 0;
// Local Y axis (north) in geocentric coords
M[1] = -clam * sphi; M[4] = -slam * sphi; M[7] = cphi;
// Local Z axis (up) in geocentric coords
M[2] = clam * cphi; M[5] = slam * cphi; M[8] = sphi;
}
} // namespace GeographicLib

62
global_fusion/Thirdparty/GeographicLib/src/LocalCartesian.cpp vendored Normal file
View File

@@ -0,0 +1,62 @@
/**
* \file LocalCartesian.cpp
* \brief Implementation for GeographicLib::LocalCartesian class
*
* Copyright (c) Charles Karney (2008-2015) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include "LocalCartesian.hpp"
namespace GeographicLib {
using namespace std;
void LocalCartesian::Reset(real lat0, real lon0, real h0) {
_lat0 = Math::LatFix(lat0);
_lon0 = Math::AngNormalize(lon0);
_h0 = h0;
_earth.Forward(_lat0, _lon0, _h0, _x0, _y0, _z0);
real sphi, cphi, slam, clam;
Math::sincosd(_lat0, sphi, cphi);
Math::sincosd(_lon0, slam, clam);
Geocentric::Rotation(sphi, cphi, slam, clam, _r);
}
void LocalCartesian::MatrixMultiply(real M[dim2_]) const {
// M = r' . M
real t[dim2_];
copy(M, M + dim2_, t);
for (size_t i = 0; i < dim2_; ++i) {
size_t row = i / dim_, col = i % dim_;
M[i] = _r[row] * t[col] + _r[row+3] * t[col+3] + _r[row+6] * t[col+6];
}
}
void LocalCartesian::IntForward(real lat, real lon, real h,
real& x, real& y, real& z,
real M[dim2_]) const {
real xc, yc, zc;
_earth.IntForward(lat, lon, h, xc, yc, zc, M);
xc -= _x0; yc -= _y0; zc -= _z0;
x = _r[0] * xc + _r[3] * yc + _r[6] * zc;
y = _r[1] * xc + _r[4] * yc + _r[7] * zc;
z = _r[2] * xc + _r[5] * yc + _r[8] * zc;
if (M)
MatrixMultiply(M);
}
void LocalCartesian::IntReverse(real x, real y, real z,
real& lat, real& lon, real& h,
real M[dim2_]) const {
real
xc = _x0 + _r[0] * x + _r[1] * y + _r[2] * z,
yc = _y0 + _r[3] * x + _r[4] * y + _r[5] * z,
zc = _z0 + _r[6] * x + _r[7] * y + _r[8] * z;
_earth.IntReverse(xc, yc, zc, lat, lon, h, M);
if (M)
MatrixMultiply(M);
}
} // namespace GeographicLib

63
global_fusion/Thirdparty/GeographicLib/src/Math.cpp vendored Normal file
View File

@@ -0,0 +1,63 @@
/**
* \file Math.cpp
* \brief Implementation for GeographicLib::Math class
*
* Copyright (c) Charles Karney (2015) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include "Math.hpp"
#if defined(_MSC_VER)
// Squelch warnings about constant conditional expressions
# pragma warning (disable: 4127)
#endif
namespace GeographicLib {
using namespace std;
template<typename T> T Math::eatanhe(T x, T es) {
return es > T(0) ? es * atanh(es * x) : -es * atan(es * x);
}
template<typename T> T Math::taupf(T tau, T es) {
T tau1 = hypot(T(1), tau),
sig = sinh( eatanhe(tau / tau1, es ) );
return hypot(T(1), sig) * tau - sig * tau1;
}
template<typename T> T Math::tauf(T taup, T es) {
static const int numit = 5;
static const T tol = sqrt(numeric_limits<T>::epsilon()) / T(10);
T e2m = T(1) - sq(es),
// To lowest order in e^2, taup = (1 - e^2) * tau = _e2m * tau; so use
// tau = taup/_e2m as a starting guess. (This starting guess is the
// geocentric latitude which, to first order in the flattening, is equal
// to the conformal latitude.) Only 1 iteration is needed for |lat| <
// 3.35 deg, otherwise 2 iterations are needed. If, instead, tau = taup
// is used the mean number of iterations increases to 1.99 (2 iterations
// are needed except near tau = 0).
tau = taup/e2m,
stol = tol * max(T(1), abs(taup));
// min iterations = 1, max iterations = 2; mean = 1.94
for (int i = 0; i < numit || GEOGRAPHICLIB_PANIC; ++i) {
T taupa = taupf(tau, es),
dtau = (taup - taupa) * (1 + e2m * sq(tau)) /
( e2m * hypot(T(1), tau) * hypot(T(1), taupa) );
tau += dtau;
if (!(abs(dtau) >= stol))
break;
}
return tau;
}
/// \cond SKIP
// Instantiate
template Math::real Math::eatanhe<Math::real>(Math::real, Math::real);
template Math::real Math::taupf<Math::real>(Math::real, Math::real);
template Math::real Math::tauf<Math::real>(Math::real, Math::real);
/// \endcond
} // namespace GeographicLib