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));
}
