amino  1.0-beta2
Lightweight Robot Utility Library
math.h
Go to the documentation of this file.
1 /* -*- mode: C; c-basic-offset: 4 -*- */
2 /* ex: set shiftwidth=4 tabstop=4 expandtab: */
3 /*
4  * Copyright (c) 2010-2011, Georgia Tech Research Corporation
5  * All rights reserved.
6  *
7  * Author(s): Neil T. Dantam <ntd@gatech.edu>
8  * Georgia Tech Humanoid Robotics Lab
9  * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
10  *
11  *
12  * This file is provided under the following "BSD-style" License:
13  *
14  *
15  * Redistribution and use in source and binary forms, with or
16  * without modification, are permitted provided that the following
17  * conditions are met:
18  *
19  * * Redistributions of source code must retain the above copyright
20  * notice, this list of conditions and the following disclaimer.
21  *
22  * * Redistributions in binary form must reproduce the above
23  * copyright notice, this list of conditions and the following
24  * disclaimer in the documentation and/or other materials provided
25  * with the distribution.
26  *
27  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
28  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
29  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
30  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
31  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
32  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
33  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
34  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
35  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
36  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
37  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
38  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
39  * POSSIBILITY OF SUCH DAMAGE.
40  *
41  */
42 #ifndef AA_MATH_H
43 #define AA_MATH_H
44 
53 /***********/
54 /* Scalars */
55 /***********/
56 
57 #ifndef AA_EPSILON
59 #define AA_EPSILON .001
60 #endif //AA_EPSILON
61 
63 #define AA_MAX(a,b) \
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; })
67 
69 #define AA_MIN(a,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; })
73 
75 AA_DEPRECATED static inline double aa_clamp( double val, double level) {
76  if( val > level ) return level;
77  if( val < -level ) return -level;
78  return val;
79 }
80 
82 static inline double aa_fclamp( double val, double min, double max) {
83  if( val > max ) return max;
84  if( val < min ) return min;
85  return val;
86 }
87 
89 static inline double aa_fdeadzone( double val, double min, double max, double deadval) {
90  if( min < val && max > val ) return deadval;
91  else return val;
92 }
93 
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;
99  }
100 }
101 
102 
104 static inline double aa_sign( double val ) {
105  if( val > 0 ) return 1;
106  if( val < 0 ) return -1;
107  return 0;
108 }
109 
111 AA_API int aa_isfok( double x );
112 
114 static inline int aa_feq( double a, double b, double tol ) {
115  return fabs(a-b) <= tol;
116 }
117 
119 AA_API int aa_veq( size_t n, const double *a, const double *b, double tol );
120 
121 
123 static inline double aa_fsq( double a ) {
124  return a * a;
125 }
126 
128 #define AA_MODULO(a,b) (((a) % (b)) + (b)) % (b);
129 
131 static inline int aa_imodulo( int a, int b ) {
132  //return ((a % b) + b) % b;
133  return AA_MODULO(a,b);
134 }
135 
137 static inline long aa_lmodulo( long a, long b ) {
138  return AA_MODULO(a,b);
139 }
140 
142 static inline int64_t aa_imodulo64( int64_t a, int64_t b ) {
143  return AA_MODULO(a,b);
144 }
145 
147 static inline int aa_iremainder( int a, int b ) {
148  return a % b;
149 }
150 
152 static inline double aa_fmodulo( double a, double b ) {
153  return fmod(fmod(a, b) + b, b);
154 }
155 
157 static inline double aa_fremainder( double a, double b ) {
158  return fmod(a , b);
159 }
160 
163 AA_API size_t aa_fminloc( size_t n, double *v );
164 
167 AA_API size_t aa_fmaxloc( size_t n, double *v );
168 
170 AA_API double aa_frand();
171 
173 AA_API double aa_frand_minmax(double min, double max);
174 
176 AA_API void aa_vrand(size_t n, double *v);
177 
178 
180 static inline double
181 aa_horner3( double x, double a0, double a1, double a2 )
182 {
183  return a0 + x * ( a1 + x*a2 );
184 }
185 
191 static inline void
192 aa_sincos( double x, double *s, double *c )
193 {
194  *s = sin(x);
195  *c = cos(x);
196 }
197 
198 
199 /********/
200 /* Stat */
201 /********/
202 
208 AA_API void aa_stat_box_muller(double x1, double x2, double *z1, double *z2);
209 
212 static inline double aa_stat_z2x(double z, double mu, double sigma) {
213  return (z * sigma) + mu;
214 }
215 
218 static inline double aa_stat_x2z(double x, double mu, double sigma) {
219  return (x-mu)/sigma;
220 }
221 
223 AA_API double aa_stat_mean( size_t n, const double *x);
224 
226 AA_API double aa_stat_std( size_t n, const double *x);
227 
238 AA_API size_t aa_stat_excluded_mean_std( size_t n, const double *x,
239  double *pmu, double *psigma,
240  double zmin, double zmax,
241  size_t max_iterations );
242 
243 
244 
245 
246 
248 AA_API double aa_stat_circ_mean( size_t n, const double *x);
249 
251 AA_API double aa_stat_circ_std( size_t n, const double *x);
252 
263 AA_API size_t aa_stat_excluded_circ_mean_std( size_t n, const double *x,
264  double *pmu, double *psigma,
265  double zmin, double zmax,
266  size_t max_iterations );
267 
268 
276 AA_API void aa_stat_vmean( size_t m, size_t n, const double *X,
277  double *mu);
278 
287 AA_API void aa_stat_vmean_cov( size_t m, size_t n, const double *X,
288  double *mu, double *E);
289 
290 
292 double aa_stat_mahalanobis( size_t m, const double *x,
293  const double *mu, const double *E_inv);
294 
295 /**********/
296 /* Angles */
297 /**********/
298 
300 static inline double aa_ang_rad2deg( double rad ) {
301  return rad*180.0/M_PI;
302 }
303 
305 static inline double aa_ang_deg2rad( double deg ) {
306  return deg*M_PI/180;
307 }
308 
309 
311 static inline double aa_ang_norm_2pi( double an ) {
312  return aa_fmodulo( an, 2*M_PI );
313 }
314 
316 static inline double aa_ang_norm_pi( double an ) {
317  //return aa_fmodulo( an + M_PI, 2*M_PI ) - M_PI;
318  return remainder( an, 2*M_PI );
319 }
320 
321 
323 static inline double aa_ang_delta( double a, double b) {
324  return aa_ang_norm_pi( aa_ang_norm_pi(a) - aa_ang_norm_pi(b) );
325 }
326 
327 
328 
329 /************************/
330 /* Dense Linear Algebra */
331 /************************/
332 
339 #define AA_MATCOL(A, lda, col) ((A)+(col)*(lda))
340 
348 #define AA_MATREF(A, lda, row, col) (AA_MATCOL(A,lda,col)[row])
349 
350 /*--- Scalar Ops ---*/
351 
353 AA_API double aa_la_min( size_t n, const double *x );
354 
356 AA_API double aa_la_max( size_t n, const double *x );
357 
361 AA_API double aa_la_dot( size_t n, const double *x, const double *y );
362 
366 AA_API double aa_la_norm( size_t n, const double *x );
367 
371 AA_API double aa_la_ssd( size_t n, const double *x, const double *y );
372 
376 AA_API double aa_la_dist( size_t n, const double *x, const double *y );
377 
378 /*--- Vector Ops ---*/
379 
383 AA_API void aa_la_sadd( size_t n, double alpha, const double *x, double *r );
384 
385 
389 AA_API void aa_la_scal( size_t n, double alpha, double *x );
390 
391 
395 AA_API void aa_la_vinc( size_t n, const double *x, double *y );
396 
400 AA_API void aa_la_sinc( size_t n, double alpha, double *x );
401 
402 
406 AA_API void aa_la_axpy( size_t n, double alpha, const double *x, double *y );
407 
408 
415 AA_API void aa_la_axpy3( size_t n, double alpha,
416  const double *x, const double *y, double *z );
417 
421 AA_API void aa_la_smul( size_t n, double alpha, const double *x, double *r );
422 
426 AA_API void aa_la_ssub( size_t n, double alpha, const double *x, double *r );
427 
431 AA_API void aa_la_sdiv( size_t n, double alpha, const double *x, double *r );
432 
436 AA_API void aa_la_vadd( size_t n, const double *x, const double *y, double *r );
437 
441 AA_API void aa_la_vsub( size_t n, const double *x, const double *y, double *r );
442 
446 AA_API void aa_la_vmul( size_t n, const double *x, const double *y, double *r );
447 
451 AA_API void aa_la_vdiv( size_t n, const double *x, const double *y, double *r );
452 
456 AA_API void aa_la_cross( const double a[3], const double b[3], double c[3] );
457 
461 AA_API void aa_la_normalize( size_t n, double *x );
462 
463 
470 AA_API double aa_la_point_plane( size_t n,
471  const double *point, const double *plane );
472 
473 /*--- Matrix Ops --- */
474 
475 
477 AA_API void aa_la_transpose( size_t n, double *A );
479 AA_API void aa_la_transpose2( size_t m, size_t n, const double *A, double *At );
480 
482 static inline void
483 aa_la_diag( size_t n, double *A, double x ) {
484  for( size_t i = 0; i < n; i ++ )
485  A[i*n+i] = x;
486 }
487 
489 static inline void
490 aa_la_ident( size_t n, double *A ) {
491  AA_MEM_ZERO(A, n*n);
492  aa_la_diag(n,A,1.0);
493 }
494 
496 static inline void
497 aa_la_mvmul( size_t m, size_t n, const double *A, const double *x, double *b ) {
498  cblas_dgemv( CblasColMajor, CblasNoTrans, (int)m, (int)n,
499  1.0, A, (int)m,
500  x, 1, 0, b, 1 );
501 }
502 
507 AA_API double aa_la_wdot( size_t n,
508  const double *x, const double *A, const double *y );
509 
510 
524 AA_API int aa_la_svd( size_t m, size_t n, const double *A, double *U, double *S, double *Vt );
525 
533 AA_API int aa_la_inv( size_t n, double *A );
534 
535 
540 AA_API void aa_la_inverse3x3_( const double R[9], double S[9] );
541 
542 
547 static inline void aa_la_inverse3x3( const double R[9], double S[9] ) {
548  aa_la_inverse3x3_(R,S);
549 }
550 
551 
555 double aa_la_det3x3( const double R[AA_RESTRICT 9] );
556 
559 AA_API double aa_la_trace( size_t n, const double *A );
560 
578 AA_API void aa_la_dpinv( size_t m, size_t n, double k, const double *A, double *A_star );
579 
580 
598 AA_API void aa_la_dzdpinv( size_t m, size_t n, double s2_min, const double *A, double *A_star ) ;
599 
613 AA_API void aa_la_dls( size_t m, size_t n, double k, const double *A, const double *b, double *x );
614 
615 
627 AA_API void aa_la_np( size_t m, size_t n,
628  const double *A, const double *A_star,
629  double *B );
630 
646 AA_API void aa_la_xlsnp( size_t m, size_t n,
647  const double *A, const double *A_star, const double *b,
648  const double *xp, double *x );
649 
650 
651 
667 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 );
668 
679 AA_API void aa_la_lls( size_t m, size_t n, size_t p, const double *A, const double *b, double *x );
680 
689 AA_API int aa_la_care_laub( size_t m, size_t n, size_t p,
690  const double *AA_RESTRICT A,
691  const double *AA_RESTRICT B,
692  const double *AA_RESTRICT C,
693  double *AA_RESTRICT X );
694 
695 
706 AA_API void aa_la_linterp( size_t n,
707  double t0, const double *X0,
708  double t1, const double *X1,
709  double ti, double *Xi );
710 
711 
724 AA_API void aa_la_quadterp( size_t n,
725  double t0, const double *X0,
726  double t1, const double *X1,
727  double t2, const double *X2,
728  double ti, double *Xi );
729 
732 AA_API void aa_la_quadterp_dx( size_t n,
733  double t0, const double *X0,
734  double t1, const double *X1,
735  double t2, const double *X2,
736  double ti, double *dXi );
737 
738 
744 AA_API void aa_la_plane_hessian( size_t n, double *plane );
745 
753 AA_API void aa_la_plane_fit( size_t m, size_t n,
754  const double *points, double *plane );
755 
756 /*--- Systems and Signals --- */
757 
770 AA_API void aa_lsim_dstep( size_t m, size_t n,
771  const double *AA_RESTRICT A,
772  const double *AA_RESTRICT B,
773  const double *AA_RESTRICT x0,
774  const double *AA_RESTRICT u,
775  double *AA_RESTRICT x1 );
776 
791 AA_API void aa_lsim_estep( size_t m, size_t n,
792  double dt,
793  const double *AA_RESTRICT A,
794  const double *AA_RESTRICT B,
795  const double *AA_RESTRICT x0,
796  const double *AA_RESTRICT u,
797  double *AA_RESTRICT x1 );
798 
799 
804 typedef void aa_sys_fun( const void *cx,
805  double t, const double *AA_RESTRICT x,
806  double *AA_RESTRICT y );
807 
826 };
827 
831 #define AA_ODE_EULER AA_ODE_RK1
832 
836 #define AA_ODE_HEUN AA_ODE_RK2
837 
838 
845 typedef int aa_ode_check( void *cx, double t, double * AA_RESTRICT x, double *AA_RESTRICT y );
846 
853 
856 
861 
866 };
867 
871 AA_API int aa_ode_sol( enum aa_ode_integrator integrator,
872  const struct aa_ode_sol_opts * AA_RESTRICT opts,
873  size_t n,
874  aa_sys_fun sys, const void *sys_cx,
875  aa_ode_check check, void *check_cx,
876  double t0, double dt0,
877  const double *AA_RESTRICT x0,
878  double *AA_RESTRICT x1 );
879 
883 AA_API int
885  const struct aa_ode_sol_opts * AA_RESTRICT opts,
886  size_t n,
887  aa_sys_fun sys, const void *sys_cx,
888  aa_ode_check check, void *check_cx,
889  double t0, double dt0,
890  const double *AA_RESTRICT x0,
891  struct aa_mem_region *region, size_t *n_points, double **path );
892 
896 typedef void aa_odestep_fixed( size_t n, aa_sys_fun sys, const void *cx,
897  double t0, double dt,
898  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
899 
903 typedef void aa_odestep_adaptive( size_t n, aa_sys_fun sys, const void *cx,
904  double t0, double dt,
905  const double *AA_RESTRICT x0,
906  double *AA_RESTRICT k,
907  double *AA_RESTRICT x_n_1,
908  double *AA_RESTRICT x_n );
909 
915 AA_API void aa_odestep_euler( size_t n, double dt,
916  const double *AA_RESTRICT dx,
917  const double *AA_RESTRICT x0,
918  double *AA_RESTRICT x1 );
919 
920 
926 AA_API void aa_odestep_rk1( size_t n, aa_sys_fun sys, const void *cx,
927  double t0, double dt,
928  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
929 
940 AA_API void aa_odestep_rk2( size_t n, aa_sys_fun sys, const void *cx,
941  double t0, double dt,
942  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
943 
954 AA_API void aa_odestep_rk4( size_t n, aa_sys_fun sys, const void *cx,
955  double t0, double dt,
956  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
957 
980 AA_API void aa_odestep_rkf45( size_t n, aa_sys_fun sys, const void *cx,
981  double t0, double dt,
982  const double *AA_RESTRICT x0,
983  double *AA_RESTRICT k,
984  double *AA_RESTRICT x4,
985  double *AA_RESTRICT x5 );
986 
1009 AA_API void aa_odestep_rkck45( size_t n, aa_sys_fun sys, const void *cx,
1010  double t0, double dt,
1011  const double *AA_RESTRICT x0,
1012  double *AA_RESTRICT k,
1013  double *AA_RESTRICT x4,
1014  double *AA_RESTRICT x5 );
1015 
1016 
1040 AA_API void aa_odestep_dorpri45( size_t n, aa_sys_fun sys, const void *cx,
1041  double t0, double dt,
1042  const double *AA_RESTRICT x0,
1043  double *AA_RESTRICT k,
1044  double *AA_RESTRICT x4,
1045  double *AA_RESTRICT x5 );
1046 
1047 
1070 AA_API void aa_odestep_rkbs23( size_t n, aa_sys_fun sys, const void *cx,
1071  double t0, double dt,
1072  const double *AA_RESTRICT x0,
1073  double *AA_RESTRICT k,
1074  double *AA_RESTRICT x4,
1075  double *AA_RESTRICT x5 );
1076 
1077 
1091 AA_API void aa_lsim_rk4step( size_t m, size_t n,
1092  double dt,
1093  const double *AA_RESTRICT A,
1094  const double *AA_RESTRICT B,
1095  const double *AA_RESTRICT x0,
1096  const double *AA_RESTRICT u,
1097  double *AA_RESTRICT x1 );
1098 
1099 
1102 typedef struct {
1103  size_t n;
1104  double *A;
1105  double *D;
1106 } aa_sys_affine_t;
1107 
1113  double t, const double *AA_RESTRICT x,
1114  double *AA_RESTRICT dx );
1115 
1116 
1117 /*--- GCC Vector Extensions --- */
1118 
1119 //typedef doulbe aa_v2df_t __attribute__ (( vector_size(2*sizeof(double)) ));
1120 
1121 #endif //AA_MATH_H
#define AA_API
calling and name mangling convention for functions
Definition: amino.h:95
#define AA_RESTRICT
Defined restrict keyword based on language flavor.
Definition: amino.h:99
AA_EXTERN const char *aa_verbf_prefix AA_DEPRECATED
don't use
Definition: debug.h:104
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.
static void aa_la_diag(size_t n, double *A, double x)
Set diagonal of A to x.
Definition: math.h:483
AA_API size_t aa_fmaxloc(size_t n, double *v)
Returns index of maximum element in array v.
AA_API void aa_la_cross(const double a[3], const double b[3], double c[3])
Cross product.
static double aa_stat_z2x(double z, double mu, double sigma)
Convert z-score to x-score a normal distribution.
Definition: math.h:212
AA_API double aa_stat_circ_std(size_t n, const double *x)
Compute standard deviation of vector x.
AA_API void aa_la_transpose(size_t n, double *A)
transpose square matrix A in place
aa_ode_integrator
The known integration methods.
Definition: math.h:811
@ AA_ODE_RK45_F
Runge-Kutta-Fehlberg integration.
Definition: math.h:819
@ AA_ODE_RK45_CK
Runge-Kutta 4-5 / Cash Karp integration.
Definition: math.h:821
@ AA_ODE_RK4
Runge-Kutta 4 integration.
Definition: math.h:817
@ AA_ODE_RK23_BS
Runge-Kutta 2-3 / Bogacki-Shampine integration.
Definition: math.h:825
@ AA_ODE_RK2
Runge-Kutta 2 integration.
Definition: math.h:815
@ AA_ODE_RK45_DP
Runge-Kutta 4-5 / Dormand-Prince integration.
Definition: math.h:823
@ AA_ODE_RK1
Runge-Kutta 1 integration.
Definition: math.h:813
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.
Definition: math.h:845
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.
AA_API void aa_la_np(size_t m, size_t n, const double *A, const double *A_star, double *B)
Computer Nullspace Projection Matrix.
AA_API void aa_la_vdiv(size_t n, const double *x, const double *y, double *r)
Elementwise division.
#define AA_MODULO(a, b)
Fortran modulo, Ada mod.
Definition: math.h:128
AA_API double aa_stat_mean(size_t n, const double *x)
Compute mean of vector x.
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_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 double aa_la_norm(size_t n, const double *x)
Euclidean norm of x.
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.
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_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_la_vsub(size_t n, const double *x, const double *y, double *r)
Elementwise subtraction.
static double aa_fmodulo(double a, double b)
Mathematical modulo, Fortran modulo, Ada mod.
Definition: math.h:152
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.
static double aa_ang_deg2rad(double deg)
convert radians to degrees
Definition: math.h:305
static double aa_ang_delta(double a, double b)
Difference between two angles, interval (-pi,pi)
Definition: math.h:323
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)
Euler / Runge-Kutta-1 integration.
AA_API double aa_frand_minmax(double min, double max)
uniform pseudo-random in [min,max]
static double aa_fremainder(double a, double b)
Mathematical remainder, Fortran mod, Ada rem.
Definition: math.h:157
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_scal(size_t n, double alpha, double *x)
vector-scalar multiplication.
static long aa_lmodulo(long a, long b)
Fortran modulo, Ada mod.
Definition: math.h:137
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_la_inverse3x3_(const double R[9], double S[9])
Inverse of 3x3 matrix R.
static double aa_fclamp(double val, double min, double max)
return val within range (min,max)
Definition: math.h:82
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 double aa_la_trace(size_t n, const double *A)
Trace of a matrix.
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.
AA_API void aa_la_vinc(size_t n, const double *x, double *y)
increment by vector.
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 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.
static int64_t aa_imodulo64(int64_t a, int64_t b)
Fortran modulo, Ada mod.
Definition: math.h:142
AA_API void aa_la_plane_hessian(size_t n, double *plane)
Convert plane to hessian normal form.
AA_API void aa_la_smul(size_t n, double alpha, const double *x, double *r)
vector-scalar multiplication.
static double aa_fsq(double a)
square
Definition: math.h:123
static int aa_imodulo(int a, int b)
Fortran modulo, Ada mod.
Definition: math.h:131
static void aa_la_ident(size_t n, double *A)
Set A to the identity matrix.
Definition: math.h:490
AA_API double aa_la_wdot(size_t n, const double *x, const double *A, const double *y)
Weighted inner product.
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 int aa_feq(double a, double b, double tol)
Fuzzy equals.
Definition: math.h:114
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.
Definition: math.h:903
static double aa_horner3(double x, double a0, double a1, double a2)
Evaluate three-term polynomial using horner's rule.
Definition: math.h:181
AA_API double aa_la_max(size_t n, const double *x)
max of vector
AA_API void aa_la_sdiv(size_t n, double alpha, const double *x, double *r)
vector-scalar division.
static double aa_ang_rad2deg(double rad)
convert radians to degrees
Definition: math.h:300
AA_API int aa_isfok(double x)
returns one if x is not infinity or NAN
AA_API double aa_la_min(size_t n, const double *x)
min of vector
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.
static AA_DEPRECATED double aa_clamp(double val, double level)
force value to be within +/- level
Definition: math.h:75
AA_API void aa_stat_vmean(size_t m, size_t n, const double *X, double *mu)
Compute mean of vectors.
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_vrand(size_t n, double *v)
fills v with random numbers in [0,1.0]
AA_API void aa_la_ssub(size_t n, double alpha, const double *x, double *r)
vector-scalar subtraction.
AA_API int aa_veq(size_t n, const double *a, const double *b, double tol)
Fuzzy equals.
AA_API void aa_la_axpy(size_t n, double alpha, const double *x, double *y)
increment by scale times vector.
static void aa_la_inverse3x3(const double R[9], double S[9])
Inverse of 3x3 matrix R.
Definition: math.h:547
static void aa_la_mvmul(size_t m, size_t n, const double *A, const double *x, double *b)
matrix-vector multiplication
Definition: math.h:497
AA_API double aa_la_point_plane(size_t n, const double *point, const double *plane)
Point Plane Distance.
AA_API void aa_la_vadd(size_t n, const double *x, const double *y, double *r)
Elementwise addition.
AA_API void aa_la_axpy3(size_t n, double alpha, const double *x, const double *y, double *z)
increment by scale times vector.
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.
double aa_la_det3x3(const double R[AA_RESTRICT 9])
Determinant of 3x3 matrix R.
static double aa_ang_norm_2pi(double an)
normalize angle on interval [0,2pi)
Definition: math.h:311
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.
static void aa_vclamp(size_t n, double *v, double min, double max)
modify each element of v to be within range (min,max)
Definition: math.h:95
static double aa_stat_x2z(double x, double mu, double sigma)
Convert x-score to z-score a normal distribution.
Definition: math.h:218
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.
double aa_stat_mahalanobis(size_t m, const double *x, const double *mu, const double *E_inv)
Mahalanobis distance.
static void aa_sincos(double x, double *s, double *c)
Portable sincos.
Definition: math.h:192
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 int aa_la_inv(size_t n, double *A)
Inverse of A.
AA_API size_t aa_fminloc(size_t n, double *v)
Returns index of minimum element in array v.
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.
static int aa_iremainder(int a, int b)
Fortran mod, Ada rem.
Definition: math.h:147
void aa_sys_fun(const void *cx, double t, const double *AA_RESTRICT x, double *AA_RESTRICT y)
A "Signal" function.
Definition: math.h:804
AA_API double aa_stat_circ_mean(size_t n, const double *x)
Compute mean of angles.
static double aa_sign(double val)
return the sign of val, one of {-1,0,1}
Definition: math.h:104
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.
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.
Definition: math.h:896
AA_API double aa_frand()
uniform pseudo-random in [0,1.0]
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 double aa_stat_std(size_t n, const double *x)
Compute standard deviation of vector x.
AA_API void aa_la_sinc(size_t n, double alpha, double *x)
increment by scalar.
AA_API double aa_la_dot(size_t n, const double *x, const double *y)
Dot product.
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 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.
AA_API void aa_la_vmul(size_t n, const double *x, const double *y, double *r)
Elementwise multiplication.
AA_API double aa_la_dist(size_t n, const double *x, const double *y)
Euclidean Distance.
static double aa_ang_norm_pi(double an)
normalize angle on interval (-pi,pi)
Definition: math.h:316
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.
AA_API void aa_la_sadd(size_t n, double alpha, const double *x, double *r)
vector-scalar addition.
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.
AA_API double aa_la_ssd(size_t n, const double *x, const double *y)
Sum of Squared Differences.
AA_API void aa_la_normalize(size_t n, double *x)
Make x unit vector.
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.
static double aa_fdeadzone(double val, double min, double max, double deadval)
apply deadzone to val
Definition: math.h:89
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.
Definition: mem.h:578
Data Structure for Region-Based memory allocation.
Definition: mem.h:199
Options for the differential equation solver.
Definition: math.h:850
double adapt_factor_inc
Factor to increse step size.
Definition: math.h:865
double adapt_tol_dec
Decrease step size if error is greater than tol_shrink.
Definition: math.h:852
double adapt_factor_dec
Factor to decrease step size.
Definition: math.h:860
double adapt_tol_inc
Increase step size if error is below tol_grow.
Definition: math.h:855
Context-struct for function aa_sys_affine.
Definition: math.h:1102
size_t n
state size
Definition: math.h:1103
double * A
state transition
Definition: math.h:1104
double * D
additive constant
Definition: math.h:1105