amino
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 
49 /***********/
50 /* Scalars */
51 /***********/
52 
53 #ifndef AA_EPSILON
54 #define AA_EPSILON .001
56 #endif //AA_EPSILON
57 
59 #define AA_MAX(a,b) \
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; })
63 
65 #define AA_MIN(a,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; })
69 
71 AA_DEPRECATED static inline double aa_clamp( double val, double level) {
72  if( val > level ) return level;
73  if( val < -level ) return -level;
74  return val;
75 }
76 
78 static inline double aa_fclamp( double val, double min, double max) {
79  if( val > max ) return max;
80  if( val < min ) return min;
81  return val;
82 }
83 
85 static inline double aa_fdeadzone( double val, double min, double max, double deadval) {
86  if( min < val && max > val ) return deadval;
87  else return val;
88 }
89 
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;
95  }
96 }
97 
98 
100 static inline double aa_sign( double val ) {
101  if( val > 0 ) return 1;
102  if( val < 0 ) return -1;
103  return 0;
104 }
105 
107 AA_API int aa_isfok( double x );
108 
110 static inline int aa_feq( double a, double b, double tol ) {
111  return fabs(a-b) <= tol;
112 }
113 
115 AA_API int aa_veq( size_t n, const double *a, const double *b, double tol );
116 
117 
119 static inline double aa_fsq( double a ) {
120  return a * a;
121 }
122 
124 #define AA_MODULO(a,b) (((a) % (b)) + (b)) % (b);
125 
127 static inline int aa_imodulo( int a, int b ) {
128  //return ((a % b) + b) % b;
129  return AA_MODULO(a,b);
130 }
131 
133 static inline long aa_lmodulo( long a, long b ) {
134  return AA_MODULO(a,b);
135 }
136 
138 static inline int64_t aa_imodulo64( int64_t a, int64_t b ) {
139  return AA_MODULO(a,b);
140 }
141 
143 static inline int aa_iremainder( int a, int b ) {
144  return a % b;
145 }
146 
148 static inline double aa_fmodulo( double a, double b ) {
149  return fmod(fmod(a, b) + b, b);
150 }
151 
153 static inline double aa_fremainder( double a, double b ) {
154  return fmod(a , b);
155 }
156 
159 AA_API size_t aa_fminloc( size_t n, double *v );
160 
163 AA_API size_t aa_fmaxloc( size_t n, double *v );
164 
166 AA_API double aa_frand();
167 
169 AA_API void aa_vrand(size_t n, double *v);
170 
171 
173 static inline double
174 aa_horner3( double x, double a0, double a1, double a2 )
175 {
176  return a0 + x * ( a1 + x*a2 );
177 }
178 
179 
180 static inline void
181 aa_sincos( double x, double *s, double *c )
182 {
183  /* This is portable, and GCC will optimize to sincos() anyway*/
184  *s = sin(x);
185  *c = cos(x);
186 }
187 
188 
189 /********/
190 /* Stat */
191 /********/
192 
198 AA_API void aa_stat_box_muller(double x1, double x2, double *z1, double *z2);
199 
202 static inline double aa_stat_z2x(double z, double mu, double sigma) {
203  return (z * sigma) + mu;
204 }
205 
208 static inline double aa_stat_x2z(double x, double mu, double sigma) {
209  return (x-mu)/sigma;
210 }
211 
213 AA_API double aa_stat_mean( size_t n, const double *x);
214 
216 AA_API double aa_stat_std( size_t n, const double *x);
217 
230 AA_API size_t aa_stat_excluded_mean_std( size_t n, const double *x,
231  double *pmu, double *psigma,
232  double zmin, double zmax,
233  size_t max_iterations );
234 
235 
236 
237 
238 
240 AA_API double aa_stat_circ_mean( size_t n, const double *x);
241 
243 AA_API double aa_stat_circ_std( size_t n, const double *x);
244 
255 AA_API size_t aa_stat_excluded_circ_mean_std( size_t n, const double *x,
256  double *pmu, double *psigma,
257  double zmin, double zmax,
258  size_t max_iterations );
259 
260 
268 AA_API void aa_stat_vmean( size_t m, size_t n, const double *X,
269  double *mu);
270 
279 AA_API void aa_stat_vmean_cov( size_t m, size_t n, const double *X,
280  double *mu, double *E);
281 
282 
284 double aa_stat_mahalanobis( size_t m, const double *x,
285  const double *mu, const double *E_inv);
286 
287 /**********/
288 /* Angles */
289 /**********/
290 
292 static inline double aa_ang_rad2deg( double rad ) {
293  return rad*180.0/M_PI;
294 }
295 
297 static inline double aa_ang_deg2rad( double deg ) {
298  return deg*M_PI/180;
299 }
300 
301 
303 static inline double aa_ang_norm_2pi( double an ) {
304  return aa_fmodulo( an, 2*M_PI );
305 }
306 
308 static inline double aa_ang_norm_pi( double an ) {
309  //return aa_fmodulo( an + M_PI, 2*M_PI ) - M_PI;
310  return remainder( an, 2*M_PI );
311 }
312 
313 
315 static inline double aa_ang_delta( double a, double b) {
316  return aa_ang_norm_pi( aa_ang_norm_pi(a) - aa_ang_norm_pi(b) );
317 }
318 
319 
320 
321 /************************/
322 /* Dense Linear Algebra */
323 /************************/
324 
331 #define AA_MATCOL(A, lda, col) ((A)+(col)*(lda))
332 
340 #define AA_MATREF(A, lda, row, col) (AA_MATCOL(A,lda,col)[row])
341 
342 /*--- Scalar Ops ---*/
343 
345 AA_API double aa_la_min( size_t n, const double *x );
346 
348 AA_API double aa_la_max( size_t n, const double *x );
349 
353 AA_API double aa_la_dot( size_t n, const double *x, const double *y );
354 
358 AA_API double aa_la_norm( size_t n, const double *x );
359 
363 AA_API double aa_la_ssd( size_t n, const double *x, const double *y );
364 
368 AA_API double aa_la_dist( size_t n, const double *x, const double *y );
369 
370 /*--- Vector Ops ---*/
371 
375 AA_API void aa_la_sadd( size_t n, double alpha, const double *x, double *r );
376 
377 
381 AA_API void aa_la_scal( size_t n, double alpha, double *x );
382 
383 
387 AA_API void aa_la_vinc( size_t n, const double *x, double *y );
388 
392 AA_API void aa_la_sinc( size_t n, double alpha, double *x );
393 
394 
398 AA_API void aa_la_axpy( size_t n, double alpha, const double *x, double *y );
399 
400 
407 AA_API void aa_la_axpy3( size_t n, double alpha,
408  const double *x, const double *y, double *z );
409 
413 AA_API void aa_la_smul( size_t n, double alpha, const double *x, double *r );
414 
418 AA_API void aa_la_ssub( size_t n, double alpha, const double *x, double *r );
419 
423 AA_API void aa_la_sdiv( size_t n, double alpha, const double *x, double *r );
424 
428 AA_API void aa_la_vadd( size_t n, const double *x, const double *y, double *r );
429 
433 AA_API void aa_la_vsub( size_t n, const double *x, const double *y, double *r );
434 
438 AA_API void aa_la_vmul( size_t n, const double *x, const double *y, double *r );
439 
443 AA_API void aa_la_vdiv( size_t n, const double *x, const double *y, double *r );
444 
448 AA_API void aa_la_cross( const double a[3], const double b[3], double c[3] );
449 
453 AA_API void aa_la_normalize( size_t n, double *x );
454 
455 
462 AA_API double aa_la_point_plane( size_t n,
463  const double *point, const double *plane );
464 
465 /*--- Matrix Ops --- */
466 
467 
469 AA_API void aa_la_transpose( size_t n, double *A );
471 AA_API void aa_la_transpose2( size_t m, size_t n, const double *A, double *At );
472 
474 static inline void
475 aa_la_diag( size_t n, double *A, double x ) {
476  for( size_t i = 0; i < n; i ++ )
477  A[i*n+i] = x;
478 }
479 
481 static inline void
482 aa_la_ident( size_t n, double *A ) {
483  AA_MEM_ZERO(A, n*n);
484  aa_la_diag(n,A,1.0);
485 }
486 
488 static inline void
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,
491  1.0, A, (int)m,
492  x, 1, 0, b, 1 );
493 }
494 
499 AA_API double aa_la_wdot( size_t n,
500  const double *x, const double *A, const double *y );
501 
502 
516 AA_API int aa_la_svd( size_t m, size_t n, const double *A, double *U, double *S, double *Vt );
517 
525 AA_API int aa_la_inv( size_t n, double *A );
526 
527 
532 AA_API void aa_la_inverse3x3_( const double R[9], double S[9] );
533 
534 
539 static inline void aa_la_inverse3x3( const double R[9], double S[9] ) {
540  aa_la_inverse3x3_(R,S);
541 }
542 
543 
547 double aa_la_det3x3( const double R[AA_RESTRICT 9] );
548 
551 AA_API double aa_la_trace( size_t n, const double *A );
552 
570 AA_API void aa_la_dpinv( size_t m, size_t n, double k, const double *A, double *A_star );
571 
572 
590 AA_API void aa_la_dzdpinv( size_t m, size_t n, double s2_min, const double *A, double *A_star ) ;
591 
605 AA_API void aa_la_dls( size_t m, size_t n, double k, const double *A, const double *b, double *x );
606 
607 
623 AA_API void aa_la_xlsnp( size_t m, size_t n,
624  const double *A, const double *A_star, const double *b,
625  const double *xp, double *x );
626 
627 
628 
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 );
645 
656 AA_API void aa_la_lls( size_t m, size_t n, size_t p, const double *A, const double *b, double *x );
657 
666 AA_API int aa_la_care_laub( size_t m, size_t n, size_t p,
667  const double *AA_RESTRICT A,
668  const double *AA_RESTRICT B,
669  const double *AA_RESTRICT C,
670  double *AA_RESTRICT X );
671 
672 
683 AA_API void aa_la_linterp( size_t n,
684  double t0, const double *X0,
685  double t1, const double *X1,
686  double ti, double *Xi );
687 
688 
701 AA_API void aa_la_quadterp( size_t n,
702  double t0, const double *X0,
703  double t1, const double *X1,
704  double t2, const double *X2,
705  double ti, double *Xi );
706 
709 AA_API void aa_la_quadterp_dx( size_t n,
710  double t0, const double *X0,
711  double t1, const double *X1,
712  double t2, const double *X2,
713  double ti, double *dXi );
714 
715 
721 AA_API void aa_la_plane_hessian( size_t n, double *plane );
722 
730 AA_API void aa_la_plane_fit( size_t m, size_t n,
731  const double *points, double *plane );
732 
733 /*--- Systems and Signals --- */
734 
747 AA_API void aa_lsim_dstep( size_t m, size_t n,
748  const double *AA_RESTRICT A,
749  const double *AA_RESTRICT B,
750  const double *AA_RESTRICT x0,
751  const double *AA_RESTRICT u,
752  double *AA_RESTRICT x1 );
753 
768 AA_API void aa_lsim_estep( size_t m, size_t n,
769  double dt,
770  const double *AA_RESTRICT A,
771  const double *AA_RESTRICT B,
772  const double *AA_RESTRICT x0,
773  const double *AA_RESTRICT u,
774  double *AA_RESTRICT x1 );
775 
776 
781 typedef void aa_sys_fun( const void *cx,
782  double t, const double *AA_RESTRICT x,
783  double *AA_RESTRICT y );
784 
803 };
804 
808 #define AA_ODE_EULER AA_ODE_RK1
809 
813 #define AA_ODE_HEUN AA_ODE_RK2
814 
815 
816 typedef int aa_ode_check( void *cx, double t, double * AA_RESTRICT x, double *AA_RESTRICT y );
817 
824 
827 
832 
837 };
838 
839 AA_API int aa_ode_sol( enum aa_ode_integrator integrator,
840  const struct aa_ode_sol_opts * AA_RESTRICT opts,
841  size_t n,
842  aa_sys_fun sys, const void *sys_cx,
843  aa_ode_check check, void *check_cx,
844  double t0, double dt0,
845  const double *AA_RESTRICT x0,
846  double *AA_RESTRICT x1 );
847 
848 typedef void aa_odestep_fixed( size_t n, aa_sys_fun sys, const void *cx,
849  double t0, double dt,
850  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
851 
852 typedef void aa_odestep_adaptive( size_t n, aa_sys_fun sys, const void *cx,
853  double t0, double dt,
854  const double *AA_RESTRICT x0,
855  double *AA_RESTRICT k,
856  double *AA_RESTRICT x_n_1,
857  double *AA_RESTRICT x_n );
858 
864 AA_API void aa_odestep_euler( size_t n, double dt,
865  const double *AA_RESTRICT dx,
866  const double *AA_RESTRICT x0,
867  double *AA_RESTRICT x1 );
868 
869 
870 AA_API void aa_odestep_rk1( size_t n, aa_sys_fun sys, const void *cx,
871  double t0, double dt,
872  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
873 
884 AA_API void aa_odestep_rk2( size_t n, aa_sys_fun sys, const void *cx,
885  double t0, double dt,
886  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
887 
898 AA_API void aa_odestep_rk4( size_t n, aa_sys_fun sys, const void *cx,
899  double t0, double dt,
900  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
901 
924 AA_API void aa_odestep_rkf45( size_t n, aa_sys_fun sys, const void *cx,
925  double t0, double dt,
926  const double *AA_RESTRICT x0,
927  double *AA_RESTRICT k,
928  double *AA_RESTRICT x4,
929  double *AA_RESTRICT x5 );
930 
953 AA_API void aa_odestep_rkck45( size_t n, aa_sys_fun sys, const void *cx,
954  double t0, double dt,
955  const double *AA_RESTRICT x0,
956  double *AA_RESTRICT k,
957  double *AA_RESTRICT x4,
958  double *AA_RESTRICT x5 );
959 
960 
984 AA_API void aa_odestep_dorpri45( size_t n, aa_sys_fun sys, const void *cx,
985  double t0, double dt,
986  const double *AA_RESTRICT x0,
987  double *AA_RESTRICT k,
988  double *AA_RESTRICT x4,
989  double *AA_RESTRICT x5 );
990 
991 
1014 AA_API void aa_odestep_rkbs23( size_t n, aa_sys_fun sys, const void *cx,
1015  double t0, double dt,
1016  const double *AA_RESTRICT x0,
1017  double *AA_RESTRICT k,
1018  double *AA_RESTRICT x4,
1019  double *AA_RESTRICT x5 );
1020 
1021 
1035 AA_API void aa_lsim_rk4step( size_t m, size_t n,
1036  double dt,
1037  const double *AA_RESTRICT A,
1038  const double *AA_RESTRICT B,
1039  const double *AA_RESTRICT x0,
1040  const double *AA_RESTRICT u,
1041  double *AA_RESTRICT x1 );
1042 
1043 
1046 typedef struct {
1047  size_t n;
1048  double *A;
1049  double *D;
1050 } aa_sys_affine_t;
1051 
1056 AA_API void aa_sys_affine( const aa_sys_affine_t *cx,
1057  double t, const double *AA_RESTRICT x,
1058  double *AA_RESTRICT dx );
1059 
1060 
1061 /*--- GCC Vector Extensions --- */
1062 
1063 //typedef doulbe aa_v2df_t __attribute__ (( vector_size(2*sizeof(double)) ));
1064 
1065 #endif //AA_MATH_H
#define AA_MODULO(a, b)
Fortran modulo, Ada mod.
Definition: math.h:124
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:85
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:821
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:798
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:119
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:71
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:315
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
Definition: math.h:292
static double aa_ang_norm_2pi(double an)
normalize angle on interval [0,2pi)
Definition: math.h:303
static void aa_la_ident(size_t n, double *A)
Set A to the identity matrix.
Definition: math.h:482
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:110
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:127
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:138
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:475
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:796
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.
Definition: math.h:539
Runge-Kutta 1 integration.
Definition: math.h:790
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:148
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:101
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:831
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:802
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:823
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:1046
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:90
static int aa_iremainder(int a, int b)
Fortran mod, Ada rem.
Definition: math.h:143
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:800
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:297
double adapt_factor_inc
Factor to increse step size.
Definition: math.h:836
void aa_sys_fun(const void *cx, double t, const double *AA_RESTRICT x, double *AA_RESTRICT y)
A "Signal" function.
Definition: math.h:781
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:792
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:153
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:91
#define AA_API
calling and name mangling convention for functions
Definition: amino.h:86
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:1047
static long aa_lmodulo(long a, long b)
Fortran modulo, Ada mod.
Definition: math.h:133
double * D
additive constant
Definition: math.h:1049
static double aa_stat_x2z(double x, double mu, double sigma)
Convert x-score to z-score a normal distribution.
Definition: math.h:208
double adapt_tol_inc
Increase step size if error is below tol_grow.
Definition: math.h:826
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:788
static double aa_sign(double val)
return the sign of val, one of {-1,0,1}
Definition: math.h:100
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:308
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:1048
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:489
static double aa_horner3(double x, double a0, double a1, double a2)
Evaluate three-term polynomial using horner's rule.
Definition: math.h:174
static double aa_fclamp(double val, double min, double max)
return val within range (min,max)
Definition: math.h:78
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:794
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:496
static double aa_stat_z2x(double z, double mu, double sigma)
Convert z-score to x-score a normal distribution.
Definition: math.h:202