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 ); } } }