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 
33 namespace WF { namespace Math {
34 
35 template<const int rows, const int columns>
36 inline 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 
43 template<const int rows, const int columns>
44 inline 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 
54 template<const int rows, const int columns>
55 bool 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 
69 template<const int rows, const int columns>
70 inline 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 
81 template<const int rows, const int columns>
82 inline 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 
93 template<const int rows, const int columns> template<const int i>
94 inline 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 
109 template<const int rows, const int columns>
110 Matrix<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 
121 template<const int rows, const int columns>
122 Matrix<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 
133 template<const int rows, const int columns>
134 Matrix<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 
145 template<const int rows, const int columns>
146 inline 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 
157 template<const int rows, const int columns>
158 inline 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 
167 template<const int rows, const int columns>
168 inline 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 
177 template<const int rows, const int columns>
178 Matrix<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 
187 template<const int rows, const int columns>
188 Matrix<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 
197 template<const int rows, const int columns>
198 inline 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 
211 template<const int rows, const int columns>
212 inline 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 
221 template<const int rows, const int columns>
222 inline 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 
232 template<const int rows, const int columns>
233 void 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 
239 template<const int rows, const int columns>
240 inline 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 
250 template<const int rows, const int columns>
251 void 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 
257 template<const int rows, const int columns>
258 inline 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 
267 template<const int rows, const int columns>
268 inline 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 
279 template<const int rows, const int columns>
280 inline 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 
292 template<const int size>
293 inline 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 
305 template<const int size>
306 inline 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 
316 double _MatrixDeterminantImpl(const int size, double* m);
317 
318 template<const int size>
319 inline 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 
330 int _MatrixInverseImpl(const int size, double* in, double* out);
331 
332 template<const int size>
333 inline 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