Added quat(v1,v2) support.

[libecef.git] / test / ecef-wgs84_test.cc

diff --git a/test/ecef-wgs84_test.cc b/test/ecef-wgs84_test.cc
index 4d19b88..cded5d3 100644 (file)
--- a/test/ecef-wgs84_test.cc
+++ b/test/ecef-wgs84_test.cc
@@ -344,7 +344,6 @@ int qtst()
   // std::cout << " " << q.v.y;
   // std::cout << " " << q.v.z;
   // std::cout << std::endl;
-
   return 0;
 }
 
@@ -393,6 +392,77 @@ int qvtst()
   std::cout << std::endl;
   */
 
+  q = quat(zAxis, vec(0,0,1));
+  if (!tst(q.a, 1)) 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(zAxis, vec(1,0,0));
+  if (!tst(q.a, cos(M_PI_4l))) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.y, 1)) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Versor Init");
+
+  q = quat(zAxis, vec(0.5,0,0));
+  if (!tst(q.a, cos(M_PI_4l))) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.y, 0.5)) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Versor Init");
+
+  q = quat(zAxis, vec(1,0,1).norm());
+  if (!tst(q.a, cos(M_PI_4l/2))) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.y, sqrt(2)/2)) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Versor Init");
+
+  q = quat(zAxis, vec(1,0,1).norm()).normVec();
+  if (!tst(q.a, cos(M_PI_4l/2))) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.x, 0)) ERR(5, "Bad Quat Versor Init");
+  // if (!tst(q.v.y, )) ERR(5, "Bad Quat Versor Init");
+  if (!tst(q.v.z, 0)) ERR(5, "Bad Quat Versor Init");
+
+  v = q * zAxis;
+  if (!tst(v.x, sqrt(2)/2)) ERR(5, "Bad Quat Versor Init");
+  if (!tst(v.y, 0)) ERR(5, "Bad Quat Versor Init");
+  if (!tst(v.z, sqrt(2)/2)) ERR(5, "Bad Quat Versor Init");
+
+  // std::cout << " " << q.a;
+  // std::cout << " " << q.v.x;
+  // std::cout << " " << q.v.y;
+  // std::cout << " " << q.v.z;
+  // std::cout << ": " << v.x;
+  //  std::cout << " " << v.y;
+  // std::cout << " " << v.z;
+  // std::cout << std::endl;
+
+  // Verify all that creating a Quaternion from two vectors work in all quadrants.
+
+  for (int x1=-5; x1<5; ++x1) for (int y1=-5; y1<5; ++y1) for (int z1=-5; z1<5; ++z1)
+    for (int x2=-5; x2<5; ++x2) for (int y2=-5; y2<5; ++y2) for (int z2=-5; z2<5; ++z2)
+    {
+    vec v1 = vec(x1, y1, z1).norm(), v2 = vec(x2, y2, z2).norm();
+    q = quat(v1, v2).normVec();
+    if (abs(q.a) >= (1 - toleranceLimit)) continue;
+
+    v = q * v1;
+    // std::cout << " " << v1.dot(v2);
+    // std::cout << " " << q.a;
+    // std::cout << ", " << v1.x;
+    // std::cout << " " << v1.y;
+    // std::cout << " " << v1.z;
+    // std::cout << ", " << v2.x;
+    // std::cout << " " << v2.y;
+    // std::cout << " " << v2.z;
+    // std::cout << ", " << v.x;
+    // std::cout << " " << v.y;
+    // std::cout << " " << v.z;
+    // std::cout << std::endl;
+    if (!tst(v.x, v2.x)) ERR(5, "Quaderant quaternion test");
+    if (!tst(v.y, v2.y)) ERR(5, "Quaderant quaternion test");
+    if (!tst(v.z, v2.z)) ERR(5, "Quaderant quaternion test");
+    }
+
   return 0;
 }
 
@@ -429,7 +499,7 @@ int quaternionAroundWorldTest()
 
 } // namespace ecef
 
-int main()
+int main(const int argc, const char *argv[])
 {
   int err=0;