27#ifndef WFMATH_MATRIX_FUNCS_H
28#define WFMATH_MATRIX_FUNCS_H
30#include <wfmath/matrix.h>
31#include <wfmath/error.h>
33namespace WF {
namespace Math {
35template<const
int rows, const
int columns>
36inline Matrix<rows,columns>::Matrix(
const Matrix<rows,columns>& m)
38 for(
int i = 0; i < rows; ++i)
39 for(
int j = 0; j < columns; ++j)
40 m_elem[i][j] = m.m_elem[i][j];
43template<const
int rows, const
int columns>
44inline bool Matrix<rows,columns>::operator==(
const Matrix<rows,columns>& m)
const
46 for(
int i = 0; i < rows; ++i)
47 for(
int j = 0; j < columns; ++j)
48 if(m_elem[i][j] != m.m_elem[i][j])
54template<const
int rows, const
int columns>
55bool Matrix<rows,columns>::operator< (
const Matrix<rows,columns>& m)
const
57 for(
int i = 0; i < rows; ++i) {
58 for(
int j = 0; j < columns; ++j) {
59 if(m_elem[i][j] < m.m_elem[i][j])
61 if(m_elem[i][j] > m.m_elem[i][j])
69template<const
int rows, const
int columns>
70inline Matrix<rows,columns> Matrix<rows,columns>::operator+(
const Matrix<rows,columns>& m)
const
72 Matrix<rows,columns> out;
74 for(
int i = 0; i < rows; ++i)
75 for(
int j = 0; j < columns; ++j)
76 out.m_elem[i][j] = this->m_elem[i][j] + m.m_elem[i][j];
81template<const
int rows, const
int columns>
82inline Matrix<rows,columns> Matrix<rows,columns>::operator-(
const Matrix<rows,columns>& m)
const
84 Matrix<rows,columns> out;
86 for(
int i = 0; i < rows; ++i)
87 for(
int j = 0; j < columns; ++j)
88 out.m_elem[i][j] = m_elem[i][j] - m.m_elem[i][j];
93template<const
int rows, const
int columns>
template<const
int i>
94inline Matrix<rows,i> Matrix<rows,columns>::operator*(
const Matrix<columns, i>& m)
const
96 Matrix<columns, i> out;
98 for(
int j = 0; j < rows; ++j) {
99 for(
int k = 0; k < i; ++k) {
100 out.m_elem[j][k] = 0;
101 for(
int l = 0; l < columns; ++l)
102 out.m_elem[j][k] += m_elem[j][l] * m.m_elem[l][k];
109template<const
int rows, const
int columns>
110Matrix<rows,columns> Matrix<rows,columns>::operator*(
const double& d)
const
112 Matrix<rows,columns> out;
114 for(
int i = 0; i < rows; ++i)
115 for(
int j = 0; j < columns; ++j)
116 out.m_elem[i][j] = m_elem[i][j] * d;
121template<const
int rows, const
int columns>
122Matrix<rows,columns>
operator*(
const double& d,
const Matrix<rows,columns>& m)
124 Matrix<rows,columns> out;
126 for(
int i = 0; i < rows; ++i)
127 for(
int j = 0; j < columns; ++j)
128 out.m_elem[i][j] = m.m_elem[i][j] * d;
133template<const
int rows, const
int columns>
134Matrix<rows,columns> Matrix<rows,columns>::operator/(
const double& d)
const
136 Matrix<rows,columns> out;
138 for(
int i = 0; i < rows; ++i)
139 for(
int j = 0; j < columns; ++j)
140 out.m_elem[i][j] = m_elem[i][j] / d;
145template<const
int rows, const
int columns>
146inline Matrix<rows,columns> Matrix<rows,columns>::operator-() const
148 Matrix<rows,columns> out;
150 for(
int i = 0; i < rows; ++i)
151 for(
int j = 0; j < columns; ++j)
152 out.m_elem[i][j] = -m_elem[i][j];
157template<const
int rows, const
int columns>
158inline Matrix<rows,columns>& Matrix<rows,columns>::operator+=(
const Matrix<rows,columns>& m)
160 for(
int i = 0; i < rows; ++i)
161 for(
int j = 0; j < columns; ++j)
162 m_elem[i][j] += m.m_elem[i][j];
167template<const
int rows, const
int columns>
168inline Matrix<rows,columns>& Matrix<rows,columns>::operator-=(
const Matrix<rows,columns>& m)
170 for(
int i = 0; i < rows; ++i)
171 for(
int j = 0; j < columns; ++j)
172 m_elem[i][j] -= m.m_elem[i][j];
177template<const
int rows, const
int columns>
178Matrix<rows,columns>& Matrix<rows,columns>::operator*=(
const double& d)
180 for(
int i = 0; i < rows; ++i)
181 for(
int j = 0; j < columns; ++j)
187template<const
int rows, const
int columns>
188Matrix<rows,columns>& Matrix<rows,columns>::operator/=(
const double& d)
190 for(
int i = 0; i < rows; ++i)
191 for(
int j = 0; j < columns; ++j)
197template<const
int rows, const
int columns>
198inline Vector<rows> Matrix<rows,columns>::operator*(
const Vector<columns>& v)
const
202 for(
int i = 0; i < rows; ++i) {
204 for(
int j = 0; j < columns; ++j)
205 out[i] += m_elem[i][j] * v[j];
211template<const
int rows, const
int columns>
212inline Matrix<rows,columns> OuterProduct(
const Vector<rows>& v1,
const Vector<columns>& v2)
214 Matrix<rows,columns> out;
216 for(
int i = 0; i < rows; ++i)
217 for(
int j = 0; j < columns; ++j)
218 out.m_elem[i][j] = v1[i] * v2[j];
221template<const
int rows, const
int columns>
222inline Vector<columns> Matrix<rows,columns>::row(
const int i)
const
226 for(
int j = 0; j < columns; ++j)
227 out[j] = m_elem[i][j];
232template<const
int rows, const
int columns>
233void Matrix<rows,columns>::setRow(
const int i,
const Vector<columns>& v)
235 for(
int j = 0; j < columns; ++j)
239template<const
int rows, const
int columns>
240inline Vector<rows> Matrix<rows,columns>::column(
const int i)
const
244 for(
int j = 0; j < rows; ++j)
245 out[j] = m_elem[j][i];
250template<const
int rows, const
int columns>
251void Matrix<rows,columns>::setColumn(
const int i,
const Vector<rows>& v)
253 for(
int j = 0; j < rows; ++j)
257template<const
int rows, const
int columns>
258inline Matrix<rows,columns>& Matrix<rows,columns>::zero()
260 for(
int i = 0; i < rows; ++i)
261 for(
int j = 0; j < columns; ++j)
267template<const
int rows, const
int columns>
268inline Matrix<columns,rows> Matrix<rows,columns>::transpose()
const
270 Matrix<columns,rows> m;
272 for(
int i = 0; i < rows; ++i)
273 for(
int j = 0; j < columns; ++j)
274 m.m_elem[j][i] = m_elem[i][j];
279template<const
int rows, const
int columns>
280inline Matrix<rows,columns>& Matrix<rows,columns>::identity()
284 for(
int i = 0; i < rows; ++i)
292template<const
int size>
293inline Matrix<size> DiagonalMatrix(
const Vector<size>& v)
299 for(
int i = 0; i < size; ++i)
300 out.m_elem[i][i] = v[i];
305template<const
int size>
306inline double Trace(
const Matrix<size>& m)
310 for(
int i = 0; i < size; ++i)
311 out += m.m_elem[i][i];
316double _MatrixDeterminantImpl(
const int size,
double* m);
318template<const
int size>
319inline double Determinant(
const Matrix<size>& m)
321 double mtmp[size*size];
323 for(
int i = 0; i < size; ++i)
324 for(
int j = 0; j < size; ++j)
325 mtmp[i*size+j] = m.elem(i, j);
327 return _MatrixDeterminantImpl(size, mtmp);
330int _MatrixInverseImpl(
const int size,
double* in,
double* out);
332template<const
int size>
333inline Matrix<size> Inverse(
const Matrix<size>& m)
335 double in[size*size], out[size*size];
337 for(
int i = 0; i < size; ++i) {
338 for(
int j = 0; j < size; ++j) {
339 in[i*size+j] = m.elem(i, j);
340 out[i*size+j] = (i == j) ? 1 : 0;
344 if(_MatrixInverseImpl(size, in, out) != 0)
345 throw DegenerateMatrix<size>(m);
349 for(
int i = 0; i < size; ++i)
350 for(
int j = 0; j < size; ++j)
351 mout.elem(i, j) = out[i*size+j];
RotMatrix< dim > operator*(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2