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]) ERR(2, "WGS84 robust");
 139
 140       wgs84Line l3(wgs84(lat,lon));
 141       wgs84Line l5(l3.p[1]/33.0);
 142       l5.trimToEllipsoid();
 143
 144       if (l3.p[1]*-1 != l5.p[0]) ERR(2, "WGS84 trim robust");
 145       if (l3.p[1] != l5.p[1]) ERR(2, "WGS84 trim robust");
 146
 147 #ifdef tlz
 148       vec p1(wgs84(lat,lon));
 149       vec p2(p1);
 150       p2.y = 0;
 151       line l6(count, line(p1, p2));
 152       if (l6.p1 != p1)
 153         {
 154         std::cout << p1.x << std::endl;
 155         std::cout << p1.y << std::endl;
 156         std::cout << p1.z << std::endl;
 157         std::cout << l6.p1.x << std::endl;
 158         std::cout << l6.p1.y << std::endl;
 159         std::cout << l6.p1.z << std::endl;
 160         ERR(2, "WGS84 robust");
 161         }
 162 #endif
 163       ++total;
 164       }
 165     }
 166   std::cout << "Total " << total << std::endl;
 167   return 0;
 168 }
 169
 170 int tstLon()
 171 {
 172   for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC)
 173     {
 174     wgs84 npole(wgs84Const::maxNorth,lon);
 175     if (!tst(0, npole.lon()*1800/M_PIl)) ERR(4, "Bad Longitude");
 176     wgs84 spole(wgs84Const::maxSouth,lon);
 177     if (!tst(0, spole.lon()*1800/M_PIl)) ERR(4, "Bad Longitude");
 178
 179     for (double lat=wgs84Const::maxNorth-LL_INC; lat >= wgs84Const::maxSouth+LL_INC; lat -= LL_INC)
 180       {
 181       wgs84 t(lat,lon);
 182       if (!tst(lon*1800/M_PIl, t.lon()*1800/M_PIl)) ERR(4, "Bad Longitude");
 183       }
 184     }
 185   return 0;
 186 }
 187
 188 int tstCos()
 189 {
 190   vec t(1,0,0);
 191
 192   if (!tst(acos(t.cos(vec(1,1,0)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
 193   if (!tst(acos(t.cos(vec(1,0,1)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
 194   if (!tst(acos(t.cos(vec(1,0,0)))*180/M_PIl, 0)) ERR(4, "Bad Cos");
 195   if (!tst(acos(t.cos(vec(1,-1,0)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
 196   if (!tst(acos(t.cos(vec(1,0,-1)))*180/M_PIl, 45)) ERR(4, "Bad Cos");
 197   if (!tst(acos(t.cos(vec(-1,0,0)))*180/M_PIl, 180)) ERR(4, "Bad Cos");
 198   if (!tst(acos(t.cos(vec(0,1,0)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
 199   if (!tst(acos(t.cos(vec(0,0,1)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
 200   if (!tst(acos(t.cos(vec(0,-1,0)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
 201   if (!tst(acos(t.cos(vec(0,0,-1)))*180/M_PIl, 90)) ERR(4, "Bad Cos");
 202
 203   for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC*100)
 204     {
 205     // std::cout << lon*1800/M_PIl << std::endl;
 206     for (double lat=wgs84Const::maxNorth; lat >= wgs84Const::maxSouth; lat -= LL_INC)
 207       {
 208       wgs84Fast t(lat,lon), n(t.north());
 209       if (!tst(lat*1800/M_PIl, t.lat()*1800/M_PIl)) ERR(4, "Bad Lat");
 210
 211       if (!tst(abs(t.sinLat()), n.distZ()/n.dist())) ERR(4, "Bad North vector");
 212
 213       wgs84Fast t2(lat,-85,10000);
 214       if (!tst(lat*1800/M_PIl, t2.lat()*1800/M_PIl, 0.0035)) ERR(4, "Bad Lat at alt");
 215       }
 216     }
 217
 218   return 0;
 219 }
 220
 221 int tst3()
 222 {
 223   for (double lat=wgs84Const::maxNorth; lat > wgs84Const::maxSouth; lat -= LL_INC)
 224     {
 225     wgs84Fast pnt(lat,0);
 226     // std::cout << " " << lat*1800/M_PIl;
 227     // std::cout << pnt.x - FindX2D(pnt.z);
 228     // std::cout << " " << Slope2D(pnt.z);
 229     // std::cout << " " << tan(lat);
 230     // std::cout << " " << tan(lat) - Slope2D(pnt.z);
 231     // if (!tst(pnt.x, pnt.rotationRadius(), 0.00001)) ERR(4, "Bad radius");
 232     if (!tst(pnt.x, pnt.distZ())) ERR(4, "Bad radius");
 233     //tlz if (!tst(tan(lat), pnt.tanLat())) ERR(4, "Bad Tan");
 234     //tlz if (!tst(pnt.x, tan(lat)*(pnt.z - TanZCrossing(pnt.z)), 0.000001)) ERR(4, "Bad Z0");
 235     if (!tst(lat, asin(pnt.sinLat()), 0.0000001)) ERR(4, "Bad WGS84 Sin");
 236
 237     pnt = wgs84(lat,0,10000);
 238     // std::cout << " " << lat*1800/M_PIl;
 239     // std::cout << " " << pnt.x;
 240     // std::cout << " " << pnt.y;
 241     // std::cout << " " << pnt.z;
 242     // std::cout << " " << TanZCrossing(pnt.z);
 243     // std::cout << " " << tan(lat);
 244     // std::cout << " " << (find2dX(pnt.z) - find2dX(pnt.z+0.1))/0.1;
 245     // std::cout << " " << pnt.x / (pnt.z - TanZCrossing(pnt.z));
 246     // std::cout << " " << pnt.z;
 247     // std::cout << " " << WGS84EarthCenter(pnt.z);
 248     // std::cout << std::endl;
 249     if (!tst(lat, asin(pnt.sinLat()), 0.00001)) ERR(4, "Bad WGS84 Sin");
 250
 251     // if (lat > wgs84Const::maxNorth-0.2) continue;  // Skip near polls TanZCrossing is better!
 252     // if (lat < wgs84Const::maxSouth+0.2) continue;  // Skip near polls TanZCrossing is better!
 253
 254     // if (!tst(pnt.x, tan(lat)*(pnt.z - TanZCrossing(pnt.z)), 0.1)) ERR(4, "Bad Z0");
 255     }
 256
 257   return 0;
 258 }
 259
 260 int qtst()
 261 {
 262   quat q;
 263
 264   q = quat(0, vec(0,0,0)).versor();
 265   if (!tst(q.a, 0)) ERR(5, "Bad Quat Versor Init");
 266   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
 267   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Versor Init");
 268   if (!tst(q.v.z, 1)) ERR(5, "Bad Quat Versor Init");
 269
 270   q = quat(3, vec(4,5,6)).conj();
 271   if (!tst(q.a, 3)) ERR(5, "Bad Quat Conj Angle");
 272   if (!tst(q.v.x, -4)) ERR(5, "Bad Quat Conj X");
 273   if (!tst(q.v.y, -5)) ERR(5, "Bad Quat Conj Y");
 274   if (!tst(q.v.z, -6)) ERR(5, "Bad Quat Conj Z");
 275   if (!tst(q.mag(), sqrt(86.0))) ERR(5, "Bad Quat Magnitude");
 276   if (!tst(q.versor().mag(), 1)) ERR(5, "Bad Quat Versor");
 277
 278   q = quat(0, zAxis) * quat(0, zAxis);
 279   if (!tst(q.a, -1)) ERR(5, "Bad Quat Angle");
 280   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat X");
 281   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Y");
 282   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Z");
 283
 284   q = quat(0, zAxis) * quat(0, vec(1,0,0));
 285   if (!tst(q.a, 0)) ERR(5, "Bad Quat Angle");
 286   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat X");
 287   if (!tst(q.v.y, 1)) ERR(5, "Bad Quat Y");
 288   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Z");
 289
 290   // q = quat(cos(M_PI_2l/2), zAxis) * quat(cos(M_PI_2l/2), zAxis);
 291   // q = quat(cos(M_PI_2l/2), zAxis * sin(M_PI_2l/2));
 292
 293   q = quat(cos(M_PI_4l/2), zAxis).normVec();
 294   q = q * q;
 295
 296   if (!tst(q.a, cos(M_PI_4l))) ERR(5, "Bad Quat Hamilton Multiple Angle");
 297   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Mult X");
 298   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Mult Y");
 299   if (!tst(q.v.norm().z, 1)) ERR(5, "Bad Quat Z");
 300
 301   q = quat(cos(M_PI_4l/4), zAxis).normVec() * quat(cos(M_PI_4l/2), zAxis).normVec();
 302   if (!tst(q.a, cos(M_PI_4l * 3/4))) ERR(5, "Bad Quat Hamilton Multiple Angle");
 303   if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Mult X");
 304   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Mult Y");
 305   if (!tst(q.v.norm().z, 1)) ERR(5, "Bad Quat Z");
 306
 307   q = quat(cos(M_PI_2l), zAxis).normVec() * quat(cos(M_PI_2l), vec(0,1,0)).normVec();
 308   if (!tst(q.a, cos(M_PI_2l))) ERR(5, "Bad Quat Hamilton Multiple Angle");
 309   if (!tst(q.v.x, -1)) ERR(5, "Bad Quat Mult X");
 310   if (!tst(q.v.y, 0)) ERR(5, "Bad Quat Mult Y");
 311   if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Mult Z");
 312
 313   q = quat(cos(M_PI_4l), zAxis).normVec() * quat(cos(M_PI_4l), vec(0,1,0)).normVec();
 314   if (!tst(q.a, cos(M_PIl/3))) ERR(5, "Bad Quat Hamilton Multiple Angle");
 315   if (!tst(q.v.x, -0.5)) ERR(5, "Bad Quat Mult X");
 316   if (!tst(q.v.y, 0.5)) ERR(5, "Bad Quat Mult Y");
 317   if (!tst(q.v.z, 0.5)) ERR(5, "Bad Quat Mult Z");
 318
 319   // std::cout << " " << q.a;
 320   // std::cout << " " << acos(q.a) * 180 / M_PIl;
 321   // std::cout << " " << q.v.x;
 322   // std::cout << " " << q.v.y;
 323   // std::cout << " " << q.v.z;
 324   // std::cout << std::endl;
 325
 326   return 0;
 327 }
 328
 329 int qvtst()
 330 {
 331   vec v;
 332   quat q(cos(M_PI_4l), zAxis);
 333   q = q.normVec();
 334
 335   v = q * vec(1,0,0);
 336   if (!tst(v.x, 0)) ERR(5, "Bad Quat rotate X");
 337   if (!tst(v.y, 1)) ERR(5, "Bad Quat rotate Y");
 338   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
 339
 340   v = q * vec(0,1,0);
 341   if (!tst(v.x, -1)) ERR(5, "Bad Quat rotate X");
 342   if (!tst(v.y, 0)) ERR(5, "Bad Quat rotate Y");
 343   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
 344
 345   v = q * vec(0,0,1);
 346   if (!tst(v.x, 0)) ERR(5, "Bad Quat rotate X");
 347   if (!tst(v.y, 0)) ERR(5, "Bad Quat rotate Y");
 348   if (!tst(v.z, 1)) ERR(5, "Bad Quat rotate Z");
 349
 350   v = q * vec(1,1,0);
 351   if (!tst(v.x, -1)) ERR(5, "Bad Quat rotate X");
 352   if (!tst(v.y, 1)) ERR(5, "Bad Quat rotate Y");
 353   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
 354
 355   q = quat(cos(M_PI_4l/2), zAxis).normVec();
 356   v = q * vec(1,0,0);
 357   if (!tst(v.x, sqrt(0.5))) ERR(5, "Bad Quat rotate X");
 358   if (!tst(v.y, sqrt(0.5))) ERR(5, "Bad Quat rotate Y");
 359   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
 360
 361   q = quat(cos(30.0 * M_PI_2l/180), zAxis).normVec();
 362   v = q * vec(5,0,0);
 363   //if (!tst(v.x, sqrt(0.5))) ERR(5, "Bad Quat rotate X");
 364   if (!tst(v.y, 2.5)) ERR(5, "Bad Quat rotate Y");
 365   if (!tst(v.z, 0)) ERR(5, "Bad Quat rotate Z");
 366
 367   /*
 368   std::cout << " " << v.x;
 369   std::cout << " " << v.y;
 370   std::cout << " " << v.z;
 371   std::cout << std::endl;
 372   */
 373
 374   return 0;
 375 }
 376
 377 int quaternionAroundWorldTest()
 378 {
 379   wgs84 start(42.89, 85.56), p; // 42.89°N 85.56°W
 380   quat q = quat(cos(M_PI_2l/18000), zAxis).normVec();           // Around the world at this latitude.
 381
 382   p = start;
 383   for (int i=0; i<36000; ++i) p = q * p;
 384   if ((start - p).dist() > 0.1) ERR(6, "Around world on latitude Quaternion test");
 385
 386   q = quat(cos(M_PI_2l/18000), zAxis.cross(start)).normVec();   // Polar around the world
 387   p = start;
 388   for (int i=0; i<36000; ++i) p = q * p;
 389   if ((start - p).dist() > 0.25) ERR(6, "Polar around world Quaternion test");
 390
 391   q = quat(cos(M_PI_2l/18000), wgs84(0,0).cross(start)).normVec();      // Around world through N0, E0
 392   p = start;
 393   for (int i=0; i<36000; ++i) p = q * p;
 394   if ((start - p).dist() > 0.25) ERR(6, "Around world through N0, W0 Quaternion test");
 395
 396   /*
 397   std::cout << " " << p.x;
 398   std::cout << " " << p.y;
 399   std::cout << " " << p.z;
 400   std::cout << " " << (start - p).dist();
 401   std::cout << std::endl;
 402   */
 403
 404   return 0;
 405 }
 406
 407
 408 } // namespace ecef
 409
 410 int main()
 411 {
 412   int err=0;
 413
 414   std::cout.precision(14);
 415   std::cout << std::scientific;
 416
 417   std::cout << "starting ecef-wgs84_test..." << std::endl;
 418
 419   err |= ecef::tstLon();
 420   err |= ecef::tst1();
 421   err |= ecef::tst2();
 422   err |= ecef::tstCos();
 423   err |= ecef::tst3();
 424   err |= ecef::qtst();
 425   err |= ecef::qvtst();
 426   err |= ecef::quaternionAroundWorldTest();
 427
 428   std::cout << "finishing ecef-wgs84_test." << std::endl;
 429
 430   return err;
 431 }