From 0df566e64545347a4a084fe32659920bf9b63333 Mon Sep 17 00:00:00 2001 From: Ty Zoerner Date: Sat, 13 May 2017 12:39:53 -0400 Subject: [PATCH] Added quaternion around world tests for accuracy and speed. --- include/ecef-quaternion.h | 3 +- test/ecef-wgs84_test.cc | 68 ++++++++++++++++++++++++++++----------- 2 files changed, 51 insertions(+), 20 deletions(-) diff --git a/include/ecef-quaternion.h b/include/ecef-quaternion.h index 2a6ebc1..d43ba7e 100644 --- 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 --- 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; -- 2.20.1