00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #ifndef LIBSWIFTNAV_LINEAR_ALGEBRA_H
00014 #define LIBSWIFTNAV_LINEAR_ALGEBRA_H
00015
00016 #include "common.h"
00017
00018 #define VEC_PRINTF(v, _n) { \
00019 printf("%s:%u <|%s| %lf", \
00020 __FILE__, __LINE__, #v, (v)[0]); \
00021 for (u32 _i = 1; _i < (u32)(_n); _i++) printf(", %lf", (v)[_i]); \
00022 printf(">\n"); \
00023 }
00024
00025 #define MAT_PRINTF(m, _r, _c) { \
00026 printf("%s:%u <|%s|\n", \
00027 __FILE__, __LINE__, #m); \
00028 for (u32 _i = 0; _i < (u32)(_r); _i++) { \
00029 printf(" [% 12lf", (m)[_i*(u32)(_c) + 0]); \
00030 for (u32 _j = 1; _j < (u32)(_c); _j++) \
00031 printf(" % 12lf", (m)[_i*(u32)(_c) + _j]); \
00032 printf("]\n"); \
00033 } \
00034 printf(">\n"); \
00035 }
00036
00037 void dmtx_printf(double *mtx, u32 m, u32 n);
00038 s32 qrdecomp_square(const double *a, u32 rows, double *qt, double *r);
00039 s32 qrdecomp(const double *a, u32 rows, u32 cols, double *qt, double *r);
00040 void qtmult(const double *qt, u32 n, const double *b, double *x);
00041 void rsolve(const double *r, u32 rows, u32 cols, const double *b, double *x);
00042 s32 qrsolve(const double *a, u32 rows, u32 cols, const double *b, double *x);
00043
00044 int matrix_inverse(u32 n, const double *const a, double *b);
00045 void matrix_multiply(u32 n, u32 m, u32 p, const double *a,
00046 const double *b, double *c);
00047 void matrix_triu(u32 n, double *M);
00048 void matrix_eye(u32 n, double *M);
00049 void matrix_udu(u32 n, double *M, double *U, double *D);
00050 void matrix_reconstruct_udu(u32 n, double *U, double *D, double *M);
00051 void matrix_add_sc(u32 n, u32 m, const double *a,
00052 const double *b, double gamma, double *c);
00053 void matrix_transpose(u32 n, u32 m, const double *a, double *b);
00054 void matrix_copy(u32 n, u32 m, const double *a, double *b);
00055
00056 int matrix_pseudoinverse(u32 n, u32 m, const double *a, double *b);
00057 int matrix_atwaiat(u32 n, u32 m, const double *a, const double *w, double *b);
00058 int matrix_ataiat(u32 n, u32 m, const double *a, double *b);
00059 int matrix_atawati(u32 n, u32 m, const double *a, const double *w, double *b);
00060 int matrix_ataati(u32 n, u32 m, const double *a, double *b);
00061
00062 double vector_dot(u32 n, const double *a, const double *b);
00063 double vector_norm(u32 n, const double *a);
00064 double vector_mean(u32 n, const double *a);
00065 void vector_normalize(u32 n, double *a);
00066 void vector_add_sc(u32 n, const double *a, const double *b,
00067 double gamma, double *c);
00068 void vector_add(u32 n, const double *a, const double *b, double *c);
00069 void vector_subtract(u32 n, const double *a,
00070 const double *b, double *c);
00071 void vector_cross(const double a[3], const double b[3], double c[3]);
00072
00073 #endif
00074