amino  1.0-beta2
Lightweight Robot Utility Library
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
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
58 #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 void aa_vrand(size_t n, double *v);
174 
175 
177 static inline double
178 aa_horner3( double x, double a0, double a1, double a2 )
179 {
180  return a0 + x * ( a1 + x*a2 );
181 }
182 
188 static inline void
189 aa_sincos( double x, double *s, double *c )
190 {
191  *s = sin(x);
192  *c = cos(x);
193 }
194 
195 
196 /********/
197 /* Stat */
198 /********/
199 
205 AA_API void aa_stat_box_muller(double x1, double x2, double *z1, double *z2);
206 
209 static inline double aa_stat_z2x(double z, double mu, double sigma) {
210  return (z * sigma) + mu;
211 }
212 
215 static inline double aa_stat_x2z(double x, double mu, double sigma) {
216  return (x-mu)/sigma;
217 }
218 
220 AA_API double aa_stat_mean( size_t n, const double *x);
221 
223 AA_API double aa_stat_std( size_t n, const double *x);
224 
237 AA_API size_t aa_stat_excluded_mean_std( size_t n, const double *x,
238  double *pmu, double *psigma,
239  double zmin, double zmax,
240  size_t max_iterations );
241 
242 
243 
244 
245 
247 AA_API double aa_stat_circ_mean( size_t n, const double *x);
248 
250 AA_API double aa_stat_circ_std( size_t n, const double *x);
251 
262 AA_API size_t aa_stat_excluded_circ_mean_std( size_t n, const double *x,
263  double *pmu, double *psigma,
264  double zmin, double zmax,
265  size_t max_iterations );
266 
267 
275 AA_API void aa_stat_vmean( size_t m, size_t n, const double *X,
276  double *mu);
277 
286 AA_API void aa_stat_vmean_cov( size_t m, size_t n, const double *X,
287  double *mu, double *E);
288 
289 
291 double aa_stat_mahalanobis( size_t m, const double *x,
292  const double *mu, const double *E_inv);
293 
294 /**********/
295 /* Angles */
296 /**********/
297 
299 static inline double aa_ang_rad2deg( double rad ) {
300  return rad*180.0/M_PI;
301 }
302 
304 static inline double aa_ang_deg2rad( double deg ) {
305  return deg*M_PI/180;
306 }
307 
308 
310 static inline double aa_ang_norm_2pi( double an ) {
311  return aa_fmodulo( an, 2*M_PI );
312 }
313 
315 static inline double aa_ang_norm_pi( double an ) {
316  //return aa_fmodulo( an + M_PI, 2*M_PI ) - M_PI;
317  return remainder( an, 2*M_PI );
318 }
319 
320 
322 static inline double aa_ang_delta( double a, double b) {
323  return aa_ang_norm_pi( aa_ang_norm_pi(a) - aa_ang_norm_pi(b) );
324 }
325 
326 
327 
328 /************************/
329 /* Dense Linear Algebra */
330 /************************/
331 
338 #define AA_MATCOL(A, lda, col) ((A)+(col)*(lda))
339 
347 #define AA_MATREF(A, lda, row, col) (AA_MATCOL(A,lda,col)[row])
348 
349 /*--- Scalar Ops ---*/
350 
352 AA_API double aa_la_min( size_t n, const double *x );
353 
355 AA_API double aa_la_max( size_t n, const double *x );
356 
360 AA_API double aa_la_dot( size_t n, const double *x, const double *y );
361 
365 AA_API double aa_la_norm( size_t n, const double *x );
366 
370 AA_API double aa_la_ssd( size_t n, const double *x, const double *y );
371 
375 AA_API double aa_la_dist( size_t n, const double *x, const double *y );
376 
377 /*--- Vector Ops ---*/
378 
382 AA_API void aa_la_sadd( size_t n, double alpha, const double *x, double *r );
383 
384 
388 AA_API void aa_la_scal( size_t n, double alpha, double *x );
389 
390 
394 AA_API void aa_la_vinc( size_t n, const double *x, double *y );
395 
399 AA_API void aa_la_sinc( size_t n, double alpha, double *x );
400 
401 
405 AA_API void aa_la_axpy( size_t n, double alpha, const double *x, double *y );
406 
407 
414 AA_API void aa_la_axpy3( size_t n, double alpha,
415  const double *x, const double *y, double *z );
416 
420 AA_API void aa_la_smul( size_t n, double alpha, const double *x, double *r );
421 
425 AA_API void aa_la_ssub( size_t n, double alpha, const double *x, double *r );
426 
430 AA_API void aa_la_sdiv( size_t n, double alpha, const double *x, double *r );
431 
435 AA_API void aa_la_vadd( size_t n, const double *x, const double *y, double *r );
436 
440 AA_API void aa_la_vsub( size_t n, const double *x, const double *y, double *r );
441 
445 AA_API void aa_la_vmul( size_t n, const double *x, const double *y, double *r );
446 
450 AA_API void aa_la_vdiv( size_t n, const double *x, const double *y, double *r );
451 
455 AA_API void aa_la_cross( const double a[3], const double b[3], double c[3] );
456 
460 AA_API void aa_la_normalize( size_t n, double *x );
461 
462 
469 AA_API double aa_la_point_plane( size_t n,
470  const double *point, const double *plane );
471 
472 /*--- Matrix Ops --- */
473 
474 
476 AA_API void aa_la_transpose( size_t n, double *A );
478 AA_API void aa_la_transpose2( size_t m, size_t n, const double *A, double *At );
479 
481 static inline void
482 aa_la_diag( size_t n, double *A, double x ) {
483  for( size_t i = 0; i < n; i ++ )
484  A[i*n+i] = x;
485 }
486 
488 static inline void
489 aa_la_ident( size_t n, double *A ) {
490  AA_MEM_ZERO(A, n*n);
491  aa_la_diag(n,A,1.0);
492 }
493 
495 static inline void
496 aa_la_mvmul( size_t m, size_t n, const double *A, const double *x, double *b ) {
497  cblas_dgemv( CblasColMajor, CblasNoTrans, (int)m, (int)n,
498  1.0, A, (int)m,
499  x, 1, 0, b, 1 );
500 }
501 
506 AA_API double aa_la_wdot( size_t n,
507  const double *x, const double *A, const double *y );
508 
509 
523 AA_API int aa_la_svd( size_t m, size_t n, const double *A, double *U, double *S, double *Vt );
524 
532 AA_API int aa_la_inv( size_t n, double *A );
533 
534 
539 AA_API void aa_la_inverse3x3_( const double R[9], double S[9] );
540 
541 
546 static inline void aa_la_inverse3x3( const double R[9], double S[9] ) {
547  aa_la_inverse3x3_(R,S);
548 }
549 
550 
554 double aa_la_det3x3( const double R[AA_RESTRICT 9] );
555 
558 AA_API double aa_la_trace( size_t n, const double *A );
559 
577 AA_API void aa_la_dpinv( size_t m, size_t n, double k, const double *A, double *A_star );
578 
579 
597 AA_API void aa_la_dzdpinv( size_t m, size_t n, double s2_min, const double *A, double *A_star ) ;
598 
612 AA_API void aa_la_dls( size_t m, size_t n, double k, const double *A, const double *b, double *x );
613 
614 
630 AA_API void aa_la_xlsnp( size_t m, size_t n,
631  const double *A, const double *A_star, const double *b,
632  const double *xp, double *x );
633 
634 
635 
651 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 );
652 
663 AA_API void aa_la_lls( size_t m, size_t n, size_t p, const double *A, const double *b, double *x );
664 
673 AA_API int aa_la_care_laub( size_t m, size_t n, size_t p,
674  const double *AA_RESTRICT A,
675  const double *AA_RESTRICT B,
676  const double *AA_RESTRICT C,
677  double *AA_RESTRICT X );
678 
679 
690 AA_API void aa_la_linterp( size_t n,
691  double t0, const double *X0,
692  double t1, const double *X1,
693  double ti, double *Xi );
694 
695 
708 AA_API void aa_la_quadterp( size_t n,
709  double t0, const double *X0,
710  double t1, const double *X1,
711  double t2, const double *X2,
712  double ti, double *Xi );
713 
716 AA_API void aa_la_quadterp_dx( size_t n,
717  double t0, const double *X0,
718  double t1, const double *X1,
719  double t2, const double *X2,
720  double ti, double *dXi );
721 
722 
728 AA_API void aa_la_plane_hessian( size_t n, double *plane );
729 
737 AA_API void aa_la_plane_fit( size_t m, size_t n,
738  const double *points, double *plane );
739 
740 /*--- Systems and Signals --- */
741 
754 AA_API void aa_lsim_dstep( size_t m, size_t n,
755  const double *AA_RESTRICT A,
756  const double *AA_RESTRICT B,
757  const double *AA_RESTRICT x0,
758  const double *AA_RESTRICT u,
759  double *AA_RESTRICT x1 );
760 
775 AA_API void aa_lsim_estep( size_t m, size_t n,
776  double dt,
777  const double *AA_RESTRICT A,
778  const double *AA_RESTRICT B,
779  const double *AA_RESTRICT x0,
780  const double *AA_RESTRICT u,
781  double *AA_RESTRICT x1 );
782 
783 
788 typedef void aa_sys_fun( const void *cx,
789  double t, const double *AA_RESTRICT x,
790  double *AA_RESTRICT y );
791 
810 };
811 
815 #define AA_ODE_EULER AA_ODE_RK1
816 
820 #define AA_ODE_HEUN AA_ODE_RK2
821 
822 
827 typedef int aa_ode_check( void *cx, double t, double * AA_RESTRICT x, double *AA_RESTRICT y );
828 
835 
838 
843 
848 };
849 
853 AA_API int aa_ode_sol( enum aa_ode_integrator integrator,
854  const struct aa_ode_sol_opts * AA_RESTRICT opts,
855  size_t n,
856  aa_sys_fun sys, const void *sys_cx,
857  aa_ode_check check, void *check_cx,
858  double t0, double dt0,
859  const double *AA_RESTRICT x0,
860  double *AA_RESTRICT x1 );
861 
865 typedef void aa_odestep_fixed( size_t n, aa_sys_fun sys, const void *cx,
866  double t0, double dt,
867  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
868 
872 typedef void aa_odestep_adaptive( size_t n, aa_sys_fun sys, const void *cx,
873  double t0, double dt,
874  const double *AA_RESTRICT x0,
875  double *AA_RESTRICT k,
876  double *AA_RESTRICT x_n_1,
877  double *AA_RESTRICT x_n );
878 
884 AA_API void aa_odestep_euler( size_t n, double dt,
885  const double *AA_RESTRICT dx,
886  const double *AA_RESTRICT x0,
887  double *AA_RESTRICT x1 );
888 
889 
890 AA_API void aa_odestep_rk1( size_t n, aa_sys_fun sys, const void *cx,
891  double t0, double dt,
892  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
893 
904 AA_API void aa_odestep_rk2( size_t n, aa_sys_fun sys, const void *cx,
905  double t0, double dt,
906  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
907 
918 AA_API void aa_odestep_rk4( size_t n, aa_sys_fun sys, const void *cx,
919  double t0, double dt,
920  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
921 
944 AA_API void aa_odestep_rkf45( size_t n, aa_sys_fun sys, const void *cx,
945  double t0, double dt,
946  const double *AA_RESTRICT x0,
947  double *AA_RESTRICT k,
948  double *AA_RESTRICT x4,
949  double *AA_RESTRICT x5 );
950 
973 AA_API void aa_odestep_rkck45( size_t n, aa_sys_fun sys, const void *cx,
974  double t0, double dt,
975  const double *AA_RESTRICT x0,
976  double *AA_RESTRICT k,
977  double *AA_RESTRICT x4,
978  double *AA_RESTRICT x5 );
979 
980 
1004 AA_API void aa_odestep_dorpri45( size_t n, aa_sys_fun sys, const void *cx,
1005  double t0, double dt,
1006  const double *AA_RESTRICT x0,
1007  double *AA_RESTRICT k,
1008  double *AA_RESTRICT x4,
1009  double *AA_RESTRICT x5 );
1010 
1011 
1034 AA_API void aa_odestep_rkbs23( size_t n, aa_sys_fun sys, const void *cx,
1035  double t0, double dt,
1036  const double *AA_RESTRICT x0,
1037  double *AA_RESTRICT k,
1038  double *AA_RESTRICT x4,
1039  double *AA_RESTRICT x5 );
1040 
1041 
1055 AA_API void aa_lsim_rk4step( size_t m, size_t n,
1056  double dt,
1057  const double *AA_RESTRICT A,
1058  const double *AA_RESTRICT B,
1059  const double *AA_RESTRICT x0,
1060  const double *AA_RESTRICT u,
1061  double *AA_RESTRICT x1 );
1062 
1063 
1066 typedef struct {
1067  size_t n;
1068  double *A;
1069  double *D;
1070 } aa_sys_affine_t;
1071 
1076 AA_API void aa_sys_affine( const aa_sys_affine_t *cx,
1077  double t, const double *AA_RESTRICT x,
1078  double *AA_RESTRICT dx );
1079 
1080 
1081 /*--- GCC Vector Extensions --- */
1082 
1083 //typedef doulbe aa_v2df_t __attribute__ (( vector_size(2*sizeof(double)) ));
1084 
1085 #endif //AA_MATH_H
#define AA_MODULO(a, b)
Fortran modulo, Ada mod.
Definition: math.h:128
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
Definition: math.h:89
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:827
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.
Definition: math.h:832
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.
Definition: math.h:805
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
Definition: math.h:123
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
Definition: math.h:75
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)
Definition: math.h:322
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 void aa_sincos(double x, double *s, double *c)
Portable sincos.
Definition: math.h:189
static double aa_ang_rad2deg(double rad)
convert radians to degrees
Definition: math.h:299
static double aa_ang_norm_2pi(double an)
normalize angle on interval [0,2pi)
Definition: math.h:310
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:865
static void aa_la_ident(size_t n, double *A)
Set A to the identity matrix.
Definition: math.h:489
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.
Definition: math.h:114
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.
Definition: math.h:131
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.
Definition: math.h:142
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.
Definition: math.h:482
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.
Definition: math.h:803
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.
Definition: math.h:872
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.
Definition: math.h:546
Runge-Kutta 1 integration.
Definition: math.h:797
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.
Definition: math.h:152
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
Definition: debug.h:104
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.
Definition: math.h:842
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.
Definition: math.h:809
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.
Definition: math.h:834
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.
Definition: math.h:1066
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.
Definition: amino.h:99
static int aa_iremainder(int a, int b)
Fortran mod, Ada rem.
Definition: math.h:147
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.
Definition: math.h:807
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
Definition: math.h:304
double adapt_factor_inc
Factor to increse step size.
Definition: math.h:847
void aa_sys_fun(const void *cx, double t, const double *AA_RESTRICT x, double *AA_RESTRICT y)
A "Signal" function.
Definition: math.h:788
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.
Definition: math.h:799
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.
Definition: math.h:157
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
#define AA_API
calling and name mangling convention for functions
Definition: amino.h:95
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.
size_t n
state size
Definition: math.h:1067
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.
Definition: math.h:137
double * D
additive constant
Definition: math.h:1069
static double aa_stat_x2z(double x, double mu, double sigma)
Convert x-score to z-score a normal distribution.
Definition: math.h:215
double adapt_tol_inc
Increase step size if error is below tol_grow.
Definition: math.h:837
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.
Definition: math.h:795
static double aa_sign(double val)
return the sign of val, one of {-1,0,1}
Definition: math.h:104
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)
Definition: math.h:315
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
Definition: math.h:1068
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:496
static double aa_horner3(double x, double a0, double a1, double a2)
Evaluate three-term polynomial using horner's rule.
Definition: math.h:178
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_axpy3(size_t n, double alpha, const double *x, const double *y, double *z)
increment by scale times vector.
Runge-Kutta 4 integration.
Definition: math.h:801
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.
Definition: mem.h:526
static double aa_stat_z2x(double z, double mu, double sigma)
Convert z-score to x-score a normal distribution.
Definition: math.h:209