Statistics
| Branch: | Revision:

adafruit_bno055 / utility / matrix.h @ c2a8045b

History | View | Annotate | Download (5.109 KB)

1 4bc1c0c1 Kevin Townsend
/*
2
    Inertial Measurement Unit Maths Library
3
    Copyright (C) 2013-2014  Samuel Cowen
4 d964148c Tony DiCola
    www.camelsoftware.com
5 4bc1c0c1 Kevin Townsend

6
    This program 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 3 of the License, or
9
    (at your option) any later version.
10

11
    This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
18
*/
19
20
#ifndef IMUMATH_MATRIX_HPP
21
#define IMUMATH_MATRIX_HPP
22
23
#include <stdlib.h>
24
#include <string.h>
25
#include <stdint.h>
26
#include <math.h>
27
28 c2a8045b Gé Vissers
#include "vector.h"
29
30 4bc1c0c1 Kevin Townsend
namespace imu
31
{
32
33
34
template <uint8_t N> class Matrix
35
{
36
public:
37 d964148c Tony DiCola
    Matrix()
38
    {
39
        int r = sizeof(double)*N;
40
        _cell = &_cell_data[0];
41 4bc1c0c1 Kevin Townsend
        memset(_cell, 0, r*r);
42 d964148c Tony DiCola
    }
43 4bc1c0c1 Kevin Townsend
44
    Matrix(const Matrix &v)
45
    {
46
        int r = sizeof(double)*N;
47 d964148c Tony DiCola
        _cell = &_cell_data[0];
48 4bc1c0c1 Kevin Townsend
        memset(_cell, 0, r*r);
49
        for (int x = 0; x < N; x++ )
50
        {
51
            for(int y = 0; y < N; y++)
52
            {
53
                _cell[x*N+y] = v._cell[x*N+y];
54
            }
55
        }
56
    }
57
58
    ~Matrix()
59
    {
60
    }
61
62
    void operator = (Matrix m)
63
    {
64
        for(int x = 0; x < N; x++)
65
        {
66
            for(int y = 0; y < N; y++)
67
            {
68
                cell(x, y) = m.cell(x, y);
69
            }
70
        }
71
    }
72
73
    Vector<N> row_to_vector(int y)
74
    {
75
        Vector<N> ret;
76
        for(int i = 0; i < N; i++)
77
        {
78
            ret[i] = _cell[y*N+i];
79
        }
80
        return ret;
81
    }
82
83
    Vector<N> col_to_vector(int x)
84
    {
85
        Vector<N> ret;
86
        for(int i = 0; i < N; i++)
87
        {
88
            ret[i] = _cell[i*N+x];
89
        }
90
        return ret;
91
    }
92
93
    void vector_to_row(Vector<N> v, int row)
94
    {
95
        for(int i = 0; i < N; i++)
96
        {
97
            cell(row, i) = v(i);
98
        }
99
    }
100
101
    void vector_to_col(Vector<N> v, int col)
102
    {
103
        for(int i = 0; i < N; i++)
104
        {
105
            cell(i, col) = v(i);
106
        }
107
    }
108
109
    double& operator ()(int x, int y)
110
    {
111
        return _cell[x*N+y];
112
    }
113
114
    double& cell(int x, int y)
115
    {
116
        return _cell[x*N+y];
117
    }
118
119
120
    Matrix operator + (Matrix m)
121
    {
122
        Matrix ret;
123
        for(int x = 0; x < N; x++)
124
        {
125
            for(int y = 0; y < N; y++)
126
            {
127
                ret._cell[x*N+y] = _cell[x*N+y] + m._cell[x*N+y];
128
            }
129
        }
130
        return ret;
131
    }
132
133
    Matrix operator - (Matrix m)
134
    {
135
        Matrix ret;
136
        for(int x = 0; x < N; x++)
137
        {
138
            for(int y = 0; y < N; y++)
139
            {
140
                ret._cell[x*N+y] = _cell[x*N+y] - m._cell[x*N+y];
141
            }
142
        }
143
        return ret;
144
    }
145
146
    Matrix operator * (double scalar)
147
    {
148
        Matrix ret;
149
        for(int x = 0; x < N; x++)
150
        {
151
            for(int y = 0; y < N; y++)
152
            {
153
                ret._cell[x*N+y] = _cell[x*N+y] * scalar;
154
            }
155
        }
156
        return ret;
157
    }
158
159
    Matrix operator * (Matrix m)
160
    {
161
        Matrix ret;
162
        for(int x = 0; x < N; x++)
163
        {
164
            for(int y = 0; y < N; y++)
165
            {
166
                Vector<N> row = row_to_vector(x);
167
                Vector<N> col = m.col_to_vector(y);
168
                ret.cell(x, y) = row.dot(col);
169
            }
170
        }
171
        return ret;
172
    }
173
174
    Matrix transpose()
175
    {
176
        Matrix ret;
177
        for(int x = 0; x < N; x++)
178
        {
179
            for(int y = 0; y < N; y++)
180
            {
181
                ret.cell(y, x) = cell(x, y);
182
            }
183
        }
184
        return ret;
185
    }
186
187
    Matrix<N-1> minor_matrix(int row, int col)
188
    {
189
        int colCount = 0, rowCount = 0;
190
        Matrix<N-1> ret;
191
        for(int i = 0; i < N; i++ )
192
        {
193
            if( i != row )
194
            {
195
                for(int j = 0; j < N; j++ )
196
                {
197
                    if( j != col )
198
                    {
199
                        ret(rowCount, colCount) = cell(i, j);
200
                        colCount++;
201
                    }
202
                }
203
                rowCount++;
204
            }
205
        }
206
        return ret;
207
    }
208
209
    double determinant()
210
    {
211
        if(N == 1)
212
            return cell(0, 0);
213
214
        float det = 0.0;
215
        for(int i = 0; i < N; i++ )
216
        {
217
            Matrix<N-1> minor = minor_matrix(0, i);
218
            det += (i%2==1?-1.0:1.0) * cell(0, i) * minor.determinant();
219
        }
220
        return det;
221
    }
222
223
    Matrix invert()
224
    {
225
        Matrix ret;
226
        float det = determinant();
227
228
        for(int x = 0; x < N; x++)
229
        {
230
            for(int y = 0; y < N; y++)
231
            {
232
                Matrix<N-1> minor = minor_matrix(y, x);
233
                ret(x, y) = det*minor.determinant();
234
                if( (x+y)%2 == 1)
235
                    ret(x, y) = -ret(x, y);
236
            }
237
        }
238
        return ret;
239
    }
240
241
private:
242
    double* _cell;
243 d964148c Tony DiCola
    double  _cell_data[N*N];
244 4bc1c0c1 Kevin Townsend
};
245
246
247
};
248
249
#endif