Branch data Line data Source code
1 : : //- Class: CubitMatrix
2 : : //- Description: This file defines the CubitMatrix class.
3 : : //- Owner: Dan Goodrich
4 : : //- Checked by:
5 : :
6 : : #include <cassert>
7 : :
8 : : #include "CubitMatrix.hpp"
9 : : #include "CubitMessage.hpp"
10 : : #include "CubitVector.hpp"
11 : : #include "CubitDefines.h"
12 : : #include "CubitFile.hpp"
13 : :
14 : 0 : CubitMatrix::CubitMatrix()
15 : : {
16 : 0 : matrixMem = NULL;
17 : 0 : matrixPtr = NULL;
18 : 0 : reset_size( 3, 3 );
19 : 0 : }
20 : :
21 : 763 : CubitMatrix::CubitMatrix( const int n, const int m )
22 : : {
23 : 763 : matrixMem = NULL;
24 : 763 : matrixPtr = NULL;
25 : 763 : reset_size( n, m );
26 : 763 : }
27 : :
28 : :
29 : 1038 : CubitMatrix::CubitMatrix( const int n )
30 : : {
31 : 1038 : matrixMem = NULL;
32 : 1038 : matrixPtr = NULL;
33 : 1038 : reset_size( n, n );
34 : :
35 : : // Initialize matrix to identity.
36 : 1038 : set_to_identity();
37 : 1038 : }
38 : :
39 : 0 : CubitMatrix::CubitMatrix (const CubitVector& vec1,
40 : : const CubitVector& vec2,
41 : 0 : const CubitVector& vec3 )
42 : : {
43 : 0 : matrixMem = NULL;
44 : 0 : matrixPtr = NULL;
45 : 0 : reset_size( 3, 3 );
46 : :
47 : : // Initialize the matrix columns to the three vectors
48 : 0 : matrixPtr[0][0] = vec1.x();
49 : 0 : matrixPtr[1][0] = vec1.y();
50 : 0 : matrixPtr[2][0] = vec1.z();
51 : 0 : matrixPtr[0][1] = vec2.x();
52 : 0 : matrixPtr[1][1] = vec2.y();
53 : 0 : matrixPtr[2][1] = vec2.z();
54 : 0 : matrixPtr[0][2] = vec3.x();
55 : 0 : matrixPtr[1][2] = vec3.y();
56 : 0 : matrixPtr[2][2] = vec3.z();
57 : 0 : }
58 : :
59 : 0 : CubitMatrix::CubitMatrix (const CubitVector& vec1,
60 : : const CubitVector& vec2,
61 : : const CubitVector& vec3,
62 : 0 : const CubitVector& vec4 )
63 : : {
64 : 0 : matrixMem = NULL;
65 : 0 : matrixPtr = NULL;
66 : 0 : reset_size( 3, 4 );
67 : :
68 : : // Initialize the matrix columns to the four vectors
69 : 0 : matrixPtr[0][0] = vec1.x();
70 : 0 : matrixPtr[1][0] = vec1.y();
71 : 0 : matrixPtr[2][0] = vec1.z();
72 : 0 : matrixPtr[0][1] = vec2.x();
73 : 0 : matrixPtr[1][1] = vec2.y();
74 : 0 : matrixPtr[2][1] = vec2.z();
75 : 0 : matrixPtr[0][2] = vec3.x();
76 : 0 : matrixPtr[1][2] = vec3.y();
77 : 0 : matrixPtr[2][2] = vec3.z();
78 : 0 : matrixPtr[0][3] = vec4.x();
79 : 0 : matrixPtr[1][3] = vec4.y();
80 : 0 : matrixPtr[2][3] = vec4.z();
81 : 0 : }
82 : :
83 : 0 : CubitMatrix::CubitMatrix(const CubitVector& vec1,
84 : 0 : const CubitVector& vec2 )
85 : : {
86 : 0 : matrixMem = NULL;
87 : 0 : matrixPtr = NULL;
88 : 0 : reset_size( 3, 3 );
89 : :
90 : 0 : this->vector_outer_product(vec1, vec2);
91 : 0 : }
92 : :
93 : 0 : CubitMatrix::CubitMatrix
94 : : (
95 : : std::vector<int> &is,
96 : : std::vector<int> &js,
97 : : std::vector<double> &es,
98 : : int n,
99 : : int m
100 : 0 : )
101 : : {
102 : 0 : matrixMem = NULL;
103 : 0 : matrixPtr = NULL;
104 : 0 : reset_size( n, m );
105 : 0 : int length = is.size();
106 [ # # ]: 0 : for ( int k = 0; k < length; k++ )
107 : : {
108 : 0 : int i = is[k];
109 : 0 : int j = js[k];
110 : 0 : matrixPtr[i][j] += es[k];
111 : : }
112 : 0 : }
113 : :
114 : 796 : CubitMatrix::CubitMatrix( const CubitMatrix &matrix )
115 : : {
116 : 796 : matrixMem = NULL;
117 : 796 : matrixPtr = NULL;
118 : 796 : reset_size( matrix.num_rows(), matrix.num_cols() );
119 : :
120 : : int ii;
121 [ + + ]: 3980 : for( ii = 0; ii < numRows; ii++ )
122 : : {
123 [ + + ]: 15920 : for( int jj = 0; jj < numCols; jj++ )
124 : 12736 : matrixPtr[ii][jj] = matrix.get( ii, jj );
125 : : }
126 : 796 : }
127 : :
128 : :
129 : :
130 : 2498 : CubitMatrix::~CubitMatrix()
131 : : {
132 [ + - ]: 2498 : delete [] matrixPtr;
133 [ + - ]: 2498 : delete [] matrixMem;
134 [ - + ]: 2498 : }
135 : :
136 : 2597 : void CubitMatrix::reset_size( const int n, const int m, double default_value )
137 : : {
138 [ - + ][ # # ]: 2597 : if ( matrixPtr ) delete [] matrixPtr;
139 [ - + ][ # # ]: 2597 : if ( matrixMem ) delete [] matrixMem;
140 : :
141 : 2597 : numRows = n;
142 : 2597 : numCols = m;
143 : :
144 [ + - ]: 2597 : matrixMem = new double[numRows*numCols];
145 [ + - ]: 2597 : matrixPtr = new double *[n];
146 : :
147 : : int ii;
148 [ + + ]: 12376 : for( ii = 0; ii < n; ii++ )
149 : : {
150 : 9779 : matrixPtr[ii] = &matrixMem[ii*numCols];
151 : : }
152 : :
153 : : // Initialize matrix to zeros.
154 [ + + ]: 12376 : for( ii = 0; ii < n; ii++ )
155 [ + + ]: 47068 : for( int jj = 0 ; jj < m; jj++ )
156 : 37289 : matrixPtr[ii][jj] = default_value;
157 : 2597 : }
158 : :
159 : 0 : void CubitMatrix::print_matrix() const
160 : : {
161 : 0 : printf( "\n\n" );
162 [ # # ]: 0 : for( int row = 0; row < numRows; row++ )
163 : : {
164 [ # # ]: 0 : for( int col = 0; col < numCols; col++ )
165 [ # # ][ # # ]: 0 : PRINT_INFO("%25.15f", matrixPtr[row][col]);
166 [ # # ][ # # ]: 0 : PRINT_INFO("\n");
167 : : }
168 : 0 : }
169 : 0 : void CubitMatrix::print_matrix( char *filename ) const
170 : : {
171 [ # # ][ # # ]: 0 : CubitFile fp( filename, "w" );
[ # # ]
172 [ # # ][ # # ]: 0 : if ( !fp )
173 : : {
174 : : printf( "CubitMatrix::print_matrix - Unable to open %s for writing\n",
175 [ # # ]: 0 : filename );
176 : 0 : return;
177 : : }
178 : :
179 [ # # ][ # # ]: 0 : for( int row = 0; row < numRows; row++ )
[ # # ]
180 : : {
181 [ # # ]: 0 : for( int col = 0; col < numCols; col++ )
182 [ # # ][ # # ]: 0 : fprintf( fp.file(), "%20.15f", matrixPtr[row][col] );
183 [ # # ][ # # ]: 0 : fprintf( fp.file(), "\n" );
184 : 0 : }
185 : : }
186 : :
187 : : // Sets this matrix equal to 'matrix'. 'this' is
188 : : // redimensioned if needed.
189 : 385 : CubitMatrix CubitMatrix::operator=(const CubitMatrix& matrix)
190 : : {
191 : : int i, j;
192 : :
193 [ + - - + ]: 770 : if (numRows != matrix.num_rows() ||
[ - + ]
194 : 385 : numCols != matrix.num_cols())
195 : : {
196 [ # # ]: 0 : delete [] matrixPtr;
197 [ # # ]: 0 : delete [] matrixMem;
198 : :
199 : 0 : numRows = matrix.num_rows();
200 : 0 : numCols = matrix.num_cols();
201 [ # # ]: 0 : matrixPtr = new double*[numRows];
202 [ # # ]: 0 : matrixMem = new double[numRows*numCols];
203 [ # # ]: 0 : for (i = 0; i < numRows; i++)
204 : 0 : matrixPtr[i] = &matrixMem[i*numCols];
205 : : }
206 : :
207 [ + + ]: 1925 : for(i = 0; i < numRows; i++ )
208 [ + + ]: 7700 : for(j = 0; j < numCols; j++)
209 : 6160 : matrixPtr[i][j] = matrix.get(i, j);
210 : :
211 : 385 : return *this;
212 : : }
213 : :
214 : :
215 : : // Multiply this ( size NxM ) with the input matrix ( size MxL ).
216 : : // return matrix of size NxL
217 : 154 : CubitMatrix CubitMatrix::operator*(const CubitMatrix& matrix ) const
218 : : {
219 : : // Check that we can multiply them.
220 [ - + ]: 154 : if(numCols != matrix.num_rows())
221 [ # # ][ # # ]: 0 : throw std::invalid_argument ("# of columns in first MUST match # of rows of second");
222 : : //assert( numCols == matrix.num_rows() );
223 : :
224 : 154 : CubitMatrix return_matrix( numRows, matrix.num_cols() );
225 : :
226 [ + + ]: 770 : for( int ii = 0; ii < numRows; ii++ )
227 : : {
228 [ + - ][ + + ]: 3080 : for( int jj = 0; jj < matrix.num_cols(); jj++ )
229 : : {
230 : 2464 : double temp = 0.0;
231 [ + + ]: 12320 : for( int kk = 0; kk < numCols; kk++ )
232 : : {
233 : : //temp += get( ii, kk ) * matrix.get( kk, jj );
234 : 9856 : temp += matrixPtr[ii][kk] * matrix.matrixPtr[kk][jj];
235 : : }
236 [ + - ]: 2464 : return_matrix.set( ii, jj, temp );
237 : : }
238 : : }
239 : 154 : return return_matrix;
240 : : }
241 : :
242 : :
243 : :
244 : : // multiply this times the input vector
245 : 154 : CubitVector CubitMatrix::operator* (const CubitVector& vector ) const
246 : : {
247 : : // Check that we can multiply them.
248 [ - + ]: 154 : if(numCols!=3)
249 : : {
250 [ # # ][ # # ]: 0 : throw std::invalid_argument("Matrix must have 3 columns");
251 : : }
252 : : //assert( numCols == 3 );
253 : :
254 : : double vec1[3];
255 : : double vec2[3];
256 : :
257 [ + - ]: 154 : vec2[0] = vector.x();
258 [ + - ]: 154 : vec2[1] = vector.y();
259 [ + - ]: 154 : vec2[2] = vector.z();
260 : :
261 [ + + ]: 616 : for( int row = 0; row < numRows; row++ )
262 : : {
263 : 462 : vec1[row] = 0.0;
264 [ + + ]: 1848 : for( int col = 0; col < numCols; col++ )
265 : : {
266 : 1386 : vec1[row] += ( matrixPtr[row][col] * vec2[col] );
267 : : }
268 : : }
269 : :
270 [ + - ]: 154 : return CubitVector( vec1[0], vec1[1], vec1[2] );
271 : : }
272 : :
273 : : // multiply this times the input vector
274 : 0 : std::vector<double> CubitMatrix::operator* (const std::vector<double> & vector) const
275 : : {
276 : : // Check that we can multiply them.
277 [ # # ]: 0 : if(numCols != (int) vector.size())
278 [ # # ][ # # ]: 0 : throw std::invalid_argument ("Columns of Matrix do not match vector size");
279 : : //assert( numCols == vector.size() );
280 : :
281 [ # # ]: 0 : std::vector<double> return_vec( numRows );
282 : :
283 [ # # ]: 0 : for( int row = 0; row < numRows; row++ )
284 : : {
285 [ # # ]: 0 : return_vec[row] = 0.0;
286 [ # # ]: 0 : for( int col = 0; col < numCols; col++ )
287 : : {
288 [ # # ][ # # ]: 0 : return_vec[row] += ( matrixPtr[row][col] * vector[col] );
289 : : }
290 : : }
291 : :
292 : 0 : return return_vec;
293 : : }
294 : :
295 : : // multiply this times the input scalar
296 : 0 : CubitMatrix CubitMatrix::operator*( double val ) const
297 : : {
298 : 0 : CubitMatrix matrix( numRows, numCols );
299 : :
300 [ # # ]: 0 : for( int row = 0; row < numRows; row++ )
301 : : {
302 [ # # ]: 0 : for( int col = 0; col < numCols; col++ )
303 : : {
304 [ # # ]: 0 : matrix.set( row, col,( matrixPtr[row][col] * val ) );
305 : : }
306 : : }
307 : 0 : return matrix;
308 : : }
309 : :
310 : : // multiply this times the input scalar
311 : 0 : CubitMatrix CubitMatrix::operator/( double val ) const
312 : : {
313 [ # # ]: 0 : if(val==0)
314 [ # # ][ # # ]: 0 : throw std::invalid_argument("Cannot Divide by Zero");
315 : : //assert( val != 0 );
316 : 0 : CubitMatrix matrix( numRows, numCols );
317 : :
318 [ # # ]: 0 : for( int ii = 0; ii < numRows; ii++ )
319 : : {
320 [ # # ]: 0 : for( int jj = 0; jj < numCols; jj++ )
321 : : {
322 [ # # ]: 0 : matrix.set( ii, jj,( matrixPtr[ii][jj] / val ) );
323 : : }
324 : : }
325 : 0 : return matrix;
326 : : }
327 : :
328 : :
329 : : // subtract this ( size NxM ) with the input matrix ( size NxM ).
330 : : // return matrix of size NxM
331 : 0 : CubitMatrix CubitMatrix::operator-(const CubitMatrix& matrix) const
332 : : {
333 : 0 : CubitMatrix return_matrix( numRows, numCols );
334 : :
335 [ # # ]: 0 : for( int ii = 0; ii < numRows; ii++ )
336 : : {
337 [ # # ]: 0 : for( int jj = 0; jj < numCols; jj++ )
338 : : {
339 : 0 : return_matrix.set( ii, jj, matrixPtr[ii][jj] -
340 [ # # ][ # # ]: 0 : matrix.get( ii, jj ));
341 : : }
342 : : }
343 : :
344 : 0 : return return_matrix;
345 : : }
346 : :
347 : : // add this ( size NxM ) with the input matrix ( size NxM ).
348 : : // return matrix of size NxM
349 : 0 : CubitMatrix CubitMatrix::operator+(const CubitMatrix& matrix ) const
350 : : {
351 : 0 : CubitMatrix return_matrix( numRows, numCols );
352 : :
353 [ # # ]: 0 : for( int ii = 0; ii < numRows; ii++ )
354 : : {
355 [ # # ]: 0 : for( int jj = 0; jj < numCols; jj++ )
356 : : {
357 : 0 : return_matrix.set( ii, jj, matrixPtr[ii][jj] +
358 [ # # ][ # # ]: 0 : matrix.get( ii, jj ));
359 : : }
360 : : }
361 : :
362 : 0 : return return_matrix;
363 : : }
364 : :
365 : :
366 : 0 : CubitMatrix& CubitMatrix::operator+=( const CubitMatrix &matrix )
367 : : {
368 [ # # ]: 0 : for( int ii = 0; ii < numRows; ii++ )
369 : : {
370 [ # # ]: 0 : for( int jj = 0; jj < numCols; jj++ )
371 : : {
372 : 0 : matrixPtr[ii][jj] += matrix.get( ii, jj );
373 : : }
374 : : }
375 : 0 : return *this;
376 : : }
377 : :
378 : 0 : CubitMatrix& CubitMatrix::operator*=(const double multiplier)
379 : : {
380 [ # # ]: 0 : for( int ii = 0; ii < numRows; ii++ )
381 : : {
382 [ # # ]: 0 : for( int jj = 0; jj < numCols; jj++ )
383 : : {
384 : 0 : matrixPtr[ii][jj] *= multiplier;
385 : : }
386 : : }
387 : 0 : return *this;
388 : : }
389 : :
390 : : // Sets the matrix to all zeros except along diagonal.
391 : : // Matrix doesn't have to be square.
392 : 1148 : void CubitMatrix::set_to_identity()
393 : : {
394 [ + + ]: 5740 : for (int i = numRows; i--; )
395 [ + + ]: 22960 : for (int j = numCols; j--; )
396 : : {
397 [ + + ]: 18368 : if (i == j)
398 : 4592 : matrixPtr[i][j] = 1;
399 : : else
400 : 13776 : matrixPtr[i][j] = 0;
401 : : }
402 : 1148 : }
403 : :
404 : : /*
405 : : // Inverts this matrix, if it is of size NxN, and a 3x3 or
406 : : // smaller.
407 : : CubitMatrix CubitMatrix::inverse()
408 : : {
409 : : CubitMatrix adj_matrix( numRows, numCols );
410 : : double det;
411 : :
412 : : if( numRows > 4 )
413 : : {
414 : : // rval = recipie_inverse();
415 : : // return rval == CUBIT_TRUE ? CUBIT_TRUE : CUBIT_FALSE;
416 : : PRINT_INFO("Can't handle matrice's greater than 3x3 yet.\n");
417 : : }
418 : :
419 : : det = determinant();
420 : : assert( fabs(det) > CUBIT_DBL_MIN );
421 : :
422 : : adj_matrix = adjoint();
423 : : return adj_matrix * ( 1.0/det );
424 : : }
425 : : */
426 : :
427 : : // Inverts this matrix, if it is size 4x4 or bigger
428 : : // uses ludcmp and lubksb from numerical recipes.
429 : 0 : CubitMatrix CubitMatrix::inverse()
430 : : {
431 : : // can't invert a non-square matrix
432 [ # # ]: 0 : if(numRows!=numCols)
433 [ # # ][ # # ]: 0 : throw std::invalid_argument ("Cannot invert a non-Square matrix");
434 : : //assert(numRows == numCols);
435 : :
436 : 0 : CubitMatrix matrix_inverse( numRows, numCols );
437 : :
438 [ # # ]: 0 : if (numRows <4)
439 : : {
440 : : double det;
441 [ # # ]: 0 : det = determinant();
442 [ # # ]: 0 : if(fabs(det) <= CUBIT_DBL_MIN)
443 [ # # ][ # # ]: 0 : throw std::invalid_argument ("Determinants Absolute value must be greater that CUBIT_DBL_MIN");
444 : : //assert( fabs(det) > CUBIT_DBL_MIN );
445 : 0 : double det_inv = 1./det;
446 : :
447 [ # # ]: 0 : if ( numRows == 1 ) {
448 [ # # ]: 0 : det = determinant();
449 [ # # ]: 0 : if(fabs(det) <= CUBIT_DBL_MIN)
450 [ # # ][ # # ]: 0 : throw std::invalid_argument ("Determinants Absolute value must be greater that CUBIT_DBL_MIN");
451 : : //assert( fabs(det) > CUBIT_DBL_MIN );
452 : :
453 [ # # ]: 0 : matrix_inverse.set(0,0, matrixPtr[0][0]);
454 : : }
455 : :
456 [ # # ]: 0 : if ( numRows == 2 ) {
457 [ # # ]: 0 : matrix_inverse.set(0,0, matrixPtr[1][1]);
458 [ # # ]: 0 : matrix_inverse.set(1,0,-matrixPtr[1][0]);
459 [ # # ]: 0 : matrix_inverse.set(0,1,-matrixPtr[0][1]);
460 [ # # ]: 0 : matrix_inverse.set(1,1, matrixPtr[0][0]);
461 : : }
462 : :
463 [ # # ]: 0 : if ( numRows == 3 ) {
464 [ # # ]: 0 : matrix_inverse.set(0,0, matrixPtr[1][1] * matrixPtr[2][2] - matrixPtr[1][2] * matrixPtr[2][1] );
465 [ # # ]: 0 : matrix_inverse.set(1,0, matrixPtr[2][0] * matrixPtr[1][2] - matrixPtr[1][0] * matrixPtr[2][2] );
466 [ # # ]: 0 : matrix_inverse.set(2,0, matrixPtr[1][0] * matrixPtr[2][1] - matrixPtr[1][1] * matrixPtr[2][0] );
467 [ # # ]: 0 : matrix_inverse.set(0,1, matrixPtr[2][1] * matrixPtr[0][2] - matrixPtr[0][1] * matrixPtr[2][2] );
468 [ # # ]: 0 : matrix_inverse.set(1,1, matrixPtr[0][0] * matrixPtr[2][2] - matrixPtr[0][2] * matrixPtr[2][0] );
469 [ # # ]: 0 : matrix_inverse.set(2,1, matrixPtr[0][1] * matrixPtr[2][0] - matrixPtr[0][0] * matrixPtr[2][1] );
470 [ # # ]: 0 : matrix_inverse.set(0,2, matrixPtr[0][1] * matrixPtr[1][2] - matrixPtr[0][2] * matrixPtr[1][1] );
471 [ # # ]: 0 : matrix_inverse.set(1,2, matrixPtr[1][0] * matrixPtr[0][2] - matrixPtr[0][0] * matrixPtr[1][2] );
472 [ # # ]: 0 : matrix_inverse.set(2,2, matrixPtr[0][0] * matrixPtr[1][1] - matrixPtr[1][0] * matrixPtr[0][1] );
473 : :
474 : : }
475 [ # # ]: 0 : matrix_inverse *= det_inv;
476 : : }
477 : : else
478 : : {
479 : :
480 : : // use numerical recipes Inverse of a Matrix
481 : :
482 : : int i, j;
483 : : double d;
484 [ # # ]: 0 : std::vector<double> indx(numRows);
485 [ # # ][ # # ]: 0 : std::vector<double> col(numRows);
486 [ # # ][ # # ]: 0 : CubitMatrix save_matrix = *this;
487 : :
488 [ # # ][ # # ]: 0 : CubitStatus rv = ludcmp(&indx[0], d);
489 [ # # ]: 0 : if(rv != CUBIT_SUCCESS)
490 [ # # ][ # # ]: 0 : throw std::invalid_argument ("rv must equal CUBIT_SUCCESS");
491 : : //assert(rv == CUBIT_SUCCESS);
492 [ # # ]: 0 : for (j=0; j<numRows; j++)
493 : : {
494 [ # # ]: 0 : for(i=0; i<numRows; i++)
495 : : {
496 [ # # ]: 0 : col[i] = 0.0;
497 : : }
498 [ # # ]: 0 : col[j] = 1.0;
499 [ # # ][ # # ]: 0 : rv = lubksb(&indx[0], &col[0]);
[ # # ]
500 [ # # ]: 0 : if(rv != CUBIT_SUCCESS)
501 [ # # ][ # # ]: 0 : throw std::invalid_argument ("rv must equal CUBIT_SUCCESS");
502 : : //assert(rv == CUBIT_SUCCESS);
503 [ # # ]: 0 : for (i=0; i<numRows; i++)
504 : : {
505 [ # # ][ # # ]: 0 : matrix_inverse.set(i,j,col[i]);
506 : : }
507 : : }
508 [ # # ][ # # ]: 0 : *this = save_matrix;
[ # # ]
509 : : }
510 : :
511 : 0 : return matrix_inverse;
512 : : }
513 : :
514 : 0 : CubitBoolean CubitMatrix::positive_definite() const
515 : : {
516 : :
517 [ # # ]: 0 : if ( matrixPtr[0][0] <= 0. ) { return CUBIT_FALSE; }
518 : :
519 : 0 : double det2x2 = matrixPtr[0][0] * matrixPtr[1][1] - matrixPtr[1][0] * matrixPtr[0][1];
520 [ # # ]: 0 : if ( det2x2 <= 0. ) { return CUBIT_FALSE; }
521 : :
522 [ # # ]: 0 : if ( determinant() <= 0. ) { return CUBIT_FALSE; }
523 : :
524 : 0 : return CUBIT_TRUE;
525 : : }
526 : :
527 : 455 : double CubitMatrix::determinant() const
528 : : {
529 : 455 : double det = 0.0;
530 : :
531 [ - + ]: 455 : if( numRows == 1 )
532 : 0 : det = matrixPtr[0][0];
533 [ - + ]: 455 : else if( numRows == 2 )
534 : 0 : det = matrixPtr[0][0] * matrixPtr[1][1] - matrixPtr[0][1]
535 : 0 : * matrixPtr[1][0];
536 [ + - ]: 455 : else if (numRows == 3)
537 : 910 : det = matrixPtr[0][0] * matrixPtr[1][1] * matrixPtr[2][2] +
538 : 910 : matrixPtr[0][1] * matrixPtr[1][2] * matrixPtr[2][0] +
539 : 910 : matrixPtr[0][2] * matrixPtr[1][0] * matrixPtr[2][1] -
540 : 910 : matrixPtr[2][0] * matrixPtr[1][1] * matrixPtr[0][2] -
541 : 455 : matrixPtr[2][1] * matrixPtr[1][2] * matrixPtr[0][0] -
542 : 455 : matrixPtr[2][2] * matrixPtr[1][0] * matrixPtr[0][1];
543 : : else
544 : : {
545 [ # # ]: 0 : for( int jj = 0; jj < numRows; jj++ )
546 : : {
547 : 0 : det += ( matrixPtr[0][jj] * cofactor( 0, jj ) );
548 : : }
549 : : }
550 : 455 : return det;
551 : : }
552 : :
553 : 0 : double CubitMatrix::cofactor( const int row, const int col ) const
554 : : {
555 : 0 : double c = 0.0;
556 [ # # ]: 0 : CubitMatrix matrix_sub( numRows - 1, numCols -1 );
557 : :
558 [ # # ][ # # ]: 0 : matrix_sub = sub_matrix( row, col );
[ # # ][ # # ]
559 : :
560 [ # # ]: 0 : c = matrix_sub.determinant();
561 [ # # ]: 0 : c = (row+col)%2 ? -1*c : c;
562 : :
563 [ # # ]: 0 : return c;
564 : : }
565 : :
566 : 0 : CubitMatrix CubitMatrix::adjoint() const
567 : : {
568 [ # # ]: 0 : CubitMatrix matrix( numRows, numRows );
569 : :
570 [ # # ]: 0 : for( int ii = 0; ii < numRows; ii++ )
571 : : {
572 [ # # ]: 0 : for( int jj = 0; jj < numRows; jj++ )
573 : : {
574 [ # # ][ # # ]: 0 : matrix.set( ii, jj, cofactor( ii, jj ) );
575 : : }
576 : : }
577 [ # # ][ # # ]: 0 : return matrix.transpose();
578 : : }
579 : :
580 : 0 : CubitMatrix CubitMatrix::transpose() const
581 : : {
582 : 0 : CubitMatrix return_matrix( numCols, numRows );
583 : :
584 [ # # ]: 0 : for( int ii = 0; ii < numRows; ii++ )
585 : : {
586 [ # # ]: 0 : for( int jj = 0; jj < numCols; jj++ )
587 : : {
588 [ # # ]: 0 : return_matrix.set( jj, ii, matrixPtr[ii][jj] );
589 : : }
590 : : }
591 : :
592 : 0 : return return_matrix;
593 : : }
594 : :
595 : : // Creates and returns a matrix that is a copy of 'this',
596 : : // except that the indicated row and column are left out.
597 : 609 : CubitMatrix CubitMatrix::sub_matrix( const int row, const int col ) const
598 : : {
599 : 609 : CubitMatrix matrix (numRows - 1, numCols - 1);
600 : :
601 : 609 : int copy_row = 0;
602 [ + + ]: 3045 : for (int source_row = 0; source_row < numRows; source_row++)
603 : : {
604 [ + + ]: 2436 : if (source_row != row)
605 : : {
606 : 1827 : int copy_col = 0;
607 [ + + ]: 9135 : for (int source_col = 0; source_col < numCols; source_col++)
608 : : {
609 [ + + ]: 7308 : if (source_col != col)
610 : : {
611 [ + - ]: 5481 : matrix.set (copy_row, copy_col, matrixPtr[source_row][source_col]);
612 : 5481 : copy_col++;
613 : : }
614 : : }
615 : 1827 : copy_row++;
616 : : }
617 : : }
618 : :
619 : 609 : return matrix;
620 : : }
621 : :
622 : : // Create a matrix containing the rows and cols of this that are true in
623 : : // rows_to_include and cols_to_include.
624 : 0 : void CubitMatrix::sub_matrix
625 : : (
626 : : const std::vector<bool> &rows_to_include,
627 : : const std::vector<bool> &cols_to_include,
628 : : CubitMatrix &submatrix
629 : : )
630 : : {
631 [ # # ]: 0 : if(numRows != (int) rows_to_include.size())
632 [ # # ][ # # ]: 0 : throw std::invalid_argument ("rows_to_include size must match numRows");
633 : : //assert( numRows == rows_to_include.size() );
634 [ # # ]: 0 : if(numCols != (int) cols_to_include.size())
635 [ # # ][ # # ]: 0 : throw std::invalid_argument ("cols_to_include size must match numCols");
636 : : //assert( numCols == cols_to_include.size() );
637 : :
638 : : int i;
639 : 0 : int nrow = 0, ncol = 0;
640 [ # # ][ # # ]: 0 : for ( i = 0; i < numRows; i++ ) if ( rows_to_include[i] ) nrow++;
641 [ # # ][ # # ]: 0 : for ( i = 0; i < numCols; i++ ) if ( cols_to_include[i] ) ncol++;
642 : 0 : submatrix.reset_size( nrow, ncol, 0.0 );
643 : :
644 [ # # ]: 0 : for ( int r = 0, new_r = 0; r < numRows; r++ )
645 : : {
646 [ # # ]: 0 : if ( !rows_to_include[r] ) continue;
647 [ # # ]: 0 : for ( int c = 0, new_c = 0; c < numCols; c++ )
648 : : {
649 [ # # ]: 0 : if ( !cols_to_include[c] ) continue;
650 : 0 : submatrix.set( new_r, new_c, get(r,c) );
651 : 0 : new_c++;
652 : : }
653 : 0 : new_r++;
654 : : }
655 : 0 : }
656 : :
657 : 0 : double CubitMatrix::inf_norm() const
658 : : {
659 : : // infinity norm = max_i sum_j | A_ij |
660 : 0 : double matrix_norm = 0., row_norm, v;
661 [ # # ]: 0 : for ( int ii = 0; ii < numRows; ii++ ) {
662 : 0 : row_norm = 0.;
663 [ # # ]: 0 : for( int jj = 0; jj < numCols; jj++ )
664 : : {
665 : 0 : v = fabs( get( ii, jj ) );
666 : 0 : row_norm += v;
667 : : }
668 [ # # ]: 0 : if ( row_norm > matrix_norm )
669 : 0 : matrix_norm = row_norm;
670 : : }
671 : 0 : return matrix_norm;
672 : : }
673 : :
674 : 0 : double CubitMatrix::frobenius_norm_squared() const
675 : : {
676 : : // frobenius norm-squared = trace( M^T M )
677 : :
678 : 0 : double matrix_norm=0;
679 [ # # ]: 0 : for ( int ii = 0; ii < numRows; ii++ ) {
680 : :
681 [ # # ]: 0 : for( int jj = 0; jj < numCols; jj++ )
682 : : {
683 : 0 : matrix_norm += matrixPtr[ii][jj] * matrixPtr[ii][jj];
684 : : }
685 : :
686 : : }
687 : :
688 : 0 : return matrix_norm;
689 : :
690 : : }
691 : :
692 : 0 : double CubitMatrix::frobenius_norm_squared_symm() const
693 : : {
694 : : // frobenius norm-squared 2 = trace[( M^T M )( M^T M )]
695 : :
696 : 0 : double matrix_norm=0;
697 [ # # ]: 0 : for ( int ii = 0; ii < numRows; ii++ )
698 : : {
699 [ # # ]: 0 : for( int jj = 0; jj < numCols; jj++ )
700 : : {
701 : 0 : double b=0;
702 [ # # ]: 0 : for ( int kk = 0; kk < numRows; kk++ )
703 : : {
704 : 0 : b += matrixPtr[kk][ii] * matrixPtr[kk][jj];
705 : : }
706 : 0 : matrix_norm += b*b;
707 : : }
708 : :
709 : : }
710 : :
711 : 0 : return matrix_norm;
712 : :
713 : : }
714 : :
715 : 0 : double CubitMatrix::frobenius_norm_squared_adj() const
716 : : {
717 : : // square of frobenius norm of adjoint
718 : :
719 : 0 : double norm=0;
720 : :
721 [ # # ]: 0 : if ( numRows == 1 ) { norm=1; }
722 : :
723 [ # # ]: 0 : if ( numRows == 2 ) {
724 : 0 : norm = this->frobenius_norm_squared();
725 : : }
726 : :
727 [ # # ]: 0 : if ( numRows == 3 ) {
728 : 0 : norm = 0.5 * ( pow( this->frobenius_norm_squared(), 2 ) - this->frobenius_norm_squared_symm() );
729 : : }
730 : :
731 [ # # ]: 0 : if ( numRows > 3 ) {
732 [ # # ]: 0 : CubitMatrix adj = this->adjoint();
733 [ # # ][ # # ]: 0 : norm = adj.frobenius_norm_squared();
734 : : }
735 : :
736 : 0 : return norm;
737 : :
738 : : }
739 : :
740 : 0 : double CubitMatrix::frobenius_norm_squared_inv() const
741 : : {
742 : : // square of frobenius norm of A-inverse
743 : :
744 : 0 : double det = this->determinant();
745 : :
746 [ # # ]: 0 : if(det==0)
747 [ # # ][ # # ]: 0 : throw std::invalid_argument ("Determinant cannot be 0");
748 : : //assert( det != 0 );
749 : :
750 : 0 : double norm=this->frobenius_norm_squared_adj()/pow(det,2);
751 : :
752 : 0 : return norm;
753 : :
754 : : }
755 : :
756 : 0 : double CubitMatrix::condition() const
757 : : {
758 : : // condition number of A using frobenius norm
759 : :
760 : 0 : double norm = ( this->frobenius_norm_squared() ) * (this->frobenius_norm_squared_inv() );
761 : :
762 : 0 : return sqrt( norm );
763 : :
764 : : }
765 : :
766 : 0 : int CubitMatrix::rank() const
767 : : {
768 : 0 : const double tol = 1E-12;
769 : 0 : int rank = 0;
770 [ # # ]: 0 : CubitMatrix tmp = *this;
771 : :
772 : : int irow;
773 [ # # ]: 0 : for ( irow = 0; irow < numRows; irow++ )
774 : : {
775 : : // make sure tmp[irow][irow] is non-zero. If it isn't, swap a row to
776 : : // make it so.
777 [ # # ]: 0 : double val = tmp.get(irow,irow);
778 [ # # ]: 0 : if ( fabs(val) < tol )
779 : : {
780 : 0 : bool found = false;
781 [ # # ]: 0 : for ( int i = irow+1; i < numRows; i++ )
782 : : {
783 [ # # ][ # # ]: 0 : if ( fabs(tmp.get(i,irow)) > 1E-4 )
784 : : {
785 : : // swap row (irow) with row (irow+i).
786 [ # # ]: 0 : for ( int icol = 0; icol < numCols; icol++ )
787 : : {
788 [ # # ]: 0 : double tmp1 = tmp.get(irow, icol);
789 [ # # ]: 0 : double tmp2 = tmp.get(i, icol);
790 [ # # ]: 0 : tmp.set(irow, icol, tmp2 );
791 [ # # ]: 0 : tmp.set(i, icol, tmp1 );
792 : 0 : found = true;
793 : : }
794 : : }
795 [ # # ]: 0 : if ( found ) break;
796 : : }
797 [ # # ]: 0 : val = tmp.get(irow,irow);
798 : : }
799 [ # # ]: 0 : if ( fabs(val) < tol )
800 : 0 : continue;
801 : :
802 : 0 : rank++;
803 : :
804 [ # # ]: 0 : for ( int icol = 0; icol < numCols; icol++ )
805 : : {
806 [ # # ]: 0 : double col_val = tmp.get(irow, icol);
807 [ # # ]: 0 : tmp.set(irow,icol, col_val/val );
808 : : }
809 : :
810 [ # # ]: 0 : for ( int jrow = irow+1; jrow < numRows; jrow++ )
811 : : {
812 [ # # ]: 0 : val = tmp.get(jrow,irow);
813 [ # # ]: 0 : if ( fabs(val) < tol )
814 : 0 : continue;
815 : :
816 [ # # ]: 0 : for ( int icol = 0; icol < numCols; icol++ )
817 : : {
818 [ # # ]: 0 : double tmp1 = tmp.get(jrow,icol) / val;
819 [ # # ]: 0 : tmp1 -= tmp.get(irow, icol);
820 [ # # ]: 0 : tmp.set(jrow,icol,tmp1 );
821 : : }
822 : : }
823 : : }
824 [ # # ]: 0 : return rank;
825 : : }
826 : :
827 : 0 : int CubitMatrix::gauss_elim( CubitVector &b )
828 : : {
829 [ # # ]: 0 : CubitVector pivot;
830 [ # # ]: 0 : int ierr = factor( pivot );
831 [ # # ][ # # ]: 0 : if ( ierr == 0 ) { solve( b, pivot ); }
832 : 0 : return ierr;
833 : : }
834 : :
835 : 0 : int CubitMatrix::factor( CubitVector &pivot )
836 : : {
837 : : double pvt[3];
838 : :
839 : 0 : const int n=3;
840 : : double s[3], tmp;
841 : :
842 : : int i,j;
843 [ # # ]: 0 : for ( i=0; i<n; i++ )
844 : : {
845 : 0 : s[i] = 0.0;
846 [ # # ]: 0 : for ( j=0; j<n; j++ )
847 : : {
848 : 0 : tmp = fabs( matrixPtr[i][j] );
849 [ # # ]: 0 : if ( tmp > s[i] )
850 : : {
851 : 0 : s[i] = tmp;
852 : : }
853 : : }
854 : :
855 [ # # ]: 0 : if ( s[i] == 0.0 )
856 : : {
857 : 0 : return(1);
858 : : }
859 : : }
860 : :
861 [ # # ]: 0 : for ( int k=0; k<n-1; k++ )
862 : : {
863 : 0 : double ck = 0.0;
864 : 0 : int i0 = -1;
865 [ # # ]: 0 : for ( i=k; i<n; i++ )
866 : : {
867 : 0 : tmp = fabs( matrixPtr[i][k] / s[i] );
868 [ # # ]: 0 : if ( tmp > ck )
869 : : {
870 : 0 : ck = tmp;
871 : 0 : i0 = i;
872 : : }
873 : : }
874 : :
875 : 0 : pvt[k] = i0;
876 [ # # ]: 0 : if ( ck == 0.0 ) { return(1); }
877 : :
878 [ # # ]: 0 : if ( i0 != k )
879 : : {
880 [ # # ]: 0 : for ( j=k; j<n; j++ )
881 : : {
882 : 0 : double swap = matrixPtr[i0][j];
883 : 0 : matrixPtr[i0][j] = matrixPtr[k][j];
884 : 0 : matrixPtr[k][j] = swap;
885 : : }
886 : : }
887 : :
888 [ # # ]: 0 : for ( i=k+1; i<n; i++ )
889 : : {
890 : 0 : double r = matrixPtr[i][k] / matrixPtr[k][k];
891 : 0 : matrixPtr[i][k] = r;
892 [ # # ]: 0 : for ( j=k+1; j<n; j++ )
893 : : {
894 : 0 : matrixPtr[i][j] -= r * matrixPtr[k][j];
895 : : }
896 : : }
897 : : }
898 : :
899 [ # # ]: 0 : pivot.set( pvt[0], pvt[1], pvt[2] );
900 : 0 : return(0);
901 : : }
902 : :
903 : 0 : void CubitMatrix::solve( CubitVector &b, const CubitVector& pivot )
904 : : {
905 : : double rhs[3];
906 [ # # ]: 0 : rhs[0] = b.x();
907 [ # # ]: 0 : rhs[1] = b.y();
908 [ # # ]: 0 : rhs[2] = b.z();
909 : :
910 : : double pvt[3];
911 [ # # ]: 0 : pvt[0] = pivot.x();
912 [ # # ]: 0 : pvt[1] = pivot.y();
913 [ # # ]: 0 : pvt[2] = pivot.z();
914 : :
915 : : int j;
916 : 0 : const int n=3;
917 [ # # ]: 0 : for ( int k=0; k<n-1; k++ )
918 : : {
919 : 0 : j=(int)pvt[k];
920 [ # # ]: 0 : if ( j != k )
921 : : {
922 : 0 : double swap = rhs[k];
923 : 0 : rhs[k] = rhs[j];
924 : 0 : rhs[j] = swap;
925 : : }
926 : :
927 [ # # ]: 0 : for ( int i=k+1; i<n; i++ )
928 : : {
929 : 0 : rhs[i] -= matrixPtr[i][k] * rhs[k];
930 : : }
931 : :
932 : : }
933 : :
934 : 0 : rhs[n-1] /= matrixPtr[n-1][n-1];
935 : :
936 [ # # ]: 0 : for ( int i=n-2; i>-1; i-- )
937 : : {
938 : 0 : double sum=0.;
939 [ # # ]: 0 : for ( j=i+1; j<n; j++ )
940 : : {
941 : 0 : sum += matrixPtr[i][j] * rhs[j];
942 : : }
943 : 0 : rhs[i] = ( rhs[i] - sum ) / matrixPtr[i][i];
944 : : }
945 : :
946 [ # # ]: 0 : b.set( rhs[0], rhs[1], rhs[2] );
947 : :
948 : 0 : }
949 : :
950 : :
951 : : // Here is the recipe for inverting a NxM matrice.
952 : : // I did not spend the time trying to convert it to Cubit style.
953 : : // Matrix is a double**
954 : : // Vector is a double*
955 : : // Scalar is a double
956 : : // int mxiRecipieInverse(Matrix M1, Matrix M2, int N)
957 : : // {
958 : : // Matrix M1_loc, M2_loc, M3_loc;
959 : : // Vector col, copycol;
960 : : // Scalar d;
961 : : // int i, j, *indx;
962 : :
963 : : // indx = ((int*)malloc((unsigned long)N*sizeof(int)))-1;
964 : :
965 : : // M1_loc = mxInitMatrixR(1, N, 1, N);
966 : : // M2_loc = mxInitMatrixR(1, N, 1, N);
967 : : // M3_loc = mxInitMatrixR(1, N, 1, N);
968 : : // col = mxInitVectorR(1, N);
969 : : // copycol = mxInitVectorR(1, N);
970 : : // if (M1_loc == NULL || M2_loc == NULL || col == NULL || indx == NULL)
971 : : // return 0;
972 : : // if (M3_loc == NULL || copycol == NULL)
973 : : // printf("\n\nCannot use Improve function\n\n");
974 : :
975 : : // /* copy the input matrix */
976 : : // for( i = 1; i <= N; i++ )
977 : : // for( j = 1; j <= N; j++ ) {
978 : : // M1_loc[i][j] = M1[i-1][j-1];
979 : : // if (M3_loc != NULL)
980 : : // M3_loc[i][j] = M1[i-1][j-1];
981 : : // M2_loc[i][j] = 0.0;
982 : : // }
983 : :
984 : : // if (!mxiLudcmp(M1_loc, N, indx, &d)) return 0;
985 : : // for (j=1; j<=N; j++) {
986 : : // for (i=1; i<=N; i++) {
987 : : // col[i]=0.0;
988 : : // if (copycol != NULL)
989 : : // copycol[i] = 0.0;
990 : : // }
991 : : // col[j] = 1.0;
992 : : // if (copycol != NULL) copycol[j] = 1.0;
993 : : // mxiLubksb(M1_loc, N, indx, col);
994 : : // if (copycol != NULL && M3_loc != NULL)
995 : : // if (!mxiImprove(M3_loc, M1_loc, N, indx, copycol, col))
996 : : // return 0;
997 : : // for (i=1; i<=N; i++) M2_loc[i][j]=col[i];
998 : : // }
999 : : // /* copy the inverted matrix */
1000 : : // for( i = 1; i <= N; i++ )
1001 : : // for( j = 1; j <= N; j++ )
1002 : : // M2[i-1][j-1] = M2_loc[i][j];
1003 : :
1004 : : // mxFreeMatrixR(M1_loc, 1, N, 1, N);
1005 : : // mxFreeMatrixR(M2_loc, 1, N, 1, N);
1006 : : // mxFreeMatrixR(M3_loc, 1, N, 1, N);
1007 : : // mxFreeVectorR(col, 1, N);
1008 : : // mxFreeVectorR(copycol, 1, N);
1009 : : // free(indx+1);
1010 : : // return 1;
1011 : : // } /* mxiRecipieInverse */
1012 : :
1013 : 0 : CubitStatus CubitMatrix::solveNxN( CubitMatrix& rhs, CubitMatrix& coef )
1014 : : {
1015 [ # # ][ # # ]: 0 : if (numRows != rhs.num_rows() ||
[ # # ][ # # ]
1016 : 0 : numRows != numCols) {
1017 : 0 : return CUBIT_FAILURE;
1018 : : }
1019 : : int i,j;
1020 : : double d;
1021 [ # # ][ # # ]: 0 : double *indx = new double [numRows];
1022 [ # # ][ # # ]: 0 : double *b = new double [numRows];
1023 : :
1024 [ # # ]: 0 : CubitStatus status = ludcmp(indx, d);
1025 [ # # ]: 0 : if (status == CUBIT_SUCCESS)
1026 : : {
1027 [ # # ][ # # ]: 0 : coef.reset_size( rhs.num_rows(), rhs.num_cols(), 0.0 );
[ # # ]
1028 [ # # ][ # # ]: 0 : for ( j = 0; j < rhs.num_cols(); j++ )
1029 : : {
1030 [ # # ]: 0 : for(i=0; i<numRows; i++)
1031 : : {
1032 [ # # ]: 0 : b[i] = rhs.get(i,j);
1033 : : }
1034 [ # # ]: 0 : status = lubksb(indx, b);
1035 [ # # ]: 0 : for (i=0; i<numRows; i++)
1036 : : {
1037 [ # # ]: 0 : coef.set(i,j,b[i]);
1038 : : }
1039 : : }
1040 : : }
1041 [ # # ]: 0 : delete [] indx;
1042 [ # # ]: 0 : delete [] b;
1043 : 0 : return status;
1044 : : }
1045 : :
1046 : 0 : CubitStatus CubitMatrix::solveNxN( const std::vector<double> &rhs, std::vector<double> &coef )
1047 : : {
1048 [ # # ][ # # ]: 0 : if (numRows != (int) rhs.size() ||
[ # # ][ # # ]
1049 : 0 : numRows != numCols) {
1050 : 0 : return CUBIT_FAILURE;
1051 : : }
1052 : : int i;
1053 : : double d;
1054 [ # # ][ # # ]: 0 : double *indx = new double [numRows];
1055 [ # # ][ # # ]: 0 : double *b = new double [numRows];
1056 : :
1057 [ # # ]: 0 : CubitStatus status = ludcmp(indx, d);
1058 [ # # ]: 0 : if (status == CUBIT_SUCCESS)
1059 : : {
1060 [ # # ]: 0 : coef.clear();
1061 [ # # ]: 0 : for(i=0; i<numRows; i++)
1062 : : {
1063 [ # # ]: 0 : b[i] = rhs[i];
1064 : : }
1065 [ # # ]: 0 : status = lubksb(indx, b);
1066 [ # # ]: 0 : for (i=0; i<numRows; i++)
1067 : : {
1068 [ # # ]: 0 : coef.push_back( b[i] );
1069 : : }
1070 : : }
1071 [ # # ]: 0 : delete [] indx;
1072 [ # # ]: 0 : delete [] b;
1073 : 0 : return status;
1074 : : }
1075 : :
1076 : : // from numerical recipies in C: Decompose a NxN matrix into
1077 : : // Upper and Lower trianglar (in place)
1078 : 0 : CubitStatus CubitMatrix::ludcmp( double *indx, double& d )
1079 : : {
1080 : 0 : int i, j, k, imax = -1;
1081 : : double big, tmp, sum;
1082 [ # # ]: 0 : double *vv = new double [numRows];
1083 [ # # ]: 0 : if (!vv) {
1084 : 0 : return CUBIT_FAILURE;
1085 : : }
1086 : :
1087 : 0 : d = 1.0; // no row interchanges yet
1088 : :
1089 : : // loop over rows to get implicit scale info
1090 : :
1091 [ # # ]: 0 : for (i=0; i<numRows; ++i){
1092 : 0 : big = 0.0;
1093 [ # # ]: 0 : for (j=0; j<numRows; ++j)
1094 [ # # ]: 0 : if ((tmp = fabs(matrixPtr[i][j])) > big)
1095 : 0 : big = tmp;
1096 [ # # ]: 0 : if (big == 0.0) {
1097 : : // note (vvyas, 3/2006): corrected array deletion
1098 : : // delete vv;
1099 [ # # ]: 0 : delete [] vv;
1100 : 0 : return CUBIT_FAILURE;
1101 : : }
1102 : 0 : vv[i] = 1.0/big;
1103 : : }
1104 : :
1105 : : // loop over columns-Crout's method
1106 : :
1107 [ # # ]: 0 : for (j=0; j<numRows; ++j){
1108 [ # # ]: 0 : for (i=0; i<j; ++i){
1109 : 0 : sum = matrixPtr[i][j];
1110 [ # # ]: 0 : for (k=0; k<i; ++k)
1111 : 0 : sum -= matrixPtr[i][k]*matrixPtr[k][j];
1112 : 0 : matrixPtr[i][j] = sum;
1113 : : }
1114 : 0 : big = 0.0; // initialize pivot search
1115 [ # # ]: 0 : for (i=j; i<numRows; ++i){
1116 : 0 : sum = matrixPtr[i][j];
1117 [ # # ]: 0 : for (k=0; k<j; ++k)
1118 : 0 : sum -= matrixPtr[i][k]*matrixPtr[k][j];
1119 : 0 : matrixPtr[i][j] = sum;
1120 [ # # ]: 0 : if ((tmp = vv[i]*fabs(sum)) > big) {
1121 : 0 : big = tmp;
1122 : 0 : imax = i;
1123 : : }
1124 : : }
1125 [ # # ]: 0 : if (j != imax) { // do we need to change rows
1126 [ # # ]: 0 : for (k=0; k<numRows; ++k) {
1127 : 0 : tmp = matrixPtr[imax][k];
1128 : 0 : matrixPtr[imax][k] = matrixPtr[j][k];
1129 : 0 : matrixPtr[j][k] = tmp;
1130 : : }
1131 : 0 : d = -d;
1132 : 0 : vv[imax] = vv[j];
1133 : : }
1134 : 0 : indx[j] = imax;
1135 [ # # ]: 0 : if (matrixPtr[j][j] == 0.0) matrixPtr[0][0] = 1.0e-20;
1136 [ # # ]: 0 : if (j != numRows-1) { // divide by the pivot element
1137 : 0 : tmp = 1.0/matrixPtr[j][j];
1138 [ # # ]: 0 : for (i=j+1; i<numRows; ++i)
1139 : 0 : matrixPtr[i][j] *= tmp;
1140 : : }
1141 : : } // go back for next column
1142 : :
1143 : : // note (vvyas 3/2006): corrected array deletion
1144 : : // delete vv;
1145 [ # # ]: 0 : delete [] vv;
1146 : :
1147 : 0 : return CUBIT_SUCCESS;
1148 : : }
1149 : :
1150 : : // from numerical recipies in C: solve [mat]{x} = {b} by back
1151 : : // substitution (mat = LU of mat)
1152 : 0 : CubitStatus CubitMatrix::lubksb( double *indx, double *b )
1153 : : {
1154 : : int i, j, ii, ip;
1155 : : double sum;
1156 : :
1157 : : // do the forward substitution
1158 : :
1159 : 0 : ii = -1;
1160 [ # # ]: 0 : for (i=0; i<numRows; ++i){
1161 : 0 : ip = (int)indx[i];
1162 : 0 : sum = b[ip];
1163 : 0 : b[ip] = b[i];
1164 [ # # ]: 0 : if (ii >= 0)
1165 [ # # ]: 0 : for (j=ii; j<=i-1; ++j)
1166 : 0 : sum -= matrixPtr[i][j]*b[j];
1167 [ # # ]: 0 : else if (sum)
1168 : 0 : ii = i;
1169 : 0 : b[i] = sum;
1170 : : }
1171 : :
1172 : : // do the back substitution
1173 : :
1174 [ # # ]: 0 : for (i=numRows-1; i>=0; --i){
1175 : 0 : sum = b[i];
1176 [ # # ]: 0 : for (j=i+1; j<numRows; ++j)
1177 : 0 : sum -= matrixPtr[i][j]*b[j];
1178 : 0 : b[i] = sum/matrixPtr[i][i]; // store a component of solution
1179 : : }
1180 : 0 : return CUBIT_SUCCESS;
1181 : : }
1182 : :
1183 : 0 : bool CubitMatrix::is_identity( double tol ) const
1184 : : {
1185 : 0 : bool ident = true;
1186 : :
1187 [ # # ][ # # ]: 0 : for (int i=0; i<numRows && ident; i++)
1188 : : {
1189 [ # # ][ # # ]: 0 : for (int j=0; j<numCols && ident; j++)
1190 : : {
1191 [ # # ]: 0 : if (i == j)
1192 : : {
1193 [ # # ]: 0 : if( fabs(matrixPtr[i][j] - 1.0) > tol)
1194 : 0 : ident = false;
1195 : : }
1196 : : else
1197 : : {
1198 [ # # ]: 0 : if(matrixPtr[i][j] > tol)
1199 : 0 : ident = false;
1200 : : }
1201 : : }
1202 : : }
1203 : :
1204 : 0 : return ident;
1205 : : }
1206 : :
1207 : 0 : bool CubitMatrix::is_equal( const CubitMatrix &other, double tol ) const
1208 : : {
1209 [ # # ]: 0 : if ( numRows != other.numRows ) return false;
1210 [ # # ]: 0 : if ( numCols != other.numCols ) return false;
1211 : :
1212 [ # # ]: 0 : for (int i=0; i<numRows; i++)
1213 : : {
1214 [ # # ]: 0 : for (int j=0; j<numCols; j++)
1215 : : {
1216 : 0 : double diff = fabs(matrixPtr[i][j] - other.matrixPtr[i][j]);
1217 [ # # ]: 0 : if(diff > tol)
1218 : 0 : return false;
1219 : : }
1220 : : }
1221 : 0 : return true;
1222 : : }
1223 : :
1224 : 0 : void CubitMatrix::plus_identity()
1225 : : {
1226 [ # # ]: 0 : for (int i=0; i<numRows; i++)
1227 : : {
1228 [ # # ]: 0 : if ( i == numCols ) break;
1229 : 0 : matrixPtr[i][i] += 1.0;
1230 : : }
1231 : 0 : }
1232 : :
1233 : 0 : bool CubitMatrix::vector_outer_product(const CubitVector& vec1,
1234 : : const CubitVector& vec2 )
1235 : : {
1236 [ # # ][ # # ]: 0 : if ( numRows != 3 || numCols != 3 )
1237 : 0 : return false;
1238 : :
1239 : : // Initialize the matrix elements using otimes (outer product)
1240 : 0 : matrixPtr[0][0] = vec1.x() * vec2.x();
1241 : 0 : matrixPtr[1][0] = vec1.y() * vec2.x();
1242 : 0 : matrixPtr[2][0] = vec1.z() * vec2.x();
1243 : 0 : matrixPtr[0][1] = vec1.x() * vec2.y();
1244 : 0 : matrixPtr[1][1] = vec1.y() * vec2.y();
1245 : 0 : matrixPtr[2][1] = vec1.z() * vec2.y();
1246 : 0 : matrixPtr[0][2] = vec1.x() * vec2.z();
1247 : 0 : matrixPtr[1][2] = vec1.y() * vec2.z();
1248 : 0 : matrixPtr[2][2] = vec1.z() * vec2.z();
1249 : 0 : return true;
1250 : : }
|
