# | Line 32 | Line 32 | |
---|---|---|
32 | #ifndef MATH_SQUAREMATRIX_HPP | |
33 | #define MATH_SQUAREMATRIX_HPP | |
34 | ||
35 | < | #include "Vector3d.hpp" |
35 | > | #include "math/RectMatrix.hpp" |
36 | ||
37 | namespace oopse { | |
38 | ||
# | Line 43 | Line 43 | namespace oopse { | |
43 | * @template Dim the dimension of the square matrix | |
44 | */ | |
45 | template<typename Real, int Dim> | |
46 | < | class SquareMatrix{ |
46 | > | class SquareMatrix : public RectMatrix<Real, Dim, Dim> { |
47 | public: | |
48 | ||
49 | /** default constructor */ | |
# | Line 53 | Line 53 | namespace oopse { | |
53 | data_[i][j] = 0.0; | |
54 | } | |
55 | ||
56 | – | /** Constructs and initializes every element of this matrix to a scalar */ |
57 | – | SquareMatrix(double s) { |
58 | – | for (unsigned int i = 0; i < Dim; i++) |
59 | – | for (unsigned int j = 0; j < Dim; j++) |
60 | – | data_[i][j] = s; |
61 | – | } |
62 | – | |
56 | /** copy constructor */ | |
57 | < | SquareMatrix(const SquareMatrix<Real, Dim>& m) { |
65 | < | *this = m; |
57 | > | SquareMatrix(const RectMatrix<Real, Dim, Dim>& m) : RectMatrix<Real, Dim, Dim>(m) { |
58 | } | |
59 | ||
68 | – | /** destructor*/ |
69 | – | ~SquareMatrix() {} |
70 | – | |
60 | /** copy assignment operator */ | |
61 | < | SquareMatrix<Real, Dim>& operator =(const SquareMatrix<Real, Dim>& m) { |
62 | < | for (unsigned int i = 0; i < Dim; i++) |
63 | < | for (unsigned int j = 0; j < Dim; j++) |
75 | < | data_[i][j] = m.data_[i][j]; |
61 | > | SquareMatrix<Real, Dim>& operator =(const RectMatrix<Real, Dim, Dim>& m) { |
62 | > | RectMatrix<Real, Dim, Dim>::operator=(m); |
63 | > | return *this; |
64 | } | |
65 | < | |
66 | < | /** |
79 | < | * Return the reference of a single element of this matrix. |
80 | < | * @return the reference of a single element of this matrix |
81 | < | * @param i row index |
82 | < | * @param j colum index |
83 | < | */ |
84 | < | double& operator()(unsigned int i, unsigned int j) { |
85 | < | return data_[i][j]; |
86 | < | } |
65 | > | |
66 | > | /** Retunrs an identity matrix*/ |
67 | ||
68 | < | /** |
69 | < | * Return the value of a single element of this matrix. |
90 | < | * @return the value of a single element of this matrix |
91 | < | * @param i row index |
92 | < | * @param j colum index |
93 | < | */ |
94 | < | double operator()(unsigned int i, unsigned int j) const { |
95 | < | return data_[i][j]; |
96 | < | } |
97 | < | |
98 | < | /** |
99 | < | * Returns a row of this matrix as a vector. |
100 | < | * @return a row of this matrix as a vector |
101 | < | * @param row the row index |
102 | < | */ |
103 | < | Vector<Real, Dim> getRow(unsigned int row) { |
104 | < | Vector<Real, Dim> v; |
105 | < | |
106 | < | for (unsigned int i = 0; i < Dim; i++) |
107 | < | v[i] = data_[row][i]; |
108 | < | |
109 | < | return v; |
110 | < | } |
111 | < | |
112 | < | /** |
113 | < | * Sets a row of this matrix |
114 | < | * @param row the row index |
115 | < | * @param v the vector to be set |
116 | < | */ |
117 | < | void setRow(unsigned int row, const Vector<Real, Dim>& v) { |
118 | < | Vector<Real, Dim> v; |
119 | < | |
120 | < | for (unsigned int i = 0; i < Dim; i++) |
121 | < | data_[row][i] = v[i]; |
122 | < | } |
123 | < | |
124 | < | /** |
125 | < | * Returns a column of this matrix as a vector. |
126 | < | * @return a column of this matrix as a vector |
127 | < | * @param col the column index |
128 | < | */ |
129 | < | Vector<Real, Dim> getColum(unsigned int col) { |
130 | < | Vector<Real, Dim> v; |
131 | < | |
132 | < | for (unsigned int i = 0; i < Dim; i++) |
133 | < | v[i] = data_[i][col]; |
134 | < | |
135 | < | return v; |
136 | < | } |
137 | < | |
138 | < | /** |
139 | < | * Sets a column of this matrix |
140 | < | * @param col the column index |
141 | < | * @param v the vector to be set |
142 | < | */ |
143 | < | void setColum(unsigned int col, const Vector<Real, Dim>& v){ |
144 | < | Vector<Real, Dim> v; |
145 | < | |
146 | < | for (unsigned int i = 0; i < Dim; i++) |
147 | < | data_[i][col] = v[i]; |
148 | < | } |
149 | < | |
150 | < | /** Negates the value of this matrix in place. */ |
151 | < | inline void negate() { |
152 | < | for (unsigned int i = 0; i < Dim; i++) |
153 | < | for (unsigned int j = 0; j < Dim; j++) |
154 | < | data_[i][j] = -data_[i][j]; |
155 | < | } |
156 | < | |
157 | < | /** |
158 | < | * Sets the value of this matrix to the negation of matrix m. |
159 | < | * @param m the source matrix |
160 | < | */ |
161 | < | inline void negate(const SquareMatrix<Real, Dim>& m) { |
162 | < | for (unsigned int i = 0; i < Dim; i++) |
163 | < | for (unsigned int j = 0; j < Dim; j++) |
164 | < | data_[i][j] = -m.data_[i][j]; |
165 | < | } |
166 | < | |
167 | < | /** |
168 | < | * Sets the value of this matrix to the sum of itself and m (*this += m). |
169 | < | * @param m the other matrix |
170 | < | */ |
171 | < | inline void add( const SquareMatrix<Real, Dim>& m ) { |
172 | < | for (unsigned int i = 0; i < Dim; i++) |
173 | < | for (unsigned int j = 0; j < Dim; j++) |
174 | < | data_[i][j] += m.data_[i][j]; |
175 | < | } |
176 | < | |
177 | < | /** |
178 | < | * Sets the value of this matrix to the sum of m1 and m2 (*this = m1 + m2). |
179 | < | * @param m1 the first matrix |
180 | < | * @param m2 the second matrix |
181 | < | */ |
182 | < | inline void add( const SquareMatrix<Real, Dim>& m1, const SquareMatrix<Real, Dim>& m2 ) { |
183 | < | for (unsigned int i = 0; i < Dim; i++) |
184 | < | for (unsigned int j = 0; j < Dim; j++) |
185 | < | data_[i][j] = m1.data_[i][j] + m2.data_[i][j]; |
186 | < | } |
187 | < | |
188 | < | /** |
189 | < | * Sets the value of this matrix to the difference of itself and m (*this -= m). |
190 | < | * @param m the other matrix |
191 | < | */ |
192 | < | inline void sub( const SquareMatrix<Real, Dim>& m ) { |
193 | < | for (unsigned int i = 0; i < Dim; i++) |
194 | < | for (unsigned int j = 0; j < Dim; j++) |
195 | < | data_[i][j] -= m.data_[i][j]; |
196 | < | } |
197 | < | |
198 | < | /** |
199 | < | * Sets the value of this matrix to the difference of matrix m1 and m2 (*this = m1 - m2). |
200 | < | * @param m1 the first matrix |
201 | < | * @param m2 the second matrix |
202 | < | */ |
203 | < | inline void sub( const SquareMatrix<Real, Dim>& m1, const Vector &m2){ |
204 | < | for (unsigned int i = 0; i < Dim; i++) |
205 | < | for (unsigned int j = 0; j < Dim; j++) |
206 | < | data_[i][j] = m1.data_[i][j] - m2.data_[i][j]; |
207 | < | } |
208 | < | |
209 | < | /** |
210 | < | * Sets the value of this matrix to the scalar multiplication of itself (*this *= s). |
211 | < | * @param s the scalar value |
212 | < | */ |
213 | < | inline void mul( double s ) { |
214 | < | for (unsigned int i = 0; i < Dim; i++) |
215 | < | for (unsigned int j = 0; j < Dim; j++) |
216 | < | data_[i][j] *= s; |
217 | < | } |
218 | < | |
219 | < | /** |
220 | < | * Sets the value of this matrix to the scalar multiplication of matrix m (*this = s * m). |
221 | < | * @param s the scalar value |
222 | < | * @param m the matrix |
223 | < | */ |
224 | < | inline void mul( double s, const SquareMatrix<Real, Dim>& m ) { |
225 | < | for (unsigned int i = 0; i < Dim; i++) |
226 | < | for (unsigned int j = 0; j < Dim; j++) |
227 | < | data_[i][j] = s * m.data_[i][j]; |
228 | < | } |
229 | < | |
230 | < | /** |
231 | < | * Sets the value of this matrix to the multiplication of this matrix and matrix m |
232 | < | * (*this = *this * m). |
233 | < | * @param m the matrix |
234 | < | */ |
235 | < | inline void mul(const SquareMatrix<Real, Dim>& m ) { |
236 | < | SquareMatrix<Real, Dim> tmp(*this); |
68 | > | static SquareMatrix<Real, Dim> identity() { |
69 | > | SquareMatrix<Real, Dim> m; |
70 | ||
71 | < | for (unsigned int i = 0; i < Dim; i++) |
72 | < | for (unsigned int j = 0; j < Dim; j++) { |
73 | < | |
74 | < | data_[i][j] = 0.0; |
75 | < | for (unsigned int k = 0; k < Dim; k++) |
76 | < | data_[i][j] = tmp.data_[i][k] * m.data_[k][j] |
244 | < | } |
245 | < | } |
246 | < | |
247 | < | /** |
248 | < | * Sets the value of this matrix to the left multiplication of matrix m into itself |
249 | < | * (*this = m * *this). |
250 | < | * @param m the matrix |
251 | < | */ |
252 | < | inline void leftmul(const SquareMatrix<Real, Dim>& m ) { |
253 | < | SquareMatrix<Real, Dim> tmp(*this); |
254 | < | |
255 | < | for (unsigned int i = 0; i < Dim; i++) |
256 | < | for (unsigned int j = 0; j < Dim; j++) { |
257 | < | |
258 | < | data_[i][j] = 0.0; |
259 | < | for (unsigned int k = 0; k < Dim; k++) |
260 | < | data_[i][j] = m.data_[i][k] * tmp.data_[k][j] |
261 | < | } |
262 | < | } |
263 | < | |
264 | < | /** |
265 | < | * Sets the value of this matrix to the multiplication of matrix m1 and matrix m2 |
266 | < | * (*this = m1 * m2). |
267 | < | * @param m1 the first matrix |
268 | < | * @param m2 the second matrix |
269 | < | */ |
270 | < | inline void mul(const SquareMatrix<Real, Dim>& m1, |
271 | < | const SquareMatrix<Real, Dim>& m2 ) { |
272 | < | for (unsigned int i = 0; i < Dim; i++) |
273 | < | for (unsigned int j = 0; j < Dim; j++) { |
274 | < | |
275 | < | data_[i][j] = 0.0; |
276 | < | for (unsigned int k = 0; k < Dim; k++) |
277 | < | data_[i][j] = m1.data_[i][k] * m2.data_[k][j] |
278 | < | } |
71 | > | for (unsigned int i = 0; i < Dim; i++) |
72 | > | for (unsigned int j = 0; j < Dim; j++) |
73 | > | if (i == j) |
74 | > | m(i, j) = 1.0; |
75 | > | else |
76 | > | m(i, j) = 0.0; |
77 | ||
78 | + | return m; |
79 | } | |
281 | – | |
282 | – | /** |
283 | – | * Sets the value of this matrix to the scalar division of itself (*this /= s ). |
284 | – | * @param s the scalar value |
285 | – | */ |
286 | – | inline void div( double s) { |
287 | – | for (unsigned int i = 0; i < Dim; i++) |
288 | – | for (unsigned int j = 0; j < Dim; j++) |
289 | – | data_[i][j] /= s; |
290 | – | } |
291 | – | |
292 | – | inline SquareMatrix<Real, Dim>& operator=(const SquareMatrix<Real, Dim>& v) { |
293 | – | if (this == &v) |
294 | – | return *this; |
295 | – | |
296 | – | for (unsigned int i = 0; i < Dim; i++) |
297 | – | data_[i] = v[i]; |
298 | – | |
299 | – | return *this; |
300 | – | } |
301 | – | |
302 | – | /** |
303 | – | * Sets the value of this matrix to the scalar division of matrix v1 (*this = v1 / s ). |
304 | – | * @paran v1 the source matrix |
305 | – | * @param s the scalar value |
306 | – | */ |
307 | – | inline void div( const SquareMatrix<Real, Dim>& v1, double s ) { |
308 | – | for (unsigned int i = 0; i < Dim; i++) |
309 | – | data_[i] = v1.data_[i] / s; |
310 | – | } |
80 | ||
81 | < | /** |
82 | < | * Multiples a scalar into every element of this matrix. |
83 | < | * @param s the scalar value |
315 | < | */ |
316 | < | SquareMatrix<Real, Dim>& operator *=(const double s) { |
317 | < | this->mul(s); |
318 | < | return *this; |
319 | < | } |
81 | > | /** Retunrs the inversion of this matrix. */ |
82 | > | SquareMatrix<Real, Dim> inverse() { |
83 | > | SquareMatrix<Real, Dim> result; |
84 | ||
85 | < | /** |
322 | < | * Divides every element of this matrix by a scalar. |
323 | < | * @param s the scalar value |
324 | < | */ |
325 | < | SquareMatrix<Real, Dim>& operator /=(const double s) { |
326 | < | this->div(s); |
327 | < | return *this; |
85 | > | return result; |
86 | } | |
87 | ||
330 | – | /** |
331 | – | * Sets the value of this matrix to the sum of the other matrix and itself (*this += m). |
332 | – | * @param m the other matrix |
333 | – | */ |
334 | – | SquareMatrix<Real, Dim>& operator += (const SquareMatrix<Real, Dim>& m) { |
335 | – | add(m); |
336 | – | return *this; |
337 | – | } |
338 | – | |
339 | – | /** |
340 | – | * Sets the value of this matrix to the differerence of itself and the other matrix (*this -= m) |
341 | – | * @param m the other matrix |
342 | – | */ |
343 | – | SquareMatrix<Real, Dim>& operator -= (const SquareMatrix<Real, Dim>& m){ |
344 | – | sub(m); |
345 | – | return *this; |
346 | – | } |
347 | – | |
348 | – | /** set this matrix to an identity matrix*/ |
349 | – | |
350 | – | void identity() { |
351 | – | for (unsigned int i = 0; i < Dim; i++) |
352 | – | for (unsigned int i = 0; i < Dim; i++) |
353 | – | if (i == j) |
354 | – | data_[i][j] = 1.0; |
355 | – | else |
356 | – | data_[i][j] = 0.0; |
357 | – | } |
358 | – | |
359 | – | /** Sets the value of this matrix to the inversion of itself. */ |
360 | – | void inverse() { |
361 | – | inverse(*this); |
362 | – | } |
363 | – | |
364 | – | /** |
365 | – | * Sets the value of this matrix to the inversion of other matrix. |
366 | – | * @ param m the source matrix |
367 | – | */ |
368 | – | void inverse(const SquareMatrix<Real, Dim>& m); |
88 | ||
370 | – | /** Sets the value of this matrix to the transpose of itself. */ |
371 | – | void transpose() { |
372 | – | for (unsigned int i = 0; i < Dim - 1; i++) |
373 | – | for (unsigned int j = i; j < Dim; j++) |
374 | – | std::swap(data_[i][j], data_[j][i]); |
375 | – | } |
89 | ||
377 | – | /** |
378 | – | * Sets the value of this matrix to the transpose of other matrix. |
379 | – | * @ param m the source matrix |
380 | – | */ |
381 | – | void transpose(const SquareMatrix<Real, Dim>& m) { |
382 | – | |
383 | – | if (this == &m) { |
384 | – | transpose(); |
385 | – | } else { |
386 | – | for (unsigned int i = 0; i < Dim; i++) |
387 | – | for (unsigned int j =0; j < Dim; j++) |
388 | – | data_[i][j] = m.data_[i][j]; |
389 | – | } |
390 | – | } |
391 | – | |
90 | /** Returns the determinant of this matrix. */ | |
91 | double determinant() const { | |
92 | < | |
92 | > | double det; |
93 | > | return det; |
94 | } | |
95 | ||
96 | /** Returns the trace of this matrix. */ | |
# | Line 408 | Line 107 | namespace oopse { | |
107 | bool isSymmetric() const { | |
108 | for (unsigned int i = 0; i < Dim - 1; i++) | |
109 | for (unsigned int j = i; j < Dim; j++) | |
110 | < | if (fabs(data_[i][j] - data_[j][i]) > epsilon) |
110 | > | if (fabs(data_[i][j] - data_[j][i]) > oopse::epsilon) |
111 | return false; | |
112 | ||
113 | return true; | |
114 | } | |
115 | ||
116 | /** Tests if this matrix is orthogona. */ | |
117 | < | bool isOrthogonal() const { |
118 | < | SquareMatrix<Real, Dim> t(*this); |
117 | > | bool isOrthogonal() { |
118 | > | SquareMatrix<Real, Dim> tmp; |
119 | ||
120 | < | t.transpose(); |
120 | > | tmp = *this * transpose(); |
121 | ||
122 | < | return isUnitMatrix(*this * t); |
122 | > | return tmp.isUnitMatrix(); |
123 | } | |
124 | ||
125 | /** Tests if this matrix is diagonal. */ | |
126 | bool isDiagonal() const { | |
127 | for (unsigned int i = 0; i < Dim ; i++) | |
128 | for (unsigned int j = 0; j < Dim; j++) | |
129 | < | if (i !=j && fabs(data_[i][j]) > epsilon) |
129 | > | if (i !=j && fabs(data_[i][j]) > oopse::epsilon) |
130 | return false; | |
131 | ||
132 | return true; | |
# | Line 439 | Line 138 | namespace oopse { | |
138 | return false; | |
139 | ||
140 | for (unsigned int i = 0; i < Dim ; i++) | |
141 | < | if (fabs(data_[i][i] - 1) > epsilon) |
141 | > | if (fabs(data_[i][i] - 1) > oopse::epsilon) |
142 | return false; | |
143 | ||
144 | return true; | |
145 | < | } |
447 | < | |
448 | < | protected: |
449 | < | double data_[Dim][Dim]; /**< matrix element */ |
145 | > | } |
146 | ||
147 | };//end SquareMatrix | |
148 | ||
453 | – | |
454 | – | /** Negate the value of every element of this matrix. */ |
455 | – | template<typename Real, int Dim> |
456 | – | inline SquareMatrix<Real, Dim> operator -(const SquareMatrix& m) { |
457 | – | SquareMatrix<Real, Dim> result(m); |
458 | – | |
459 | – | result.negate(); |
460 | – | |
461 | – | return result; |
462 | – | } |
463 | – | |
464 | – | /** |
465 | – | * Return the sum of two matrixes (m1 + m2). |
466 | – | * @return the sum of two matrixes |
467 | – | * @param m1 the first matrix |
468 | – | * @param m2 the second matrix |
469 | – | */ |
470 | – | template<typename Real, int Dim> |
471 | – | inline SquareMatrix<Real, Dim> operator + (const SquareMatrix<Real, Dim>& m1, |
472 | – | const SquareMatrix<Real, Dim>& m2) { |
473 | – | SquareMatrix<Real, Dim>result; |
474 | – | |
475 | – | result.add(m1, m2); |
476 | – | |
477 | – | return result; |
478 | – | } |
479 | – | |
480 | – | /** |
481 | – | * Return the difference of two matrixes (m1 - m2). |
482 | – | * @return the sum of two matrixes |
483 | – | * @param m1 the first matrix |
484 | – | * @param m2 the second matrix |
485 | – | */ |
486 | – | template<typename Real, int Dim> |
487 | – | inline SquareMatrix<Real, Dim> operator - (const SquareMatrix<Real, Dim>& m1, |
488 | – | const SquareMatrix<Real, Dim>& m2) { |
489 | – | SquareMatrix<Real, Dim>result; |
490 | – | |
491 | – | result.sub(m1, m2); |
492 | – | |
493 | – | return result; |
494 | – | } |
495 | – | |
496 | – | /** |
497 | – | * Return the multiplication of two matrixes (m1 * m2). |
498 | – | * @return the multiplication of two matrixes |
499 | – | * @param m1 the first matrix |
500 | – | * @param m2 the second matrix |
501 | – | */ |
502 | – | template<typename Real, int Dim> |
503 | – | inline SquareMatrix<Real, Dim> operator *(const SquareMatrix<Real, Dim>& m1, |
504 | – | const SquareMatrix<Real, Dim>& m2) { |
505 | – | SquareMatrix<Real, Dim> result; |
506 | – | |
507 | – | result.mul(m1, m2); |
508 | – | |
509 | – | return result; |
510 | – | } |
511 | – | |
512 | – | /** |
513 | – | * Return the multiplication of matrixes m and vector v (m * v). |
514 | – | * @return the multiplication of matrixes and vector |
515 | – | * @param m the matrix |
516 | – | * @param v the vector |
517 | – | */ |
518 | – | template<typename Real, int Dim> |
519 | – | inline Vector<Real, Dim> operator *(const SquareMatrix<Real, Dim>& m, |
520 | – | const SquareMatrix<Real, Dim>& v) { |
521 | – | Vector<Real, Dim> result; |
522 | – | |
523 | – | for (unsigned int i = 0; i < Dim ; i++) |
524 | – | for (unsigned int j = 0; j < Dim ; j++) |
525 | – | result[i] += m(i, j) * v[j]; |
526 | – | |
527 | – | return result; |
528 | – | } |
149 | } | |
150 | #endif //MATH_SQUAREMATRIX_HPP |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |