# | 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++) |
74 | < | for (unsigned int j = 0; j < Dim; j++) |
75 | < | data_[i][j] = m.data_[i][j]; |
76 | < | } |
77 | < | |
78 | < | /** |
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 | < | } |
87 | < | |
88 | < | /** |
89 | < | * 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); |
237 | < | |
238 | < | for (unsigned int i = 0; i < Dim; i++) |
239 | < | for (unsigned int j = 0; j < Dim; j++) { |
240 | < | |
241 | < | data_[i][j] = 0.0; |
242 | < | for (unsigned int k = 0; k < Dim; k++) |
243 | < | 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 | < | } |
279 | < | |
280 | < | } |
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 | < | |
61 | > | SquareMatrix<Real, Dim>& operator =(const RectMatrix<Real, Dim, Dim>& m) { |
62 | > | RectMatrix<Real, Dim, Dim>::operator=(m); |
63 | return *this; | |
64 | } | |
65 | < | |
66 | < | /** |
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 | < | } |
65 | > | |
66 | > | /** Retunrs an identity matrix*/ |
67 | ||
68 | < | /** |
69 | < | * Multiples a scalar into every element of this matrix. |
70 | < | * @param s the scalar value |
315 | < | */ |
316 | < | SquareMatrix<Real, Dim>& operator *=(const double s) { |
317 | < | this->mul(s); |
318 | < | return *this; |
319 | < | } |
320 | < | |
321 | < | /** |
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; |
328 | < | } |
329 | < | |
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() { |
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 i = 0; i < Dim; i++) |
72 | > | for (unsigned int j = 0; j < Dim; j++) |
73 | if (i == j) | |
74 | < | data_[i][j] = 1.0; |
74 | > | m(i, j) = 1.0; |
75 | else | |
76 | < | data_[i][j] = 0.0; |
357 | < | } |
76 | > | m(i, j) = 0.0; |
77 | ||
78 | < | /** 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); |
369 | < | |
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]); |
78 | > | return m; |
79 | } | |
80 | ||
81 | < | /** |
82 | < | * Sets the value of this matrix to the transpose of other matrix. |
83 | < | * @ 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 | < | } |
81 | > | /** Retunrs the inversion of this matrix. */ |
82 | > | SquareMatrix<Real, Dim> inverse() { |
83 | > | SquareMatrix<Real, Dim> result; |
84 | ||
85 | + | return result; |
86 | + | } |
87 | + | |
88 | /** Returns the determinant of this matrix. */ | |
89 | double determinant() const { | |
90 | < | |
90 | > | double det; |
91 | > | return det; |
92 | } | |
93 | ||
94 | /** Returns the trace of this matrix. */ | |
# | Line 408 | Line 105 | namespace oopse { | |
105 | bool isSymmetric() const { | |
106 | for (unsigned int i = 0; i < Dim - 1; i++) | |
107 | for (unsigned int j = i; j < Dim; j++) | |
108 | < | if (fabs(data_[i][j] - data_[j][i]) > epsilon) |
108 | > | if (fabs(data_[i][j] - data_[j][i]) > oopse::epsilon) |
109 | return false; | |
110 | ||
111 | return true; | |
112 | } | |
113 | ||
114 | < | /** Tests if this matrix is orthogona. */ |
115 | < | bool isOrthogonal() const { |
116 | < | SquareMatrix<Real, Dim> t(*this); |
114 | > | /** Tests if this matrix is orthogonal. */ |
115 | > | bool isOrthogonal() { |
116 | > | SquareMatrix<Real, Dim> tmp; |
117 | ||
118 | < | t.transpose(); |
118 | > | tmp = *this * transpose(); |
119 | ||
120 | < | return isUnitMatrix(*this * t); |
120 | > | return tmp.isDiagonal(); |
121 | } | |
122 | ||
123 | /** Tests if this matrix is diagonal. */ | |
124 | bool isDiagonal() const { | |
125 | for (unsigned int i = 0; i < Dim ; i++) | |
126 | for (unsigned int j = 0; j < Dim; j++) | |
127 | < | if (i !=j && fabs(data_[i][j]) > epsilon) |
127 | > | if (i !=j && fabs(data_[i][j]) > oopse::epsilon) |
128 | return false; | |
129 | ||
130 | return true; | |
# | Line 439 | Line 136 | namespace oopse { | |
136 | return false; | |
137 | ||
138 | for (unsigned int i = 0; i < Dim ; i++) | |
139 | < | if (fabs(data_[i][i] - 1) > epsilon) |
139 | > | if (fabs(data_[i][i] - 1) > oopse::epsilon) |
140 | return false; | |
141 | ||
142 | return true; | |
143 | + | } |
144 | + | |
145 | + | void diagonalize() { |
146 | + | jacobi(m, eigenValues, ortMat); |
147 | } | |
447 | – | |
448 | – | protected: |
449 | – | double data_[Dim][Dim]; /**< matrix element */ |
148 | ||
149 | + | /** |
150 | + | * Finds the eigenvalues and eigenvectors of a symmetric matrix |
151 | + | * @param eigenvals a reference to a vector3 where the |
152 | + | * eigenvalues will be stored. The eigenvalues are ordered so |
153 | + | * that eigenvals[0] <= eigenvals[1] <= eigenvals[2]. |
154 | + | * @return an orthogonal matrix whose ith column is an |
155 | + | * eigenvector for the eigenvalue eigenvals[i] |
156 | + | */ |
157 | + | SquareMatrix<Real, Dim> findEigenvectors(Vector<Real, Dim>& eigenValues) { |
158 | + | SquareMatrix<Real, Dim> ortMat; |
159 | + | |
160 | + | if ( !isSymmetric()){ |
161 | + | throw(); |
162 | + | } |
163 | + | |
164 | + | SquareMatrix<Real, Dim> m(*this); |
165 | + | jacobi(m, eigenValues, ortMat); |
166 | + | |
167 | + | return ortMat; |
168 | + | } |
169 | + | /** |
170 | + | * Jacobi iteration routines for computing eigenvalues/eigenvectors of |
171 | + | * real symmetric matrix |
172 | + | * |
173 | + | * @return true if success, otherwise return false |
174 | + | * @param a source matrix |
175 | + | * @param w output eigenvalues |
176 | + | * @param v output eigenvectors |
177 | + | */ |
178 | + | void jacobi(const SquareMatrix<Real, Dim>& a, |
179 | + | Vector<Real, Dim>& w, |
180 | + | SquareMatrix<Real, Dim>& v); |
181 | };//end SquareMatrix | |
182 | ||
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); |
183 | ||
184 | < | result.negate(); |
184 | > | #define ROT(a,i,j,k,l) g=a(i, j);h=a(k, l);a(i, j)=g-s*(h+g*tau);a(k, l)=h+s*(g-h*tau) |
185 | > | #define MAX_ROTATIONS 60 |
186 | ||
187 | < | return result; |
187 | > | template<Real, int Dim> |
188 | > | void SquareMatrix<Real, int Dim>::jacobi(SquareMatrix<Real, Dim>& a, |
189 | > | Vector<Real, Dim>& w, |
190 | > | SquareMatrix<Real, Dim>& v) { |
191 | > | const int N = Dim; |
192 | > | int i, j, k, iq, ip; |
193 | > | double tresh, theta, tau, t, sm, s, h, g, c; |
194 | > | double tmp; |
195 | > | Vector<Real, Dim> b, z; |
196 | > | |
197 | > | // initialize |
198 | > | for (ip=0; ip<N; ip++) |
199 | > | { |
200 | > | for (iq=0; iq<N; iq++) v(ip, iq) = 0.0; |
201 | > | v(ip, ip) = 1.0; |
202 | } | |
203 | < | |
204 | < | /** |
205 | < | * Return the sum of two matrixes (m1 + m2). |
206 | < | * @return the sum of two matrixes |
207 | < | * @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; |
203 | > | for (ip=0; ip<N; ip++) |
204 | > | { |
205 | > | b(ip) = w(ip) = a(ip, ip); |
206 | > | z(ip) = 0.0; |
207 | > | } |
208 | ||
209 | < | result.add(m1, m2); |
209 | > | // begin rotation sequence |
210 | > | for (i=0; i<MAX_ROTATIONS; i++) |
211 | > | { |
212 | > | sm = 0.0; |
213 | > | for (ip=0; ip<2; ip++) |
214 | > | { |
215 | > | for (iq=ip+1; iq<N; iq++) sm += fabs(a(ip, iq)); |
216 | > | } |
217 | > | if (sm == 0.0) break; |
218 | ||
219 | < | return result; |
220 | < | } |
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; |
219 | > | if (i < 4) tresh = 0.2*sm/(9); |
220 | > | else tresh = 0.0; |
221 | ||
222 | < | result.sub(m1, m2); |
222 | > | for (ip=0; ip<2; ip++) |
223 | > | { |
224 | > | for (iq=ip+1; iq<N; iq++) |
225 | > | { |
226 | > | g = 100.0*fabs(a(ip, iq)); |
227 | > | if (i > 4 && (fabs(w(ip))+g) == fabs(w(ip)) |
228 | > | && (fabs(w(iq))+g) == fabs(w(iq))) |
229 | > | { |
230 | > | a(ip, iq) = 0.0; |
231 | > | } |
232 | > | else if (fabs(a(ip, iq)) > tresh) |
233 | > | { |
234 | > | h = w(iq) - w(ip); |
235 | > | if ( (fabs(h)+g) == fabs(h)) t = (a(ip, iq)) / h; |
236 | > | else |
237 | > | { |
238 | > | theta = 0.5*h / (a(ip, iq)); |
239 | > | t = 1.0 / (fabs(theta)+sqrt(1.0+theta*theta)); |
240 | > | if (theta < 0.0) t = -t; |
241 | > | } |
242 | > | c = 1.0 / sqrt(1+t*t); |
243 | > | s = t*c; |
244 | > | tau = s/(1.0+c); |
245 | > | h = t*a(ip, iq); |
246 | > | z(ip) -= h; |
247 | > | z(iq) += h; |
248 | > | w(ip) -= h; |
249 | > | w(iq) += h; |
250 | > | a(ip, iq)=0.0; |
251 | > | for (j=0;j<ip-1;j++) |
252 | > | { |
253 | > | ROT(a,j,ip,j,iq); |
254 | > | } |
255 | > | for (j=ip+1;j<iq-1;j++) |
256 | > | { |
257 | > | ROT(a,ip,j,j,iq); |
258 | > | } |
259 | > | for (j=iq+1; j<N; j++) |
260 | > | { |
261 | > | ROT(a,ip,j,iq,j); |
262 | > | } |
263 | > | for (j=0; j<N; j++) |
264 | > | { |
265 | > | ROT(v,j,ip,j,iq); |
266 | > | } |
267 | > | } |
268 | > | } |
269 | > | } |
270 | ||
271 | < | return result; |
271 | > | for (ip=0; ip<N; ip++) |
272 | > | { |
273 | > | b(ip) += z(ip); |
274 | > | w(ip) = b(ip); |
275 | > | z(ip) = 0.0; |
276 | > | } |
277 | } | |
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; |
278 | ||
279 | < | result.mul(m1, m2); |
279 | > | if ( i >= MAX_ROTATIONS ) |
280 | > | return false; |
281 | ||
282 | < | return result; |
282 | > | // sort eigenfunctions |
283 | > | for (j=0; j<N; j++) |
284 | > | { |
285 | > | k = j; |
286 | > | tmp = w(k); |
287 | > | for (i=j; i<N; i++) |
288 | > | { |
289 | > | if (w(i) >= tmp) |
290 | > | { |
291 | > | k = i; |
292 | > | tmp = w(k); |
293 | > | } |
294 | > | } |
295 | > | if (k != j) |
296 | > | { |
297 | > | w(k) = w(j); |
298 | > | w(j) = tmp; |
299 | > | for (i=0; i<N; i++) |
300 | > | { |
301 | > | tmp = v(i, j); |
302 | > | v(i, j) = v(i, k); |
303 | > | v(i, k) = tmp; |
304 | > | } |
305 | > | } |
306 | } | |
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; |
307 | ||
308 | < | for (unsigned int i = 0; i < Dim ; i++) |
309 | < | for (unsigned int j = 0; j < Dim ; j++) |
310 | < | result[i] += m(i, j) * v[j]; |
311 | < | |
312 | < | return result; |
308 | > | // insure eigenvector consistency (i.e., Jacobi can compute |
309 | > | // vectors that are negative of one another (.707,.707,0) and |
310 | > | // (-.707,-.707,0). This can reek havoc in |
311 | > | // hyperstreamline/other stuff. We will select the most |
312 | > | // positive eigenvector. |
313 | > | int numPos; |
314 | > | for (j=0; j<N; j++) |
315 | > | { |
316 | > | for (numPos=0, i=0; i<N; i++) if ( v(i, j) >= 0.0 ) numPos++; |
317 | > | if ( numPos < 2 ) for(i=0; i<N; i++) v(i, j) *= -1.0; |
318 | } | |
319 | + | |
320 | + | return true; |
321 | } | |
322 | + | |
323 | + | #undef ROT |
324 | + | #undef MAX_ROTATIONS |
325 | + | |
326 | + | } |
327 | + | |
328 | + | |
329 | + | } |
330 | #endif //MATH_SQUAREMATRIX_HPP |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |