Statistics
| Branch: | Revision:

adafruit_bno055 / utility / matrix.h @ 463eabf7

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