| 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 |  |  | 
| 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 */ | 
| 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. */ | 
| 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; | 
| 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 | + | bool jacobi(const SquareMatrix<Real, Dim>& a, Vector<Real, Dim>& w, | 
| 179 | + | SquareMatrix<Real, Dim>& v); | 
| 180 |  | };//end SquareMatrix | 
| 181 |  |  | 
| 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); | 
| 182 |  |  | 
| 183 | < | result.negate(); | 
| 183 | > | #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) | 
| 184 | > | #define MAX_ROTATIONS 60 | 
| 185 |  |  | 
| 186 | < | return result; | 
| 186 | > | template<typename Real, int Dim> | 
| 187 | > | bool SquareMatrix<Real, Dim>::jacobi(const SquareMatrix<Real, Dim>& a, Vector<Real, Dim>& w, | 
| 188 | > | SquareMatrix<Real, Dim>& v) { | 
| 189 | > | const int N = Dim; | 
| 190 | > | int i, j, k, iq, ip; | 
| 191 | > | double tresh, theta, tau, t, sm, s, h, g, c; | 
| 192 | > | double tmp; | 
| 193 | > | Vector<Real, Dim> b, z; | 
| 194 | > |  | 
| 195 | > | // initialize | 
| 196 | > | for (ip=0; ip<N; ip++) | 
| 197 | > | { | 
| 198 | > | for (iq=0; iq<N; iq++) v(ip, iq) = 0.0; | 
| 199 | > | v(ip, ip) = 1.0; | 
| 200 |  | } | 
| 201 | < |  | 
| 202 | < | /** | 
| 203 | < | * Return the sum of two matrixes  (m1 + m2). | 
| 204 | < | * @return the sum of two matrixes | 
| 205 | < | * @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; | 
| 201 | > | for (ip=0; ip<N; ip++) | 
| 202 | > | { | 
| 203 | > | b(ip) = w(ip) = a(ip, ip); | 
| 204 | > | z(ip) = 0.0; | 
| 205 | > | } | 
| 206 |  |  | 
| 207 | < | result.add(m1, m2); | 
| 207 | > | // begin rotation sequence | 
| 208 | > | for (i=0; i<MAX_ROTATIONS; i++) | 
| 209 | > | { | 
| 210 | > | sm = 0.0; | 
| 211 | > | for (ip=0; ip<2; ip++) | 
| 212 | > | { | 
| 213 | > | for (iq=ip+1; iq<N; iq++) sm += fabs(a(ip, iq)); | 
| 214 | > | } | 
| 215 | > | if (sm == 0.0) break; | 
| 216 |  |  | 
| 217 | < | return result; | 
| 218 | < | } | 
| 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; | 
| 217 | > | if (i < 4) tresh = 0.2*sm/(9); | 
| 218 | > | else tresh = 0.0; | 
| 219 |  |  | 
| 220 | < | result.sub(m1, m2); | 
| 220 | > | for (ip=0; ip<2; ip++) | 
| 221 | > | { | 
| 222 | > | for (iq=ip+1; iq<N; iq++) | 
| 223 | > | { | 
| 224 | > | g = 100.0*fabs(a(ip, iq)); | 
| 225 | > | if (i > 4 && (fabs(w(ip))+g) == fabs(w(ip)) | 
| 226 | > | && (fabs(w(iq))+g) == fabs(w(iq))) | 
| 227 | > | { | 
| 228 | > | a(ip, iq) = 0.0; | 
| 229 | > | } | 
| 230 | > | else if (fabs(a(ip, iq)) > tresh) | 
| 231 | > | { | 
| 232 | > | h = w(iq) - w(ip); | 
| 233 | > | if ( (fabs(h)+g) == fabs(h)) t = (a(ip, iq)) / h; | 
| 234 | > | else | 
| 235 | > | { | 
| 236 | > | theta = 0.5*h / (a(ip, iq)); | 
| 237 | > | t = 1.0 / (fabs(theta)+sqrt(1.0+theta*theta)); | 
| 238 | > | if (theta < 0.0) t = -t; | 
| 239 | > | } | 
| 240 | > | c = 1.0 / sqrt(1+t*t); | 
| 241 | > | s = t*c; | 
| 242 | > | tau = s/(1.0+c); | 
| 243 | > | h = t*a(ip, iq); | 
| 244 | > | z(ip) -= h; | 
| 245 | > | z(iq) += h; | 
| 246 | > | w(ip) -= h; | 
| 247 | > | w(iq) += h; | 
| 248 | > | a(ip, iq)=0.0; | 
| 249 | > | for (j=0;j<ip-1;j++) | 
| 250 | > | { | 
| 251 | > | ROT(a,j,ip,j,iq); | 
| 252 | > | } | 
| 253 | > | for (j=ip+1;j<iq-1;j++) | 
| 254 | > | { | 
| 255 | > | ROT(a,ip,j,j,iq); | 
| 256 | > | } | 
| 257 | > | for (j=iq+1; j<N; j++) | 
| 258 | > | { | 
| 259 | > | ROT(a,ip,j,iq,j); | 
| 260 | > | } | 
| 261 | > | for (j=0; j<N; j++) | 
| 262 | > | { | 
| 263 | > | ROT(v,j,ip,j,iq); | 
| 264 | > | } | 
| 265 | > | } | 
| 266 | > | } | 
| 267 | > | } | 
| 268 |  |  | 
| 269 | < | return result; | 
| 269 | > | for (ip=0; ip<N; ip++) | 
| 270 | > | { | 
| 271 | > | b(ip) += z(ip); | 
| 272 | > | w(ip) = b(ip); | 
| 273 | > | z(ip) = 0.0; | 
| 274 | > | } | 
| 275 |  | } | 
| 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; | 
| 276 |  |  | 
| 277 | < | result.mul(m1, m2); | 
| 277 | > | if ( i >= MAX_ROTATIONS ) | 
| 278 | > | return false; | 
| 279 |  |  | 
| 280 | < | return result; | 
| 280 | > | // sort eigenfunctions | 
| 281 | > | for (j=0; j<N; j++) | 
| 282 | > | { | 
| 283 | > | k = j; | 
| 284 | > | tmp = w(k); | 
| 285 | > | for (i=j; i<N; i++) | 
| 286 | > | { | 
| 287 | > | if (w(i) >= tmp) | 
| 288 | > | { | 
| 289 | > | k = i; | 
| 290 | > | tmp = w(k); | 
| 291 | > | } | 
| 292 | > | } | 
| 293 | > | if (k != j) | 
| 294 | > | { | 
| 295 | > | w(k) = w(j); | 
| 296 | > | w(j) = tmp; | 
| 297 | > | for (i=0; i<N; i++) | 
| 298 | > | { | 
| 299 | > | tmp = v(i, j); | 
| 300 | > | v(i, j) = v(i, k); | 
| 301 | > | v(i, k) = tmp; | 
| 302 | > | } | 
| 303 | > | } | 
| 304 |  | } | 
| 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; | 
| 305 |  |  | 
| 306 | < | for (unsigned int i = 0; i < Dim ; i++) | 
| 307 | < | for (unsigned int j = 0; j < Dim ; j++) | 
| 308 | < | result[i] += m(i, j) * v[j]; | 
| 309 | < |  | 
| 310 | < | return result; | 
| 306 | > | //    insure eigenvector consistency (i.e., Jacobi can compute | 
| 307 | > | //    vectors that are negative of one another (.707,.707,0) and | 
| 308 | > | //    (-.707,-.707,0). This can reek havoc in | 
| 309 | > | //    hyperstreamline/other stuff. We will select the most | 
| 310 | > | //    positive eigenvector. | 
| 311 | > | int numPos; | 
| 312 | > | for (j=0; j<N; j++) | 
| 313 | > | { | 
| 314 | > | for (numPos=0, i=0; i<N; i++) if ( v(i, j) >= 0.0 ) numPos++; | 
| 315 | > | if ( numPos < 2 ) for(i=0; i<N; i++) v(i, j) *= -1.0; | 
| 316 |  | } | 
| 317 | + |  | 
| 318 | + | return true; | 
| 319 |  | } | 
| 320 | + |  | 
| 321 | + | #undef ROT | 
| 322 | + | #undef MAX_ROTATIONS | 
| 323 | + |  | 
| 324 | + | } | 
| 325 | + |  | 
| 326 |  | #endif //MATH_SQUAREMATRIX_HPP |