Added quaternion around world tests for accuracy and speed.

diff --git a/include/ecef-quaternion.h b/include/ecef-quaternion.h
index 2a6ebc1..d43ba7e 100644 (file)
--- a/include/ecef-quaternion.h
+++ b/include/ecef-quaternion.h
@@ -42,8 +42,7 @@ struct quat
   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
+  vec operator*(const vec v2) const {return (*this * quat(0,v2) * conj()).v;}                  // Rotate vector by quaternion (32nS on 2.1 GHz Intel E6420)
   };
 
 }

diff --git a/test/ecef-wgs84_test.cc b/test/ecef-wgs84_test.cc
index d96d56a..2bd9bb8 100644 (file)
--- a/test/ecef-wgs84_test.cc
+++ b/test/ecef-wgs84_test.cc
@@ -22,11 +22,11 @@
 
 namespace ecef
 {
-static bool tst(const double i, const double j=0, const double t=0.0000001) 
+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;}
 
@@ -52,13 +52,13 @@ int tst1()
 
   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)) 
+  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)) 
+  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)) 
+  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;
@@ -82,7 +82,7 @@ int tst1()
 
   for (double lon=wgs84Const::maxEast; lon > wgs84Const::maxWest; lon -= LL_INC)
     {
-    if (!tst(cent.dist(wgs84(0.0, lon)), 6378137.0)) 
+    if (!tst(cent.dist(wgs84(0.0, lon)), 6378137.0))
       {
       std::cout << lon << ": " << cent.dist(wgs84(0.0, lon)) - 6378137.0 << std::endl;
 
@@ -100,7 +100,7 @@ int tst2()
   // 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;
@@ -149,7 +149,7 @@ int tst2()
       vec p2(p1);
       p2.y = 0;
       line l6(count, line(p1, p2));
-      if (l6.p1 != p1) 
+      if (l6.p1 != p1)
        {
        std::cout << p1.x << std::endl;
        std::cout << p1.y << std::endl;
@@ -223,7 +223,7 @@ 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 << " " << lat*1800/M_PIl;
     // std::cout << pnt.x - FindX2D(pnt.z);
     // std::cout << " " << Slope2D(pnt.z);
     // std::cout << " " << tan(lat);
@@ -235,16 +235,16 @@ int tst3()
     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 << " " << 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 << " " << WGS84EarthCenter(pnt.z);
     // std::cout << std::endl;
     if (!tst(lat, asin(pnt.sinLat()), 0.00001)) ERR(4, "Bad WGS84 Sin");
 
@@ -374,6 +374,37 @@ int qvtst()
   return 0;
 }
 
+int quaternionAroundWorldTest()
+{
+  wgs84 start(42.89, 85.56), p;        // 42.89°N 85.56°W
+  quat q = quat(cos(M_PI_2l/18000), zAxis).normVec();          // Around the world at this latitude.
+
+  p = start;
+  for (int i=0; i<36000; ++i) p = q * p;
+  if ((start - p).dist() > 0.1) ERR(6, "Around world on latitude Quaternion test");
+
+  q = quat(cos(M_PI_2l/18000), zAxis.cross(start)).normVec();  // Polar around the world
+  p = start;
+  for (int i=0; i<36000; ++i) p = q * p;
+  if ((start - p).dist() > 0.25) ERR(6, "Polar around world Quaternion test");
+
+  q = quat(cos(M_PI_2l/18000), wgs84(0,0).cross(start)).normVec();     // Around world through N0, E0
+  p = start;
+  for (int i=0; i<36000; ++i) p = q * p;
+  if ((start - p).dist() > 0.25) ERR(6, "Around world through N0, W0 Quaternion test");
+
+  /*
+  std::cout << " " << p.x;
+  std::cout << " " << p.y;
+  std::cout << " " << p.z;
+  std::cout << " " << (start - p).dist();
+  std::cout << std::endl;
+  */
+
+  return 0;
+}
+
+
 } // namespace ecef
 
 int main()
@@ -392,6 +423,7 @@ int main()
   err |= ecef::tst3();
   err |= ecef::qtst();
   err |= ecef::qvtst();
+  err |= ecef::quaternionAroundWorldTest();
 
   std::cout << "finishing ecef-wgs84_test." << std::endl;