void qmul() {
double a[4], b[4], c1[4], c2[4];
double va[4], vb[4];
aa_vrand( 4, a );
aa_vrand( 4, b );
aa_tf_qnormalize(a);
aa_tf_qnormalize(b);
aa_vec_d4_st( va, aa_vec_d4_ld(a) );
aa_vec_d4_st( vb, aa_vec_d4_ld(b) );
aveq( "lda", 4, a, va, .000 );
aveq( "ldb", 4, b, vb, .000 );
aa_tf_qmul(a,b,c1);
aa_vecm_qmul(a,b,c2);
aveq( "quat-equal", 4, c1, c2, .0001 );
aa_tick("qmul non-vec: ");
for( size_t i = 0; i < N; i ++ ) {
aa_tf_qmul(a,b,c1);
}
aa_tock();
aa_tick("qmul vec: ");
for( size_t i = 0; i < N; i ++ ) {
aa_vecm_qmul(a,b,c1);
}
aa_tock();
}
void qrot() {
double a[4], b[4], c1[4], c2[4];
aa_vrand( 4, a );
aa_vrand( 4, b );
aa_tf_qnormalize(a);
aa_tf_qnormalize(b);
aa_tf_qrot(a,b,c1);
aa_vecm_qrot(a,b,c2);
aveq( "qrot-equal", 3, c1, c2, .0001 );
aa_tick("qrot non-vec: ");
for( size_t i = 0; i < N; i ++ ) {
aa_tf_qrot(a,b,c1);
}
aa_tock();
aa_tick("qrot vec: ");
for( size_t i = 0; i < N; i ++ ) {
aa_vecm_qrot(a,b,c1);
}
aa_tock();
}
void duqumul() {
double q0[4], q1[4];
double v0[3], v1[3];
double d0[8], d1[8];
double ra[8], rb[8];
aa_vrand( 4, q0 );
aa_vrand( 4, q1 );
aa_tf_qnormalize(q0);
aa_tf_qnormalize(q1);
aa_vrand( 3, v0 );
aa_vrand( 3, v1 );
aa_tf_qv2duqu( q0, v0, d0 );
aa_tf_qv2duqu( q1, v1, d1 );
aa_tf_duqu_mul(q0,q1,ra);
aa_vecm_duqu_mul(q0,q1,rb);
aveq( "duqu_mul-equal", 8, ra, rb, .0001 );
aa_tick("duqu-mul non-vec: ");
for( size_t i = 0; i < N; i ++ ) {
aa_tf_duqu_mul(d0,d1,ra);
}
aa_tock();
aa_tick("duqu-mul vec: ");
for( size_t i = 0; i < N; i ++ ) {
aa_vecm_duqu_mul(q0,q1,rb);
}
aa_tock();
}
void tfmul() {
double q0[4], q1[4];
double T0[12], T1[12];
double Ua[12], Ub[12];
aa_vrand( 4, q0 );
aa_vrand( 4, q1 );
aa_tf_qnormalize(q0);
aa_tf_qnormalize(q1);
aa_tf_quat2rotmat( q0, T0 );
aa_tf_quat2rotmat( q1, T1 );
aa_vrand( 3, T0+9 );
aa_vrand( 3, T1+9 );
// cross
aa_tf_12chain(T0,T1,Ua);
aa_vecm_tfmul(T0,T1,Ub);
aveq( "tfmul-equal", 12, Ua, Ub, .0001 );
aa_tick("tfmul non-vec: ");
for( size_t i = 0; i < N; i ++ ) {
aa_tf_12chain(T0,T1,Ua);
}
aa_tock();
aa_tick("tfmul vec: ");
for( size_t i = 0; i < N; i ++ ) {
aa_vecm_tfmul(T0,T1,Ub);
}
aa_tock();
}
void rel_q() {
// random transforms
double q0[4], qrel[4], q1[4];
aa_vrand(4,q0);
aa_vrand(4,q1);
aa_tf_qnormalize(q0);
aa_tf_qnormalize(q1);
aa_tf_qcmul(q0, q1, qrel );
// random velocity, point 0
double dx0[3]={0}, dq0[4];
aa_vrand(3,dx0);
aa_tf_qvel2diff( q0, dx0, dq0 );
// computed velocity, point 1
double dq1[4], dx1[3];
aa_tf_qmul( dq0, qrel, dq1 );
aa_tf_qdiff2vel( q1, dq1, dx1 );
// integrate both velocities
double q0_1[4], q1_1[4];
aa_tf_qsvel( q0, dx0, .1, q0_1 );
aa_tf_qsvel( q1, dx1, .1, q1_1 );
// new relative orientation
double qrel_1[4];
aa_tf_qcmul( q0_1, q1_1, qrel_1 );
// check
aveq("relq", 4, qrel, qrel_1, .000001);
}
void rel_d() {
// random transforms
double q0[4], v0[3], q1[4], v1[4];
aa_vrand(4,q0);
aa_vrand(4,q1);
aa_tf_qnormalize(q0);
aa_tf_qnormalize(q1);
aa_vrand(3,v0);
aa_vrand(3,v1);
// dual quat transforms
double d0[8], drel[8], d1[8], d1p[8];
aa_tf_qv2duqu(q0,v0, d0);
aa_tf_qv2duqu(q1,v1, d1);
// d0 * drel = d1
// drel = conj(d0) * d1
aa_tf_duqu_cmul( d0, d1, drel );
aa_tf_duqu_mul( d0, drel, d1p );
aveq("duqu-relmul", 8, d1, d1p, .001 );
// random velocity
double dx0[6], dd0[8];
aa_vrand(6,dx0);
aa_tf_duqu_vel2diff(d0, dx0, dd0);
// second velocity
// d1 = d0*drel
// d1/dt = d0/dt * drel + d0 * drel/dt, and drel/dt = 0
double dd1[8];
aa_tf_duqu_mul( dd0, drel, dd1 );
// integrate
double d0_1[8], d1_1[8];
double dt = .1;
aa_tf_duqu_sdiff( d0, dd0, dt, d0_1 );
aa_tf_duqu_sdiff( d1, dd1, dt, d1_1 );
aa_tf_duqu_normalize( d0_1 );
aa_tf_duqu_normalize( d1_1 );
// new relative
double drel_1[8];
// drel = d0*inv(d1)
aa_tf_duqu_cmul( d0_1, d1_1, drel_1 );
// twist
double d0_1t[8], d1_1t[8], drel_1t[8];
aa_tf_duqu_svel( d0, dx0, dt, d0_1t );
aa_tf_duqu_sdiff( d1, dd1, dt, d1_1t );
aa_tf_duqu_cmul( d0_1t, d1_1t, drel_1t );
// check
aveq("rel_d", 8, drel, drel_1t, 1e-6);
}
int aa_tf_qnormalize__( double q[4] )
{
aa_tf_qnormalize(q);
return 0;
}
