54 #define AA_EPSILON .001
60 ({ const __typeof__(a) aa_$_max_a = (a); \
61 const __typeof__(b) aa_$_max_b = (b); \
62 (aa_$_max_a > aa_$_max_b) ? aa_$_max_a : aa_$_max_b; })
66 ({ const __typeof__(a) aa_$_min_a = (a); \
67 const __typeof__(b) aa_$_min_b = (b); \
68 (aa_$_min_a < aa_$_min_b) ? aa_$_min_a : aa_$_min_b; })
72 if( val > level )
return level;
73 if( val < -level )
return -level;
78 static inline double aa_fclamp(
double val,
double min,
double max) {
79 if( val > max )
return max;
80 if( val < min )
return min;
85 static inline double aa_fdeadzone(
double val,
double min,
double max,
double deadval) {
86 if( min < val && max > val )
return deadval;
91 static inline void aa_vclamp(
size_t n,
double *v,
double min,
double max) {
92 for(
size_t i = 0; i < n; i++ ) {
93 if( v[i] > max ) v[i] = max;
94 else if( v[i] < min ) v[i] = min;
101 if( val > 0 )
return 1;
102 if( val < 0 )
return -1;
110 static inline int aa_feq(
double a,
double b,
double tol ) {
111 return fabs(a-b) <= tol;
115 AA_API int aa_veq(
size_t n,
const double *a,
const double *b,
double tol );
119 static inline double aa_fsq(
double a ) {
124 #define AA_MODULO(a,b) (((a) % (b)) + (b)) % (b);
149 return fmod(fmod(a, b) + b, b);
176 return a0 + x * ( a1 + x*a2 );
181 aa_sincos(
double x,
double *s,
double *c )
202 static inline double aa_stat_z2x(
double z,
double mu,
double sigma) {
203 return (z * sigma) + mu;
208 static inline double aa_stat_x2z(
double x,
double mu,
double sigma) {
231 double *pmu,
double *psigma,
232 double zmin,
double zmax,
233 size_t max_iterations );
256 double *pmu,
double *psigma,
257 double zmin,
double zmax,
258 size_t max_iterations );
280 double *mu,
double *E);
285 const double *mu,
const double *E_inv);
293 return rad*180.0/M_PI;
310 return remainder( an, 2*M_PI );
331 #define AA_MATCOL(A, lda, col) ((A)+(col)*(lda))
340 #define AA_MATREF(A, lda, row, col) (AA_MATCOL(A,lda,col)[row])
375 AA_API void aa_la_sadd(
size_t n,
double alpha,
const double *x,
double *r );
398 AA_API void aa_la_axpy(
size_t n,
double alpha,
const double *x,
double *y );
408 const double *x,
const double *y,
double *z );
413 AA_API void aa_la_smul(
size_t n,
double alpha,
const double *x,
double *r );
418 AA_API void aa_la_ssub(
size_t n,
double alpha,
const double *x,
double *r );
423 AA_API void aa_la_sdiv(
size_t n,
double alpha,
const double *x,
double *r );
428 AA_API void aa_la_vadd(
size_t n,
const double *x,
const double *y,
double *r );
433 AA_API void aa_la_vsub(
size_t n,
const double *x,
const double *y,
double *r );
438 AA_API void aa_la_vmul(
size_t n,
const double *x,
const double *y,
double *r );
443 AA_API void aa_la_vdiv(
size_t n,
const double *x,
const double *y,
double *r );
463 const double *point,
const double *plane );
476 for(
size_t i = 0; i < n; i ++ )
489 aa_la_mvmul(
size_t m,
size_t n,
const double *A,
const double *x,
double *b ) {
490 cblas_dgemv( CblasColMajor, CblasNoTrans, (
int)m, (
int)n,
500 const double *x,
const double *A,
const double *y );
516 AA_API int aa_la_svd(
size_t m,
size_t n,
const double *A,
double *U,
double *S,
double *Vt );
570 AA_API void aa_la_dpinv(
size_t m,
size_t n,
double k,
const double *A,
double *A_star );
590 AA_API void aa_la_dzdpinv(
size_t m,
size_t n,
double s2_min,
const double *A,
double *A_star ) ;
605 AA_API void aa_la_dls(
size_t m,
size_t n,
double k,
const double *A,
const double *b,
double *x );
624 const double *A,
const double *A_star,
const double *b,
625 const double *xp,
double *x );
644 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 );
656 AA_API void aa_la_lls(
size_t m,
size_t n,
size_t p,
const double *A,
const double *b,
double *x );
684 double t0,
const double *X0,
685 double t1,
const double *X1,
686 double ti,
double *Xi );
702 double t0,
const double *X0,
703 double t1,
const double *X1,
704 double t2,
const double *X2,
705 double ti,
double *Xi );
710 double t0,
const double *X0,
711 double t1,
const double *X1,
712 double t2,
const double *X2,
713 double ti,
double *dXi );
731 const double *points,
double *plane );
808 #define AA_ODE_EULER AA_ODE_RK1
813 #define AA_ODE_HEUN AA_ODE_RK2
843 aa_ode_check check,
void *check_cx,
844 double t0,
double dt0,
848 typedef void aa_odestep_fixed(
size_t n,
aa_sys_fun sys,
const void *cx,
849 double t0,
double dt,
852 typedef void aa_odestep_adaptive(
size_t n,
aa_sys_fun sys,
const void *cx,
853 double t0,
double dt,
871 double t0,
double dt,
885 double t0,
double dt,
899 double t0,
double dt,
925 double t0,
double dt,
954 double t0,
double dt,
985 double t0,
double dt,
1015 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
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.
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)
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.
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.
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 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.