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
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 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 
240 AA_API size_t aa_stat_excluded_mean_std( size_t n, const double *x,
241  double *pmu, double *psigma,
242  double zmin, double zmax,
243  size_t max_iterations );
244 
245 
246 
247 
248 
250 AA_API double aa_stat_circ_mean( size_t n, const double *x);
251 
253 AA_API double aa_stat_circ_std( size_t n, const double *x);
254 
265 AA_API size_t aa_stat_excluded_circ_mean_std( size_t n, const double *x,
266  double *pmu, double *psigma,
267  double zmin, double zmax,
268  size_t max_iterations );
269 
270 
278 AA_API void aa_stat_vmean( size_t m, size_t n, const double *X,
279  double *mu);
280 
289 AA_API void aa_stat_vmean_cov( size_t m, size_t n, const double *X,
290  double *mu, double *E);
291 
292 
294 double aa_stat_mahalanobis( size_t m, const double *x,
295  const double *mu, const double *E_inv);
296 
297 /**********/
298 /* Angles */
299 /**********/
300 
302 static inline double aa_ang_rad2deg( double rad ) {
303  return rad*180.0/M_PI;
304 }
305 
307 static inline double aa_ang_deg2rad( double deg ) {
308  return deg*M_PI/180;
309 }
310 
311 
313 static inline double aa_ang_norm_2pi( double an ) {
314  return aa_fmodulo( an, 2*M_PI );
315 }
316 
318 static inline double aa_ang_norm_pi( double an ) {
319  //return aa_fmodulo( an + M_PI, 2*M_PI ) - M_PI;
320  return remainder( an, 2*M_PI );
321 }
322 
323 
325 static inline double aa_ang_delta( double a, double b) {
326  return aa_ang_norm_pi( aa_ang_norm_pi(a) - aa_ang_norm_pi(b) );
327 }
328 
329 
330 
331 /************************/
332 /* Dense Linear Algebra */
333 /************************/
334 
341 #define AA_MATCOL(A, lda, col) ((A)+(col)*(lda))
342 
350 #define AA_MATREF(A, lda, row, col) (AA_MATCOL(A,lda,col)[row])
351 
352 /*--- Scalar Ops ---*/
353 
355 AA_API double aa_la_min( size_t n, const double *x );
356 
358 AA_API double aa_la_max( size_t n, const double *x );
359 
363 AA_API double aa_la_dot( size_t n, const double *x, const double *y );
364 
368 AA_API double aa_la_norm( size_t n, const double *x );
369 
373 AA_API double aa_la_ssd( size_t n, const double *x, const double *y );
374 
378 AA_API double aa_la_dist( size_t n, const double *x, const double *y );
379 
380 /*--- Vector Ops ---*/
381 
385 AA_API void aa_la_sadd( size_t n, double alpha, const double *x, double *r );
386 
387 
391 AA_API void aa_la_scal( size_t n, double alpha, double *x );
392 
393 
397 AA_API void aa_la_vinc( size_t n, const double *x, double *y );
398 
402 AA_API void aa_la_sinc( size_t n, double alpha, double *x );
403 
404 
408 AA_API void aa_la_axpy( size_t n, double alpha, const double *x, double *y );
409 
410 
417 AA_API void aa_la_axpy3( size_t n, double alpha,
418  const double *x, const double *y, double *z );
419 
423 AA_API void aa_la_smul( size_t n, double alpha, const double *x, double *r );
424 
428 AA_API void aa_la_ssub( size_t n, double alpha, const double *x, double *r );
429 
433 AA_API void aa_la_sdiv( size_t n, double alpha, const double *x, double *r );
434 
438 AA_API void aa_la_vadd( size_t n, const double *x, const double *y, double *r );
439 
443 AA_API void aa_la_vsub( size_t n, const double *x, const double *y, double *r );
444 
448 AA_API void aa_la_vmul( size_t n, const double *x, const double *y, double *r );
449 
453 AA_API void aa_la_vdiv( size_t n, const double *x, const double *y, double *r );
454 
458 AA_API void aa_la_cross( const double a[3], const double b[3], double c[3] );
459 
463 AA_API void aa_la_normalize( size_t n, double *x );
464 
465 
472 AA_API double aa_la_point_plane( size_t n,
473  const double *point, const double *plane );
474 
475 /*--- Matrix Ops --- */
476 
477 
479 AA_API void aa_la_transpose( size_t n, double *A );
481 AA_API void aa_la_transpose2( size_t m, size_t n, const double *A, double *At );
482 
484 static inline void
485 aa_la_diag( size_t n, double *A, double x ) {
486  for( size_t i = 0; i < n; i ++ )
487  A[i*n+i] = x;
488 }
489 
491 static inline void
492 aa_la_ident( size_t n, double *A ) {
493  AA_MEM_ZERO(A, n*n);
494  aa_la_diag(n,A,1.0);
495 }
496 
498 static inline void
499 aa_la_mvmul( size_t m, size_t n, const double *A, const double *x, double *b ) {
500  cblas_dgemv( CblasColMajor, CblasNoTrans, (int)m, (int)n,
501  1.0, A, (int)m,
502  x, 1, 0, b, 1 );
503 }
504 
509 AA_API double aa_la_wdot( size_t n,
510  const double *x, const double *A, const double *y );
511 
512 
526 AA_API int aa_la_svd( size_t m, size_t n, const double *A, double *U, double *S, double *Vt );
527 
535 AA_API int aa_la_inv( size_t n, double *A );
536 
537 
542 AA_API void aa_la_inverse3x3_( const double R[9], double S[9] );
543 
544 
549 static inline void aa_la_inverse3x3( const double R[9], double S[9] ) {
550  aa_la_inverse3x3_(R,S);
551 }
552 
553 
557 double aa_la_det3x3( const double R[AA_RESTRICT 9] );
558 
561 AA_API double aa_la_trace( size_t n, const double *A );
562 
580 AA_API void aa_la_dpinv( size_t m, size_t n, double k, const double *A, double *A_star );
581 
582 
600 AA_API void aa_la_dzdpinv( size_t m, size_t n, double s2_min, const double *A, double *A_star ) ;
601 
615 AA_API void aa_la_dls( size_t m, size_t n, double k, const double *A, const double *b, double *x );
616 
617 
633 AA_API void aa_la_xlsnp( size_t m, size_t n,
634  const double *A, const double *A_star, const double *b,
635  const double *xp, double *x );
636 
637 
638 
654 AA_API void aa_la_dlsnp( size_t m, size_t n, double k, const double *A, const double *b, const double *xp, double *x );
655 
666 AA_API void aa_la_lls( size_t m, size_t n, size_t p, const double *A, const double *b, double *x );
667 
676 AA_API int aa_la_care_laub( size_t m, size_t n, size_t p,
677  const double *AA_RESTRICT A,
678  const double *AA_RESTRICT B,
679  const double *AA_RESTRICT C,
680  double *AA_RESTRICT X );
681 
682 
693 AA_API void aa_la_linterp( size_t n,
694  double t0, const double *X0,
695  double t1, const double *X1,
696  double ti, double *Xi );
697 
698 
711 AA_API void aa_la_quadterp( size_t n,
712  double t0, const double *X0,
713  double t1, const double *X1,
714  double t2, const double *X2,
715  double ti, double *Xi );
716 
719 AA_API void aa_la_quadterp_dx( size_t n,
720  double t0, const double *X0,
721  double t1, const double *X1,
722  double t2, const double *X2,
723  double ti, double *dXi );
724 
725 
731 AA_API void aa_la_plane_hessian( size_t n, double *plane );
732 
740 AA_API void aa_la_plane_fit( size_t m, size_t n,
741  const double *points, double *plane );
742 
743 /*--- Systems and Signals --- */
744 
757 AA_API void aa_lsim_dstep( size_t m, size_t n,
758  const double *AA_RESTRICT A,
759  const double *AA_RESTRICT B,
760  const double *AA_RESTRICT x0,
761  const double *AA_RESTRICT u,
762  double *AA_RESTRICT x1 );
763 
778 AA_API void aa_lsim_estep( size_t m, size_t n,
779  double dt,
780  const double *AA_RESTRICT A,
781  const double *AA_RESTRICT B,
782  const double *AA_RESTRICT x0,
783  const double *AA_RESTRICT u,
784  double *AA_RESTRICT x1 );
785 
786 
791 typedef void aa_sys_fun( const void *cx,
792  double t, const double *AA_RESTRICT x,
793  double *AA_RESTRICT y );
794 
813 };
814 
818 #define AA_ODE_EULER AA_ODE_RK1
819 
823 #define AA_ODE_HEUN AA_ODE_RK2
824 
825 
832 typedef int aa_ode_check( void *cx, double t, double * AA_RESTRICT x, double *AA_RESTRICT y );
833 
840 
843 
848 
853 };
854 
858 AA_API int aa_ode_sol( enum aa_ode_integrator integrator,
859  const struct aa_ode_sol_opts * AA_RESTRICT opts,
860  size_t n,
861  aa_sys_fun sys, const void *sys_cx,
862  aa_ode_check check, void *check_cx,
863  double t0, double dt0,
864  const double *AA_RESTRICT x0,
865  double *AA_RESTRICT x1 );
866 
870 AA_API int
871 aa_ode_path( 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  struct aa_mem_region *region, size_t *n_points, double **path );
879 
883 typedef void aa_odestep_fixed( size_t n, aa_sys_fun sys, const void *cx,
884  double t0, double dt,
885  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
886 
890 typedef void aa_odestep_adaptive( size_t n, aa_sys_fun sys, const void *cx,
891  double t0, double dt,
892  const double *AA_RESTRICT x0,
893  double *AA_RESTRICT k,
894  double *AA_RESTRICT x_n_1,
895  double *AA_RESTRICT x_n );
896 
902 AA_API void aa_odestep_euler( size_t n, double dt,
903  const double *AA_RESTRICT dx,
904  const double *AA_RESTRICT x0,
905  double *AA_RESTRICT x1 );
906 
907 
908 AA_API void aa_odestep_rk1( size_t n, aa_sys_fun sys, const void *cx,
909  double t0, double dt,
910  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
911 
922 AA_API void aa_odestep_rk2( size_t n, aa_sys_fun sys, const void *cx,
923  double t0, double dt,
924  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
925 
936 AA_API void aa_odestep_rk4( size_t n, aa_sys_fun sys, const void *cx,
937  double t0, double dt,
938  const double *AA_RESTRICT x0, double *AA_RESTRICT x1 );
939 
962 AA_API void aa_odestep_rkf45( size_t n, aa_sys_fun sys, const void *cx,
963  double t0, double dt,
964  const double *AA_RESTRICT x0,
965  double *AA_RESTRICT k,
966  double *AA_RESTRICT x4,
967  double *AA_RESTRICT x5 );
968 
991 AA_API void aa_odestep_rkck45( size_t n, aa_sys_fun sys, const void *cx,
992  double t0, double dt,
993  const double *AA_RESTRICT x0,
994  double *AA_RESTRICT k,
995  double *AA_RESTRICT x4,
996  double *AA_RESTRICT x5 );
997 
998 
1022 AA_API void aa_odestep_dorpri45( size_t n, aa_sys_fun sys, const void *cx,
1023  double t0, double dt,
1024  const double *AA_RESTRICT x0,
1025  double *AA_RESTRICT k,
1026  double *AA_RESTRICT x4,
1027  double *AA_RESTRICT x5 );
1028 
1029 
1052 AA_API void aa_odestep_rkbs23( size_t n, aa_sys_fun sys, const void *cx,
1053  double t0, double dt,
1054  const double *AA_RESTRICT x0,
1055  double *AA_RESTRICT k,
1056  double *AA_RESTRICT x4,
1057  double *AA_RESTRICT x5 );
1058 
1059 
1073 AA_API void aa_lsim_rk4step( size_t m, size_t n,
1074  double dt,
1075  const double *AA_RESTRICT A,
1076  const double *AA_RESTRICT B,
1077  const double *AA_RESTRICT x0,
1078  const double *AA_RESTRICT u,
1079  double *AA_RESTRICT x1 );
1080 
1081 
1084 typedef struct {
1085  size_t n;
1086  double *A;
1087  double *D;
1088 } aa_sys_affine_t;
1089 
1094 AA_API void aa_sys_affine( const aa_sys_affine_t *cx,
1095  double t, const double *AA_RESTRICT x,
1096  double *AA_RESTRICT dx );
1097 
1098 
1099 /*--- GCC Vector Extensions --- */
1100 
1101 //typedef doulbe aa_v2df_t __attribute__ (( vector_size(2*sizeof(double)) ));
1102 
1103 #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:832
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:837
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:808
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:325
AA_API void aa_la_vadd(size_t n, const double *x, const double *y, double *r)
Elementwise addition.
AA_API void aa_lsim_estep(size_t m, size_t n, double dt, const double *AA_RESTRICT A, const double *AA_RESTRICT B, const double *AA_RESTRICT x0, const double *AA_RESTRICT u, double *AA_RESTRICT x1)
Linear simulation step with euler integration.
AA_API int aa_la_svd(size_t m, size_t n, const double *A, double *U, double *S, double *Vt)
Singular Value Decomposition of A.
AA_API double aa_la_dist(size_t n, const double *x, const double *y)
Euclidean Distance.
AA_API double aa_frand_minmax(double min, double max)
uniform pseudo-random in [min,max]
static void aa_sincos(double x, double *s, double *c)
Portable sincos.
Definition: math.h:192
static double aa_ang_rad2deg(double rad)
convert radians to degrees
Definition: math.h:302
static double aa_ang_norm_2pi(double an)
normalize angle on interval [0,2pi)
Definition: math.h:313
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:883
static void aa_la_ident(size_t n, double *A)
Set A to the identity matrix.
Definition: math.h:492
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:485
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:806
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:890
Data Structure for Region-Based memory allocation.
Definition: mem.h:198
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:549
Runge-Kutta 1 integration.
Definition: math.h:800
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&#39;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&#39;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:847
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:812
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:839
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:1084
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:810
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:307
double adapt_factor_inc
Factor to increse step size.
Definition: math.h:852
void aa_sys_fun(const void *cx, double t, const double *AA_RESTRICT x, double *AA_RESTRICT y)
A "Signal" function.
Definition: math.h:791
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:802
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:1085
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:1087
static double aa_stat_x2z(double x, double mu, double sigma)
Convert x-score to z-score a normal distribution.
Definition: math.h:218
double adapt_tol_inc
Increase step size if error is below tol_grow.
Definition: math.h:842
AA_API void aa_odestep_rkck45(size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT k, double *AA_RESTRICT x4, double *AA_RESTRICT x5)
Runge-Kutta-4-5 (Cash-Karp Method) integration.
AA_API void aa_la_vmul(size_t n, const double *x, const double *y, double *r)
Elementwise multiplication.
AA_API void aa_la_lls(size_t m, size_t n, size_t p, const double *A, const double *b, double *x)
Linear Least Squares.
AA_API size_t aa_fminloc(size_t n, double *v)
Returns index of minimum element in array v.
AA_API int aa_ode_path(enum aa_ode_integrator integrator, const struct aa_ode_sol_opts *AA_RESTRICT opts, size_t n, aa_sys_fun sys, const void *sys_cx, aa_ode_check check, void *check_cx, double t0, double dt0, const double *AA_RESTRICT x0, struct aa_mem_region *region, size_t *n_points, double **path)
Compute the path followed during integration.
AA_API size_t aa_stat_excluded_mean_std(size_t n, const double *x, double *pmu, double *psigma, double zmin, double zmax, size_t max_iterations)
Compute mean and standard deviation, excluding outliers.
AA_API double aa_la_dot(size_t n, const double *x, const double *y)
Dot product.
AA_API void aa_odestep_dorpri45(size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT k, double *AA_RESTRICT x4, double *AA_RESTRICT x5)
Runge-Kutta-4-5 (Dormand-Prince Method) integration.
AA_API void aa_la_dzdpinv(size_t m, size_t n, double s2_min, const double *A, double *A_star)
Deadzone, Damped Pseudo Inverse of A.
aa_ode_integrator
The known integration methods.
Definition: math.h:798
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:318
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:1086
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:499
static double aa_horner3(double x, double a0, double a1, double a2)
Evaluate three-term polynomial using horner&#39;s rule.
Definition: math.h:181
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:804
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:557
static double aa_stat_z2x(double z, double mu, double sigma)
Convert z-score to x-score a normal distribution.
Definition: math.h:212