Statistics
| Branch: | Revision:

adafruit_bno055 / utility / matrix.h @ e8e79779

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