coursepointer 0.3.4

Converts waypoints into Garmin FIT course points
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78

#include <iostream>
#include <iomanip>
#include <GeographicLib/Utility.hpp>
#include <GeographicLib/Gnomonic.hpp>
#include <GeographicLib/Geodesic.hpp>

class vector3 {
public:
  GeographicLib::Math::real _x, _y, _z;
  vector3(GeographicLib::Math::real x,
          GeographicLib::Math::real y,
          GeographicLib::Math::real z = 1) throw()
    : _x(x)
    , _y(y)
    , _z(z) {}
  vector3 cross(const vector3& b) const throw() {
    return vector3(_y * b._z - _z * b._y,
                   _z * b._x - _x * b._z,
                   _x * b._y - _y * b._x);
  }
  void norm() throw() {
    _x /= _z;
    _y /= _z;
    _z = 1;
  }
};

int main() {
  GeographicLib::Utility::set_digits();
  GeographicLib::Math::real lata1, lona1, lata2, lona2;
  GeographicLib::Math::real latb1, lonb1, latb2, lonb2;
  std::cin >> lata1 >> lona1 >> lata2 >> lona2
           >> latb1 >> lonb1 >> latb2 >> lonb2;
  const GeographicLib::Geodesic& geod = GeographicLib::Geodesic::WGS84();
  const GeographicLib::Gnomonic gn(geod);
  GeographicLib::Math::real
    lat0 = (lata1 + lata2 + latb1 + latb2)/4,
    // Possibly need to deal with longitudes wrapping around
    lon0 = (lona1 + lona2 + lonb1 + lonb2)/4;
  std::cout << std::setprecision(16);
  std::cout << "Initial guess " << lat0 << " " << lon0 << "\n";
  GeographicLib::Math::real s120 = 1, s121;
  for (int i = 0; i < 20; ++i) {
    GeographicLib::Math::real xa1, ya1, xa2, ya2;
    GeographicLib::Math::real xb1, yb1, xb2, yb2;
    gn.Forward(lat0, lon0, lata1, lona1, xa1, ya1);
    gn.Forward(lat0, lon0, lata2, lona2, xa2, ya2);
    gn.Forward(lat0, lon0, latb1, lonb1, xb1, yb1);
    gn.Forward(lat0, lon0, latb2, lonb2, xb2, yb2);
    // See Hartley and Zisserman, Multiple View Geometry, Sec. 2.2.1
    vector3 va1(xa1, ya1); vector3 va2(xa2, ya2);
    vector3 vb1(xb1, yb1); vector3 vb2(xb2, yb2);
    // la is homogeneous representation of line A1,A2
    // lb is homogeneous representation of line B1,B2
    vector3 la = va1.cross(va2);
    vector3 lb = vb1.cross(vb2);
    // p0 is homogeneous representation of intersection of la and lb
    vector3 p0 = la.cross(lb);
    p0.norm();
    GeographicLib::Math::real lat1, lon1;
    gn.Reverse(lat0, lon0, p0._x, p0._y, lat1, lon1);
    geod.Inverse(lat1, lon1, lat0, lon1, s121);
    std::cout << "Increment " << s121 << " ratio " << s121/s120 << "\n";
    lat0 = lat1;
    lon0 = lon1;
    s120 = s121;
  }
  std::cout << "Final result " << lat0 << " " << lon0 << "\n";
  GeographicLib::Math::real azi1, azi2;
  geod.Inverse(lata1, lona1, lat0, lon0, azi1, azi2);
  std::cout << "Azimuths on line A " << azi2 << " ";
  geod.Inverse(lat0, lon0, lata2, lona2, azi1, azi2);
  std::cout << azi1 << "\n";
  geod.Inverse(latb1, lonb1, lat0, lon0, azi1, azi2);
  std::cout << "Azimuths on line B " << azi2 << " ";
  geod.Inverse(lat0, lon0, latb2, lonb2, azi1, azi2);
  std::cout << azi1 << "\n";
}