Esempi in C++ (Cpp) per aa_tf_quat2rotvec

Esempio n. 1

File: aa_fuzz_tf.c Progetto: kingdwd/amino

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

Esempio n. 2

File: aa_fuzz_tf.c Progetto: kingdwd/amino

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

Esempio n. 3

File: plot_test.c Progetto: golems/reflex

void plot_viax() {
    aa_mem_region_t reg;
    aa_mem_region_init( &reg, 1024*32 );

    struct rfx_trajx_point_list *plist = rfx_trajx_point_list_alloc( &reg );

    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, &reg );
    rfx_trajx_parablend_generate( plist, &reg );

    rfx_trajx_seg_list_plot( seglist, .001, NULL );
    return;

    struct rfx_trajx_seg_list *testlist =
        rfx_trajx_seg_list_alloc( &reg );
    {
        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( &reg, 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( &reg );
    {
        struct rfx_trajx_seg *test =
            rfx_trajx_seg_lerp_slerp_alloc( &reg, 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( &reg );
}