wfmath 1.0.3
A math library for the Worldforge system.
oldmatrix_funcs.h
1// -*-C++-*-
2// matrix_funcs.h (Matrix<> template functions)
3//
4// The WorldForge Project
5// Copyright (C) 2001 The WorldForge Project
6//
7// This program is free software; you can redistribute it and/or modify
8// it under the terms of the GNU General Public License as published by
9// the Free Software Foundation; either version 2 of the License, or
10// (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License
18// along with this program; if not, write to the Free Software
19// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
20//
21// For information about WorldForge and its authors, please contact
22// the Worldforge Web Site at http://www.worldforge.org.
23
24// Author: Ron Steinke
25// Created: 2001-12-7
26
27#ifndef WFMATH_MATRIX_FUNCS_H
28#define WFMATH_MATRIX_FUNCS_H
29
30#include <wfmath/matrix.h>
31#include <wfmath/error.h>
32
33namespace WF { namespace Math {
34
35template<const int rows, const int columns>
36inline Matrix<rows,columns>::Matrix(const Matrix<rows,columns>& m)
37{
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];
41}
42
43template<const int rows, const int columns>
44inline bool Matrix<rows,columns>::operator==(const Matrix<rows,columns>& m) const
45{
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])
49 return false;
50
51 return true;
52}
53
54template<const int rows, const int columns>
55bool Matrix<rows,columns>::operator< (const Matrix<rows,columns>& m) const
56{
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])
60 return true;
61 if(m_elem[i][j] > m.m_elem[i][j])
62 return false;
63 }
64 }
65
66 return false;
67}
68
69template<const int rows, const int columns>
70inline Matrix<rows,columns> Matrix<rows,columns>::operator+(const Matrix<rows,columns>& m) const
71{
72 Matrix<rows,columns> out;
73
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];
77
78 return out;
79}
80
81template<const int rows, const int columns>
82inline Matrix<rows,columns> Matrix<rows,columns>::operator-(const Matrix<rows,columns>& m) const
83{
84 Matrix<rows,columns> out;
85
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];
89
90 return out;
91}
92
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
95{
96 Matrix<columns, i> out;
97
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];
103 }
104 }
105
106 return out;
107}
108
109template<const int rows, const int columns>
110Matrix<rows,columns> Matrix<rows,columns>::operator*(const double& d) const
111{
112 Matrix<rows,columns> out;
113
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;
117
118 return out;
119}
120
121template<const int rows, const int columns>
122Matrix<rows,columns> operator*(const double& d, const Matrix<rows,columns>& m)
123{
124 Matrix<rows,columns> out;
125
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;
129
130 return out;
131}
132
133template<const int rows, const int columns>
134Matrix<rows,columns> Matrix<rows,columns>::operator/(const double& d) const
135{
136 Matrix<rows,columns> out;
137
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;
141
142 return out;
143}
144
145template<const int rows, const int columns>
146inline Matrix<rows,columns> Matrix<rows,columns>::operator-() const // Unary minus
147{
148 Matrix<rows,columns> out;
149
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];
153
154 return out;
155}
156
157template<const int rows, const int columns>
158inline Matrix<rows,columns>& Matrix<rows,columns>::operator+=(const Matrix<rows,columns>& m)
159{
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];
163
164 return *this;
165}
166
167template<const int rows, const int columns>
168inline Matrix<rows,columns>& Matrix<rows,columns>::operator-=(const Matrix<rows,columns>& m)
169{
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];
173
174 return *this;
175}
176
177template<const int rows, const int columns>
178Matrix<rows,columns>& Matrix<rows,columns>::operator*=(const double& d)
179{
180 for(int i = 0; i < rows; ++i)
181 for(int j = 0; j < columns; ++j)
182 m_elem[i][j] *= d;
183
184 return *this;
185}
186
187template<const int rows, const int columns>
188Matrix<rows,columns>& Matrix<rows,columns>::operator/=(const double& d)
189{
190 for(int i = 0; i < rows; ++i)
191 for(int j = 0; j < columns; ++j)
192 m_elem[i][j] /= d;
193
194 return *this;
195}
196
197template<const int rows, const int columns>
198inline Vector<rows> Matrix<rows,columns>::operator*(const Vector<columns>& v) const
199{
200 Vector<rows> out;
201
202 for(int i = 0; i < rows; ++i) {
203 out[i] = 0;
204 for(int j = 0; j < columns; ++j)
205 out[i] += m_elem[i][j] * v[j];
206 }
207
208 return out;
209}
210
211template<const int rows, const int columns>
212inline Matrix<rows,columns> OuterProduct(const Vector<rows>& v1, const Vector<columns>& v2)
213{
214 Matrix<rows,columns> out;
215
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];
219}
220
221template<const int rows, const int columns>
222inline Vector<columns> Matrix<rows,columns>::row(const int i) const
223{
224 Vector<columns> out;
225
226 for(int j = 0; j < columns; ++j)
227 out[j] = m_elem[i][j];
228
229 return out;
230}
231
232template<const int rows, const int columns>
233void Matrix<rows,columns>::setRow(const int i, const Vector<columns>& v)
234{
235 for(int j = 0; j < columns; ++j)
236 m_elem[i][j] = v[j];
237}
238
239template<const int rows, const int columns>
240inline Vector<rows> Matrix<rows,columns>::column(const int i) const
241{
242 Vector<columns> out;
243
244 for(int j = 0; j < rows; ++j)
245 out[j] = m_elem[j][i];
246
247 return out;
248}
249
250template<const int rows, const int columns>
251void Matrix<rows,columns>::setColumn(const int i, const Vector<rows>& v)
252{
253 for(int j = 0; j < rows; ++j)
254 m_elem[j][i] = v[j];
255}
256
257template<const int rows, const int columns>
258inline Matrix<rows,columns>& Matrix<rows,columns>::zero()
259{
260 for(int i = 0; i < rows; ++i)
261 for(int j = 0; j < columns; ++j)
262 m_elem[i][j] = 0;
263
264 return *this;
265}
266
267template<const int rows, const int columns>
268inline Matrix<columns,rows> Matrix<rows,columns>::transpose() const
269{
270 Matrix<columns,rows> m;
271
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];
275
276 return m;
277}
278
279template<const int rows, const int columns>
280inline Matrix<rows,columns>& Matrix<rows,columns>::identity()
281{
282 Vector<rows> v;
283
284 for(int i = 0; i < rows; ++i)
285 v[i] = 1;
286
287 this->diagonal(v);
288
289 return *this;
290}
291
292template<const int size>
293inline Matrix<size> DiagonalMatrix(const Vector<size>& v)
294{
295 Matrix<size> out;
296
297 out.zero();
298
299 for(int i = 0; i < size; ++i)
300 out.m_elem[i][i] = v[i];
301
302 return out;
303}
304
305template<const int size>
306inline double Trace(const Matrix<size>& m)
307{
308 double out = 0;
309
310 for(int i = 0; i < size; ++i)
311 out += m.m_elem[i][i];
312
313 return out;
314}
315
316double _MatrixDeterminantImpl(const int size, double* m);
317
318template<const int size>
319inline double Determinant(const Matrix<size>& m)
320{
321 double mtmp[size*size]; // Scratch space
322
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);
326
327 return _MatrixDeterminantImpl(size, mtmp);
328}
329
330int _MatrixInverseImpl(const int size, double* in, double* out);
331
332template<const int size>
333inline Matrix<size> Inverse(const Matrix<size>& m)
334{
335 double in[size*size], out[size*size]; // Scratch space
336
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;
341 }
342 }
343
344 if(_MatrixInverseImpl(size, in, out) != 0)
345 throw DegenerateMatrix<size>(m);
346
347 Matrix<size> mout;
348
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];
352
353 return mout;
354}
355
356}} // namespace WF::Math
357
358#endif // WFMATH_MATRIX_FUNCS_H
RotMatrix< dim > operator*(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2