test/ecef-wgs84_test.cc

   1 /*
   2   Earth Centered Earth Fixed (ECEF) WGS84 Navigation Utility Copyright © 2013-2017  Infinite Delta Corp
   3
   4   This program is free software: you can redistribute it and/or modify
   5   it under the terms of the GNU Lesser General Public License as published by
   6   the Free Software Foundation, either version 2 of the License, or
   7   (at your option) any later version.
   8
   9   This program is distributed in the hope that it will be useful,
  10   but WITHOUT ANY WARRANTY; without even the implied warranty of
  11   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12   GNU General Public License for more details.
  13
  14   You should have received a copy of the GNU Lesser General Public License
  15   along with this program.  If not, see <http://www.gnu.org/licenses/>.
  16
  17 */
  18
  19 #include <iostream>
  20 #include <ecef-quaternion.h>
  21 #include <ecef-wgs84.h>
  22
  23 namespace ecef
  24 {
  25 static bool tst(const double i, const double j=0, const double t=0.0000001)
  26   {if (i>=j ? i-j <= t : j-i <= t) return true;
  27   std::cout << i << " " << j << " " << i-j << " " << t << " ";
  28   return false;
  29   }
  30
  31 #define ERR(e,m) {std::cout << "Test Error: \"" << m << "\" in " << __FILE__ << ":" << __LINE__ << std::endl; return e;}
  32
  33 #define LL_INC wgs84Const::maxNorth/1000.0
  34
  35 int tst1()
  36 {
  37   vec cent;
  38   wgs84 pos;
  39
  40   if (pos.x != 6378137.0) ERR(1, "Default X is not Wgs84 Radius");
  41   if (pos.y != 0.0) ERR(1, "Default Y is not zero");
  42   if (pos.z != 0.0) ERR(1, "Default Z is not zero");
  43   // std::cout << pos.x << std::endl;
  44   // std::cout << pos.y << std::endl;
  45   // std::cout << pos.z << std::endl;
  46
  47   if (vec(1.0, 0.0, 0.0) + vec(0.0, 2.0, 0.0) + vec(0.0, 0.0, 3.0) !=
  48       vec(1.0, 2.0, 3.0)) ERR(1, "Bad vector addition");
  49
  50   if (vec(1.0, 0.0, 0.0) - vec(2.0, 3.0, 0.0) - vec(0.0, 0.0, 4.0) !=
  51       vec(-1.0, -3.0, -4.0)) ERR(1, "Bad vector subtraction");
  52
  53   if (vec(1.0, 2.0, 3.0).dot(vec(4.0, 5.0, 6.0)) != 32.0) ERR(1, "Bad vector dot product");
  54
  55   if (vec(1.0, 0.0, 0.0).cross(vec(0.0, 1.0, 0.0)) != vec(0.0, 0.0, 1.0))
  56         ERR(1, "Bad vector cross product");
  57
  58   if (vec(0.0, 1.0, 0.0).cross(vec(0.0, 1.0, 0.0)) != vec(0.0, 0.0, 0.0))
  59         ERR(1, "Bad vector cross product");
  60
  61   if (vec(0.0, 0.0, 1.0).cross(vec(0.0, 1.0, 0.0)) != vec(-1.0, 0.0, 0.0))
  62         ERR(1, "Bad vector cross product");
  63
  64   // std::cout << pos.dist(vec(6378137.0, 0.0, 0.0)) << std::endl;
  65
  66   if (!(pos == vec(6378137.0, 0.0, 0.0))) ERR(1, "Default pos is not prime meridian");
  67   if (pos != vec(6378137.0, 0.0, 0.0)) ERR(1, "Default pos is not prime meridian");
  68
  69   // std::cout << cent.dist(pos)- 6378137.0 << std::endl;
  70
  71   if (cent.dist(pos) != 6378137.0) ERR(1, "Equator Radius not Wgs84");
  72   if (wgs84Const::c != 6356752.3142451792954l) ERR(1, "Minor axis not Wgs84");
  73
  74   // std::cout << wgs84(wgs84Const::maxNorth, 0.0).x << std::endl;
  75   // std::cout << wgs84(wgs84Const::maxNorth, 0.0).y << std::endl;
  76   // std::cout << wgs84(wgs84Const::maxNorth, 0.0).z << std::endl;
  77
  78   // std::cout << a * (1-1/fr) << std::endl;
  79
  80   if (wgs84(wgs84Const::maxNorth, 0.0).z != wgs84Const::c) ERR(1, "Bad North");
  81   if (!tst(wgs84(wgs84Const::maxNorth, 0.0).z, 6356752.31424518l)) ERR(1, "Bad North");
  82
  83   for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC)
  84     {
  85     if (!tst(cent.dist(wgs84(0.0, lon)), 6378137.0))
  86       {
  87       std::cout << lon << ": " << cent.dist(wgs84(0.0, lon)) - 6378137.0 << std::endl;
  88
  89       ERR(1, "World Equator Radius not Wgs84");
  90       }
  91     }
  92
  93   return 0;
  94 }
  95
  96 int tst2()
  97 {
  98   int total=0;
  99   wgs84Line line;
 100   // line l1(vec(6378137.0/2,0,0));
 101   // line l1(vec(6000000.0,0,0));
 102   // line l2(count, l1);
 103
 104   // std::cout << count << std::endl;
 105   // std::cout << l2.p1.x << std::endl;
 106   // std::cout << l2.p1.y << std::endl;
 107   // std::cout << l2.p1.z << std::endl;
 108   // std::cout << l2.p2.x << std::endl;
 109   // std::cout << l2.p2.y << std::endl;
 110   // std::cout << l2.p2.z << std::endl;
 111
 112   if (line.trimToEllipsoid() != 2) ERR(2, "WGS84 Crossing count");
 113   if (line.p[0] != vec(-6378137.0,0,0)) ERR(2, "WGS84 Crossing");
 114   if (line.p[1] != vec( 6378137.0,0,0)) ERR(2, "WGS84 Crossing");
 115
 116   line = wgs84Line(vec(0,1,0));
 117   if (line.trimToEllipsoid() != 2) ERR(2, "WGS84 Crossing count");
 118   // std::cout << " " << line.p[0].x << std::endl;
 119   // std::cout << " " << line.p[0].y << std::endl;
 120   // std::cout << " " << line.p[0].z << std::endl;
 121   // std::cout << std::endl;
 122   if (line.p[0] != vec(0,-6378137.0,0)) ERR(2, "WGS84 Crossing");
 123   if (line.p[1] != vec(0, 6378137.0,0)) ERR(2, "WGS84 Crossing");
 124
 125   line = wgs84Line(vec(0,0,1));
 126   if (line.trimToEllipsoid() != 2) ERR(2, "WGS84 Crossing count");
 127   if (line.p[0] != vec(0,0,-6356752.31424518)) ERR(2, "WGS84 Crossing");
 128   if (line.p[1] != vec(0,0, 6356752.31424518)) ERR(2, "WGS84 Crossing");
 129
 130   for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC)
 131     {
 132     for (double lat=wgs84Const::maxNorth; lat > wgs84Const::maxSouth; lat -= LL_INC)
 133       {
 134       wgs84Line l1(wgs84(lat,lon), wgs84(wgs84Const::maxNorth/2, wgs84Const::maxNorth));
 135       wgs84Line l2(l1);
 136       int count = l2.trimToEllipsoid();
 137       if (count > 0 && l1.p[0] != l2.p[0]) ERR(2, "WGS84 robust");
 138       if (count > 1 && l1.p[1] != l2.p[1])
 139         {
 140         // ERR(2, "WGS84 robust");
 141
 142         std::cout << "Second point trim error at lat/lon " << lat * 180.0 / M_PIl;
 143         std::cout << "/" << lon * 180.0 / M_PIl;
 144         // std::cout << std::endl;
 145         // std::cout << " " << l1.p[0].x;
 146         // std::cout << " " << l1.p[0].y;
 147         // std::cout << " " << l1.p[0].z;
 148         // std::cout << " " << l1.p[1].x;
 149         // std::cout << " " << l1.p[1].y;
 150         // std::cout << " " << l1.p[1].z;
 151         // std::cout << std::endl;
 152         // std::cout << " " << l2.p[0].x;
 153         // std::cout << " " << l2.p[0].y;
 154         // std::cout << " " << l2.p[0].z;
 155         // std::cout << " " << l2.p[1].x;
 156         // std::cout << " " << l2.p[1].y;
 157         // std::cout << " " << l2.p[1].z;
 158         std::cout << std::endl;
 159         }
 160
 161
 162       wgs84Line l3(wgs84(lat,lon));
 163       wgs84Line l5(l3.p[1]/33.0);
 164       l5.trimToEllipsoid();
 165
 166       if (l3.p[1]*-1 != l5.p[0]) ERR(2, "WGS84 trim robust");
 167       if (l3.p[1] != l5.p[1]) ERR(2, "WGS84 trim robust");
 168
 169 #ifdef tlz
 170       vec p1(wgs84(lat,lon));
 171       vec p2(p1);
 172       p2.y = 0;
 173       line l6(count, line(p1, p2));
 174       if (l6.p1 != p1)
 175         {
 176         std::cout << p1.x << std::endl;
 177         std::cout << p1.y << std::endl;
 178         std::cout << p1.z << std::endl;
 179         std::cout << l6.p1.x << std::endl;
 180         std::cout << l6.p1.y << std::endl;
 181         std::cout << l6.p1.z << std::endl;
 182         ERR(2, "WGS84 robust");
 183         }
 184 #endif
 185       ++total;
 186       }
 187     }
 188   std::cout << "Total " << total << std::endl;
 189   return 0;
 190 }
 191
 192 int tstLon()
 193 {
 194   for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC)
 195     {
 196     wgs84 npole(wgs84Const::maxNorth,lon);
 197     if (!tst(0, npole.lon()*1800/M_PIl)) ERR(4, "Bad Longitude");
 198     wgs84 spole(wgs84Const::maxSouth,lon);
 199     if (!tst(0, spole.lon()*1800/M_PIl)) ERR(4, "Bad Longitude");
 200
 201     for (double lat=wgs84Const::maxNorth-LL_INC; lat >= wgs84Const::maxSouth+LL_INC; lat -= LL_INC)
 202       {
 203       wgs84 t(lat,lon);
 204       if (!tst(lon*1800/M_PIl, t.lon()*1800/M_PIl)) ERR(4, "Bad Longitude");
 205       }
 206     }
 207   return 0;
 208 }
 209
 210 int tstCos()
 211 {
 212   vec t(1,0,0);
 213
 214   if (!tst(acos(t.cos(vec(1,1,0)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
 215   if (!tst(acos(t.cos(vec(1,0,1)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
 216   if (!tst(acos(t.cos(vec(1,0,0)))*180/M_PIl, 0)) ERR(4, "Bad Cos");
 217   if (!tst(acos(t.cos(vec(1,-1,0)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
 218   if (!tst(acos(t.cos(vec(1,0,-1)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
 219   if (!tst(acos(t.cos(vec(-1,0,0)))*180/M_PIl, 180)) ERR(4, "Bad Cos");
 220   if (!tst(acos(t.cos(vec(0,1,0)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
 221   if (!tst(acos(t.cos(vec(0,0,1)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
 222   if (!tst(acos(t.cos(vec(0,-1,0)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
 223   if (!tst(acos(t.cos(vec(0,0,-1)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
 224
 225   for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC*100)
 226     {
 227     // std::cout << lon*1800/M_PIl << std::endl;
 228     for (double lat=wgs84Const::maxNorth; lat >= wgs84Const::maxSouth; lat -= LL_INC)
 229       {
 230       wgs84Fast t(lat,lon), n(t.north());
 231       if (!tst(lat*1800/M_PIl, t.lat()*1800/M_PIl)) ERR(4, "Bad Lat");
 232
 233       if (!tst(abs(t.sinLat()), n.distZ()/n.dist())) ERR(4, "Bad North vector");
 234
 235       wgs84Fast t2(lat,-85,10000);
 236       if (!tst(lat*1800/M_PIl, t2.lat()*1800/M_PIl, 0.0035)) ERR(4, "Bad Lat at alt");
 237       }
 238     }
 239
 240   return 0;
 241 }
 242
 243 int tst3()
 244 {
 245   for (double lat=wgs84Const::maxNorth; lat > wgs84Const::maxSouth; lat -= LL_INC)
 246     {
 247     wgs84Fast pnt(lat,0);
 248     // std::cout << " " << lat*1800/M_PIl;
 249     // std::cout << pnt.x - FindX2D(pnt.z);
 250     // std::cout << " " << Slope2D(pnt.z);
 251     // std::cout << " " << tan(lat);
 252     // std::cout << " " << tan(lat) - Slope2D(pnt.z);
 253     // if (!tst(pnt.x, pnt.rotationRadius(), 0.00001)) ERR(4, "Bad radius");
 254     if (!tst(pnt.x, pnt.distZ())) ERR(4, "Bad radius");
 255     //tlz if (!tst(tan(lat), pnt.tanLat())) ERR(4, "Bad Tan");
 256     //tlz if (!tst(pnt.x, tan(lat)*(pnt.z - TanZCrossing(pnt.z)), 0.000001)) ERR(4, "Bad Z0");
 257     if (!tst(lat, asin(pnt.sinLat()), 0.0000001)) ERR(4, "Bad WGS84 Sin");
 258
 259     pnt = wgs84(lat,0,10000);
 260     // std::cout << " " << lat*1800/M_PIl;
 261     // std::cout << " " << pnt.x;
 262     // std::cout << " " << pnt.y;
 263     // std::cout << " " << pnt.z;
 264     // std::cout << " " << TanZCrossing(pnt.z);
 265     // std::cout << " " << tan(lat);
 266     // std::cout << " " << (find2dX(pnt.z) - find2dX(pnt.z+0.1))/0.1;
 267     // std::cout << " " << pnt.x / (pnt.z - TanZCrossing(pnt.z));
 268     // std::cout << " " << pnt.z;
 269     // std::cout << " " << WGS84EarthCenter(pnt.z);
 270     // std::cout << std::endl;
 271     if (!tst(lat, asin(pnt.sinLat()), 0.00001)) ERR(4, "Bad WGS84 Sin");
 272
 273     // if (lat > wgs84Const::maxNorth-0.2) continue;  // Skip near polls TanZCrossing is better!
 274     // if (lat < wgs84Const::maxSouth+0.2) continue;  // Skip near polls TanZCrossing is better!
 275
 276     // if (!tst(pnt.x, tan(lat)*(pnt.z - TanZCrossing(pnt.z)), 0.1)) ERR(4, "Bad Z0");
 277     }
 278
 279   return 0;
 280 }
 281
 282 int qtst()
 283 {
 284   quat q;
 285
 286   q = quat(0, vec(0,0,0)).versor();
 287   if (!tst(q.a, 0)) ERR(5, "Bad Quat Versor Init");
 288   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
 289   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Versor Init");
 290   if (!tst(q.v.z, 1)) ERR(5, "Bad Quat Versor Init");
 291
 292   q = quat(3, vec(4,5,6)).conj();
 293   if (!tst(q.a, 3)) ERR(5, "Bad Quat Conj Angle");
 294   if (!tst(q.v.x, -4)) ERR(5, "Bad Quat Conj X");
 295   if (!tst(q.v.y, -5)) ERR(5, "Bad Quat Conj Y");
 296   if (!tst(q.v.z, -6)) ERR(5, "Bad Quat Conj Z");
 297   if (!tst(q.mag(), sqrt(86.0))) ERR(5, "Bad Quat Magnitude");
 298   if (!tst(q.versor().mag(), 1)) ERR(5, "Bad Quat Versor");
 299
 300   q = quat(0, zAxis) * quat(0, zAxis);
 301   if (!tst(q.a, -1)) ERR(5, "Bad Quat Angle");
 302   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat X");
 303   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Y");
 304   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Z");
 305
 306   q = quat(0, zAxis) * quat(0, vec(1,0,0));
 307   if (!tst(q.a, 0)) ERR(5, "Bad Quat Angle");
 308   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat X");
 309   if (!tst(q.v.y, 1)) ERR(5, "Bad Quat Y");
 310   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Z");
 311
 312   // q = quat(cos(M_PI_2l/2), zAxis) * quat(cos(M_PI_2l/2), zAxis);
 313   // q = quat(cos(M_PI_2l/2), zAxis * sin(M_PI_2l/2));
 314
 315   q = quat(cos(M_PI_4l/2), zAxis).normVec();
 316   q = q * q;
 317
 318   if (!tst(q.a, cos(M_PI_4l))) ERR(5, "Bad Quat Hamilton Multiple Angle");
 319   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Mult X");
 320   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Mult Y");
 321   if (!tst(q.v.norm().z, 1)) ERR(5, "Bad Quat Z");
 322
 323   q = quat(cos(M_PI_4l/4), zAxis).normVec() * quat(cos(M_PI_4l/2), zAxis).normVec();
 324   if (!tst(q.a, cos(M_PI_4l * 3/4))) ERR(5, "Bad Quat Hamilton Multiple Angle");
 325   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Mult X");
 326   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Mult Y");
 327   if (!tst(q.v.norm().z, 1)) ERR(5, "Bad Quat Z");
 328
 329   q = quat(cos(M_PI_2l), zAxis).normVec() * quat(cos(M_PI_2l), vec(0,1,0)).normVec();
 330   if (!tst(q.a, cos(M_PI_2l))) ERR(5, "Bad Quat Hamilton Multiple Angle");
 331   if (!tst(q.v.x, -1)) ERR(5, "Bad Quat Mult X");
 332   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Mult Y");
 333   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Mult Z");
 334
 335   q = quat(cos(M_PI_4l), zAxis).normVec() * quat(cos(M_PI_4l), vec(0,1,0)).normVec();
 336   if (!tst(q.a, cos(M_PIl/3))) ERR(5, "Bad Quat Hamilton Multiple Angle");
 337   if (!tst(q.v.x, -0.5)) ERR(5, "Bad Quat Mult X");
 338   if (!tst(q.v.y, 0.5)) ERR(5, "Bad Quat Mult Y");
 339   if (!tst(q.v.z, 0.5)) ERR(5, "Bad Quat Mult Z");
 340
 341   // std::cout << " " << q.a;
 342   // std::cout << " " << acos(q.a) * 180 / M_PIl;
 343   // std::cout << " " << q.v.x;
 344   // std::cout << " " << q.v.y;
 345   // std::cout << " " << q.v.z;
 346   // std::cout << std::endl;
 347   return 0;
 348 }
 349
 350 int qvtst()
 351 {
 352   vec v;
 353   quat q(cos(M_PI_4l), zAxis);
 354   q = q.normVec();
 355
 356   v = q * vec(1,0,0);
 357   if (!tst(v.x, 0)) ERR(5, "Bad Quat rotate X");
 358   if (!tst(v.y, 1)) ERR(5, "Bad Quat rotate Y");
 359   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
 360
 361   v = q * vec(0,1,0);
 362   if (!tst(v.x, -1)) ERR(5, "Bad Quat rotate X");
 363   if (!tst(v.y, 0)) ERR(5, "Bad Quat rotate Y");
 364   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
 365
 366   v = q * vec(0,0,1);
 367   if (!tst(v.x, 0)) ERR(5, "Bad Quat rotate X");
 368   if (!tst(v.y, 0)) ERR(5, "Bad Quat rotate Y");
 369   if (!tst(v.z, 1)) ERR(5, "Bad Quat rotate Z");
 370
 371   v = q * vec(1,1,0);
 372   if (!tst(v.x, -1)) ERR(5, "Bad Quat rotate X");
 373   if (!tst(v.y, 1)) ERR(5, "Bad Quat rotate Y");
 374   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
 375
 376   q = quat(cos(M_PI_4l/2), zAxis).normVec();
 377   v = q * vec(1,0,0);
 378   if (!tst(v.x, sqrt(0.5))) ERR(5, "Bad Quat rotate X");
 379   if (!tst(v.y, sqrt(0.5))) ERR(5, "Bad Quat rotate Y");
 380   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
 381
 382   q = quat(cos(30.0 * M_PI_2l/180), zAxis).normVec();
 383   v = q * vec(5,0,0);
 384   //if (!tst(v.x, sqrt(0.5))) ERR(5, "Bad Quat rotate X");
 385   if (!tst(v.y, 2.5)) ERR(5, "Bad Quat rotate Y");
 386   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
 387
 388   /*
 389   std::cout << " " << v.x;
 390   std::cout << " " << v.y;
 391   std::cout << " " << v.z;
 392   std::cout << std::endl;
 393   */
 394
 395   q = quat(zAxis, vec(0,0,1));
 396   if (!tst(q.a, 1)) ERR(5, "Bad Quat Versor Init");
 397   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
 398   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Versor Init");
 399   if (!tst(q.v.z, 1)) ERR(5, "Bad Quat Versor Init");
 400
 401   q = quat(zAxis, vec(1,0,0));
 402   if (!tst(q.a, cos(M_PI_4l))) ERR(5, "Bad Quat Versor Init");
 403   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
 404   if (!tst(q.v.y, 1)) ERR(5, "Bad Quat Versor Init");
 405   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Versor Init");
 406
 407   q = quat(zAxis, vec(0.5,0,0));
 408   if (!tst(q.a, cos(M_PI_4l))) ERR(5, "Bad Quat Versor Init");
 409   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
 410   if (!tst(q.v.y, 0.5)) ERR(5, "Bad Quat Versor Init");
 411   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Versor Init");
 412
 413   q = quat(zAxis, vec(1,0,1).norm());
 414   if (!tst(q.a, cos(M_PI_4l/2))) ERR(5, "Bad Quat Versor Init");
 415   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
 416   if (!tst(q.v.y, sqrt(2)/2)) ERR(5, "Bad Quat Versor Init");
 417   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Versor Init");
 418
 419   q = quat(zAxis, vec(1,0,1).norm()).normVec();
 420   if (!tst(q.a, cos(M_PI_4l/2))) ERR(5, "Bad Quat Versor Init");
 421   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
 422   // if (!tst(q.v.y, )) ERR(5, "Bad Quat Versor Init");
 423   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Versor Init");
 424
 425   v = q * zAxis;
 426   if (!tst(v.x, sqrt(2)/2)) ERR(5, "Bad Quat Versor Init");
 427   if (!tst(v.y, 0)) ERR(5, "Bad Quat Versor Init");
 428   if (!tst(v.z, sqrt(2)/2)) ERR(5, "Bad Quat Versor Init");
 429
 430   // std::cout << " " << q.a;
 431   // std::cout << " " << q.v.x;
 432   // std::cout << " " << q.v.y;
 433   // std::cout << " " << q.v.z;
 434   // std::cout << ": " << v.x;
 435   //  std::cout << " " << v.y;
 436   // std::cout << " " << v.z;
 437   // std::cout << std::endl;
 438
 439   // Verify all that creating a Quaternion from two vectors work in all quadrants.
 440
 441   for (int x1=-5; x1<5; ++x1) for (int y1=-5; y1<5; ++y1) for (int z1=-5; z1<5; ++z1)
 442     for (int x2=-5; x2<5; ++x2) for (int y2=-5; y2<5; ++y2) for (int z2=-5; z2<5; ++z2)
 443     {
 444     vec v1 = vec(x1, y1, z1).norm(), v2 = vec(x2, y2, z2).norm();
 445     q = quat(v1, v2).normVec();
 446     if (abs(q.a) >= (1 - toleranceLimit)) continue;
 447
 448     v = q * v1;
 449     // std::cout << " " << v1.dot(v2);
 450     // std::cout << " " << q.a;
 451     // std::cout << ", " << v1.x;
 452     // std::cout << " " << v1.y;
 453     // std::cout << " " << v1.z;
 454     // std::cout << ", " << v2.x;
 455     // std::cout << " " << v2.y;
 456     // std::cout << " " << v2.z;
 457     // std::cout << ", " << v.x;
 458     // std::cout << " " << v.y;
 459     // std::cout << " " << v.z;
 460     // std::cout << std::endl;
 461     if (!tst(v.x, v2.x)) ERR(5, "Quaderant quaternion test");
 462     if (!tst(v.y, v2.y)) ERR(5, "Quaderant quaternion test");
 463     if (!tst(v.z, v2.z)) ERR(5, "Quaderant quaternion test");
 464     }
 465
 466   return 0;
 467 }
 468
 469 int quaternionAroundWorldTest()
 470 {
 471   wgs84 start(42.89, 85.56), p; // 42.89°N 85.56°W
 472   quat q = quat(cos(M_PI_2l/18000), zAxis).normVec();           // Around the world at this latitude.
 473
 474   p = start;
 475   for (int i=0; i<36000; ++i) p = q * p;
 476   if ((start - p).dist() > 0.1) ERR(6, "Around world on latitude Quaternion test");
 477
 478   q = quat(cos(M_PI_2l/18000), zAxis.cross(start)).normVec();   // Polar around the world
 479   p = start;
 480   for (int i=0; i<36000; ++i) p = q * p;
 481   if ((start - p).dist() > 0.25) ERR(6, "Polar around world Quaternion test");
 482
 483   q = quat(cos(M_PI_2l/18000), wgs84(0,0).cross(start)).normVec();      // Around world through N0, E0
 484   p = start;
 485   for (int i=0; i<36000; ++i) p = q * p;
 486   if ((start - p).dist() > 0.25) ERR(6, "Around world through N0, W0 Quaternion test");
 487
 488   /*
 489   std::cout << " " << p.x;
 490   std::cout << " " << p.y;
 491   std::cout << " " << p.z;
 492   std::cout << " " << (start - p).dist();
 493   std::cout << std::endl;
 494   */
 495
 496   return 0;
 497 }
 498
 499
 500 } // namespace ecef
 501
 502 int main(const int argc, const char *argv[])
 503 {
 504   int err=0;
 505
 506   std::cout.precision(14);
 507   std::cout << std::scientific;
 508
 509   std::cout << "starting ecef-wgs84_test..." << std::endl;
 510
 511   err |= ecef::tstLon();
 512   err |= ecef::tst1();
 513   err |= ecef::tst2();
 514   err |= ecef::tstCos();
 515   err |= ecef::tst3();
 516   err |= ecef::qtst();
 517   err |= ecef::qvtst();
 518   err |= ecef::quaternionAroundWorldTest();
 519
 520   std::cout << "finishing ecef-wgs84_test." << std::endl;
 521
 522   return err;
 523 }