math.h

Go to the documentation of this file.

/* -*- mode: C; c-basic-offset: 4 -*- */
/* ex: set shiftwidth=4 tabstop=4 expandtab: */
/*
 * Copyright (c) 2010-2011, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s): Neil T. Dantam <ntd@gatech.edu>
 * Georgia Tech Humanoid Robotics Lab
 * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
 *
 *
 * This file is provided under the following "BSD-style" License:
 *
 *
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 *
 */
#ifndef AA_MATH_H
#define AA_MATH_H

/***********/
/* Scalars */
/***********/

#ifndef AA_EPSILON
#define AA_EPSILON .001
#endif //AA_EPSILON

#define AA_MAX(a,b) \
    ({ const __typeof__(a) aa_$_max_a = (a); \
       const __typeof__(b) aa_$_max_b = (b); \
       (aa_$_max_a > aa_$_max_b) ? aa_$_max_a : aa_$_max_b; })

#define AA_MIN(a,b) \
    ({ const __typeof__(a) aa_$_min_a = (a); \
       const __typeof__(b) aa_$_min_b = (b); \
       (aa_$_min_a < aa_$_min_b) ? aa_$_min_a : aa_$_min_b; })

AA_DEPRECATED static inline double aa_clamp( double val, double level) {
    if( val > level )  return level;
    if( val < -level ) return -level;
    return val;
}

static inline double aa_fclamp( double val, double min, double max) {
    if( val > max )  return max;
    if( val < min ) return min;
    return val;
}

static inline double aa_fdeadzone( double val, double min, double max, double deadval) {
    if( min < val && max > val )  return deadval;
    else return val;
}

static inline void aa_vclamp( size_t n, double *v, double min, double max) {
    for( size_t i = 0; i < n; i++ ) {
        if( v[i] > max )  v[i] = max;
        else if( v[i] < min ) v[i] = min;
    }
}


static inline double aa_sign( double val ) {
    if( val > 0 )  return 1;
    if( val < 0 ) return -1;
    return 0;
}

AA_API int aa_isfok( double x );

static inline int aa_feq( double a, double b, double tol ) {
    return fabs(a-b) <= tol;
}

AA_API int aa_veq( size_t n, const double *a, const double *b, double tol );


static inline double aa_fsq( double a ) {
    return a * a;
}

#define AA_MODULO(a,b) (((a) % (b)) + (b)) % (b);

static inline int aa_imodulo( int a, int b ) {
    //return ((a % b) + b) % b;
    return AA_MODULO(a,b);
}

static inline long aa_lmodulo( long a, long b ) {
    return AA_MODULO(a,b);
}

static inline int64_t aa_imodulo64( int64_t a, int64_t b ) {
    return AA_MODULO(a,b);
}

static inline int aa_iremainder( int a, int b ) {
    return a % b;
}

static inline double aa_fmodulo( double a, double b ) {
    return fmod(fmod(a, b) + b,  b);
}

static inline double aa_fremainder( double a, double b ) {
    return fmod(a , b);
}

AA_API size_t aa_fminloc( size_t n, double *v );

AA_API size_t aa_fmaxloc( size_t n, double *v );

AA_API double aa_frand();

AA_API double aa_frand_minmax(double min, double max);

AA_API void aa_vrand(size_t n, double *v);


static inline double
aa_horner3( double x, double a0, double a1, double a2 )
{
    return a0 + x * ( a1 + x*a2 );
}

static inline void
aa_sincos( double x, double *s, double *c )
{
    *s = sin(x);
    *c = cos(x);
}


/********/
/* Stat */
/********/

AA_API void aa_stat_box_muller(double x1, double x2, double *z1, double *z2);

static inline double aa_stat_z2x(double z, double mu, double sigma) {
    return (z * sigma) + mu;
}

static inline double aa_stat_x2z(double x, double mu, double sigma) {
    return (x-mu)/sigma;
}

AA_API double aa_stat_mean( size_t n, const double *x);

AA_API double aa_stat_std( size_t n, const double *x);

AA_API size_t aa_stat_excluded_mean_std( size_t n, const double *x,
                                         double *pmu, double *psigma,
                                         double zmin, double zmax,
                                         size_t max_iterations );





AA_API double aa_stat_circ_mean( size_t n, const double *x);

AA_API double aa_stat_circ_std( size_t n, const double *x);

AA_API size_t aa_stat_excluded_circ_mean_std( size_t n, const double *x,
                                              double *pmu, double *psigma,
                                              double zmin, double zmax,
                                              size_t max_iterations );


AA_API void aa_stat_vmean( size_t m, size_t n, const double *X,
                           double *mu);

AA_API void aa_stat_vmean_cov( size_t m, size_t n, const double *X,
                               double *mu, double *E);


double aa_stat_mahalanobis( size_t m, const double *x,
                            const double *mu, const double *E_inv);

/**********/
/* Angles */
/**********/

static inline double aa_ang_rad2deg( double rad ) {
    return rad*180.0/M_PI;
}

static inline double aa_ang_deg2rad( double deg ) {
    return deg*M_PI/180;
}


static inline double aa_ang_norm_2pi( double an ) {
    return aa_fmodulo( an, 2*M_PI );
}

static inline double aa_ang_norm_pi( double an ) {
    //return aa_fmodulo( an + M_PI, 2*M_PI ) - M_PI;
    return remainder( an, 2*M_PI );
}


static inline double aa_ang_delta( double a, double b) {
    return aa_ang_norm_pi( aa_ang_norm_pi(a) - aa_ang_norm_pi(b) );
}



/************************/
/* Dense Linear Algebra */
/************************/

#define AA_MATCOL(A, lda, col) ((A)+(col)*(lda))

#define AA_MATREF(A, lda, row, col) (AA_MATCOL(A,lda,col)[row])

/*--- Scalar Ops ---*/

AA_API double aa_la_min( size_t n, const double *x );

AA_API double aa_la_max( size_t n, const double *x );

AA_API double aa_la_dot( size_t n, const double *x, const double *y );

AA_API double aa_la_norm( size_t n, const double *x );

AA_API double aa_la_ssd( size_t n, const double *x, const double *y );

AA_API double aa_la_dist( size_t n, const double *x, const double *y );

/*--- Vector Ops ---*/

AA_API void aa_la_sadd( size_t n, double alpha, const double *x, double *r );


AA_API void aa_la_scal( size_t n, double alpha, double *x  );


AA_API void aa_la_vinc( size_t n, const double *x, double *y  );

AA_API void aa_la_sinc( size_t n, double alpha, double *x  );


AA_API void aa_la_axpy( size_t n, double alpha, const double *x, double *y  );


AA_API void aa_la_axpy3( size_t n, double alpha,
                         const double *x, const double *y, double *z );

AA_API void aa_la_smul( size_t n, double alpha, const double *x, double *r );

AA_API void aa_la_ssub( size_t n, double alpha, const double *x, double *r );

AA_API void aa_la_sdiv( size_t n, double alpha, const double *x, double *r );

AA_API void aa_la_vadd( size_t n, const double *x, const double *y, double *r );

AA_API void aa_la_vsub( size_t n, const double *x, const double *y, double *r );

AA_API void aa_la_vmul( size_t n, const double *x, const double *y, double *r );

AA_API void aa_la_vdiv( size_t n, const double *x, const double *y, double *r );

AA_API void aa_la_cross( const double a[3], const double b[3], double c[3] );

AA_API void aa_la_normalize( size_t n, double *x );


AA_API double aa_la_point_plane( size_t n,
                                 const double *point, const double *plane );

/*--- Matrix Ops --- */


AA_API void aa_la_transpose( size_t n, double *A  );
AA_API void aa_la_transpose2( size_t m, size_t n, const double *A, double *At  );

static inline void
aa_la_diag( size_t n, double *A, double x ) {
    for( size_t i = 0; i < n; i ++ )
        A[i*n+i] = x;
}

static inline void
aa_la_ident( size_t n, double *A ) {
    AA_MEM_ZERO(A, n*n);
    aa_la_diag(n,A,1.0);
}

static inline void
aa_la_mvmul( size_t m, size_t n, const double *A, const double *x, double *b ) {
    cblas_dgemv( CblasColMajor, CblasNoTrans, (int)m, (int)n,
                 1.0, A, (int)m,
                 x, 1, 0, b, 1 );
}

AA_API double aa_la_wdot( size_t n,
                          const double *x, const double *A, const double *y );


AA_API int aa_la_svd( size_t m, size_t n, const double *A, double *U, double *S, double *Vt );

AA_API int aa_la_inv( size_t n, double *A );


AA_API void aa_la_inverse3x3_( const double R[9], double S[9] );


static inline  void aa_la_inverse3x3( const double R[9], double S[9] ) {
    aa_la_inverse3x3_(R,S);
}


double aa_la_det3x3( const double R[AA_RESTRICT 9] );

AA_API double aa_la_trace( size_t n, const double *A );

AA_API void aa_la_dpinv( size_t m, size_t n, double k,  const double *A, double *A_star );


AA_API void aa_la_dzdpinv( size_t m, size_t n, double s2_min, const double *A, double *A_star ) ;

AA_API void aa_la_dls( size_t m, size_t n, double k,  const double *A, const double *b, double *x );


AA_API void aa_la_np( size_t m, size_t n,
                      const double *A, const double *A_star,
                      double *B );

AA_API void aa_la_xlsnp( size_t m, size_t n,
                         const double *A, const double *A_star, const double *b,
                         const double *xp, double *x );



AA_API void aa_la_dlsnp( size_t m, size_t n, double k,  const double *A, const double *b, const double *xp, double *x );

AA_API void aa_la_lls( size_t m, size_t n, size_t p, const double *A, const double *b, double *x );

AA_API int aa_la_care_laub( size_t m, size_t n, size_t p,
                            const double *AA_RESTRICT A,
                            const double *AA_RESTRICT B,
                            const double *AA_RESTRICT C,
                            double *AA_RESTRICT X );


AA_API void aa_la_linterp( size_t n,
                           double t0, const double *X0,
                           double t1, const double *X1,
                           double ti, double *Xi );


AA_API void aa_la_quadterp( size_t n,
                            double t0, const double *X0,
                            double t1, const double *X1,
                            double t2, const double *X2,
                            double ti, double *Xi );

AA_API void aa_la_quadterp_dx( size_t n,
                               double t0, const double *X0,
                               double t1, const double *X1,
                               double t2, const double *X2,
                               double ti, double *dXi );


AA_API void aa_la_plane_hessian( size_t n, double *plane );

AA_API void aa_la_plane_fit( size_t m, size_t n,
                             const double *points, double *plane );

/*--- Systems and Signals --- */

AA_API void aa_lsim_dstep( size_t m, size_t n,
                           const double *AA_RESTRICT A,
                           const double *AA_RESTRICT B,
                           const double *AA_RESTRICT x0,
                           const double *AA_RESTRICT u,
                           double *AA_RESTRICT x1 );

AA_API void aa_lsim_estep( size_t m, size_t n,
                           double dt,
                           const double *AA_RESTRICT A,
                           const double *AA_RESTRICT B,
                           const double *AA_RESTRICT x0,
                           const double *AA_RESTRICT u,
                           double *AA_RESTRICT x1 );


typedef void aa_sys_fun( const void *cx,
                         double t, const double *AA_RESTRICT x,
                         double *AA_RESTRICT y );

enum aa_ode_integrator {
    AA_ODE_RK1,
    AA_ODE_RK2,
    AA_ODE_RK4,
    AA_ODE_RK45_F,
    AA_ODE_RK45_CK,
    AA_ODE_RK45_DP,
    AA_ODE_RK23_BS
};

#define AA_ODE_EULER AA_ODE_RK1

#define AA_ODE_HEUN AA_ODE_RK2


typedef int aa_ode_check( void *cx, double t, double * AA_RESTRICT x, double *AA_RESTRICT y );

struct aa_ode_sol_opts {
    double adapt_tol_dec;

    double adapt_tol_inc;

    double adapt_factor_dec;

    double adapt_factor_inc;
};

AA_API int aa_ode_sol( enum aa_ode_integrator integrator,
                       const struct aa_ode_sol_opts * AA_RESTRICT opts,
                       size_t n,
                       aa_sys_fun sys, const void *sys_cx,
                       aa_ode_check check, void *check_cx,
                       double t0, double dt0,
                       const double *AA_RESTRICT x0,
                       double *AA_RESTRICT x1 );

AA_API int
aa_ode_path( enum aa_ode_integrator integrator,
             const struct aa_ode_sol_opts * AA_RESTRICT opts,
             size_t n,
             aa_sys_fun sys, const void *sys_cx,
             aa_ode_check check, void *check_cx,
             double t0, double dt0,
             const double *AA_RESTRICT x0,
             struct aa_mem_region *region, size_t *n_points, double **path );

typedef void aa_odestep_fixed( size_t n, aa_sys_fun sys, const void *cx,
                               double t0, double dt,
                               const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );

typedef void aa_odestep_adaptive( size_t n, aa_sys_fun sys, const void *cx,
                                  double t0, double dt,
                                  const double *AA_RESTRICT x0,
                                  double *AA_RESTRICT k,
                                  double *AA_RESTRICT x_n_1,
                                  double *AA_RESTRICT x_n );

AA_API void aa_odestep_euler( size_t n, double dt,
                              const double *AA_RESTRICT dx,
                              const double *AA_RESTRICT x0,
                              double *AA_RESTRICT x1 );


AA_API void aa_odestep_rk1( size_t n, aa_sys_fun sys, const void *cx,
                            double t0, double dt,
                            const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );

AA_API void aa_odestep_rk2( size_t n, aa_sys_fun sys, const void *cx,
                            double t0, double dt,
                            const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );

AA_API void aa_odestep_rk4( size_t n, aa_sys_fun sys, const void *cx,
                            double t0, double dt,
                            const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );

AA_API void aa_odestep_rkf45( size_t n, aa_sys_fun sys, const void *cx,
                              double t0, double dt,
                              const double *AA_RESTRICT x0,
                              double *AA_RESTRICT k,
                              double *AA_RESTRICT x4,
                              double *AA_RESTRICT x5 );

AA_API void aa_odestep_rkck45( size_t n, aa_sys_fun sys, const void *cx,
                               double t0, double dt,
                               const double *AA_RESTRICT x0,
                               double *AA_RESTRICT k,
                               double *AA_RESTRICT x4,
                               double *AA_RESTRICT x5 );


AA_API void aa_odestep_dorpri45( size_t n, aa_sys_fun sys, const void *cx,
                                 double t0, double dt,
                                 const double *AA_RESTRICT x0,
                                 double *AA_RESTRICT k,
                                 double *AA_RESTRICT x4,
                                 double *AA_RESTRICT x5 );


AA_API void aa_odestep_rkbs23( size_t n, aa_sys_fun sys, const void *cx,
                               double t0, double dt,
                               const double *AA_RESTRICT x0,
                               double *AA_RESTRICT k,
                               double *AA_RESTRICT x4,
                               double *AA_RESTRICT x5 );


AA_API void aa_lsim_rk4step( size_t m, size_t n,
                             double dt,
                             const double *AA_RESTRICT A,
                             const double *AA_RESTRICT B,
                             const double *AA_RESTRICT x0,
                             const double *AA_RESTRICT u,
                             double *AA_RESTRICT x1 );


typedef struct {
    size_t n;   
    double *A;  
    double *D;  
} aa_sys_affine_t;

AA_API void aa_sys_affine( const aa_sys_affine_t *cx,
                           double t, const double *AA_RESTRICT x,
                           double *AA_RESTRICT dx );


/*--- GCC Vector Extensions --- */

//typedef doulbe aa_v2df_t __attribute__ (( vector_size(2*sizeof(double)) ));

#endif //AA_MATH_H