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);
}
Example #2
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);
}
Example #3
0
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);
}
Example #7
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);
}
Example #9
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);
}
Example #10
0
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);
}