Statistics
| Branch: | Revision:

adafruit_bno055 / utility / matrix.h @ 2d9324e5

History | View | Annotate | Download (4.951 KB)

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

6
    Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
7

8
    This program is free software: you can redistribute it and/or modify
9
    it under the terms of the GNU General Public License as published by
10
    the Free Software Foundation, either version 3 of the License, or
11
    (at your option) any later version.
12

13
    This program is distributed in the hope that it will be useful,
14
    but WITHOUT ANY WARRANTY; without even the implied warranty of
15
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16
    GNU General Public License for more details.
17

18
    You should have received a copy of the GNU General Public License
19
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
20
*/
21

    
22
#ifndef IMUMATH_MATRIX_HPP
23
#define IMUMATH_MATRIX_HPP
24

    
25
#include <string.h>
26
#include <stdint.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() const
196
    {
197
        // specialization for N == 1 given below this class
198
        double det = 0.0, sign = 1.0;
199
        for (int i = 0; i < N; ++i, sign = -sign)
200
            det += sign * cell(0, i) * minor_matrix(0, i).determinant();
201
        return det;
202
    }
203

    
204
    Matrix invert() const
205
    {
206
        Matrix ret;
207
        double det = determinant();
208

    
209
        for (int i = 0; i < N; i++)
210
        {
211
            for (int j = 0; j < N; j++)
212
            {
213
                ret(i, j) = minor_matrix(j, i).determinant() / det;
214
                if ((i+j)%2 == 1)
215
                    ret(i, j) = -ret(i, j);
216
            }
217
        }
218
        return ret;
219
    }
220

    
221
    double trace() const
222
    {
223
        double tr = 0.0;
224
        for (int i = 0; i < N; ++i)
225
            tr += cell(i, i);
226
        return tr;
227
    }
228

    
229
private:
230
    double _cell_data[N*N];
231
};
232

    
233

    
234
template<>
235
inline double Matrix<1>::determinant() const
236
{
237
    return cell(0, 0);
238
}
239

    
240
};
241

    
242
#endif
243