Move from SVN to using GIT.

diff --git a/doc-references/Geodetic_Datum.pdf b/doc-references/Geodetic_Datum.pdf
new file mode 100644 (file)
index 0000000..533020e
Binary files /dev/null and b/doc-references/Geodetic_Datum.pdf differ

diff --git a/doc-references/ngs-ecef.pdf b/doc-references/ngs-ecef.pdf
new file mode 100644 (file)
index 0000000..4cd2340
Binary files /dev/null and b/doc-references/ngs-ecef.pdf differ

diff --git a/doc-references/wgs84fin.pdf b/doc-references/wgs84fin.pdf
new file mode 100644 (file)
index 0000000..a542638
Binary files /dev/null and b/doc-references/wgs84fin.pdf differ

diff --git a/include/ecef-quaternion.h b/include/ecef-quaternion.h
new file mode 100644 (file)
index 0000000..2a6ebc1
--- /dev/null
+++ b/include/ecef-quaternion.h
@@ -0,0 +1,50 @@
+/* 
+  C++ Linear Algegra Quaternion Copyright © 2017  Infinite Delta Corp
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU Lesser 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 Lesser General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+*/
+
+#ifndef ECEF_QUATERNION_H
+#define ECEF_QUATERNION_H
+
+#include "ecef-vector.h"
+
+namespace ecef
+{
+
+struct quat
+  {
+  double a;
+  vec v;
+ 
+  quat() {a=0.0; v=zAxis;}
+  quat(const double a, const vec v) {this->a=a; this->v=v;}
+
+  quat operator*(const quat q) const {return quat(a*q.a - v.dot(q.v), q.v*a + v*q.a + v.cross(q.v));}
+  quat operator*(const double s) const {return quat(a*s, v*s);}
+  quat operator/(const double s) const {return quat(a/s, v/s);}
+
+  quat conj() const {return quat(a, -v);}
+  // double magSqr() const {return a*a + v.x*v.x + v.y*v.y + v.z*v.z;}
+  double mag() const {double ms=a*a + v.x*v.x + v.y*v.y + v.z*v.z; return tolerance(ms) ? 0.0 : sqrt(ms);}
+  quat versor() const {double m=mag(); return tolerance(m) ? quat(0, zAxis) : *this/m;}                // Unit Quaternion
+  quat normVec() const {double s=a*a;  s= a<=1 ? sqrt(1-s) : 0; return quat(a, v.norm(s));}    // Normalize Quaternion by only scalling the vector
+
+  // vec operator*(const vec v2) const {return (*this * quat(0,v2) * conj()).v;}               // Rotate vector by quaternion
+  vec operator*(const vec v2) const {return (quat(-v.dot(v2), v2*a + v.cross(v2)) * conj()).v;}        //  but skip the initial mult-by-zero
+  };
+
+}
+#endif

diff --git a/include/ecef-vector.h b/include/ecef-vector.h
new file mode 100644 (file)
index 0000000..6b01eed
--- /dev/null
+++ b/include/ecef-vector.h
@@ -0,0 +1,71 @@
+/* 
+  C++ Linear Algegra Vector Copyright © 2013-2017  Infinite Delta Corp
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU Lesser 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 Lesser General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+*/
+
+#ifndef ECEF_VECTOR_H
+#define ECEF_VECTOR_H
+
+#include <math.h>
+
+namespace ecef
+{
+const double toleranceLimit = 0.0000001;
+inline double sqr(const double a) {return a*a;}
+inline bool tolerance(const double d) {return d <= toleranceLimit;}
+inline double abs(const double a) {return (a>=0) ? a : -a;}
+inline double sign(const double a) {return (a>=0) ? 1 : -1;}
+
+struct vec
+  {
+  double x, y, z;
+  
+  vec() {x=y=z=0.0;};
+  vec(const double x, const double y, const double z) {this->x=x; this->y=y; this->z=z;};
+
+  double distSqr(const vec v=vec()) const {return sqr(x-v.x)+sqr(y-v.y)+sqr(z-v.z);}
+  double dist(const vec v=vec()) const {return sqrt(distSqr(v));}
+  double distZ() const {return vec(x,y,0).dist();}
+
+  bool operator==(const vec v) const {return tolerance(distSqr(v));}
+  bool operator!=(const vec v) const {return !tolerance(distSqr(v));}
+
+  vec operator+(const vec v) const {return vec(x+v.x, y+v.y, z+v.z);}
+  vec operator-(const vec v) const {return vec(x-v.x, y-v.y, z-v.z);}
+  vec operator*(const vec v) const {return vec(x*v.x, y*v.y, z*v.z);}
+  vec operator*(const double s) const {return vec(x*s, y*s, z*s);}
+  vec operator/(const double s) const {return vec(x/s, y/s, z/s);}
+  vec operator-() const {return vec(-x, -y, -z);}
+  
+  double dot(const vec v) const {return x*v.x + y*v.y + z*v.z;}
+  vec cross(const vec v) const {return vec(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);}
+  vec doubleCross(const vec v) const {return cross(v).cross(-v);} // http://en.wikipedia.org/wiki/Dot_product#Triple_product_expansion
+  double cos(const vec v) const {return dot(v)/(dist()*v.dist());}
+  double mag() const {return sqrt(x*x + y*y + z*z);}
+  vec norm(const double s=1) const {double m = mag(); return tolerance(m) ? vec(0,0,1) * s : *this * s / m;}
+  };
+
+const vec zAxis(0,0,1);
+
+struct line
+  {
+  vec p[2];    // Two points define a line
+
+  line(const vec end) {p[0]=vec(); p[1]=end;}
+  line(const vec start, const vec end) {p[0]=start; p[1]=end;}
+  };
+}
+#endif

diff --git a/include/ecef-wgs84.h b/include/ecef-wgs84.h
new file mode 100644 (file)
index 0000000..9ad65dd
--- /dev/null
+++ b/include/ecef-wgs84.h
@@ -0,0 +1,95 @@
+/* 
+  Earth Centered Earth Fixed (ECEF) WGS84 Navigation Utility Copyright © 2013-2017  Infinite Delta Corp
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU Lesser 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 Lesser General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+*/
+
+#ifndef ECEF_WGS84_H
+#define ECEF_WGS84_H
+
+#include <ecef-vector.h>
+
+// http://en.wikipedia.org/wiki/Geodetic_system
+// wgs84fin.pdf -> TR8350.2
+// http://proceedings.esri.com/library/userconf/proc14/papers/1034_755.pdf
+
+// http://www.ngs.noaa.gov/AERO/aero.html
+// http://geodesy.noaa.gov/AERO/UDDFdat.htm
+// http://www.ngs.noaa.gov/AERO/uddf/GREAT-LAKES/MICHIGAN/GRR__01B.F77
+
+namespace ecef
+{
+  namespace wgs84Const
+    {
+    const double a                     = 6378137.0;            // Semi-major Axis Meters
+    const double fr                    = 298.257223563;        // Flattening recipical
+
+    const double f                     = 1/fr;                 // Flattening
+    const double c                     = a*(1-f);              // Minor Axis Meters
+    const double c2                    = c*c;                  // Square Minor Axis M^2
+    const double e2                    = 2*f-f*f;              // First Eccentricity Squared
+    const double EarthCenterScale      = 1-a*a/c2;             // WGS84 Earth Center Z scaler
+    const double maxEast               = M_PIl;
+    const double maxWest               = -M_PIl;
+    const double maxNorth              = M_PI_2l;
+    const double maxSouth              = -M_PI_2l;
+    }
+
+struct wgs84 : public vec
+  {
+  wgs84() : vec(wgs84Const::a,0,0) {}
+  wgs84(const vec v) : vec(v) {}
+  wgs84(const double lat, const double lon, const double h=0.0)
+    {
+    double slat = ::sin(lat);
+    double clat = ::cos(lat);
+    double n = wgs84Const::a/sqrt(1-wgs84Const::e2*sqr(slat));
+    x = (n+h)*clat*::cos(lon);
+    y = (n+h)*clat*::sin(lon);
+    z = (n*(1-wgs84Const::e2)+h)*slat;
+    }
+  // wgs84& operator=(const vec v) {x=v.x; y=v.y; z=v.z; return *this;}
+
+  double lon() const {double r=distZ(); return tolerance(r) ? 0 : ::acos(x/r)*sign(y);}
+  };
+
+struct wgs84Line : public line
+  {
+  wgs84Line() : line(wgs84()) {} 
+  wgs84Line(const vec end) : line(end) {} 
+  wgs84Line(const wgs84 end) : line(end) {} 
+  wgs84Line(const wgs84 start, const wgs84 end) : line(start, end) {}
+  int trimToEllipsoid();
+  };
+
+// Fast: +/- 0.0004% @0m, or 0.2% @10,000m altitude
+//   Assuming altitude is zero (0) or insignificant, everything can be based on WGS84 earth center.
+
+struct wgs84Fast : public wgs84
+  {
+  wgs84Fast() : wgs84() {}
+  wgs84Fast(const wgs84 p) : wgs84(p) {}
+  wgs84Fast(const double lat, const double lon, const double h=0.0) : wgs84(lat, lon, h) {}
+
+  wgs84 center() const {return vec(0,0,z*wgs84Const::EarthCenterScale);} // gravity earth center
+  double tZ() const {return z * (1 - wgs84Const::EarthCenterScale);}
+  wgs84 up() const {return vec(x,y,tZ());}
+  wgs84 north() const {return zAxis.doubleCross(up());} // North tangent vector 
+  double sinLat() const {return tZ() / sqrt(sqr(x) + sqr(y) + sqr(tZ()));}
+  double lat() const {return ::asin(sinLat());}
+  };
+}
+
+#endif

diff --git a/source/ecef-wgs84.cc b/source/ecef-wgs84.cc
new file mode 100644 (file)
index 0000000..c6b7ab4
--- /dev/null
+++ b/source/ecef-wgs84.cc
@@ -0,0 +1,43 @@
+/*
+  Earth Centered Earth Fixed (ECEF) WGS84 Navigation Utility Copyright ©  2013-2017  Infinite Delta Corp
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU Lesser 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 Lesser General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+*/
+
+#include <ecef-wgs84.h>
+
+namespace ecef
+{
+
+inline double myFunc(const vec v)
+{
+  double g2 = sqr(wgs84Const::a);
+  double h2 = sqr(wgs84Const::a * (1-1/wgs84Const::fr));
+  return v.x/g2 + v.y/g2 + v.z/h2;
+}
+
+int wgs84Line::trimToEllipsoid()
+{
+  vec v(p[0] - p[1]), e(p[1]);
+  double d = sqr(2*myFunc(v*e)) - 4*myFunc(v*v)*(myFunc(e*e)-1);
+  if (d < 0) return 0;
+  d = sqrt(d);
+  double t = (-2*myFunc(v*e) + d) / (2*myFunc(v*v));
+  p[0] = e + v*t;
+  t = (-2*myFunc(v*e) - d) / (2*myFunc(v*v));
+  p[1] = e + v*t;
+  return (p[0] == p[1]) ? 1 : 2;
+}
+}

diff --git a/test/Makefile b/test/Makefile
new file mode 100644 (file)
index 0000000..d437150
--- /dev/null
+++ b/test/Makefile
@@ -0,0 +1,36 @@
+# 
+#  Earth Centered Earth Fixed Navigation Copyright © 2013-2017  Infinite Delta Corp
+#
+#  This program is free software: you can redistribute it and/or modify
+#  it under the terms of the GNU Lesser 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 Lesser General Public License
+#  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+#
+
+VPATH=../include:../source
+CXXFLAGS=-Wall -g -O2 -I../include
+
+TRGS=ecef-wgs84_test
+OBJS=ecef-wgs84.o
+INC=ecef-vector.h ecef-wgs84.h ecef-quaternion.h
+
+all : $(TRGS)
+
+ecef-wgs84_test : ecef-wgs84_test.cc $(OBJS) $(INC)
+
+$(OBJS) : $(INC)
+
+clean:
+       rm -f $(TRGS) $(OBJS)
+
+test: all
+       ./ecef-wgs84_test
+

diff --git a/test/ecef-wgs84_test.cc b/test/ecef-wgs84_test.cc
new file mode 100644 (file)
index 0000000..d96d56a
--- /dev/null
+++ b/test/ecef-wgs84_test.cc
@@ -0,0 +1,399 @@
+/*
+  Earth Centered Earth Fixed (ECEF) WGS84 Navigation Utility Copyright © 2013-2017  Infinite Delta Corp
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU Lesser 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 Lesser General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+*/
+
+#include <iostream>
+#include <ecef-quaternion.h>
+#include <ecef-wgs84.h>
+
+namespace ecef
+{
+static bool tst(const double i, const double j=0, const double t=0.0000001) 
+  {if (i>=j ? i-j <= t : j-i <= t) return true;
+  std::cout << i << " " << j << " " << i-j << " " << t << " ";
+  return false;
+  } 
+
+#define ERR(e,m) {std::cout << "Test Error: \"" << m << "\" in " << __FILE__ << ":" << __LINE__ << std::endl; return e;}
+
+#define LL_INC wgs84Const::maxNorth/1000.0
+
+int tst1()
+{
+  vec cent;
+  wgs84 pos;
+
+  if (pos.x != 6378137.0) ERR(1, "Default X is not Wgs84 Radius");
+  if (pos.y != 0.0) ERR(1, "Default Y is not zero");
+  if (pos.z != 0.0) ERR(1, "Default Z is not zero");
+  // std::cout << pos.x << std::endl;
+  // std::cout << pos.y << std::endl;
+  // std::cout << pos.z << std::endl;
+
+  if (vec(1.0, 0.0, 0.0) + vec(0.0, 2.0, 0.0) + vec(0.0, 0.0, 3.0) !=
+      vec(1.0, 2.0, 3.0)) ERR(1, "Bad vector addition");
+
+  if (vec(1.0, 0.0, 0.0) - vec(2.0, 3.0, 0.0) - vec(0.0, 0.0, 4.0) !=
+      vec(-1.0, -3.0, -4.0)) ERR(1, "Bad vector subtraction");
+
+  if (vec(1.0, 2.0, 3.0).dot(vec(4.0, 5.0, 6.0)) != 32.0) ERR(1, "Bad vector dot product");
+
+  if (vec(1.0, 0.0, 0.0).cross(vec(0.0, 1.0, 0.0)) != vec(0.0, 0.0, 1.0)) 
+       ERR(1, "Bad vector cross product");
+
+  if (vec(0.0, 1.0, 0.0).cross(vec(0.0, 1.0, 0.0)) != vec(0.0, 0.0, 0.0)) 
+       ERR(1, "Bad vector cross product");
+
+  if (vec(0.0, 0.0, 1.0).cross(vec(0.0, 1.0, 0.0)) != vec(-1.0, 0.0, 0.0)) 
+       ERR(1, "Bad vector cross product");
+
+  // std::cout << pos.dist(vec(6378137.0, 0.0, 0.0)) << std::endl;
+
+  if (!(pos == vec(6378137.0, 0.0, 0.0))) ERR(1, "Default pos is not prime meridian");
+  if (pos != vec(6378137.0, 0.0, 0.0)) ERR(1, "Default pos is not prime meridian");
+
+  // std::cout << cent.dist(pos)- 6378137.0 << std::endl;
+
+  if (cent.dist(pos) != 6378137.0) ERR(1, "Equator Radius not Wgs84");
+  if (wgs84Const::c != 6356752.3142451792954l) ERR(1, "Minor axis not Wgs84");
+
+  // std::cout << wgs84(wgs84Const::maxNorth, 0.0).x << std::endl;
+  // std::cout << wgs84(wgs84Const::maxNorth, 0.0).y << std::endl;
+  // std::cout << wgs84(wgs84Const::maxNorth, 0.0).z << std::endl;
+
+  // std::cout << a * (1-1/fr) << std::endl;
+
+  if (wgs84(wgs84Const::maxNorth, 0.0).z != wgs84Const::c) ERR(1, "Bad North");
+  if (!tst(wgs84(wgs84Const::maxNorth, 0.0).z, 6356752.31424518l)) ERR(1, "Bad North");
+
+  for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC)
+    {
+    if (!tst(cent.dist(wgs84(0.0, lon)), 6378137.0)) 
+      {
+      std::cout << lon << ": " << cent.dist(wgs84(0.0, lon)) - 6378137.0 << std::endl;
+
+      ERR(1, "World Equator Radius not Wgs84");
+      }
+    }
+
+  return 0;
+}
+
+int tst2()
+{
+  int total=0;
+  wgs84Line line;
+  // line l1(vec(6378137.0/2,0,0));
+  // line l1(vec(6000000.0,0,0));
+  // line l2(count, l1);
+  
+  // std::cout << count << std::endl;
+  // std::cout << l2.p1.x << std::endl;
+  // std::cout << l2.p1.y << std::endl;
+  // std::cout << l2.p1.z << std::endl;
+  // std::cout << l2.p2.x << std::endl;
+  // std::cout << l2.p2.y << std::endl;
+  // std::cout << l2.p2.z << std::endl;
+
+  if (line.trimToEllipsoid() != 2) ERR(2, "WGS84 Crossing count");
+  if (line.p[0] != vec(-6378137.0,0,0)) ERR(2, "WGS84 Crossing");
+  if (line.p[1] != vec( 6378137.0,0,0)) ERR(2, "WGS84 Crossing");
+
+  line = wgs84Line(vec(0,1,0));
+  if (line.trimToEllipsoid() != 2) ERR(2, "WGS84 Crossing count");
+  // std::cout << " " << line.p[0].x << std::endl;
+  // std::cout << " " << line.p[0].y << std::endl;
+  // std::cout << " " << line.p[0].z << std::endl;
+  // std::cout << std::endl;
+  if (line.p[0] != vec(0,-6378137.0,0)) ERR(2, "WGS84 Crossing");
+  if (line.p[1] != vec(0, 6378137.0,0)) ERR(2, "WGS84 Crossing");
+
+  line = wgs84Line(vec(0,0,1));
+  if (line.trimToEllipsoid() != 2) ERR(2, "WGS84 Crossing count");
+  if (line.p[0] != vec(0,0,-6356752.31424518)) ERR(2, "WGS84 Crossing");
+  if (line.p[1] != vec(0,0, 6356752.31424518)) ERR(2, "WGS84 Crossing");
+
+  for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC)
+    {
+    for (double lat=wgs84Const::maxNorth; lat > wgs84Const::maxSouth; lat -= LL_INC)
+      {
+      wgs84Line l1(wgs84(lat,lon), wgs84(wgs84Const::maxNorth/2, wgs84Const::maxNorth));
+      wgs84Line l2(l1);
+      int count = l2.trimToEllipsoid();
+      if (count > 0 && l1.p[0] != l2.p[0]) ERR(2, "WGS84 robust");
+      if (count > 1 && l1.p[1] != l2.p[1]) ERR(2, "WGS84 robust");
+
+      wgs84Line l3(wgs84(lat,lon));
+      wgs84Line l5(l3.p[1]/33.0);
+      l5.trimToEllipsoid();
+
+      if (l3.p[1]*-1 != l5.p[0]) ERR(2, "WGS84 trim robust");
+      if (l3.p[1] != l5.p[1]) ERR(2, "WGS84 trim robust");
+
+#ifdef tlz
+      vec p1(wgs84(lat,lon));
+      vec p2(p1);
+      p2.y = 0;
+      line l6(count, line(p1, p2));
+      if (l6.p1 != p1) 
+       {
+       std::cout << p1.x << std::endl;
+       std::cout << p1.y << std::endl;
+       std::cout << p1.z << std::endl;
+       std::cout << l6.p1.x << std::endl;
+       std::cout << l6.p1.y << std::endl;
+       std::cout << l6.p1.z << std::endl;
+       ERR(2, "WGS84 robust");
+       }
+#endif
+      ++total;
+      }
+    }
+  std::cout << "Total " << total << std::endl;
+  return 0;
+}
+
+int tstLon()
+{
+  for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC)
+    {
+    wgs84 npole(wgs84Const::maxNorth,lon);
+    if (!tst(0, npole.lon()*1800/M_PIl)) ERR(4, "Bad Longitude");
+    wgs84 spole(wgs84Const::maxSouth,lon);
+    if (!tst(0, spole.lon()*1800/M_PIl)) ERR(4, "Bad Longitude");
+
+    for (double lat=wgs84Const::maxNorth-LL_INC; lat >= wgs84Const::maxSouth+LL_INC; lat -= LL_INC)
+      {
+      wgs84 t(lat,lon);
+      if (!tst(lon*1800/M_PIl, t.lon()*1800/M_PIl)) ERR(4, "Bad Longitude");
+      }
+    }
+  return 0;
+}
+
+int tstCos()
+{
+  vec t(1,0,0);
+
+  if (!tst(acos(t.cos(vec(1,1,0)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
+  if (!tst(acos(t.cos(vec(1,0,1)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
+  if (!tst(acos(t.cos(vec(1,0,0)))*180/M_PIl, 0)) ERR(4, "Bad Cos");
+  if (!tst(acos(t.cos(vec(1,-1,0)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
+  if (!tst(acos(t.cos(vec(1,0,-1)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
+  if (!tst(acos(t.cos(vec(-1,0,0)))*180/M_PIl, 180)) ERR(4, "Bad Cos");
+  if (!tst(acos(t.cos(vec(0,1,0)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
+  if (!tst(acos(t.cos(vec(0,0,1)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
+  if (!tst(acos(t.cos(vec(0,-1,0)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
+  if (!tst(acos(t.cos(vec(0,0,-1)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
+
+  for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC*100)
+    {
+    // std::cout << lon*1800/M_PIl << std::endl;
+    for (double lat=wgs84Const::maxNorth; lat >= wgs84Const::maxSouth; lat -= LL_INC)
+      {
+      wgs84Fast t(lat,lon), n(t.north());
+      if (!tst(lat*1800/M_PIl, t.lat()*1800/M_PIl)) ERR(4, "Bad Lat");
+
+      if (!tst(abs(t.sinLat()), n.distZ()/n.dist())) ERR(4, "Bad North vector");
+
+      wgs84Fast t2(lat,-85,10000);
+      if (!tst(lat*1800/M_PIl, t2.lat()*1800/M_PIl, 0.0035)) ERR(4, "Bad Lat at alt");
+      }
+    }
+
+  return 0;
+}
+
+int tst3()
+{
+  for (double lat=wgs84Const::maxNorth; lat > wgs84Const::maxSouth; lat -= LL_INC)
+    {
+    wgs84Fast pnt(lat,0);
+    // std::cout << " " << lat*1800/M_PIl; 
+    // std::cout << pnt.x - FindX2D(pnt.z);
+    // std::cout << " " << Slope2D(pnt.z);
+    // std::cout << " " << tan(lat);
+    // std::cout << " " << tan(lat) - Slope2D(pnt.z);
+    // if (!tst(pnt.x, pnt.rotationRadius(), 0.00001)) ERR(4, "Bad radius");
+    if (!tst(pnt.x, pnt.distZ())) ERR(4, "Bad radius");
+    //tlz if (!tst(tan(lat), pnt.tanLat())) ERR(4, "Bad Tan");
+    //tlz if (!tst(pnt.x, tan(lat)*(pnt.z - TanZCrossing(pnt.z)), 0.000001)) ERR(4, "Bad Z0");
+    if (!tst(lat, asin(pnt.sinLat()), 0.0000001)) ERR(4, "Bad WGS84 Sin");
+
+    pnt = wgs84(lat,0,10000);
+    // std::cout << " " << lat*1800/M_PIl; 
+    // std::cout << " " << pnt.x; 
+    // std::cout << " " << pnt.y; 
+    // std::cout << " " << pnt.z; 
+    // std::cout << " " << TanZCrossing(pnt.z); 
+    // std::cout << " " << tan(lat); 
+    // std::cout << " " << (find2dX(pnt.z) - find2dX(pnt.z+0.1))/0.1; 
+    // std::cout << " " << pnt.x / (pnt.z - TanZCrossing(pnt.z)); 
+    // std::cout << " " << pnt.z;
+    // std::cout << " " << WGS84EarthCenter(pnt.z); 
+    // std::cout << std::endl;
+    if (!tst(lat, asin(pnt.sinLat()), 0.00001)) ERR(4, "Bad WGS84 Sin");
+
+    // if (lat > wgs84Const::maxNorth-0.2) continue;  // Skip near polls TanZCrossing is better!
+    // if (lat < wgs84Const::maxSouth+0.2) continue;  // Skip near polls TanZCrossing is better!
+
+    // if (!tst(pnt.x, tan(lat)*(pnt.z - TanZCrossing(pnt.z)), 0.1)) ERR(4, "Bad Z0");
+    }
+
+  return 0;
+}
+
+int qtst()
+{
+  quat q;
+
+  q = quat(0, vec(0,0,0)).versor();
+  if (!tst(q.a, 0)) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.z, 1)) ERR(5, "Bad Quat Versor Init");
+
+  q = quat(3, vec(4,5,6)).conj();
+  if (!tst(q.a, 3)) ERR(5, "Bad Quat Conj Angle");
+  if (!tst(q.v.x, -4)) ERR(5, "Bad Quat Conj X");
+  if (!tst(q.v.y, -5)) ERR(5, "Bad Quat Conj Y");
+  if (!tst(q.v.z, -6)) ERR(5, "Bad Quat Conj Z");
+  if (!tst(q.mag(), sqrt(86.0))) ERR(5, "Bad Quat Magnitude");
+  if (!tst(q.versor().mag(), 1)) ERR(5, "Bad Quat Versor");
+
+  q = quat(0, zAxis) * quat(0, zAxis);
+  if (!tst(q.a, -1)) ERR(5, "Bad Quat Angle");
+  if (!tst(q.v.x, 0)) ERR(5, "Bad Quat X");
+  if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Y");
+  if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Z");
+
+  q = quat(0, zAxis) * quat(0, vec(1,0,0));
+  if (!tst(q.a, 0)) ERR(5, "Bad Quat Angle");
+  if (!tst(q.v.x, 0)) ERR(5, "Bad Quat X");
+  if (!tst(q.v.y, 1)) ERR(5, "Bad Quat Y");
+  if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Z");
+
+  // q = quat(cos(M_PI_2l/2), zAxis) * quat(cos(M_PI_2l/2), zAxis);
+  // q = quat(cos(M_PI_2l/2), zAxis * sin(M_PI_2l/2));
+
+  q = quat(cos(M_PI_4l/2), zAxis).normVec();
+  q = q * q;
+
+  if (!tst(q.a, cos(M_PI_4l))) ERR(5, "Bad Quat Hamilton Multiple Angle");
+  if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Mult X");
+  if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Mult Y");
+  if (!tst(q.v.norm().z, 1)) ERR(5, "Bad Quat Z");
+
+  q = quat(cos(M_PI_4l/4), zAxis).normVec() * quat(cos(M_PI_4l/2), zAxis).normVec();
+  if (!tst(q.a, cos(M_PI_4l * 3/4))) ERR(5, "Bad Quat Hamilton Multiple Angle");
+  if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Mult X");
+  if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Mult Y");
+  if (!tst(q.v.norm().z, 1)) ERR(5, "Bad Quat Z");
+
+  q = quat(cos(M_PI_2l), zAxis).normVec() * quat(cos(M_PI_2l), vec(0,1,0)).normVec();
+  if (!tst(q.a, cos(M_PI_2l))) ERR(5, "Bad Quat Hamilton Multiple Angle");
+  if (!tst(q.v.x, -1)) ERR(5, "Bad Quat Mult X");
+  if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Mult Y");
+  if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Mult Z");
+
+  q = quat(cos(M_PI_4l), zAxis).normVec() * quat(cos(M_PI_4l), vec(0,1,0)).normVec();
+  if (!tst(q.a, cos(M_PIl/3))) ERR(5, "Bad Quat Hamilton Multiple Angle");
+  if (!tst(q.v.x, -0.5)) ERR(5, "Bad Quat Mult X");
+  if (!tst(q.v.y, 0.5)) ERR(5, "Bad Quat Mult Y");
+  if (!tst(q.v.z, 0.5)) ERR(5, "Bad Quat Mult Z");
+
+  // std::cout << " " << q.a;
+  // std::cout << " " << acos(q.a) * 180 / M_PIl;
+  // std::cout << " " << q.v.x;
+  // std::cout << " " << q.v.y;
+  // std::cout << " " << q.v.z;
+  // std::cout << std::endl;
+
+  return 0;
+}
+
+int qvtst()
+{
+  vec v;
+  quat q(cos(M_PI_4l), zAxis);
+  q = q.normVec();
+
+  v = q * vec(1,0,0);
+  if (!tst(v.x, 0)) ERR(5, "Bad Quat rotate X");
+  if (!tst(v.y, 1)) ERR(5, "Bad Quat rotate Y");
+  if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
+
+  v = q * vec(0,1,0);
+  if (!tst(v.x, -1)) ERR(5, "Bad Quat rotate X");
+  if (!tst(v.y, 0)) ERR(5, "Bad Quat rotate Y");
+  if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
+
+  v = q * vec(0,0,1);
+  if (!tst(v.x, 0)) ERR(5, "Bad Quat rotate X");
+  if (!tst(v.y, 0)) ERR(5, "Bad Quat rotate Y");
+  if (!tst(v.z, 1)) ERR(5, "Bad Quat rotate Z");
+
+  v = q * vec(1,1,0);
+  if (!tst(v.x, -1)) ERR(5, "Bad Quat rotate X");
+  if (!tst(v.y, 1)) ERR(5, "Bad Quat rotate Y");
+  if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
+
+  q = quat(cos(M_PI_4l/2), zAxis).normVec();
+  v = q * vec(1,0,0);
+  if (!tst(v.x, sqrt(0.5))) ERR(5, "Bad Quat rotate X");
+  if (!tst(v.y, sqrt(0.5))) ERR(5, "Bad Quat rotate Y");
+  if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
+
+  q = quat(cos(30.0 * M_PI_2l/180), zAxis).normVec();
+  v = q * vec(5,0,0);
+  //if (!tst(v.x, sqrt(0.5))) ERR(5, "Bad Quat rotate X");
+  if (!tst(v.y, 2.5)) ERR(5, "Bad Quat rotate Y");
+  if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
+
+  /*
+  std::cout << " " << v.x;
+  std::cout << " " << v.y;
+  std::cout << " " << v.z;
+  std::cout << std::endl;
+  */
+
+  return 0;
+}
+
+} // namespace ecef
+
+int main()
+{
+  int err=0;
+
+  std::cout.precision(14);
+  std::cout << std::scientific;
+
+  std::cout << "starting ecef-wgs84_test..." << std::endl;
+
+  err |= ecef::tstLon();
+  err |= ecef::tst1();
+  err |= ecef::tst2();
+  err |= ecef::tstCos();
+  err |= ecef::tst3();
+  err |= ecef::qtst();
+  err |= ecef::qvtst();
+
+  std::cout << "finishing ecef-wgs84_test." << std::endl;
+
+  return err;
+}