void test_basic_quaternion_multiply(void) {
Quaternion q = { 1, 0, 0, 0 };
Quaternion r = { 1, 0, 0, 0 };
Quaternion t;
Vector3F eulers;
QuaternionMultiply(&q, &r, &t);
QuaternionPrint(&t);
assert(t.a == 1);
assert(t.b == 0);
assert(t.c == 0);
assert(t.d == 0);
q.a = 0.707107;
q.b = 0.707107;
QuaternionMultiply(&q, &r, &t);
QuaternionPrint(&t);
QuaternionToEulers(&t, &eulers);
assert(NearEqual(eulers.a, PI_DIV_2, 0.001));
assert(eulers.b == 0);
assert(eulers.c == 0);
QuaternionMultiply(&q, &q, &t);
QuaternionPrint(&t);
QuaternionToEulers(&t, &eulers);
assert(NearEqual(eulers.a, PI, 0.001));
assert(eulers.b == 0);
assert(eulers.c == 0);
}
bool NearEqual(
const Type &x ,
const std::complex<Type> &y ,
const Type &r ,
const Type & a )
{
return NearEqual(std::complex<Type>(x, Type(0)), y, r, a);
}
bool NearEqual(
const std::complex<Type> &x ,
const Type &y ,
const Type &r ,
const Type & a )
{
return NearEqual(x, std::complex<Type>(y, Type(0)), r, a);
}
void test_basic_quaternion_from_eulers(void) {
Quaternion q = { 0, 0, 0, 0 };
Vector3F eulers = { 0, 0, 0 };
QuaternionFromEulers(&eulers, &q);
QuaternionPrint(&q);
Vector4F res = { 1, 0, 0, 0 };
assert(Vector4FEqual(&res, &q));
eulers.a = PI;
QuaternionFromEulers(&eulers, &q);
QuaternionPrint(&q);
assert(NearEqual(q.a, 0, .0001));
assert(NearEqual(q.b, 1, .0001));
assert(NearEqual(q.c, 0, .0001));
assert(NearEqual(q.d, 0, .0001));
eulers.a = PI_DIV_2;
QuaternionFromEulers(&eulers, &q);
QuaternionPrint(&q);
assert(NearEqual(q.a, 0.70, 0.01));
assert(NearEqual(q.b, 0.70, 0.01));
assert(q.c == 0);
assert(q.d == 0);
eulers.a = 0;
eulers.b = PI_DIV_2;
QuaternionFromEulers(&eulers, &q);
QuaternionPrint(&q);
assert(NearEqual(q.a, 0.70, 0.01));
assert(q.b == 0);
assert(NearEqual(q.c, 0.70, 0.01));
assert(q.d == 0);
eulers.b = 0;
eulers.c = PI_DIV_2;
QuaternionFromEulers(&eulers, &q);
QuaternionPrint(&q);
assert(NearEqual(q.a, 0.70, 0.01));
assert(q.b == 0);
assert(q.c == 0);
assert(NearEqual(q.d, 0.70, 0.01));
}
void test_basic_quaternion_to_eulers(void) {
Vector3F eulers;
Quaternion q = { 1, 0, 0, 0 };
QuaternionToEulers(&q, &eulers);
Vector3fPrint(&eulers);
assert(eulers.a == 0);
assert(eulers.b == 0);
assert(eulers.c == 0);
q.a = -0.0;
q.b = 1.0;
QuaternionToEulers(&q, &eulers);
Vector3fPrint(&eulers);
assert(NearEqual(eulers.a, PI, 0.001));
assert(eulers.b == 0);
assert(eulers.c == 0);
q.a = 0.707141;
q.b = 0.7073;
QuaternionToEulers(&q, &eulers);
Vector3fPrint(&eulers);
assert(NearEqual(eulers.a, PI_DIV_2, 0.001));
assert(eulers.b == 0);
assert(eulers.c == 0);
q.c = q.b;
q.b = 0;
QuaternionToEulers(&q, &eulers);
Vector3fPrint(&eulers);
assert(eulers.a == 0);
assert(NearEqual(eulers.b, PI_DIV_2, 0.001));
assert(eulers.c == 0);
q.d = q.c;
q.c = 0;
QuaternionToEulers(&q, &eulers);
Vector3fPrint(&eulers);
assert(eulers.a == 0);
assert(eulers.b == 0);
assert(NearEqual(eulers.c, PI_DIV_2, 0.001));
}
void test_quaternion_lerp(void) {
Quaternion a, b, c;
QuaternionZero(&a);
Vector3F eulers = {.1, 0, 0};
QuaternionFromEulers(&eulers, &b);
QuaternionLerp(&a, &b, .2, &c);
QuaternionToEulers(&c, &eulers);
Vector3fPrint(&eulers);
assert(NearEqual(eulers.a, 0.02, 0.001));
assert(eulers.b == 0);
assert(eulers.c == 0);
}
bool FunCheck(
ADFun<Base> &f ,
Fun &g ,
const Vector &x ,
const Base &r ,
const Base &a )
{ bool ok = true;
size_t m = f.Range();
Vector yf = f.Forward(0, x);
Vector yg = g(x);
size_t i;
for(i = 0; i < m; i++)
ok &= NearEqual(yf[i], yg[i], r, a);
return ok;
}
void test_quaternion_difference(void) {
Quaternion a, b, c;
Vector3F eulers;
eulers.a = 1;
eulers.b = 0;
eulers.c = 0;
QuaternionFromEulers(&eulers, &a);
eulers.a = .5;
eulers.b = 0;
eulers.c = 0;
QuaternionFromEulers(&eulers, &b);
QuaternionDifference(&a, &b, &c);
QuaternionToEulers(&c, &eulers);
Vector3fPrint(&eulers);
assert(NearEqual(eulers.a, 0.5, 0.001));
assert(eulers.b == 0);
assert(eulers.c == 0);
}
CPPAD_INLINE_FRIEND_TEMPLATE_FUNCTION
bool NearEqual(const Base &x, const VecAD_reference<Base> &y,
const Base &r, const Base &a)
{ return NearEqual(x, y.ADBase(), r, a);
}
CPPAD_INLINE_FRIEND_TEMPLATE_FUNCTION
bool NearEqual(
const AD<Base> &x, const Base &y, const Base &r, const Base &a)
{ return NearEqual(x.value_, y, r, a);
}
