static void rotvec(double *q) { double e[3], R[9], vr[3]; aa_tf_quat2rotmat(q, R); assert( aa_tf_isrotmat(R) ); aa_tf_quat2rotvec(q, e); aa_tf_rotmat2rotvec(R, vr); aveq("rotvec", 3, e, vr, .001 ); { double ee[9], eln[3], qe[4], rln[3]; aa_tf_rotmat_expv( e, ee ); aa_tf_rotmat_lnv( ee, rln ); aveq("rotmat_lnv", 3, e, rln, 1e-6 ); aa_tf_rotmat2quat( ee, qe ); aa_tf_quat2rotvec( qe, eln ); aveq("rotmat_expv", 3, e, eln, 1e-6 ); } { double aa[4], ee[9], eln[3], qe[4]; aa_tf_rotvec2axang(e, aa); aa_tf_rotmat_exp_aa( aa, ee ); aa_tf_rotmat2quat( ee, qe ); aa_tf_quat2rotvec( qe, eln ); aveq("rotmat_exp_aa", 3, e, eln, 1e-6 ); } { double Rtmp[9]; aa_tf_rotmat_xy( R+0, R+3, Rtmp ); aveq( "rotmat_xy", 9, R, Rtmp, 1e-6 ); aa_tf_rotmat_yz( R+3, R+6, Rtmp ); aveq( "rotmat_yz", 9, R, Rtmp, 1e-6 ); aa_tf_rotmat_zx( R+6, R+0, Rtmp ); aveq( "rotmat_zx", 9, R, Rtmp, 1e-6 ); } }
static void euler_helper( const double e[4], fun_type e2r, fun_type e2q ) { double R[9], q[4]; e2r(e[0], e[1], e[2], R); aa_tf_isrotmat(R) ; e2q(e[0], e[1], e[2], q); double vq[3], vr[3]; aa_tf_quat2rotvec(q, vq); aa_tf_rotmat2rotvec(R, vr); aveq("euler-vecs", 3, vq, vr, .001 ); }
void plot_viax() { aa_mem_region_t reg; aa_mem_region_init( ®, 1024*32 ); struct rfx_trajx_point_list *plist = rfx_trajx_point_list_alloc( ® ); double theta = M_PI*.9; //double X[5][5] = { {0,0,0}, {1,0,0}, {1,1,0}, {1,1,1}, {0,0,0} }; //double E[5][5] = { {0,0,0}, {M_PI_2,0,0}, {M_PI_2,M_PI_2,0}, {M_PI_2,M_PI_2,M_PI_2}, {0,0,0} }; double X[2][3] = { {0,0,0}, {0,0,0} }; double E[2][3] = { {theta,0,0}, {theta,theta,0} }; double R[5][4]; double RV[5][3]; size_t n = 2; for( size_t i = 0; i < n; i ++ ) { aa_tf_eulerzyx2quat( E[i][0], E[i][1], E[i][2], R[i] ); aa_tf_quat2rotvec( R[i], RV[i] ); rfx_trajx_point_list_addb_qv( plist, 5*(double)i, 1, R[i], X[i] ); } //rfx_trajx_generate( pT ); //rfx_trajx_plot( pT, .001, NULL ); struct rfx_trajx_seg_list *seglist = //rfx_trajx_splend_generate( plist, ® ); rfx_trajx_parablend_generate( plist, ® ); rfx_trajx_seg_list_plot( seglist, .001, NULL ); return; struct rfx_trajx_seg_list *testlist = rfx_trajx_seg_list_alloc( ® ); { double x_i[6], x_f[6]; AA_MEM_CPY(x_i, X[0], 3 ); AA_MEM_CPY(x_i+3, RV[0], 3 ); AA_MEM_CPY(x_f, X[1], 3 ); AA_MEM_CPY(x_f+3, RV[1], 3 ); struct rfx_trajx_seg *test = rfx_trajx_seg_lerp_rv_alloc( ®, 0, 1, 0, x_i, 1, x_f ) ; rfx_trajx_seg_list_add( testlist, test ); } struct rfx_trajx_seg_list *testlist2 = rfx_trajx_seg_list_alloc( ® ); { struct rfx_trajx_seg *test = rfx_trajx_seg_lerp_slerp_alloc( ®, 0, 1, 0, X[0], R[0], 1, X[1], R[1] ) ; rfx_trajx_seg_list_add( testlist2, test ); } rfx_trajx_seg_list_plot( testlist, .001, NULL ); aa_mem_region_destroy( ® ); }