58 #define AA_EPSILON .001 64 ({ const __typeof__(a) aa_$_max_a = (a); \ 65 const __typeof__(b) aa_$_max_b = (b); \ 66 (aa_$_max_a > aa_$_max_b) ? aa_$_max_a : aa_$_max_b; }) 70 ({ const __typeof__(a) aa_$_min_a = (a); \ 71 const __typeof__(b) aa_$_min_b = (b); \ 72 (aa_$_min_a < aa_$_min_b) ? aa_$_min_a : aa_$_min_b; }) 76 if( val > level )
return level;
77 if( val < -level )
return -level;
82 static inline double aa_fclamp(
double val,
double min,
double max) {
83 if( val > max )
return max;
84 if( val < min )
return min;
89 static inline double aa_fdeadzone(
double val,
double min,
double max,
double deadval) {
90 if( min < val && max > val )
return deadval;
95 static inline void aa_vclamp(
size_t n,
double *v,
double min,
double max) {
96 for(
size_t i = 0; i < n; i++ ) {
97 if( v[i] > max ) v[i] = max;
98 else if( v[i] < min ) v[i] = min;
105 if( val > 0 )
return 1;
106 if( val < 0 )
return -1;
114 static inline int aa_feq(
double a,
double b,
double tol ) {
115 return fabs(a-b) <= tol;
119 AA_API int aa_veq(
size_t n,
const double *a,
const double *b,
double tol );
123 static inline double aa_fsq(
double a ) {
128 #define AA_MODULO(a,b) (((a) % (b)) + (b)) % (b); 153 return fmod(fmod(a, b) + b, b);
183 return a0 + x * ( a1 + x*a2 );
212 static inline double aa_stat_z2x(
double z,
double mu,
double sigma) {
213 return (z * sigma) + mu;
218 static inline double aa_stat_x2z(
double x,
double mu,
double sigma) {
241 double *pmu,
double *psigma,
242 double zmin,
double zmax,
243 size_t max_iterations );
266 double *pmu,
double *psigma,
267 double zmin,
double zmax,
268 size_t max_iterations );
290 double *mu,
double *E);
295 const double *mu,
const double *E_inv);
303 return rad*180.0/M_PI;
320 return remainder( an, 2*M_PI );
341 #define AA_MATCOL(A, lda, col) ((A)+(col)*(lda)) 350 #define AA_MATREF(A, lda, row, col) (AA_MATCOL(A,lda,col)[row]) 385 AA_API void aa_la_sadd(
size_t n,
double alpha,
const double *x,
double *r );
408 AA_API void aa_la_axpy(
size_t n,
double alpha,
const double *x,
double *y );
418 const double *x,
const double *y,
double *z );
423 AA_API void aa_la_smul(
size_t n,
double alpha,
const double *x,
double *r );
428 AA_API void aa_la_ssub(
size_t n,
double alpha,
const double *x,
double *r );
433 AA_API void aa_la_sdiv(
size_t n,
double alpha,
const double *x,
double *r );
438 AA_API void aa_la_vadd(
size_t n,
const double *x,
const double *y,
double *r );
443 AA_API void aa_la_vsub(
size_t n,
const double *x,
const double *y,
double *r );
448 AA_API void aa_la_vmul(
size_t n,
const double *x,
const double *y,
double *r );
453 AA_API void aa_la_vdiv(
size_t n,
const double *x,
const double *y,
double *r );
473 const double *point,
const double *plane );
486 for(
size_t i = 0; i < n; i ++ )
499 aa_la_mvmul(
size_t m,
size_t n,
const double *A,
const double *x,
double *b ) {
500 cblas_dgemv( CblasColMajor, CblasNoTrans, (
int)m, (
int)n,
510 const double *x,
const double *A,
const double *y );
526 AA_API int aa_la_svd(
size_t m,
size_t n,
const double *A,
double *U,
double *S,
double *Vt );
580 AA_API void aa_la_dpinv(
size_t m,
size_t n,
double k,
const double *A,
double *A_star );
600 AA_API void aa_la_dzdpinv(
size_t m,
size_t n,
double s2_min,
const double *A,
double *A_star ) ;
615 AA_API void aa_la_dls(
size_t m,
size_t n,
double k,
const double *A,
const double *b,
double *x );
634 const double *A,
const double *A_star,
const double *b,
635 const double *xp,
double *x );
654 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 );
666 AA_API void aa_la_lls(
size_t m,
size_t n,
size_t p,
const double *A,
const double *b,
double *x );
694 double t0,
const double *X0,
695 double t1,
const double *X1,
696 double ti,
double *Xi );
712 double t0,
const double *X0,
713 double t1,
const double *X1,
714 double t2,
const double *X2,
715 double ti,
double *Xi );
720 double t0,
const double *X0,
721 double t1,
const double *X1,
722 double t2,
const double *X2,
723 double ti,
double *dXi );
741 const double *points,
double *plane );
818 #define AA_ODE_EULER AA_ODE_RK1 823 #define AA_ODE_HEUN AA_ODE_RK2 863 double t0,
double dt0,
876 double t0,
double dt0,
878 struct aa_mem_region *region,
size_t *n_points,
double **path );
884 double t0,
double dt,
891 double t0,
double dt,
909 double t0,
double dt,
923 double t0,
double dt,
937 double t0,
double dt,
963 double t0,
double dt,
992 double t0,
double dt,
1023 double t0,
double dt,
1053 double t0,
double dt,
#define AA_MODULO(a, b)
Fortran modulo, Ada mod.
AA_API double aa_la_min(size_t n, const double *x)
min of vector
AA_API double aa_la_norm(size_t n, const double *x)
Euclidean norm of x.
static double aa_fdeadzone(double val, double min, double max, double deadval)
apply deadzone to val
int aa_ode_check(void *cx, double t, double *AA_RESTRICT x, double *AA_RESTRICT y)
Function signature to check whether a differential equation has reached its goal. ...
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)
Runge-Kutta-4 integration.
Options for the differential equation solver.
AA_API double aa_la_trace(size_t n, const double *A)
Trace of a matrix.
AA_API void aa_la_dls(size_t m, size_t n, double k, const double *A, const double *b, double *x)
Damped Least Squares.
AA_API void aa_stat_vmean_cov(size_t m, size_t n, const double *X, double *mu, double *E)
Compute sample covariance of vectors.
AA_API void aa_la_vsub(size_t n, const double *x, const double *y, double *r)
Elementwise subtraction.
Runge-Kutta 4-5 / Cash Karp integration.
AA_API void aa_la_ssub(size_t n, double alpha, const double *x, double *r)
vector-scalar subtraction.
static double aa_fsq(double a)
square
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)
Runge-Kutta-4-5 (Fehlberg Method) integration.
AA_API void aa_la_vinc(size_t n, const double *x, double *y)
increment by vector.
AA_API double aa_la_ssd(size_t n, const double *x, const double *y)
Sum of Squared Differences.
static AA_DEPRECATED double aa_clamp(double val, double level)
force value to be within +/- level
AA_API void aa_la_vdiv(size_t n, const double *x, const double *y, double *r)
Elementwise division.
static double aa_ang_delta(double a, double b)
Difference between two angles, interval (-pi,pi)
AA_API void aa_la_vadd(size_t n, const double *x, const double *y, double *r)
Elementwise addition.
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)
Linear simulation step with euler integration.
AA_API int aa_la_svd(size_t m, size_t n, const double *A, double *U, double *S, double *Vt)
Singular Value Decomposition of A.
AA_API double aa_la_dist(size_t n, const double *x, const double *y)
Euclidean Distance.
AA_API double aa_frand_minmax(double min, double max)
uniform pseudo-random in [min,max]
static void aa_sincos(double x, double *s, double *c)
Portable sincos.
static double aa_ang_rad2deg(double rad)
convert radians to degrees
static double aa_ang_norm_2pi(double an)
normalize angle on interval [0,2pi)
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)
Function signature for a fixed integration step.
static void aa_la_ident(size_t n, double *A)
Set A to the identity matrix.
AA_API void aa_la_plane_hessian(size_t n, double *plane)
Convert plane to hessian normal form.
static int aa_feq(double a, double b, double tol)
Fuzzy equals.
AA_API void aa_la_cross(const double a[3], const double b[3], double c[3])
Cross product.
static int aa_imodulo(int a, int b)
Fortran modulo, Ada mod.
AA_API double aa_la_point_plane(size_t n, const double *point, const double *plane)
Point Plane Distance.
AA_API void aa_stat_vmean(size_t m, size_t n, const double *X, double *mu)
Compute mean of vectors.
AA_API int aa_veq(size_t n, const double *a, const double *b, double tol)
Fuzzy equals.
static int64_t aa_imodulo64(int64_t a, int64_t b)
Fortran modulo, Ada mod.
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)
Solve the continuous-time Riccati equation.
static void aa_la_diag(size_t n, double *A, double x)
Set diagonal of A to x.
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)
Quadratic interpolation, derivative.
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)
Quadratic interpolation.
Runge-Kutta-Fehlberg integration.
AA_API void aa_la_transpose2(size_t m, size_t n, const double *A, double *At)
transpose m*n matrix A into n*m matrix At
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)
Linear simulation step with Runge-Kutta-4 integration.
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)
Linear simulation step in discrete time.
AA_API void aa_la_inverse3x3_(const double R[9], double S[9])
Inverse of 3x3 matrix R.
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)
Function signature for an adaptive integration step.
Data Structure for Region-Based memory allocation.
AA_API double aa_stat_circ_std(size_t n, const double *x)
Compute standard deviation of vector x.
static void aa_la_inverse3x3(const double R[9], double S[9])
Inverse of 3x3 matrix R.
Runge-Kutta 1 integration.
double aa_la_det3x3(const double R[AA_RESTRICT 9])
Determinant of 3x3 matrix R.
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)
Euler / Runge-Kutta-1 integration.
AA_API void aa_la_dpinv(size_t m, size_t n, double k, const double *A, double *A_star)
Damped Pseudo Inverse of A.
AA_API void aa_sys_affine(const aa_sys_affine_t *cx, double t, const double *AA_RESTRICT x, double *AA_RESTRICT dx)
Affine system model function.
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)
Runge-Kutta-2 (Heun's Method) integration.
AA_API void aa_la_axpy(size_t n, double alpha, const double *x, double *y)
increment by scale times vector.
static double aa_fmodulo(double a, double b)
Mathematical modulo, Fortran modulo, Ada mod.
AA_API void aa_la_sinc(size_t n, double alpha, double *x)
increment by scalar.
AA_EXTERN const char *aa_verbf_prefix AA_DEPRECATED
don't use
AA_API double aa_stat_mean(size_t n, const double *x)
Compute mean of vector x.
double adapt_factor_dec
Factor to decrease step size.
AA_API void aa_la_normalize(size_t n, double *x)
Make x unit vector.
AA_API int aa_isfok(double x)
returns one if x is not infinity or NAN
Runge-Kutta 2-3 / Bogacki-Shampine integration.
double aa_stat_mahalanobis(size_t m, const double *x, const double *mu, const double *E_inv)
Mahalanobis distance.
double adapt_tol_dec
Decrease step size if error is greater than tol_shrink.
AA_API double aa_stat_circ_mean(size_t n, const double *x)
Compute mean of angles.
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)
Damped Least Squares with Nullspace projection.
Context-struct for function aa_sys_affine.
AA_API double aa_la_max(size_t n, const double *x)
max of vector
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)
Runge-Kutta-2-3 (Bogacki-Shampine Method) integration.
#define AA_RESTRICT
Defined restrict keyword based on language flavor.
static int aa_iremainder(int a, int b)
Fortran mod, Ada rem.
AA_API void aa_la_linterp(size_t n, double t0, const double *X0, double t1, const double *X1, double ti, double *Xi)
Linear interpolation.
AA_API double aa_frand()
uniform pseudo-random in [0,1.0]
Runge-Kutta 4-5 / Dormand-Prince integration.
AA_API void aa_la_transpose(size_t n, double *A)
transpose square matrix A in place
AA_API double aa_stat_std(size_t n, const double *x)
Compute standard deviation of vector x.
static double aa_ang_deg2rad(double deg)
convert radians to degrees
double adapt_factor_inc
Factor to increse step size.
void aa_sys_fun(const void *cx, double t, const double *AA_RESTRICT x, double *AA_RESTRICT y)
A "Signal" function.
AA_API int aa_la_inv(size_t n, double *A)
Inverse of A.
AA_API void aa_la_scal(size_t n, double alpha, double *x)
vector-scalar multiplication.
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)
Least Squares with Nullspace projection.
Runge-Kutta 2 integration.
AA_API void aa_vrand(size_t n, double *v)
fills v with random numbers in [0,1.0]
static double aa_fremainder(double a, double b)
Mathematical remainder, Fortran mod, Ada rem.
static void aa_vclamp(size_t n, double *v, double min, double max)
modify each element of v to be within range (min,max)
#define AA_API
calling and name mangling convention for functions
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)
Compute mean and standard deviation, excluding outliers.
AA_API void aa_la_sadd(size_t n, double alpha, const double *x, double *r)
vector-scalar addition.
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)
Solve an ordinary differential equation.
static long aa_lmodulo(long a, long b)
Fortran modulo, Ada mod.
double * D
additive constant
static double aa_stat_x2z(double x, double mu, double sigma)
Convert x-score to z-score a normal distribution.
double adapt_tol_inc
Increase step size if error is below tol_grow.
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)
Runge-Kutta-4-5 (Cash-Karp Method) integration.
AA_API void aa_la_vmul(size_t n, const double *x, const double *y, double *r)
Elementwise multiplication.
AA_API void aa_la_lls(size_t m, size_t n, size_t p, const double *A, const double *b, double *x)
Linear Least Squares.
AA_API size_t aa_fminloc(size_t n, double *v)
Returns index of minimum element in array v.
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)
Compute the path followed during integration.
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)
Compute mean and standard deviation, excluding outliers.
AA_API double aa_la_dot(size_t n, const double *x, const double *y)
Dot product.
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)
Runge-Kutta-4-5 (Dormand-Prince Method) integration.
AA_API void aa_la_dzdpinv(size_t m, size_t n, double s2_min, const double *A, double *A_star)
Deadzone, Damped Pseudo Inverse of A.
aa_ode_integrator
The known integration methods.
static double aa_sign(double val)
return the sign of val, one of {-1,0,1}
AA_API double aa_la_wdot(size_t n, const double *x, const double *A, const double *y)
Weighted inner product.
static double aa_ang_norm_pi(double an)
normalize angle on interval (-pi,pi)
AA_API void aa_la_sdiv(size_t n, double alpha, const double *x, double *r)
vector-scalar division.
AA_API void aa_la_smul(size_t n, double alpha, const double *x, double *r)
vector-scalar multiplication.
AA_API void aa_la_plane_fit(size_t m, size_t n, const double *points, double *plane)
Fit a plane to a set of points.
double * A
state transition
static void aa_la_mvmul(size_t m, size_t n, const double *A, const double *x, double *b)
matrix-vector multiplication
static double aa_horner3(double x, double a0, double a1, double a2)
Evaluate three-term polynomial using horner's rule.
static double aa_fclamp(double val, double min, double max)
return val within range (min,max)
AA_API void aa_la_axpy3(size_t n, double alpha, const double *x, const double *y, double *z)
increment by scale times vector.
Runge-Kutta 4 integration.
AA_API size_t aa_fmaxloc(size_t n, double *v)
Returns index of maximum element in array v.
AA_API void aa_stat_box_muller(double x1, double x2, double *z1, double *z2)
Generate 2 gaussian random numbers with stddev=1 from two uniform random numbers in interval (0...
#define AA_MEM_ZERO(dst, n_elem)
Set n_elem elements at dst to zero.
static double aa_stat_z2x(double z, double mu, double sigma)
Convert z-score to x-score a normal distribution.