Statistics
| Branch: | Revision:

adafruit_bno055 / utility / matrix.h @ 771690b8

History | View | Annotate | Download (4.941 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
        _cell = &_cell_data[0];
40
        memset(_cell, 0, N*N*sizeof(double));
41
    }
42

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

    
52
    ~Matrix()
53
    {
54
    }
55

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

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

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

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

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

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

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

    
119

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

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

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

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

    
164
    Matrix transpose()
165
    {
166
        Matrix ret;
167
        for(int x = 0; x < N; x++)
168
        {
169
            for(int y = 0; y < N; y++)
170
            {
171
                ret.cell(y, x) = cell(x, y);
172
            }
173
        }
174
        return ret;
175
    }
176

    
177
    Matrix<N-1> minor_matrix(int row, int col)
178
    {
179
        int colCount = 0, rowCount = 0;
180
        Matrix<N-1> ret;
181
        for(int i = 0; i < N; i++ )
182
        {
183
            if( i != row )
184
            {
185
                for(int j = 0; j < N; j++ )
186
                {
187
                    if( j != col )
188
                    {
189
                        ret(rowCount, colCount) = cell(i, j);
190
                        colCount++;
191
                    }
192
                }
193
                rowCount++;
194
            }
195
        }
196
        return ret;
197
    }
198

    
199
    double determinant()
200
    {
201
        if(N == 1)
202
            return cell(0, 0);
203

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

    
213
    Matrix invert()
214
    {
215
        Matrix ret;
216
        float det = determinant();
217

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

    
231
private:
232
    double* _cell;
233
    double  _cell_data[N*N];
234
};
235

    
236

    
237
};
238

    
239
#endif
240