AA_API void
aa_tf_qutr_mul( const double A[AA_RESTRICT 7], const double B[AA_RESTRICT 7],
double C[AA_RESTRICT 7] )
{
aa_tf_qv_chain( A+AA_TF_QUTR_Q, A+AA_TF_QUTR_V,
B+AA_TF_QUTR_Q, B+AA_TF_QUTR_V,
C+AA_TF_QUTR_Q, C+AA_TF_QUTR_V );
}
void SceneFramePrismatic::tf_rel( const double *q, double _E[7] )
{
(void)q;
double x[3];
double qo = q[this->config_index] + this->offset;
for( size_t i = 0; i < 3; i ++ ) {
x[i] = qo*this->axis[i];
}
// TODO: remove redundant operations
aa_tf_qv_chain( E + AA_TF_QUTR_Q, E+AA_TF_QUTR_V,
aa_tf_quat_ident, x,
_E + AA_TF_QUTR_Q, _E+AA_TF_QUTR_V );
}
void SceneFrameRevolute::tf_rel( const double *q, double _E[7] )
{
(void)q;
double h[4];
double a[3];
double qo = q[this->config_index] + this->offset;
for( size_t i = 0; i < 3; i ++ ) {
a[i] = qo * axis[i];
}
// TODO: call qln directly
aa_tf_rotvec2quat(a, h);
// TODO: remove redundant operation
aa_tf_qv_chain( E + AA_TF_QUTR_Q, E+AA_TF_QUTR_V,
h, aa_tf_vec_ident,
_E + AA_TF_QUTR_Q, _E+AA_TF_QUTR_V );
}
static void duqu() {
// random tf
aa_tf_tfmat_t T;
aa_tf_duqu_t H;
double E[7];
double S_ident[8] = AA_TF_DUQU_IDENT_INITIALIZER;
double Q_ident[4] = AA_TF_QUAT_IDENT_INITIALIZER;
double v_ident[3] = {0};
double p0[3];
rand_tf( E, H.data, T.data );
aa_vrand( 3, p0 );
{
double A[8], B[8];
aa_vrand(8,A);
aa_vrand(8,B);
// mul
{
double A_L[8*8], B_R[8*8];
double C[8], Cl[8], Cr[8];
aa_tf_duqu_mul(A,B,C);
aa_tf_duqu_matrix_l(A, A_L, 8);
cblas_dgemv( CblasColMajor, CblasNoTrans, 8, 8,
1.0, A_L, 8,
B, 1,
0, Cl, 1 );
aveq( "duqu-mul-L", 8, C, Cl, 1e-6 );
aa_tf_duqu_matrix_r(B, B_R, 8);
cblas_dgemv( CblasColMajor, CblasNoTrans, 8, 8,
1.0, B_R, 8,
A, 1,
0, Cr, 1 );
aveq( "duqu-mul-R", 8, C, Cr, 1e-6 );
}
// add / sub
{
double Ca[8], Cs[8], mB[8];
for( size_t i = 0; i < 8; i ++ ) mB[i] = -B[i];
aa_tf_duqu_add(A,B,Ca);
aa_tf_duqu_sub(A,mB,Cs);
aveq( "duqu-add-sub", 8, Ca, Cs, 1e-6 );
double Cra[4], Crs[4];
double Cda[4], Cds[4];
aa_tf_duqu_sub(A,B,Cs);
aa_tf_qadd(A+AA_TF_DUQU_REAL, B+AA_TF_DUQU_REAL,Cra);
aa_tf_qadd(A+AA_TF_DUQU_DUAL, B+AA_TF_DUQU_DUAL,Cda);
aa_tf_qsub(A+AA_TF_DUQU_REAL, B+AA_TF_DUQU_REAL,Crs);
aa_tf_qsub(A+AA_TF_DUQU_DUAL, B+AA_TF_DUQU_DUAL,Cds);
aveq( "duqu-qadd-real", 4, Cra, Ca+AA_TF_DUQU_REAL, 1e-6);
aveq( "duqu-qadd-dual", 4, Cda, Ca+AA_TF_DUQU_DUAL, 1e-6);
aveq( "duqu-qsub-real", 4, Crs, Cs+AA_TF_DUQU_REAL, 1e-6);
aveq( "duqu-qsub-dual", 4, Cds, Cs+AA_TF_DUQU_DUAL, 1e-6);
}
}
//double q[4], v[3], p0[3];
//aa_vrand( 3, v );
//aa_tf_qurand( q );
//AA_MEM_SET( v, 0, 3 );
// tfmat
//aa_tf_quat2rotmat(q, T.R);
//AA_MEM_CPY( &T.t.x, v, 3 );
// dual quat
//aa_tf_qv2duqu( q, v, H.data );
//aa_tf_qv2duqu( aa_tf_quat_ident, v, H_tran.data );
// check trans
double hv[3];
aa_tf_duqu_trans(H.data, hv);
aveq("duqu-trans", 3, T.v.data, hv, .001 );
//double nreal,ndual;
//aa_tf_duqu_norm( H.data, &nreal, &ndual );
//printf("norm: %f + %f \\epsilon \n", nreal, ndual );
// transform points
double p1H[3], p1qv[3], p1T[3];
aa_tf_12( T.data, p0, p1T );
aa_tf_tf_qv( H.real.data, T.v.data, p0, p1qv );
aa_tf_tf_duqu( H.data, p0, p1H );
aveq( "tf-qv", 3, p1T, p1qv, .001 );
aveq( "tf-duqu", 3, p1T, p1H, .001 );
// conjugate
{
double S_conj[8];
double qv_conj[7], E_conj[7];
double SSc[8], EEc[7];
double Scv[3];
aa_tf_duqu_conj(H.data, S_conj);
aa_tf_qv_conj(H.real.data, T.v.data, qv_conj, qv_conj+4);
aa_tf_qutr_conj(E, E_conj);
aa_tf_duqu_trans(S_conj, Scv);
aveq( "duqu/qutr conj q", 4, S_conj, E_conj, 1e-6 );
aveq( "duqu/qv conj q", 4, S_conj, qv_conj, 1e-6 );
aveq( "duqu/qutr conj v", 3, Scv, E_conj+4, 1e-6 );
aveq( "duqu/qv conj v", 3, Scv, qv_conj+4, 1e-6 );
aa_tf_duqu_mul( H.data, S_conj, SSc );
aa_tf_qv_chain( H.real.data, T.v.data, qv_conj, qv_conj+4, EEc, EEc+4 );
aveq( "duqu conj", 8, SSc, S_ident, 1e-6 );
aveq( "qv conj q", 4, EEc, Q_ident, 1e-6 );
aveq( "qv conj v", 3, EEc+4, v_ident, 1e-6 );
}
// derivative
{
double dx[6], dd[8], dq[4];
aa_vrand(6, dx);
double dt = aa_frand() / 100;
aa_tf_duqu_vel2diff( H.data, dx, dd );
aa_tf_qvel2diff( H.real.data, dx+3, dq );
// back to velocity
double dx1[6];
aa_tf_duqu_diff2vel( H.data, dd, dx1 );
aveq( "duqu-vel invert", 6, dx, dx1, .001 );
// integrate
double H1[8], q1[4], v1[3], H1qv[8];
double H1_sdd[8], H1_sdx[8];
for( size_t i = 0; i < 8; i ++ ) H1[i] = H.data[i] + dd[i]*dt; // some numerical error here...
for( size_t i = 0; i < 3; i ++ ) v1[i] = T.v.data[i] + dx[i]*dt;
aa_tf_duqu_normalize( H1 );
aa_tf_qsvel( H.real.data, dx+3, dt, q1 );
aa_tf_qv2duqu( q1, v1, H1qv );
aveq( "duqu-vel_real", 4, dq, dd, .001 );
aveq( "duqu-vel-int real", 4, H1, H1qv, .001 );
aveq( "duqu-vel-int dual", 4, H1+4, H1qv+4, .001 );
aa_tf_duqu_svel( H.data, dx, dt, H1_sdx );
aa_tf_duqu_sdiff( H.data, dd, dt, H1_sdd );
aveq( "duqu-int vel", 8, H1qv, H1_sdx, .001 );
aveq( "duqu-int diff", 8, H1_sdx, H1_sdd, .0001 );
/* // twist */
double tw[8], dxtw[6];
aa_tf_duqu_vel2twist(H.data, dx, tw );
aa_tf_duqu_twist2vel(H.data, tw, dxtw );
aveq( "duqu twist<->vel", 6, dx, dxtw, 1e-6 );
}
// exponential
{
double expd[8], lnexpd[8];
aa_tf_duqu_exp(H.data, expd );
aa_tf_duqu_ln( expd, lnexpd );
aveq( "duqu-exp-ln", 8, H.data, lnexpd, .001 );
aa_tf_duqu_ln( H.data, lnexpd );
aa_tf_duqu_exp(lnexpd, expd );
aveq( "duqu-ln-exp", 8, H.data, expd, .001 );
}
// Logarithm
{
double HI[8], HIln[8], dxi[6], dx0[6] = {0};
aa_tf_duqu_mulc( H.data, H.data, HI );
aa_tf_duqu_ln(HI, HIln);
aa_tf_duqu_twist2vel(HI, HIln, dxi );
aveq( "duqu ln 0 near", 6, dx0, dxi, .0001 );
aa_tf_duqu_ln(aa_tf_duqu_ident, HIln);
aa_tf_duqu_twist2vel(HI, HIln, dxi );
aveq( "duqu ln 0 exact", 6, dx0, dxi, 0.0 );
}
// Pure translation
{
double S[8], v[3], v1[3];
aa_vrand(3,v);
aa_tf_xyz2duqu( v[0], v[1], v[2], S );
aa_tf_duqu_trans(S, v1);
aveq( "duqu trans orientation", 4, S, aa_tf_quat_ident, 0.0 );
aveq( "duqu trans translation", 3, v, v1, 1e-6 );
}
}
