+
+]]>
+
+ 179.981620
+ 66.000167
+ 66
+
+
+ 2021-11-10T11:00:47Z
+
+ #track
+
+ 179.981620,66.000167
+
+
+
+
+ Path
+ #lineStyle
+
+ 1
+
+-179.979040,66.000103
+-179.979030,66.000188
+-179.979470,66.000023
+-179.980080,66.000045
+-179.980480,66.000177
+-179.980670,66.000109
+-179.981970,66.000167
+-179.983200,66.000075
+-179.984290,66.000134
+-179.985190,66.000121
+-179.985580,66.000197
+-179.986710,66.000158
+-179.988050,66.000138
+-179.989060,66.000171
+-179.989700,66.000050
+-179.991300,66.000066
+-179.991830,66.000046
+-179.993400,66.000131
+-179.993840,66.000088
+-179.995040,66.000165
+-179.995960,66.000196
+-179.997370,66.000158
+-179.997980,66.000103
+-179.999030,66.000106
+-179.999560,66.000057
+179.999150,66.000042
+179.998310,66.000150
+179.996750,66.000193
+179.996210,66.000128
+179.994930,66.000091
+179.993620,66.000191
+179.992930,66.000057
+179.992110,66.000098
+179.991380,66.000040
+179.990450,66.000114
+179.988540,66.000077
+179.987830,66.000013
+179.986990,66.000176
+179.986060,66.000021
+179.985380,66.000093
+179.984290,66.000199
+179.983110,66.000184
+179.982510,66.000122
+179.981820,66.000102
+179.981830,66.000147
+179.981750,66.000071
+179.981780,66.000073
+179.981620,66.000167
+
+
+
+
+
+
+
diff --git a/src/core/nvector.cc b/src/core/nvector.cc
new file mode 100644
index 000000000..a35efc691
--- /dev/null
+++ b/src/core/nvector.cc
@@ -0,0 +1,450 @@
+/*
+ Copyright (C) 2018, 2021 Robert Lipe, gpsbabel.org
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+ */
+
+// https://en.wikipedia.org/wiki/N-vector
+// http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf
+
+#include
+#include
+#include
+#include
+#include
+
+#include "nvector.h"
+#include "vector3d.h"
+
+namespace gpsbabel
+{
+
+LatLon::LatLon(double latitude, double longitude)
+{
+ lat = latitude;
+ lon = longitude;
+}
+
+NVector::NVector(double latitude_degrees, double longitude_degrees)
+{
+ // This implements equation 3.
+
+ // The coordinate system choice matches
+ // A_Nonsingular_Horizontal_Position_Representation.pdf
+ // The E frame:
+ // from the earths center
+ // x points to North pole
+ // y points towards latitude 0, longitude +90 (east)
+ // z points to latitude 0, longitude +180, the antimeridian
+ double latitude_radians = latitude_degrees * kRadiansPerDegree;
+ double longitude_radians = longitude_degrees * kRadiansPerDegree;
+ x_ = sin(latitude_radians);
+ y_ = sin(longitude_radians)*cos(latitude_radians);
+ z_ = -cos(longitude_radians)*cos(latitude_radians);
+}
+
+NVector::NVector(const Vector3D& v)
+{
+ x_ = v.getx();
+ y_ = v.gety();
+ z_ = v.getz();
+}
+
+std::pair PVector::toNVectorAndHeight() const
+{
+ // This implements equation 23.
+ constexpr double a = WGS84_SEMI_MAJOR_AXIS_METERS;
+ constexpr double a2 = a * a;
+ constexpr double e2 = WGS84_ECCENTRICITY_SQUARED;
+ constexpr double e4 = e2 * e2;
+
+ double px2 = x_ * x_;
+ double py2 = y_ * y_;
+ double pz2 = z_ * z_;
+
+ double q = ((1.0-e2)/(a2)) * px2;
+ double p = (py2 + pz2) / (a2);
+ double r = (p+q-e4) / 6.0;
+ double s = (e4*p*q) / (4.0*r*r*r);
+ double t = cbrt(1.0 + s + sqrt(s*(2.0+s)));
+ double u = r * (1.0 + t + (1.0/t));
+ double v = sqrt(u*u + e4*q);
+ double w = e2 * ((u+v-q)/(2.0*v));
+ double k = sqrt(u+v+w*w) - w;
+ double d = k*sqrt(py2 + pz2) / (k+e2);
+ double sf = 1.0 / (sqrt(d*d + px2));
+ double sf2 = k / (k+e2);
+
+ double height = ((k+e2-1.0)/k) * sqrt(d*d+px2);
+ double nx = sf * x_;
+ double ny = sf * sf2 * y_;
+ double nz = sf * sf2 * z_;
+
+ return {Vector3D(nx, ny, nz), height};
+}
+
+double NVector::latitude() const
+{
+ // This implements equation 6.
+ double latitude_radians = atan2(x_, sqrt(y_*y_ + z_*z_));
+ double latitude_degrees = latitude_radians * kDegreesPerRadian;
+ return latitude_degrees;
+}
+
+double NVector::longitude() const
+{
+ // This implements equation 5.
+ double longitude_radians = atan2(y_, -z_);
+ double longitude_degrees = longitude_radians * kDegreesPerRadian;
+ return longitude_degrees;
+}
+
+// great circle distance in radians
+double NVector::distance_radians(const NVector& n_EA_E, const NVector& n_EB_E)
+{
+ // This implements equation 16 using arctan for numerical accuracy.
+ double result = atan2(crossProduct(n_EA_E, n_EB_E).norm(),
+ dotProduct(n_EA_E, n_EB_E));
+ return result;
+}
+
+// great circle distance in meters
+double NVector::distance(const NVector& n_EA_E, const NVector& n_EB_E)
+{
+ double result = distance_radians(n_EA_E, n_EB_E) * MEAN_EARTH_RADIUS_METERS;
+ return result;
+}
+
+double NVector::distance(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees)
+{
+ NVector n_EA_E(latitude_a_degrees, longitude_a_degrees);
+ NVector n_EB_E(latitude_b_degrees, longitude_b_degrees);
+ double result = distance(n_EA_E, n_EB_E);
+ return result;
+}
+
+double NVector::azimuth_radians(const NVector& n_EA_E, const NVector& n_EB_E, double height_a, double height_b)
+{
+ PVector p_EA_E(n_EA_E, height_a);
+ PVector p_EB_E(n_EB_E, height_b);
+ Vector3D p_AB_E = p_EB_E - p_EA_E;
+
+ // equation 9 (normalized)
+ Vector3D kaeast = crossProduct(Vector3D(1.0, 0.0, 0.0), n_EA_E).normalize();
+ // equation 10 (normalized, n_EA_E and kaeast are perpendicular unit vectors)
+ Vector3D kanorth = crossProduct(n_EA_E, kaeast);
+
+ // equation 11
+ // Ren = [kanorth(normalized) kaeast(normalized) -n_EA_E];
+ // and a rotation from the E frame to the N frame (North, East, Down).
+ // P_AB_N = R_EN' * p_AB_E
+ double P_AB_N_x = kanorth*p_AB_E;
+ double P_AB_N_y = kaeast*p_AB_E;
+ // double P_AB_N_z = (-n_EA_E)*p_AB_E;
+
+ double azimuth = atan2(P_AB_N_y, P_AB_N_x);
+ return azimuth;
+}
+
+double NVector::azimuth(const NVector& n_EA_E, const NVector& n_EB_E, double height_a, double height_b)
+{
+ double azimuth = azimuth_radians(n_EA_E, n_EB_E, height_a, height_b);
+ double azimuth_degrees = azimuth * kDegreesPerRadian;
+ return azimuth_degrees;
+}
+
+// returns values in the range [0.0,360)
+double NVector::azimuth_true_degrees(const NVector& n_EA_E, const NVector& n_EB_E, double height_a, double height_b)
+{
+ double result = azimuth(n_EA_E, n_EB_E, height_a, height_b);
+ if (result < 0.0) {
+ result += 360.0;
+ }
+ if (result >= 360.0) {
+ result = 0.0;
+ }
+ return result;
+}
+
+double NVector::azimuth(double latitude_a_degrees, double longitude_a_degrees,
+ double latitude_b_degrees, double longitude_b_degrees,
+ double height_a, double height_b)
+{
+ NVector n_EA_E(latitude_a_degrees, longitude_a_degrees);
+ NVector n_EB_E(latitude_b_degrees, longitude_b_degrees);
+ return azimuth(n_EA_E, n_EB_E, height_a, height_b);
+}
+
+// returns values in the range [0.0,360)
+double NVector::azimuth_true_degrees(double latitude_a_degrees, double longitude_a_degrees,
+ double latitude_b_degrees, double longitude_b_degrees,
+ double height_a, double height_b)
+{
+ double result = azimuth(latitude_a_degrees, longitude_a_degrees,
+ latitude_b_degrees, longitude_b_degrees,
+ height_a, height_b);
+ if (result < 0.0) {
+ result += 360.0;
+ }
+ if (result >= 360.0) {
+ result = 0.0;
+ }
+ return result;
+}
+
+#if 0
+// This interpolation is nonlinear!
+NVector NVector::linepart(const NVector& n_EA_E, const NVector& n_EB_E, double fraction)
+{
+ Vector3D n_ER_E = (n_EA_E + (n_EB_E - n_EA_E)*fraction).normalize();
+ return n_ER_E;
+}
+#elif 0
+NVector NVector::linepart(const NVector& n_EA_E, const NVector& n_EB_E, double fraction)
+{
+ // see example 8.
+ double sab_rad = distance_radians(n_EA_E, n_EB_E);
+ double sar_rad = fraction*sab_rad;
+ double az_rad = azimuth_radians(n_EA_E, n_EB_E);
+
+ // equation 9 (normalized)
+ Vector3D kaeast = crossProduct(Vector3D(1.0, 0.0, 0.0), n_EA_E).normalize();
+ // equation 10 (normalized, n_EA_E and kaeast are perpendicular unit vectors)
+ Vector3D kanorth = crossProduct(n_EA_E, kaeast);
+
+ Vector3D d_EA_E = kanorth*cos(az_rad) + kaeast*sin(az_rad);
+ Vector3D n_ER_E = n_EA_E*cos(sar_rad) + d_EA_E*sin(sar_rad);
+ return n_ER_E;
+}
+#else
+NVector NVector::linepart(const NVector& n_EA_E, const NVector& n_EB_E, double fraction)
+{
+ double dp_a_b = dotProduct(n_EA_E, n_EB_E);
+ if (dp_a_b >= (1.0-8.0*DBL_EPSILON)) {
+ // The points are so close we will have trouble constructing a basis.
+ // Since they are so close the non-linearities in direct n vector
+ // interpolation are negligible.
+ Vector3D result = (n_EA_E + (n_EB_E-n_EA_E)*fraction).normalize();
+ return result;
+ }
+ if (dp_a_b <= (-1.0+8.0*DBL_EPSILON)) {
+ // The points are so close to be exactly opposite each other that
+ // there isn't a unique great circle between them.
+ // Unless fraction is 1.0 or 0.0 there isn't a unique answer.
+ Vector3D result(nan(""), nan(""), nan(""));
+ return result;
+ }
+ // Form an orthonormal basis with one component perpendicular to the great
+ // circle containing A and B, and one component being A.
+ // Call this the W frame.
+ // The columns of the rotation matrix from E to W are w1, w2 and w3.
+ Vector3D w1 = n_EA_E;
+ Vector3D w2 = crossProduct(n_EB_E, n_EA_E).normalize();
+ Vector3D w3 = crossProduct(n_EA_E,w2);
+ // Rotate A and B to the W frame.
+ // Vector3D n_EA_W = Vector3D(w1*n_EA_E, w2*n_EA_E, w3*n_EA_E);
+ Vector3D n_EB_W = Vector3D(w1*n_EB_E, w2*n_EB_E, w3*n_EB_E);
+ // By construction n_EA_W.y is (1, 0, 0),
+ // n_EB_W.y is zero,
+ // and both n_EA_W and n_EB_W are unit vectors.
+ // The information is all in the angle between them,
+ // which is all in n_EB_W.z and n_EB_W.x.
+ double theta = atan2(n_EB_W.getz(), n_EB_W.getx());
+ // We define the interpolated point as the point on the great circle
+ // between A and B whose great circle distance from A is the given fraction
+ // of the great circle distance between A and B. For a spheroid the
+ // distance is proportional to the angle between the vectors.
+ // The interpolated point is thus:
+ Vector3D n_EX_W = Vector3D(cos(fraction*theta), 0.0, sin(fraction*theta));
+ // Translate the interpolated point back to the E frame.
+ // We need to invert the matrix composed of the column vectors w1, w2, w3.
+ // Since this matrix is orthogonal it's inverse equals it's transpose.
+ Vector3D wt1 = Vector3D(w1.getx(), w2.getx(), w3.getx());
+ Vector3D wt2 = Vector3D(w1.gety(), w2.gety(), w3.gety());
+ Vector3D wt3 = Vector3D(w1.getz(), w2.getz(), w3.getz());
+ Vector3D n_EX_E = Vector3D(wt1*n_EX_W, wt2*n_EX_W, wt3*n_EX_W);
+ return n_EX_E;
+}
+#endif
+
+LatLon NVector::linepart(double latitude_a_degrees, double longitude_a_degrees,
+ double latitude_b_degrees, double longitude_b_degrees,
+ double fraction)
+{
+ NVector n_EA_E(latitude_a_degrees, longitude_a_degrees);
+ NVector n_EB_E(latitude_b_degrees, longitude_b_degrees);
+ NVector n_ER_E = linepart(n_EA_E, n_EB_E, fraction);
+ LatLon result(n_ER_E.latitude(), n_ER_E.longitude());
+ return result;
+}
+
+// Find the point of closest approach to Y on the great circle arc AB.
+// The great circle arc AB is the shortest of the two possibilities.
+#if 0
+// This implementation works and passes regression.
+// However, it seems harder to understand than the next implementation.
+NVector NVector::crossTrackProjection(const NVector& n_EA_E, const NVector& n_EB_E, const NVector& n_EY_E)
+{
+ // Compute the normal to the great circle defined by A and B:
+ Vector3D a_cross_b = crossProduct(n_EA_E, n_EB_E);
+ // Because a_cross_b is normal to the plane defined by n_EA_E and n_EB_E,
+ // any great circle defined by the point defined by a_cross_b and
+ // any other point, e.g. Y,
+ // will cross the great circle including the points A and B perpendicularly.
+ // Thus, if we find the intersection of the two great circles we will
+ // have found the projection of Y onto the great circle defined
+ // by A and B.
+ Vector3D x = a_cross_b;
+ // Compute the normal to the great circle defined by Y and X:
+ Vector3D x_cross_y = crossProduct(x, n_EY_E);
+ // A candidate projection from X onto the great circle defined by A and B is
+ // c_candidate, the other possibility is -c_candidate.
+ Vector3D c_candidate = crossProduct(a_cross_b, x_cross_y).normalize();
+ // pick the candidate closest to Y.
+ Vector3D c = dotProduct(c_candidate, n_EY_E) >= 0.0 ? c_candidate : -c_candidate;
+ // find the midpoint between A and B.
+ Vector3D m = (n_EA_E + n_EB_E).normalize();
+ // Now we have a point C on the great circle defined by A and B,
+ // and the midpoint M of this arc.
+ // If C is on the arc defined by A and B then the point of closest
+ // approach to Y is C. But if C is not on the arc, then the point
+ // of closest approach is either A or B, whichever is close to C.
+ // Note that A, B, C, M all are all on the great circle defined by A and B, so
+ // the dot product of any of these two unit vectors monotonically decreases
+ // the farther apart they are on the great circle.
+ Vector3D result;
+ if (dotProduct(n_EA_E, m) < dotProduct(c, m)) {
+ result = c;
+ } else if (dotProduct(n_EA_E, c) < dotProduct(n_EB_E, c)) {
+ result = n_EB_E;
+ } else {
+ result = n_EA_E;
+ }
+ return result;
+}
+#else
+NVector NVector::crossTrackProjection(const NVector& n_EA_E, const NVector& n_EB_E, const NVector& n_EY_E)
+{
+ // Form an orthonormal basis with one component perpendicular to the great
+ // circle containing A and B, and one component being A.
+ // Call this the W frame.
+ // The columns of the rotation matrix from E to W are w1, w2 and w3.
+ Vector3D w1 = n_EA_E;
+ Vector3D w2 = crossProduct(n_EB_E, n_EA_E).normalize();
+ Vector3D w3 = crossProduct(n_EA_E,w2);
+ // Rotate Y to the W frame.
+ Vector3D n_EY_W = Vector3D(w1*n_EY_E, w2*n_EY_E, w3*n_EY_E);
+ // By construction n_EA_W.y is (1, 0, 0),
+ // n_EB_W.y is zero,
+ // and both n_EA_W and n_EB_W are unit vectors.
+ // The projection of Y onto the great circle defined by A and B
+ // is just the n_EY_W with the y component set to zero.
+ Vector3D n_EC_W = Vector3D(n_EY_W.getx(), 0.0, n_EY_W.getz()).normalize();
+ // Translate the projected point back to the E frame.
+ // We need to invert the matrix composed of the column vectors w1, w2, w3.
+ // Since this matrix is orthogonal it's inverse equals it's transpose.
+ Vector3D wt1 = Vector3D(w1.getx(), w2.getx(), w3.getx());
+ Vector3D wt2 = Vector3D(w1.gety(), w2.gety(), w3.gety());
+ Vector3D wt3 = Vector3D(w1.getz(), w2.getz(), w3.getz());
+ Vector3D n_EC_E = Vector3D(wt1*n_EC_W, wt2*n_EC_W, wt3*n_EC_W);
+ // find the midpoint between A and B.
+ Vector3D n_EM_E = (n_EA_E + n_EB_E).normalize();
+ // Now we have a point C on the great circle defined by A and B,
+ // and the midpoint M of this arc.
+ // If C is on the arc defined by A and B then the point of closest
+ // approach to Y is C. But if C is not on the arc, then the point
+ // of closest approach is either A or B, whichever is close to C.
+ // Note that A, B, C, M all are all on the great circle defined by A and B, so
+ // the dot product of any of these two unit vectors monotonically decreases
+ // the farther apart they are on the great circle.
+ Vector3D result;
+ if (dotProduct(n_EA_E, n_EM_E) < dotProduct(n_EC_E, n_EM_E)) {
+ result = n_EC_E;
+ } else if (dotProduct(n_EA_E, n_EC_E) < dotProduct(n_EB_E, n_EC_E)) {
+ result = n_EB_E;
+ } else {
+ result = n_EA_E;
+ }
+ return result;
+}
+#endif
+
+LatLon NVector::crossTrackProjection(double latitude_a_degrees, double longitude_a_degrees,
+ double latitude_b_degrees, double longitude_b_degrees,
+ double latitude_x_degrees, double longitude_x_degrees)
+{
+ NVector n_EA_E(latitude_a_degrees, longitude_a_degrees);
+ NVector n_EB_E(latitude_b_degrees, longitude_b_degrees);
+ NVector n_EX_E(latitude_x_degrees, longitude_x_degrees);
+ NVector n_EC_E = crossTrackProjection(n_EA_E, n_EB_E, n_EX_E);
+ LatLon result(NVector(n_EC_E).latitude(), NVector(n_EC_E).longitude());
+ return result;
+}
+
+// Find the minimum distance to point X when traveling from A to B.
+#if 0
+// This doesn't work if we are more than 90 degrees away
+double NVector::crossTrackDistance(const NVector& n_EA_E, const NVector& n_EB_E, const NVector& n_EX_E)
+{
+ Vector3D c_E = crossProduct(n_EA_E, n_EB_E).normalize();
+ double result = fabs((atan2(crossProduct(c_E, n_EX_E).norm(),
+ dotProduct(c_E, n_EX_E)) - M_PI/2.0)) * MEAN_EARTH_RADIUS_METERS;
+ return result;
+}
+#else
+double NVector::crossTrackDistance(const NVector& n_EA_E, const NVector& n_EB_E, const NVector& n_EX_E)
+{
+ NVector n_EP_E = crossTrackProjection(n_EA_E, n_EB_E, n_EX_E);
+ double result = distance(n_EP_E, n_EX_E);
+ return result;
+}
+#endif
+
+double NVector::crossTrackDistance(double latitude_a_degrees, double longitude_a_degrees,
+ double latitude_b_degrees, double longitude_b_degrees,
+ double latitude_x_degrees, double longitude_x_degrees)
+{
+ NVector n_EA_E(latitude_a_degrees, longitude_a_degrees);
+ NVector n_EB_E(latitude_b_degrees, longitude_b_degrees);
+ NVector n_EX_E(latitude_x_degrees, longitude_x_degrees);
+ double result = crossTrackDistance(n_EA_E, n_EB_E, n_EX_E);
+ return result;
+}
+
+PVector::PVector(const NVector& n_EA_E, double h=0.0)
+{
+ // This implements equation 22.
+
+ // a semi-major axis, b semi-minor axis, f flattening
+ // a/b = 1/(1-f)
+ // aspect ratio = b/a
+ constexpr double b = WGS84_SEMI_MINOR_AXIS_METERS;
+ constexpr double asq_over_bsq = 1.0 / (WGS84_ASPECT_RATIO * WGS84_ASPECT_RATIO);
+
+ double denom = sqrt(n_EA_E.getx()*n_EA_E.getx() + n_EA_E.gety()*n_EA_E.gety()*asq_over_bsq + n_EA_E.getz()*n_EA_E.getz()*asq_over_bsq);
+ x_ = (b/denom)*n_EA_E.getx() + h*n_EA_E.getx();
+ y_ = (b/denom)*asq_over_bsq*n_EA_E.gety() + h*n_EA_E.gety();
+ z_ = (b/denom)*asq_over_bsq*n_EA_E.getz() + h*n_EA_E.getz();
+}
+
+PVector::PVector(const Vector3D& v)
+{
+ x_ = v.getx();
+ y_ = v.gety();
+ z_ = v.getz();
+}
+
+} // namespace gpsbabel
diff --git a/src/core/nvector.h b/src/core/nvector.h
new file mode 100644
index 000000000..04ce9d211
--- /dev/null
+++ b/src/core/nvector.h
@@ -0,0 +1,97 @@
+/*
+ Copyright (C) 2018, 2021 Robert Lipe, gpsbabel.org
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+ */
+
+#ifndef NVECTOR_H
+#define NVECTOR_H
+
+#include "defs.h"
+#include "vector3d.h"
+
+namespace gpsbabel
+{
+
+#define COMPARE_BEARING_TO_GRTCIRC
+#ifdef COMPARE_BEARING_TO_GRTCIRC
+constexpr double MEAN_EARTH_RADIUS_METERS = 6378137.0;
+#else
+constexpr double MEAN_EARTH_RADIUS_METERS = 6371000.0;
+#endif
+constexpr double WGS84_SEMI_MAJOR_AXIS_METERS = 6378137.0; // a
+#ifdef COMPARE_BEARING_TO_GRTCIRC
+constexpr double WGS84_FLATTENING = 0.0;
+#else
+constexpr double WGS84_FLATTENING = 1.0/298.257223563; // (a-b)/a
+#endif
+constexpr double WGS84_ASPECT_RATIO = 1.0 - WGS84_FLATTENING; // b/a
+constexpr double WGS84_SEMI_MINOR_AXIS_METERS = WGS84_SEMI_MAJOR_AXIS_METERS * WGS84_ASPECT_RATIO; // b
+constexpr double WGS84_ECCENTRICITY_SQUARED = 1.0 - (WGS84_ASPECT_RATIO * WGS84_ASPECT_RATIO);
+
+constexpr double kRadiansPerDegree = M_PI/180.0;
+constexpr double kDegreesPerRadian = 1.0/kRadiansPerDegree;
+
+class PVector;
+
+class LatLon
+{
+public:
+ LatLon(double latitude, double longitude);
+
+ double lat;
+ double lon;
+};
+
+class NVector : public Vector3D
+{
+
+public:
+ NVector() = default;
+ NVector(double latitude_degrees, double longitude_degrees);
+ NVector(const Vector3D& v);
+
+ [[nodiscard]] double latitude() const;
+ [[nodiscard]] double longitude() const;
+
+ static double distance_radians(const NVector& n_EA_E, const NVector& n_EB_E);
+ static double distance(const NVector& n_EA_E, const NVector& n_EB_E);
+ static double distance(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees);
+ static double azimuth_radians(const NVector& n_EA_E, const NVector& n_EB_E, double height_a = 0.0, double height_b = 0.0);
+ static double azimuth(const NVector& n_EA_E, const NVector& n_EB_E, double height_a = 0.0, double height_b = 0.0);
+ static double azimuth(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees, double height_a = 0.0, double height_b = 0.0);
+ static double azimuth_true_degrees(const NVector& n_EA_E, const NVector& n_EB_E, double height_a = 0.0, double height_b = 0.0);
+ static double azimuth_true_degrees(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees, double height_a = 0.0, double height_b = 0.0);
+ static NVector linepart(const NVector& n_EA_E, const NVector& n_EB_E, double fraction);
+ static LatLon linepart(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees, double fraction);
+ static NVector crossTrackProjection(const NVector& n_EA_E, const NVector& n_EB_E, const NVector& n_EY_E);
+ static LatLon crossTrackProjection(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees, double latitude_x_degrees, double longitude_x_degrees);
+ static double crossTrackDistance(const NVector& n_EA_E, const NVector& n_EB_E, const NVector& n_EX_E);
+ static double crossTrackDistance(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees, double latitude_x_degrees, double longitude_x_degrees);
+};
+
+class PVector : public Vector3D
+{
+public:
+ PVector() = default;
+ PVector(const NVector& n_EA_E, double h);
+ PVector(const Vector3D& v);
+
+ [[nodiscard]] std::pair toNVectorAndHeight() const;
+};
+
+} // namespace gpsbabel
+#endif // NVECTOR_H
diff --git a/src/core/vector3d.cc b/src/core/vector3d.cc
new file mode 100644
index 000000000..a59b44a72
--- /dev/null
+++ b/src/core/vector3d.cc
@@ -0,0 +1,156 @@
+/*
+ Copyright (C) 2018, 2021 Robert Lipe, gpsbabel.org
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+ */
+
+// https://en.wikipedia.org/wiki/N-vector
+// http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf
+
+#include
+#include
+#include "vector3d.h"
+
+namespace gpsbabel
+{
+
+Vector3D::Vector3D(double xi, double yi, double zi)
+{
+ x_ = xi;
+ y_ = yi;
+ z_ = zi;
+}
+
+double Vector3D::norm() const
+{
+ double norm = sqrt(x_*x_ + y_*y_ + z_*z_);
+ return norm;
+}
+
+Vector3D& Vector3D::normalize()
+{
+ *this = *this/this->norm();
+ return *this;
+}
+
+Vector3D& Vector3D::operator+=(const Vector3D& rhs)
+{
+ x_ += rhs.x_;
+ y_ += rhs.y_;
+ z_ += rhs.z_;
+ return *this;
+}
+
+Vector3D& Vector3D::operator-=(const Vector3D& rhs)
+{
+ x_ -= rhs.x_;
+ y_ -= rhs.y_;
+ z_ -= rhs.z_;
+ return *this;
+}
+
+Vector3D& Vector3D::operator*=(double rhs)
+{
+ x_ *= rhs;
+ y_ *= rhs;
+ z_ *= rhs;
+ return *this;
+}
+
+Vector3D& Vector3D::operator/=(double rhs)
+{
+ x_ /= rhs;
+ y_ /= rhs;
+ z_ /= rhs;
+ return *this;
+}
+
+Vector3D Vector3D::operator+ (const Vector3D& rhs) const
+{
+ Vector3D result = *this;
+ result += rhs;
+ return result;
+}
+
+Vector3D Vector3D::operator- (const Vector3D& rhs) const
+{
+ Vector3D result = *this;
+ result -= rhs;
+ return result;
+}
+
+Vector3D Vector3D::operator* (double rhs) const
+{
+ Vector3D result = *this;
+ result *= rhs;
+ return result;
+}
+
+Vector3D Vector3D::operator/ (double rhs) const
+{
+ Vector3D result = *this;
+ result /= rhs;
+ return result;
+}
+
+Vector3D Vector3D::operator- () const
+{
+ Vector3D result(-x_,-y_,-z_);
+ return result;
+}
+
+double Vector3D::operator* (const Vector3D& b) const
+{
+ double result = x_*b.x_ + y_*b.y_ + z_*b.z_;
+ return result;
+}
+
+double Vector3D::dotProduct(const Vector3D& a, const Vector3D& b)
+{
+ double dotproduct = a.x_*b.x_ + a.y_*b.y_ + a.z_*b.z_;
+ return dotproduct;
+}
+
+Vector3D Vector3D::crossProduct(const Vector3D& b, const Vector3D& c)
+{
+ // a = b x c
+ Vector3D a;
+ a.x_ = b.y_*c.z_ - b.z_*c.y_;
+ a.y_ = b.z_*c.x_ - b.x_*c.z_;
+ a.z_ = b.x_*c.y_ - b.y_*c.x_;
+ return a;
+}
+
+Vector3D operator* (double lhs, const Vector3D& rhs)
+{
+ Vector3D result = rhs*lhs;
+ return result;
+}
+
+std::ostream& operator<< (std::ostream& os, const Vector3D& v)
+{
+ os << '(' << v.getx() << ", " << v.gety() << ", " << v.getz() << ')';
+ return os;
+}
+
+QDebug operator<< (QDebug debug, const Vector3D& v)
+{
+ QDebugStateSaver saver(debug);
+ debug.nospace() << '(' << v.getx() << ", " << v.gety() << ", " << v.getz() << ')';
+ return debug;
+}
+
+} // namespace gpsbabel
diff --git a/src/core/vector3d.h b/src/core/vector3d.h
new file mode 100644
index 000000000..bd9ac5474
--- /dev/null
+++ b/src/core/vector3d.h
@@ -0,0 +1,66 @@
+/*
+ Copyright (C) 2018, 2021 Robert Lipe, gpsbabel.org
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+ */
+
+#ifndef VECTOR3D_H
+#define VECTOR3D_H
+
+#include
+
+#include
+
+namespace gpsbabel
+{
+
+class Vector3D
+{
+public:
+ Vector3D() = default;
+ Vector3D(double xi, double yi, double zi);
+
+ [[nodiscard]] double norm() const;
+ [[nodiscard]] double getx() const {return x_;}
+ [[nodiscard]] double gety() const {return y_;}
+ [[nodiscard]] double getz() const {return z_;}
+ Vector3D& normalize();
+
+ Vector3D& operator+=(const Vector3D& rhs);
+ Vector3D& operator-=(const Vector3D& rhs);
+ Vector3D& operator*=(double rhs);
+ Vector3D& operator/=(double rhs);
+ Vector3D operator+ (const Vector3D& rhs) const;
+ Vector3D operator- (const Vector3D& rhs) const;
+ Vector3D operator* (double rhs) const;
+ Vector3D operator/ (double rhs) const;
+ Vector3D operator- () const;
+ double operator* (const Vector3D& b) const;
+
+ static Vector3D crossProduct(const Vector3D& b, const Vector3D& c);
+ static double dotProduct(const Vector3D& a, const Vector3D& b);
+
+protected:
+ double x_{};
+ double y_{};
+ double z_{};
+};
+Vector3D operator* (double lhs, const Vector3D& rhs);
+std::ostream& operator<< (std::ostream& os, const Vector3D& v);
+QDebug operator<<(QDebug debug, const Vector3D& v);
+
+} // namespace gpsbabel
+#endif // VECTOR3D_H
diff --git a/testo.d/kml.test b/testo.d/kml.test
index 2863b86bd..2b3664e74 100644
--- a/testo.d/kml.test
+++ b/testo.d/kml.test
@@ -69,6 +69,10 @@ compare ${REFERENCE}/track/Placemark-Track-1~kml.gpx ${TMPDIR}/Placemark-Track-1
gpsbabel -T -i random,points=20,seed=33,nodelay -f dummy -o kml,track -F ${TMPDIR}/realtime.kml
compare ${REFERENCE}/realtime.kml ${TMPDIR}/realtime.kml
+# kml bounding box
+gpsbabel -t -i unicsv -f ${REFERENCE}/track/sim.csv -o kml -F ${TMPDIR}/sim.kml
+compare ${REFERENCE}/track/sim.kml ${TMPDIR}/sim.kml
+
if [ "${RUNNINGVALGRIND}" != "0" ]; then
set -e
if which xmllint > /dev/null;