static void theta2quat() { double theta = (aa_frand() - 0.5) * 2 * M_PI; double qx[4], qy[4], qz[4]; double Rx[9], Ry[9], Rz[9]; double qRx[4], qRy[4], qRz[4]; aa_tf_xangle2rotmat( theta, Rx ); aa_tf_yangle2rotmat( theta, Ry ); aa_tf_zangle2rotmat( theta, Rz ); aa_tf_xangle2quat( theta, qx ); aa_tf_yangle2quat( theta, qy ); aa_tf_zangle2quat( theta, qz ); aa_tf_rotmat2quat( Rx, qRx ); aa_tf_rotmat2quat( Ry, qRy ); aa_tf_rotmat2quat( Rz, qRz ); aa_tf_qminimize( qx ); aa_tf_qminimize( qRx ); aa_tf_qminimize( qy ); aa_tf_qminimize( qRy ); aa_tf_qminimize( qz ); aa_tf_qminimize( qRz ); aveq("xangle2quat", 4, qx, qRx, 1e-6 ); aveq("yangle2quat", 4, qy, qRy, 1e-6 ); aveq("xangle2quat", 4, qz, qRz, 1e-6 ); }
void regress_1() { aa_mem_region_t reg; aa_mem_region_init( ®, 1024*32 ); // Create list for way points struct rfx_trajx_point_list *plist = rfx_trajx_point_list_alloc( ® ); double r0[4] = {0,1,0,0}; double rr[6][4]; double rrel[4][4]; double x0[3] = {0}; aa_tf_zangle2quat(.5*M_PI, rrel[0]); aa_tf_eulerzyx2quat(0, 0, .10*M_PI, rrel[1]); aa_tf_eulerzyx2quat(0, 0, -.2*M_PI, rrel[2]); aa_tf_eulerzyx2quat(-.5*M_PI, 0, 0, rrel[3]); AA_MEM_CPY(rr[0], r0, 4); for( size_t i = 0; i < 4; i ++ ) { aa_tf_qmul( rr[i], rrel[i], rr[i+1] ); } AA_MEM_CPY(rr[5], r0, 4); size_t diff = 2; for( size_t i = 0; i < sizeof(rr)/sizeof(rr[0])-diff; i ++ ) { rfx_trajx_point_list_addb_qv( plist, 5*(double)i, 1, rr[i+diff], x0 ); } // generate trajectory struct rfx_trajx_seg_list *seglist = //rfx_trajx_splend_generate( plist, ® ); rfx_trajx_parablend_generate( plist, ® ); // plot trajectory rfx_trajx_seg_list_plot( seglist, .001, NULL ); }