int __EXPORT quaternionTest() { printf("Test Quaternion\t\t: "); // test default ctor Quaternion q; ASSERT(equal(q.getA(), 1.0f)); ASSERT(equal(q.getB(), 0.0f)); ASSERT(equal(q.getC(), 0.0f)); ASSERT(equal(q.getD(), 0.0f)); // test float ctor q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); ASSERT(equal(q.getA(), 0.1825742f)); ASSERT(equal(q.getB(), 0.3651484f)); ASSERT(equal(q.getC(), 0.5477226f)); ASSERT(equal(q.getD(), 0.7302967f)); // test euler ctor q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); // test dcm ctor q = Quaternion(Dcm()); ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); // TODO test derivative // test accessors q.setA(0.1f); q.setB(0.2f); q.setC(0.3f); q.setD(0.4f); ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); printf("PASS\n"); return 0; }
int __EXPORT eulerAnglesTest() { printf("Test EulerAngles\t: "); EulerAngles euler(0.1f, 0.2f, 0.3f); // test ctor ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); ASSERT(equal(euler.getPhi(), 0.1f)); ASSERT(equal(euler.getTheta(), 0.2f)); ASSERT(equal(euler.getPsi(), 0.3f)); // test dcm ctor euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); // test quat ctor euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); // test assignment euler.setPhi(0.4f); euler.setTheta(0.5f); euler.setPsi(0.6f); ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); printf("PASS\n"); return 0; }
int __EXPORT quaternionTest() { printf("Test Quaternion\t\t: "); // test default ctor Quaternion q; ASSERT(equal(q.getA(), 1)); ASSERT(equal(q.getB(), 0)); ASSERT(equal(q.getC(), 0)); ASSERT(equal(q.getD(), 0)); // test float ctor q = Quaternion(0, 1, 0, 0); ASSERT(equal(q.getA(), 0)); ASSERT(equal(q.getB(), 1)); ASSERT(equal(q.getC(), 0)); ASSERT(equal(q.getD(), 0)); // test euler ctor q = Quaternion(EulerAngles(0, 0, 0)); ASSERT(equal(q.getA(), 1)); ASSERT(equal(q.getB(), 0)); ASSERT(equal(q.getC(), 0)); ASSERT(equal(q.getD(), 0)); // test dcm ctor q = Quaternion(Dcm()); ASSERT(equal(q.getA(), 1)); ASSERT(equal(q.getB(), 0)); ASSERT(equal(q.getC(), 0)); ASSERT(equal(q.getD(), 0)); // TODO test derivative // test accessors q.setA(0.1); q.setB(0.2); q.setC(0.3); q.setD(0.4); ASSERT(equal(q.getA(), 0.1)); ASSERT(equal(q.getB(), 0.2)); ASSERT(equal(q.getC(), 0.3)); ASSERT(equal(q.getD(), 0.4)); printf("PASS\n"); return 0; }
EulerAngles::EulerAngles(const Quaternion &q) : Vector(3) { (*this) = EulerAngles(Dcm(q)); }