Statistics
| Branch: | Revision:

adafruit_bno055 / utility / matrix.h @ 2d9324e5

History | View | Annotate | Download (4.951 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 b5582106 Gé Vissers
    Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
7

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