Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
pprz_simple_matrix.h
Go to the documentation of this file.
1/*
2 * Copyright (C) 2009-2014 The Paparazzi Team
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
27#ifndef PPRZ_SIMPLE_MATRIX_H
28#define PPRZ_SIMPLE_MATRIX_H
29
30#ifdef __cplusplus
31extern "C" {
32#endif
33
34#include <float.h> /* for FLT_EPSILON */
35#include <math.h>
36#ifdef HAVE_STDIO
37#include <stdio.h> /* for printf'ing warnings */
38#define warn_message printf
39#else
40#define warn_message(...) do { } while(0)
41#endif
42
43//
44// C = A*B A:(i,k) B:(k,j) C:(i,j)
45//
46#define MAT_MUL(_i, _k, _j, C, A, B) { \
47 int l,c,m; \
48 for (l=0; l<_i; l++) \
49 for (c=0; c<_j; c++) { \
50 C[l][c] = 0.; \
51 for (m=0; m<_k; m++) \
52 C[l][c] += A[l][m]*B[m][c]; \
53 } \
54 }
55
56//
57// C = A*B' A:(i,k) B:(j,k) C:(i,j)
58//
59#define MAT_MUL_T(_i, _k, _j, C, A, B) { \
60 int l,c,m; \
61 for (l=0; l<_i; l++) \
62 for (c=0; c<_j; c++) { \
63 C[l][c] = 0.; \
64 for (m=0; m<_k; m++) \
65 C[l][c] += A[l][m]*B[c][m]; \
66 } \
67 }
68
69
70//
71// C = A-B
72//
73#define MAT_SUB(_i, _j, C, A, B) { \
74 int l,c; \
75 for (l=0; l<_i; l++) \
76 for (c=0; c<_j; c++) \
77 C[l][c] = A[l][c] - B[l][c]; \
78 }
79
86#define MAT_MUL_VECT(_n, o, a, b) { \
87 int l,m; \
88 for (l = 0; l < _n; l++) { \
89 o[l] = 0; \
90 for (m = 0; m < _n; m++) { \
91 o[l] += a[l][m] * b[m]; \
92 } \
93 } \
94 }
95
96//
97// invS = 1/det(S) com(S)'
98//
99#define MAT_INV33(_invS, _S) { \
100 const float m00 = _S[1][1]*_S[2][2] - _S[1][2]*_S[2][1]; \
101 const float m10 = _S[0][1]*_S[2][2] - _S[0][2]*_S[2][1]; \
102 const float m20 = _S[0][1]*_S[1][2] - _S[0][2]*_S[1][1]; \
103 const float m01 = _S[1][0]*_S[2][2] - _S[1][2]*_S[2][0]; \
104 const float m11 = _S[0][0]*_S[2][2] - _S[0][2]*_S[2][0]; \
105 const float m21 = _S[0][0]*_S[1][2] - _S[0][2]*_S[1][0]; \
106 const float m02 = _S[1][0]*_S[2][1] - _S[1][1]*_S[2][0]; \
107 const float m12 = _S[0][0]*_S[2][1] - _S[0][1]*_S[2][0]; \
108 const float m22 = _S[0][0]*_S[1][1] - _S[0][1]*_S[1][0]; \
109 float det = _S[0][0]*m00 - _S[1][0]*m10 + _S[2][0]*m20; \
110 if (fabs(det) < FLT_EPSILON) { \
111 /* If the determinant is too small then set it to epsilon preserving sign. */ \
112 warn_message("warning: %s:%d MAT_INV33 trying to invert non-invertable matrix '%s' and put result in '%s'.\n", __FILE__, __LINE__, #_S, #_invS); \
113 det = copysignf(FLT_EPSILON, det); \
114 } \
115 _invS[0][0] = m00 / det; \
116 _invS[1][0] = -m01 / det; \
117 _invS[2][0] = m02 / det; \
118 _invS[0][1] = -m10 / det; \
119 _invS[1][1] = m11 / det; \
120 _invS[2][1] = -m12 / det; \
121 _invS[0][2] = m20 / det; \
122 _invS[1][2] = -m21 / det; \
123 _invS[2][2] = m22 / det; \
124 }
125
126//
127// C = A+B
128//
129#define MAT_SUM(_i, _j, C, A, B) { \
130 int l,c; \
131 for (l=0; l<_i; l++) \
132 for (c=0; c<_j; c++) \
133 C[l][c] = A[l][c] + B[l][c]; \
134 }
135
136
137//
138// C = A+(B*k)
139//
140#define MAT_SUM_c(_i, _j, C, A, B,k_) { \
141 int l,c; \
142 for (l=0; l<_i; l++) \
143 for (c=0; c<_j; c++) \
144 C[l][c] = A[l][c] + B[l][c]*k_; \
145 }
146
147//
148// C = c*A*B A:(i,k) B:(k,j) C:(i,j)
149//
150#define MAT_MUL_c(_i, _k, _j, C, A, B,c_) { \
151 int l,c,m; \
152 for (l=0; l<_i; l++) \
153 for (c=0; c<_j; c++) { \
154 C[l][c] = 0.; \
155 for (m=0; m<_k; m++) \
156 C[l][c] += c_*A[l][m]*B[m][c]; \
157 } \
158 }
159
160
161#define MAT_PRINT(_i, _j,A) { \
162 int l,c; \
163 printf("float "); \
164 printf(#A); \
165 printf(" = {\n"); \
166 for (l=0; l<_i; l++){ \
167 printf("{"); \
168 for (c=0; c<_j; c++){ \
169 if(c<(_j-1)){printf("%f,",A[l][c]);} \
170 else{printf("%f",A[l][c]);} \
171 } \
172 if(l<(_i-1)){ \
173 printf("} ,\n");} \
174 else{printf("}\n");} \
175 } \
176 printf("}\n"); \
177 }
178
179#ifdef __cplusplus
180} /* extern "C" */
181#endif
182
183#endif /* PPRZ_SIMPLE_MATRIX_H */