ADD: new track message, Entity class and Position class
This commit is contained in:
Henry Winkel
403
libs/geographiclib/develop/LevelEllipsoid.cpp
Normal file
403
libs/geographiclib/develop/LevelEllipsoid.cpp
Normal file
@@ -0,0 +1,403 @@
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <vector>
|
||||
#include <array>
|
||||
#include <limits>
|
||||
#include <algorithm>
|
||||
#include <boost/numeric/odeint.hpp>
|
||||
|
||||
#include <GeographicLib/Math.hpp>
|
||||
#include <GeographicLib/Utility.hpp>
|
||||
#include <GeographicLib/NormalGravity.hpp>
|
||||
|
||||
using namespace GeographicLib;
|
||||
namespace ode = boost::numeric::odeint;
|
||||
typedef Math::real real;
|
||||
typedef std::array<real, 2> point;
|
||||
|
||||
// Initialize with the end points of a line
|
||||
class LineDistance {
|
||||
point _a, _b;
|
||||
real _l, _nx, _ny;
|
||||
public:
|
||||
LineDistance(const point& a, const point& b);
|
||||
real Distance(const point& p) const;
|
||||
// negative on left of line, pos on right
|
||||
real Displacement(const point& p) const;
|
||||
};
|
||||
|
||||
// Use Ramer-Douglas-Peucker to simplify a polyline
|
||||
class LineSimplifier {
|
||||
// Simplify range [a,b]
|
||||
static void InternalSimplify(std::vector<point>& p, unsigned a, unsigned b,
|
||||
real thresh);
|
||||
public:
|
||||
static void Simplify(std::vector<point>& p, real thresh);
|
||||
};
|
||||
|
||||
inline LineDistance::LineDistance(const point& a, const point& b)
|
||||
: _a(a), _b(b) {
|
||||
using std::hypot;
|
||||
real dx = _b[0] - _a[0], dy = _b[1] - _a[1];
|
||||
_l = hypot(dx, dy);
|
||||
if (_l > std::numeric_limits<real>::epsilon()) {
|
||||
_nx = dx / _l; _ny = dy / _l;
|
||||
} else {
|
||||
_l = 0; _nx = 1; _ny = 0;
|
||||
}
|
||||
}
|
||||
|
||||
inline real LineDistance::Distance(const point& p) const {
|
||||
using std::abs; using std::hypot;
|
||||
real x = p[0] - _a[0], y = p[1] - _a[1];
|
||||
if (_l != 0) {
|
||||
real X = x * _nx + y * _ny, Y = abs(x * _ny - y * _nx);
|
||||
X = X < 0 ? -X : (X > _l ? X - _l : 0);
|
||||
return X != 0 ? hypot(X, Y) : Y;
|
||||
} else
|
||||
return hypot(x, y);
|
||||
}
|
||||
|
||||
inline real LineDistance::Displacement(const point& p) const {
|
||||
real x = p[0] - _a[0], y = p[1] - _a[1];
|
||||
return x * _ny - y * _nx;
|
||||
}
|
||||
|
||||
inline void LineSimplifier::Simplify(std::vector<point>& p, real thresh) {
|
||||
using std::isnan;
|
||||
unsigned n = p.size();
|
||||
InternalSimplify(p, 0, n-1, thresh);
|
||||
unsigned i = 0, j = 0;
|
||||
while (i < n) { // Squeeze out nans
|
||||
if (!isnan(p[i][0])) { p[j] = p[i]; ++j; }
|
||||
++i;
|
||||
}
|
||||
p.resize(j);
|
||||
}
|
||||
|
||||
void LineSimplifier::InternalSimplify(std::vector<point>& p,
|
||||
unsigned a, unsigned b,
|
||||
real thresh) {
|
||||
if (b - a + 1 <= 2) return;
|
||||
// Assume b > a+1, thresh > 0
|
||||
real maxdist = -1;
|
||||
unsigned maxind = (a + b) / 2; // initial value isn't used
|
||||
LineDistance dist(p[a], p[b]);
|
||||
for (unsigned i = a + 1; i <= b - 1; ++i) {
|
||||
real d = dist.Distance(p[i]);
|
||||
if (d > maxdist) { maxdist = d; maxind = i; }
|
||||
}
|
||||
if (maxdist > thresh) {
|
||||
InternalSimplify(p, a, maxind, thresh);
|
||||
InternalSimplify(p, maxind, b, thresh);
|
||||
} else {
|
||||
for (unsigned i = a+1; i <= b-1; ++i)
|
||||
p[i][0] = Math::NaN();
|
||||
}
|
||||
}
|
||||
|
||||
class PointTest {
|
||||
real _xmin, _xmax, _ymin, _ymax;
|
||||
public:
|
||||
PointTest(real xmin, real xmax, real ymin, real ymax)
|
||||
: _xmin(xmin)
|
||||
, _xmax(xmax)
|
||||
, _ymin(ymin)
|
||||
, _ymax(ymax) {}
|
||||
bool operator() (const point &p) const {
|
||||
return p[0] >= _xmin && p[0] <= _xmax && p[1] >= _ymin && p[1] <= _ymax;
|
||||
}
|
||||
};
|
||||
|
||||
class GravityInt {
|
||||
const NormalGravity& _grav;
|
||||
unsigned _dir;
|
||||
public:
|
||||
GravityInt(const NormalGravity& grav, unsigned dir)
|
||||
: _grav(grav)
|
||||
, _dir(dir & 3u)
|
||||
{}
|
||||
void operator() (const point& p, point &d, const real /*t*/) const {
|
||||
real Y = 0, gX, gY, gZ;
|
||||
_grav.U(p[0], Y, p[1], gX, gY, gZ);
|
||||
Math::norm(gX, gZ);
|
||||
switch (_dir) {
|
||||
case 0: d[0] = gX; d[1] = gZ; break; // parallel to g
|
||||
case 1: d[0] = -gZ; d[1] = gX; break; // counterclockwise 90d from g
|
||||
case 2: d[0] = -gX; d[1] = -gZ; break; // antiparallel to g
|
||||
case 3:
|
||||
default: d[0] = gZ; d[1] = -gX; break; // clockwise 90d from g
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
class GravityFollow {
|
||||
public:
|
||||
static void follow(const GravityInt& sys, const point& p0, real t0, real ds,
|
||||
std::vector<point>& points, const PointTest& box) {
|
||||
ode::result_of::make_dense_output
|
||||
< ode::runge_kutta_dopri5< point, real > >::type integrator =
|
||||
ode::make_dense_output(real(1.0e-8), real(0*1.0e-8),
|
||||
ode::runge_kutta_dopri5< point, real >() );
|
||||
integrator.initialize(p0, t0, real(1e-2));
|
||||
integrator.do_step(sys);
|
||||
int n = 1, i = 1;
|
||||
point out;
|
||||
while (true) {
|
||||
real tout = i * ds;
|
||||
while (integrator.current_time() < tout) {
|
||||
integrator.do_step(sys); ++n;
|
||||
}
|
||||
integrator.calc_state(tout, out);
|
||||
points.push_back(out);
|
||||
if (!box(out)) break;
|
||||
++i;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// Evaluate gravity potential or gradient along 1 axis
|
||||
class GravityEval {
|
||||
const NormalGravity& _grav;
|
||||
bool _zaxis, _grad;
|
||||
real _disp;
|
||||
public:
|
||||
GravityEval(const NormalGravity& grav, bool zaxis, bool grad, real disp)
|
||||
: _grav(grav)
|
||||
, _zaxis(zaxis)
|
||||
, _grad(grad)
|
||||
, _disp(disp) {};
|
||||
real operator() (real v) const {
|
||||
real
|
||||
X = _zaxis ? _disp : v,
|
||||
Y = 0,
|
||||
Z = _zaxis ? v : _disp,
|
||||
gX, gY, gZ,
|
||||
U = _grav.U(X, Y, Z, gX, gY, gZ);
|
||||
return _grad ? gX : U;
|
||||
}
|
||||
};
|
||||
|
||||
class Interpolator {
|
||||
private:
|
||||
int _maxit;
|
||||
real _eps;
|
||||
public:
|
||||
Interpolator()
|
||||
: _maxit(20), _eps(sqrt(std::numeric_limits<real>::epsilon())) {}
|
||||
real operator() (const GravityEval& gravfun,
|
||||
real x0, real x1, real val) {
|
||||
using std::abs;
|
||||
real y0 = gravfun(x0) - val, y1;
|
||||
for (int i = 0, trip = 0; i < _maxit; ++i) {
|
||||
y1 = gravfun(x1) - val;
|
||||
if (y1 == y0) break;
|
||||
real x2 = x1 - y1 * (x1 - x0) / (y1 - y0);
|
||||
x0 = x1; x1 = x2; y0 = y1;
|
||||
if (abs(x0 - x1) <= _eps) ++trip;
|
||||
if (trip > 2) break;
|
||||
}
|
||||
return x1;
|
||||
}
|
||||
};
|
||||
|
||||
void dump(const std::vector<point>& points) {
|
||||
for (auto p = points.begin(); p != points.end(); ++p) {
|
||||
std::cout << (*p)[0] << "," << (*p)[1] << ";\n";
|
||||
}
|
||||
}
|
||||
|
||||
int main() {
|
||||
Utility::set_digits();
|
||||
using std::sqrt;
|
||||
real a = 1, GM = 1, omega = 3/Math::real(10), f = 2/Math::real(10);
|
||||
real gX, gY, gZ, b = (1 - f) * a, E = a * sqrt(f * (2 - f));
|
||||
real thresh = real(0.5e-3);
|
||||
NormalGravity grav(a, GM, omega, f, true);
|
||||
real xmax = 3, ymax = 2;
|
||||
PointTest box(0, xmax, 0, ymax);
|
||||
Interpolator intpol;
|
||||
real
|
||||
X0 = a,
|
||||
U0 = grav.U(X0, 0, 0, gX, gY, gZ),
|
||||
Uc = grav.U(0, 0, 0, gX, gY, gZ);
|
||||
real Xnull, Unull;
|
||||
{
|
||||
GravityEval eval(grav, false, true, 0);
|
||||
Xnull = intpol(eval, 1, 2, 0);
|
||||
Unull = grav.U(Xnull, 0, 0, gX, gY, gZ);
|
||||
}
|
||||
int ndiv = 20;
|
||||
real del = (U0 - Unull) / 20;
|
||||
std::cout << std::setprecision(6);
|
||||
std::cout << "a=" << a << "; b=" << b << "; Rnull=" << Xnull
|
||||
<< "; xmax=" << xmax << "; ymax=" << ymax << ";\n";
|
||||
std::cout << "q={}; p={}; qa={}; pa={};\n";
|
||||
std::vector<point> points;
|
||||
point p0;
|
||||
int k = 0;
|
||||
for (int i = 0; i <= 90; i += 10) {
|
||||
points.clear();
|
||||
real s, c;
|
||||
Math::sincosd(real(i), s, c);
|
||||
p0[0] = a * c; p0[1] = b * s;
|
||||
points.push_back(p0);
|
||||
if (i == 0) {
|
||||
p0[0] = xmax; p0[1] = 0;
|
||||
points.push_back(p0);
|
||||
} else if (i == 90) {
|
||||
p0[0] = 0; p0[1] = ymax;
|
||||
points.push_back(p0);
|
||||
} else {
|
||||
GravityInt sys(grav, 2u);
|
||||
GravityFollow::follow(sys, p0, real(0), 1/real(100), points, box);
|
||||
}
|
||||
LineSimplifier::Simplify(points, thresh);
|
||||
std::cout << "q{" << ++k << "}=[\n";
|
||||
dump(points);
|
||||
std::cout << "];\n";
|
||||
}
|
||||
{
|
||||
points.clear();
|
||||
p0[0] = Xnull; p0[1] = 0;
|
||||
points.push_back(p0);
|
||||
p0[0] = Xnull; p0[1] = real(0.001);
|
||||
points.push_back(p0);
|
||||
GravityInt sys(grav, 2u);
|
||||
GravityFollow::follow(sys, p0, real(0), 1/real(100), points, box);
|
||||
LineSimplifier::Simplify(points, thresh);
|
||||
std::cout << "q{" << ++k << "}=[\n";
|
||||
dump(points);
|
||||
std::cout << "];\n";
|
||||
}
|
||||
for (int i = 1; i <= 5; ++i) {
|
||||
points.clear();
|
||||
p0[0] = xmax;
|
||||
p0[1] = real(i == 1 ? 0.45 :
|
||||
(i == 2 ? 0.9 :
|
||||
(i == 3 ? 1.25 :
|
||||
(i == 4 ? 1.6 : 1.9))));
|
||||
points.push_back(p0);
|
||||
if (!box(p0)) break;
|
||||
GravityInt sys(grav, 2u);
|
||||
GravityFollow::follow(sys, p0, real(0), 1/real(100), points, box);
|
||||
LineSimplifier::Simplify(points, thresh);
|
||||
std::cout << "q{" << ++k << "}=[\n";
|
||||
dump(points);
|
||||
std::cout << "];\n";
|
||||
}
|
||||
k = 0;
|
||||
for (int i = 0; i <= 90; i += 10) {
|
||||
points.clear();
|
||||
real s, c;
|
||||
Math::sincosd(real(i), s, c);
|
||||
p0[0] = a * c; p0[1] = b * s;
|
||||
points.push_back(p0);
|
||||
if (i == 0) {
|
||||
p0[0] = E; p0[1] = 0;
|
||||
points.push_back(p0);
|
||||
} else if (i == 90) {
|
||||
p0[0] = 0; p0[1] = 0;
|
||||
points.push_back(p0);
|
||||
} else {
|
||||
GravityInt sys(grav, 0u);
|
||||
GravityFollow::follow(sys, p0, real(0), 1/real(100), points, box);
|
||||
}
|
||||
LineSimplifier::Simplify(points, thresh);
|
||||
std::cout << "qa{" << ++k << "}=[\n";
|
||||
dump(points);
|
||||
std::cout << "];\n";
|
||||
}
|
||||
k = 0;
|
||||
{
|
||||
GravityEval eval(grav, false, false, 0);
|
||||
for (int i = 0; i < ndiv; ++i) {
|
||||
points.clear();
|
||||
real U = U0 - del * i;
|
||||
p0[0] = intpol(eval, X0, Xnull, U); p0[1] = 0;
|
||||
points.push_back(p0);
|
||||
GravityInt sysa(grav, 3u);
|
||||
GravityFollow::follow(sysa, p0, real(0), 1/real(100), points, box);
|
||||
LineSimplifier::Simplify(points, thresh);
|
||||
std::cout << "p{" << ++k << "}=[\n";
|
||||
dump(points);
|
||||
std::cout << "];\n";
|
||||
}
|
||||
for (int i = ndiv - 1;; --i) {
|
||||
points.clear();
|
||||
real U = U0 - del * i;
|
||||
p0[0] = intpol(eval, Xnull + 3, Xnull, U); p0[1] = 0;
|
||||
if (!box(p0)) break;
|
||||
points.push_back(p0);
|
||||
GravityInt sysa(grav, 1u);
|
||||
GravityFollow::follow(sysa, p0, real(0), 1/real(100), points, box);
|
||||
LineSimplifier::Simplify(points, thresh);
|
||||
std::cout << "p{" << ++k << "}=[\n";
|
||||
dump(points);
|
||||
std::cout << "];\n";
|
||||
}
|
||||
}
|
||||
{
|
||||
GravityEval eval(grav, true, false, 0);
|
||||
for (int i = ndiv + 1;; ++i) {
|
||||
points.clear();
|
||||
real U = U0 - del * i;
|
||||
p0[1] = intpol(eval, X0, Xnull, U); p0[0] = 0;
|
||||
if (!box(p0)) break;
|
||||
points.push_back(p0);
|
||||
GravityInt sysa(grav, 1u);
|
||||
GravityFollow::follow(sysa, p0, real(0), 1/real(100), points, box);
|
||||
LineSimplifier::Simplify(points, thresh);
|
||||
std::cout << "p{" << ++k << "}=[\n";
|
||||
dump(points);
|
||||
std::cout << "];\n";
|
||||
}
|
||||
}
|
||||
{
|
||||
real dy = real(0.02);
|
||||
GravityEval eval(grav, false, false, dy);
|
||||
points.clear();
|
||||
p0[0] = Xnull; p0[1] = 0;
|
||||
points.push_back(p0);
|
||||
p0[0] = intpol(eval, Xnull - real(0.1), Xnull, Unull); p0[1] = dy;
|
||||
points.push_back(p0);
|
||||
GravityInt sysa(grav, 3u);
|
||||
GravityFollow::follow(sysa, p0, real(0), 1/real(100), points, box);
|
||||
LineSimplifier::Simplify(points, thresh);
|
||||
std::cout << "p{" << ++k << "}=[\n";
|
||||
dump(points);
|
||||
std::cout << "];\n";
|
||||
}
|
||||
{
|
||||
real dy = real(0.02);
|
||||
GravityEval eval(grav, false, false, dy);
|
||||
points.clear();
|
||||
p0[0] = Xnull; p0[1] = 0;
|
||||
points.push_back(p0);
|
||||
p0[0] = intpol(eval, Xnull + real(0.1), Xnull, Unull); p0[1] = dy;
|
||||
points.push_back(p0);
|
||||
GravityInt sysa(grav, 1u);
|
||||
GravityFollow::follow(sysa, p0, real(0), 1/real(100), points, box);
|
||||
LineSimplifier::Simplify(points, thresh);
|
||||
std::cout << "p{" << ++k << "}=[\n";
|
||||
dump(points);
|
||||
std::cout << "];\n";
|
||||
}
|
||||
k = 0;
|
||||
{
|
||||
GravityEval eval(grav, false, false, 0);
|
||||
for (int i = 1;; ++i) {
|
||||
points.clear();
|
||||
real U = U0 + 5 * del * i;
|
||||
if (U > Uc) break;
|
||||
p0[0] = intpol(eval, 0, X0, U); p0[1] = 0;
|
||||
points.push_back(p0);
|
||||
GravityInt sysa(grav, 3u);
|
||||
GravityFollow::follow(sysa, p0, real(0), 1/real(100), points, box);
|
||||
LineSimplifier::Simplify(points, thresh);
|
||||
std::cout << "pa{" << ++k << "}=[\n";
|
||||
dump(points);
|
||||
std::cout << "];\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user