From: Ty Zoerner <ty@infinitedelta.com>
Date: Sun, 20 May 2018 21:34:09 +0000 (-0400)
Subject: Added quat(v1,v2) support.
X-Git-Url: http://www.infinitedelta.com/gitweb/?p=libecef.git;a=commitdiff_plain

Added quat(v1,v2) support.
---

diff --git a/include/ecef-quaternion.h b/include/ecef-quaternion.h
index d43ba7e..3ca9350 100644
--- a/include/ecef-quaternion.h
+++ b/include/ecef-quaternion.h
@@ -26,11 +26,12 @@ namespace ecef
 
 struct quat
   {
-  double a;
+  double a;	// cos(Angle / 2)
   vec v;
  
   quat() {a=0.0; v=zAxis;}
   quat(const double a, const vec v) {this->a=a; this->v=v;}
+  quat(const vec v1, const vec v2) {a=v1.dot(v2); v=zAxis; if (abs(a)<(1-toleranceLimit)) {a=sqrt((1+a)/2); v=v1.cross(v2);}}
 
   quat operator*(const quat q) const {return quat(a*q.a - v.dot(q.v), q.v*a + v*q.a + v.cross(q.v));}
   quat operator*(const double s) const {return quat(a*s, v*s);}
diff --git a/test/ecef-wgs84_test.cc b/test/ecef-wgs84_test.cc
index 4d19b88..cded5d3 100644
--- 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;