Statistics
| Branch: | Revision:

adafruit_bno055 / utility / matrix.h @ 67f3cff5

History | View | Annotate | Download (5.075 KB)

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

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