Statistics
| Branch: | Revision:

adafruit_bno055 / utility / matrix.h @ 55604844

History | View | Annotate | Download (4.761 KB)

1
/*
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
#include "vector.h"
29

    
30
namespace imu
31
{
32

    
33

    
34
template <uint8_t N> class Matrix
35
{
36
public:
37
    Matrix()
38
    {
39
        memset(_cell_data, 0, N*N*sizeof(double));
40
    }
41

    
42
    Matrix(const Matrix &m)
43
    {
44
        for (int ij = 0; ij < N*N; ++ij)
45
        {
46
            _cell_data[ij] = m._cell_data[ij];
47
        }
48
    }
49

    
50
    ~Matrix()
51
    {
52
    }
53

    
54
    Matrix& operator=(const Matrix& m)
55
    {
56
        for (int ij = 0; ij < N*N; ++ij)
57
        {
58
            _cell_data[ij] = m._cell_data[ij];
59
        }
60
        return *this;
61
    }
62

    
63
    Vector<N> row_to_vector(int i) const
64
    {
65
        Vector<N> ret;
66
        for (int j = 0; j < N; j++)
67
        {
68
            ret[j] = cell(i, j);
69
        }
70
        return ret;
71
    }
72

    
73
    Vector<N> col_to_vector(int j) const
74
    {
75
        Vector<N> ret;
76
        for (int i = 0; i < N; i++)
77
        {
78
            ret[i] = cell(i, j);
79
        }
80
        return ret;
81
    }
82

    
83
    void vector_to_row(const Vector<N>& v, int i)
84
    {
85
        for (int j = 0; j < N; j++)
86
        {
87
            cell(i, j) = v[j];
88
        }
89
    }
90

    
91
    void vector_to_col(const Vector<N>& v, int j)
92
    {
93
        for (int i = 0; i < N; i++)
94
        {
95
            cell(i, j) = v[i];
96
        }
97
    }
98

    
99
    double operator()(int i, int j) const
100
    {
101
        return cell(i, j);
102
    }
103
    double& operator()(int i, int j)
104
    {
105
        return cell(i, j);
106
    }
107

    
108
    double cell(int i, int j) const
109
    {
110
        return _cell_data[i*N+j];
111
    }
112
    double& cell(int i, int j)
113
    {
114
        return _cell_data[i*N+j];
115
    }
116

    
117

    
118
    Matrix operator+(const Matrix& m) const
119
    {
120
        Matrix ret;
121
        for (int ij = 0; ij < N*N; ++ij)
122
        {
123
            ret._cell_data[ij] = _cell_data[ij] + m._cell_data[ij];
124
        }
125
        return ret;
126
    }
127

    
128
    Matrix operator-(const Matrix& m) const
129
    {
130
        Matrix ret;
131
        for (int ij = 0; ij < N*N; ++ij)
132
        {
133
            ret._cell_data[ij] = _cell_data[ij] - m._cell_data[ij];
134
        }
135
        return ret;
136
    }
137

    
138
    Matrix operator*(double scalar) const
139
    {
140
        Matrix ret;
141
        for (int ij = 0; ij < N*N; ++ij)
142
        {
143
            ret._cell_data[ij] = _cell_data[ij] * scalar;
144
        }
145
        return ret;
146
    }
147

    
148
    Matrix operator*(const Matrix& m) const
149
    {
150
        Matrix ret;
151
        for (int i = 0; i < N; i++)
152
        {
153
            Vector<N> row = row_to_vector(i);
154
            for (int j = 0; j < N; j++)
155
            {
156
                ret(i, j) = row.dot(m.col_to_vector(j));
157
            }
158
        }
159
        return ret;
160
    }
161

    
162
    Matrix transpose() const
163
    {
164
        Matrix ret;
165
        for (int i = 0; i < N; i++)
166
        {
167
            for (int j = 0; j < N; j++)
168
            {
169
                ret(j, i) = cell(i, j);
170
            }
171
        }
172
        return ret;
173
    }
174

    
175
    Matrix<N-1> minor_matrix(int row, int col) const
176
    {
177
        Matrix<N-1> ret;
178
        for (int i = 0, im = 0; i < N; i++)
179
        {
180
            if (i == row)
181
                continue;
182

    
183
            for (int j = 0, jm = 0; j < N; j++)
184
            {
185
                if (j != col)
186
                {
187
                    ret(im, jm++) = cell(i, j);
188
                }
189
            }
190
            im++;
191
        }
192
        return ret;
193
    }
194

    
195
    double determinant()
196
    {
197
        if(N == 1)
198
            return cell(0, 0);
199

    
200
        float det = 0.0;
201
        for(int i = 0; i < N; i++ )
202
        {
203
            Matrix<N-1> minor = minor_matrix(0, i);
204
            det += (i%2==1?-1.0:1.0) * cell(0, i) * minor.determinant();
205
        }
206
        return det;
207
    }
208

    
209
    Matrix invert()
210
    {
211
        Matrix ret;
212
        float det = determinant();
213

    
214
        for(int x = 0; x < N; x++)
215
        {
216
            for(int y = 0; y < N; y++)
217
            {
218
                Matrix<N-1> minor = minor_matrix(y, x);
219
                ret(x, y) = det*minor.determinant();
220
                if( (x+y)%2 == 1)
221
                    ret(x, y) = -ret(x, y);
222
            }
223
        }
224
        return ret;
225
    }
226

    
227
private:
228
    double  _cell_data[N*N];
229
};
230

    
231

    
232
};
233

    
234
#endif
235