px4-firmware/apps/px4/attitude_estimator_bm/matrix.h

157 lines
3.4 KiB
C

/*
* matrix.h
*
* Created on: 18.11.2010
* Author: Laurens Mackay
*/
#ifndef MATRIX_H_
#define MATRIX_H_
typedef float m_elem;
typedef struct {
int rows;
int cols;
m_elem *a;
} matrix_t;
typedef struct {
float x;
float y;
float z;
} float_vect3;
#define M(m,i,j) m.a[m.cols*i+j]
///* This is the datatype used for the math and non-type specific ops. */
//
//matrix_t matrix_create(const int rows, const int cols, m_elem * a);
///* matrix C = matrix A + matrix B , both of size m x n */
//void matrix_add(const matrix_t a, const matrix_t b, matrix_t c);
//
///* matrix C = matrix A - matrix B , all of size m x n */
//void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c);
//
///* matrix C = matrix A x matrix B , A(a_rows x a_cols), B(a_cols x b_cols) */
//void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c);
//
//void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c);
//
//void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c);
//
///* matrix C = A*B'*/
//void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c);
static inline matrix_t matrix_create(const int rows, const int cols, m_elem *a)
{
matrix_t ret;
ret.rows = rows;
ret.cols = cols;
ret.a = a;
return ret;
}
static inline void matrix_add(const matrix_t a, const matrix_t b, matrix_t c)
{
if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols
!= c.cols) {
//debug_message_buffer("matrix_add: Dimension mismatch");
}
for (int i = 0; i < c.rows; i++) {
for (int j = 0; j < c.cols; j++) {
M(c, i, j) = M(a, i, j) + M(b, i, j);
}
}
}
static inline void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c)
{
if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols
!= c.cols) {
//debug_message_buffer("matrix_sub: Dimension mismatch");
}
for (int i = 0; i < c.rows; i++) {
for (int j = 0; j < c.cols; j++) {
M(c, i, j) = M(a, i, j) - M(b, i, j);
}
}
}
static inline void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c)
{
if (a.rows != c.rows || b.cols != c.cols || a.cols != b.rows) {
//debug_message_buffer("matrix_mult: Dimension mismatch");
}
for (int i = 0; i < a.rows; i++) {
for (int j = 0; j < b.cols; j++) {
M(c, i, j) = 0;
for (int k = 0; k < a.cols; k++) {
M(c, i, j) += M(a, i, k) * M(b, k, j);
}
}
}
}
static inline void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c)
{
if (a.rows != c.rows || b.rows != c.cols || a.cols != b.cols) {
//debug_message_buffer("matrix_mult: Dimension mismatch");
}
for (int i = 0; i < a.rows; i++) {
for (int j = 0; j < b.cols; j++) {
M(c, i, j) = 0;
for (int k = 0; k < a.cols; k++) {
M(c, i, j) += M(a, i, k) * M(b, j, k);
}
}
}
}
static inline void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c)
{
if (a.rows != c.rows || a.cols != c.cols) {
//debug_message_buffer("matrix_mult_scalar: Dimension mismatch");
}
for (int i = 0; i < c.rows; i++) {
for (int j = 0; j < c.cols; j++) {
M(c, i, j) = f * M(a, i, j);
}
}
}
static inline void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c)
{
if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols
!= c.cols) {
//debug_message_buffer("matrix_mult_element: Dimension mismatch");
}
for (int i = 0; i < c.rows; i++) {
for (int j = 0; j < c.cols; j++) {
M(c, i, j) = M(a, i, j) * M(b, i, j);
}
}
}
#endif /* MATRIX_H_ */