Exemplo n.º 1
0
AA_API void aa_cv_rgb2hsv(const double rgb[3], double hsv[3]) {
    double r = rgb[0];
    double g = rgb[1];
    double b = rgb[2];

    // using the polar coordinates method (not hexagon)
    double alpha = 0.5 * (2*r - g - b);
    double beta = (sqrt(3) / 2.0) * (g - b);
    double c = sqrt(alpha*alpha + beta*beta);

    // H
    hsv[0] = atan2(beta, alpha);

    // V
    double tmp = AA_MAX(r,g);
    hsv[2] = AA_MAX(tmp,b);

    // S
    if( aa_feq(c,0,0) ) {
        hsv[1] = 0;
    } else {
        hsv[1] = c / hsv[2];
    }

}
Exemplo n.º 2
0
void aafeq( const char * name, double a, double b, double tol ) {
    if( !aa_feq( a, b, tol) ) {
        fprintf( stderr, "FAILED: %s\n",name);
        fprintf( stderr, "a: %f, b: %f\n", a, b);
        abort();
    }
}
Exemplo n.º 3
0
void test_feq( const char *name, double a, double b, double tol )
{
    if( !aa_feq(a,b,tol) ) {
        fprintf( stderr, "FAILED: %s, %f != %f\n",name, a, b);
        abort();
    }
}
Exemplo n.º 4
0
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 );
        }


    }
}
Exemplo n.º 5
0
int aa_veq(size_t n, const double *a, const double *b, double tol ) {
    for( size_t i = 0; i < n; i ++ ) {
        if( ! aa_feq( a[i], b[i], tol ) ) return 0;
    }
    return 1;
}
Exemplo n.º 6
0
AA_API void aa_rx_sg_tf_update
( const struct aa_rx_sg *scene_graph,
  size_t n_q,
  const double *q0,
  const double *q,
  size_t n_tf,
  const double *TF_rel0, size_t ld_rel0,
  const double *TF_abs0, size_t ld_abs0,
  double *TF_rel, size_t ld_rel,
  double *TF_abs, size_t ld_abs )
{
    aa_rx_sg_ensure_clean_frames( scene_graph );

    amino::SceneGraph *sg = scene_graph->sg;

    bool updated[sg->frames.size()];

    size_t i_frame = 0;
    for( size_t i_rel0 = 0, i_abs0 = 0,
             i_rel = 0, i_abs = 0;
         i_frame < n_tf && i_frame < sg->frames.size();
         i_frame++, i_rel += ld_rel, i_abs += ld_abs,
             i_rel0 += ld_rel0, i_abs0 += ld_abs0
        )
    {
        amino::SceneFrame *f = sg->frames[(aa_rx_frame_id)i_frame];
        enum aa_rx_frame_type type = f->type;
        bool update_abs = 0;
        bool in_global =  f->in_global();

        const double *E_rel0 = TF_rel0 + i_rel0;
        const double *E_abs0 = TF_abs0 + i_abs0;
        double *E_rel  = TF_rel + i_rel;
        double *E_abs  = TF_abs + i_abs;

        switch( type ) {
        case AA_RX_FRAME_FIXED:
            f->tf_rel(q, E_rel);
            update_abs = !in_global && updated[f->parent_id];
            break;
        case AA_RX_FRAME_REVOLUTE:
        case AA_RX_FRAME_PRISMATIC: {
            amino::SceneFrameJoint *fj = static_cast<amino::SceneFrameJoint*>(f);
            if( aa_feq(q0[fj->config_index], q[fj->config_index], 0 )) {
                AA_MEM_CPY(E_rel, E_rel0, 7);
                update_abs = !in_global && updated[f->parent_id];
            } else {
                f->tf_rel(q, E_rel);
                update_abs = 1;
            }
            break; }
        }

        if( update_abs ) {
            // chain to global
            if( in_global ) {
                // TODO: can we somehow get rid of this branch?
                //       maybe a separate type for global frames
                AA_MEM_CPY(E_abs, E_rel, 7);
            } else {
                assert( f->parent_id < (ssize_t)i_frame );
                double *E_abs_parent = TF_abs + (ld_abs * f->parent_id);;
                aa_tf_qutr_mul(E_abs_parent, E_rel, E_abs);
            }
            updated[i_frame] = 1;
        } else {
            AA_MEM_CPY(E_abs, E_abs0, 7);
            updated[i_frame] = 0;
        }
    }
}
Exemplo n.º 7
0
void aneq( double a, double b, double tol ) {
    assert( !aa_feq(a,b,tol) );
}
Exemplo n.º 8
0
AA_API int aa_valid_vunit( double *values, size_t n, double tol ) {
    return aa_feq( aa_la_norm(n, values), 1.0, tol ) ? 0 : 1;
}