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();
}
AA_API void
aa_rx_sg_chain_jacobian( const struct aa_rx_sg *sg,
size_t n_tf, const double *TF_abs, size_t ld_TF,
size_t n_frames, aa_rx_frame_id *chain_frames,
size_t n_configs, aa_rx_frame_id *chain_configs,
double *J, size_t ld_J )
{
(void)chain_configs; /* Use these for multivariate frame support */
aa_rx_frame_id frame_ee = chain_frames[n_frames-1];
const double *pe = &TF_abs[(size_t)frame_ee * ld_TF] + AA_TF_QUTR_T;
for( size_t i_frame=0, i_config=0;
i_frame < n_frames && i_config < n_configs;
i_frame++ )
{
double *Jr = J + AA_TF_DX_W; // rotational part
double *Jt = J + AA_TF_DX_V; // translational part
aa_rx_frame_id frame = chain_frames[i_frame];
assert( frame >= 0 );
assert( (size_t)frame < n_tf );
enum aa_rx_frame_type ft = aa_rx_sg_frame_type(sg, frame);
switch(ft) {
case AA_RX_FRAME_FIXED:
break;
case AA_RX_FRAME_REVOLUTE:
case AA_RX_FRAME_PRISMATIC: {
const double *a = aa_rx_sg_frame_axis(sg, frame);
const double *E = TF_abs + (size_t)frame*ld_TF;
const double *q = E+AA_TF_QUTR_Q;
const double *t = E+AA_TF_QUTR_T;
switch(ft) {
case AA_RX_FRAME_REVOLUTE: {
aa_tf_qrot(q,a,Jr);
double tmp[3];
for( size_t j = 0; j < 3; j++ ) tmp[j] = pe[j] - t[j];
aa_tf_cross(Jr, tmp, Jt);
break;
}
case AA_RX_FRAME_PRISMATIC:
AA_MEM_ZERO(Jr, 3);
aa_tf_qrot(q,a,Jt);
break;
default: assert(0);
}
i_config++;
J += ld_J;
break;
} /* end joint */
} /* end switch */
} /* end for */
}
static void mzlook( const double eye[3],
const double target[3],
const double up[3] )
{
{
double R[9] = {0};
aa_tf_rotmat_mzlook(eye,target,up,R);
assert( aa_tf_isrotmat(R) );
}
double g_E_cam[7];
double cam_E_g[7];
aa_tf_qutr_mzlook(eye,target,up,g_E_cam);
aa_tf_qutr_conj(g_E_cam, cam_E_g);
{
double x[3];
aa_tf_qutr_tf(g_E_cam, aa_tf_vec_ident, x);
aveq("mzlook eye 0", 3, eye, x, 1e-6 );
aa_tf_qutr_tf(cam_E_g, eye, x);
aveq("mzlook eye 1", 3, aa_tf_vec_ident, x, 1e-6 );
}
{
double ell[3];
for(size_t i=0; i<3; i++) ell[i] = -(target[i] - eye[i]);
aa_tf_vnormalize(ell);
double z[3];
aa_tf_qrot(cam_E_g, ell, z);
aveq("mzlook rot", 3, z, aa_tf_vec_z, 1e-6 );
}
}
AA_API void
aa_tf_qv_conj( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
double qc[AA_RESTRICT 4], double vc[AA_RESTRICT 3] )
{
aa_tf_qconj( q, qc );
aa_tf_qrot( qc, v, vc );
FOR_VEC(i) vc[i] = -vc[i];
}
AA_API void
aa_tf_tf_qv( const double quat[AA_RESTRICT 4],
const double v[AA_RESTRICT 3],
const double p0[AA_RESTRICT 3],
double p1[AA_RESTRICT 4] )
{
aa_tf_qrot( quat, p0, p1 );
FOR_VEC(i) p1[i] += v[i];
}
static void quat(double E[2][7]) {
double u;
double *q1 = E[0];
double *q2 = E[0];
u = aa_frand();
{
double qg[4], qa[4];
aa_tf_qslerp( u, q1, q2, qg );
aa_tf_qslerpalg( u, q1, q2, qa );
aveq("slerp", 4, qg, qa, .001 );
double dqg[4], dqa[4];
aa_tf_qslerpdiff( u, q1, q2, dqg );
aa_tf_qslerpdiffalg( u, q1, q2, dqa );
aveq("slerpdiff", 4, dqg, dqa, .001 );
}
// mul
{
double Ql[16], Qr[16];
double y0[4], y1[4], y2[4];
aa_tf_qmatrix_l(q1, Ql, 4);
aa_tf_qmatrix_r(q2, Qr, 4);
aa_tf_qmul(q1,q2, y0);
cblas_dgemv( CblasColMajor, CblasNoTrans, 4, 4,
1.0, Ql, 4,
q2, 1,
0, y1, 1 );
cblas_dgemv( CblasColMajor, CblasNoTrans, 4, 4,
1.0, Qr, 4,
q1, 1,
0, y2, 1 );
aveq( "qmul-1", 4, y0, y1, 1e-6 );
aveq( "qmul-2", 4, y0, y2, 1e-6 );
}
// average
{
double qq[8], p[4], s[4];
AA_MEM_CPY( qq, q1, 4 );
AA_MEM_CPY( qq+4, q2, 4 );
double w[2] = {.5,.5};
aa_tf_quat_davenport( 2, w, qq, 4, p );
aa_tf_qslerp( .5, q1, q2, s );
aa_tf_qminimize( p );
aa_tf_qminimize( s );
aveq("davenport-2", 4, p, s, 1e-4 );
}
double R1[9], R2[9], Rr[9], qr[4], qrr[4];
aa_tf_quat2rotmat(q1, R1);
aa_tf_quat2rotmat(q2, R2);
aa_tf_9rel( R1, R2, Rr );
aa_tf_qrel( q1, q2, qr );
aa_tf_rotmat2quat( Rr, qrr );
aa_tf_qminimize( qr );
aa_tf_qminimize( qrr );
aveq("qrel", 4, qr, qrr, .001 );
// minimize
{
double qmin[4], axang[4];
aa_tf_qminimize2( q1, qmin );
test( "quat-minimize", aa_feq( fabs(q1[3]), qmin[3], 0) );
aa_tf_quat2axang( qmin, axang );
test( "quat-minimize-angle", fabs(axang[3]) <= M_PI );
}
// mulc
{
double q1c[4], q2c[4], t1[4], t2[4];
aa_tf_qconj(q1, q1c);
aa_tf_qconj(q2, q2c);
aa_tf_qmul(q1,q2c,t1);
aa_tf_qmulc(q1,q2,t2);
aveq("qmulc", 4, t1, t2, .001 );
aa_tf_qmul(q1c,q2,t1);
aa_tf_qcmul(q1,q2,t2);
aveq("qcmul", 4, t1, t2, .001 );
}
// conj. props
{
// p*q = conj(conj(q) * conj(p))
double c1[4], c2[4], c2c1[4], cc2c1[4], q1q2[4];
aa_tf_qconj(q1,c1);
aa_tf_qconj(q2,c2);
aa_tf_qmul(c2,c1,c2c1);
aa_tf_qmul(q1,q2,q1q2);
aa_tf_qconj(c2c1,cc2c1);
aveq("conjprop", 4, q1q2, cc2c1, .0001);
}
// exp
{
double q1e[4], q1eln[4];
aa_tf_qexp(q1, q1e);
aa_tf_qln(q1e, q1eln);
aveq("exp-log", 4, q1, q1eln, .00001 );
aa_tf_qln(q1, q1eln);
aa_tf_qexp(q1eln, q1e);
aveq("log-exp", 4, q1, q1e, .00001 );
}
// diff
double w[3]={0}, dq[4], wdq[3];
aa_vrand( 3, w );
aa_tf_qvel2diff( q1, w, dq );
aa_tf_qdiff2vel( q1, dq, wdq );
aveq("qveldiff", 3, w, wdq, .000001);
// integrate
double qn_rk1[4], qn_vrk1[4], qn_vrk4[4], qn_vexp[4], qn_dq[4], w0[3] = {0};
double dt = .02;
aa_tf_qrk1( q1, dq, dt, qn_rk1 );
aa_tf_qvelrk1( q1, w, dt, qn_vrk1 );
aa_tf_qvelrk4( q1, w, dt, qn_vrk4 );
aa_tf_qsvel( q1, w, dt, qn_vexp );
aa_tf_qsdiff( q1, dq, dt, qn_dq );
aveq("qvelrk1", 4, qn_rk1, qn_vrk1, .001 );
aveq("qvelrk4", 4, qn_rk1, qn_vrk4, .001 );
aveq("qvelexp", 4, qn_vrk4, qn_vexp, .0001);
aveq("qvelsdiff", 4, qn_vexp, qn_dq, .001 );
aa_tf_qsvel( q1, w0, dt, qn_vexp );
aveq("qvelsvel0", 4, q1, qn_vexp, .000 );
{
double Rb[9], qR[4];
aa_tf_qsvel( q1, w, dt, qn_vexp );
aa_tf_rotmat_svel( R1, w, dt, Rb );
aa_tf_rotmat2quat( Rb, qR );
aa_tf_qminimize( qn_vexp);
aa_tf_qminimize( qR );
aveq("rotmat_svel", 4, qn_vexp, qR, 1e-4 );
}
// vectors
{
double *v0 = E[0] + AA_TF_QUTR_T;
double *v1 = E[1] + AA_TF_QUTR_T;
double q[4], vp[3];
// identify case
aa_tf_vecs2quat( v0, v0, q);
aveq( "vecs2quat-ident", 4, q, aa_tf_quat_ident, 1e-6 );
// regular case
aa_tf_vecs2quat( v0, v1, q);
aa_tf_qrot(q,v0,vp);
// normalize result
{
double n0 = sqrt(v0[0]*v0[0] + v0[1]*v0[1] + v0[2]*v0[2] );
double n1 = sqrt(v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2] );
double vp1[3];
for( size_t i = 0; i < 3; i ++ ) {
vp1[i] = n0*v1[i] / n1;
}
aveq("vecs2quat", 3, vp, vp1, 1e-6 );
}
// inverted case
double v0n[3] = {-v0[0], -v0[1], -v0[2]};
aa_tf_vecs2quat( v0, v0n, q);
aa_tf_qrot(q,v0,vp);
{
double n0 = sqrt(v0[0]*v0[0] + v0[1]*v0[1] + v0[2]*v0[2] );
double n1 = sqrt(v0n[0]*v0n[0] + v0n[1]*v0n[1] + v0n[2]*v0n[2] );
double vp1[3];
for( size_t i = 0; i < 3; i ++ ) {
vp1[i] = n0*v0n[i] / n1;
}
aveq("vecs2quat-degenerate", 3, vp, vp1, 1e-6 );
}
}
}
