// std::cout << " " << q.v.y;
// std::cout << " " << q.v.z;
// std::cout << std::endl;
-
return 0;
}
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;
}
} // namespace ecef
-int main()
+int main(const int argc, const char *argv[])
{
int err=0;