1 /*
2 Copyright (C) 1998 Dennis Roddeman
3 email: dennis.roddeman@feat.nl
4
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation; either version 2 of the License, or
8 (at your option) any later version.
9
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License
16 along with this program; if not, write to the Free Software Foundation
17 59 Temple Place, Suite 330, Boston, MA, 02111-1307, USA
18 */
19
20 #include "tochnog.h"
21
22 #define EPS_Q 1.e-10
23 #define EPS_DET 1.e-10
24
array_add(double a[],double b[],double c[],long int n)25 void array_add( double a[], double b[], double c[], long int n )
26
27 {
28 register long int i=0;
29
30 for ( i=0; i<n; i++ ) c[i] = a[i] + b[i];
31
32 }
33
array_distance(double a[],double b[],double work[],long int n)34 double array_distance( double a[], double b[], double work[], long int n )
35
36 {
37 array_subtract( a, b, work, n );
38 return array_size( work, n );
39 }
40
array_inproduct(double a[],double b[],long int n)41 double array_inproduct( double a[], double b[], long int n )
42
43 {
44 register long int i=0;
45 double result=0.;
46
47 for ( i=0; i<n; i++ ) result += a[i]*b[i];
48 return result;
49
50 }
51
array_member(long int list[],long int i,long int n,long int & indx)52 long int array_member( long int list[], long int i, long int n, long int &indx )
53
54 {
55 register long int j=0, found=0;
56
57 indx = -1;
58
59 while ( j<n && !found ) {
60 if ( list[j]==i ) {
61 indx = j;
62 found = 1;
63 }
64 j++;
65 }
66 return found;
67
68 }
69
array_move(long int from[],long int to[],long int n)70 void array_move( long int from[], long int to[], long int n )
71
72 {
73 long int i=0;
74
75 for ( i=0; i<n; i++ ) to[i] = from[i];
76
77 }
78
79
array_move(double from[],double to[],long int n)80 void array_move( double from[], double to[], long int n )
81
82 {
83 register long int i=0;
84
85 for ( i=0; i<n; i++ ) to[i] = from[i];
86
87 }
88
array_set(double * ptr,double value,long int n)89 void array_set( double *ptr, double value, long int n )
90
91 {
92 register long int i=0;
93
94 for ( i=0; i<n; i++ ) *(ptr+i) = value;
95 }
96
array_set(long int * ptr,long int value,long int n)97 void array_set( long int *ptr, long int value, long int n )
98
99 {
100 register long int i=0;
101
102 for ( i=0; i<n; i++ ) *(ptr+i) = value;
103
104 }
105
106
array_multiply(double a[],double b[],double c,long int n)107 void array_multiply( double a[], double b[], double c, long int n )
108
109 {
110 register long int i=0;
111
112 for ( i=0; i<n; i++ ) b[i] = c * a[i];
113
114 }
115
array_normalize(double a[],long int n)116 long int array_normalize( double a[], long int n )
117
118 {
119 long int i=0;
120 double l=0;
121
122 l = array_size( a, n );
123 if ( l<1.e-10 ) return 0;
124 for ( i=0; i<n; i++ ) a[i] = a[i] / l;
125
126 return 1;
127 }
128
array_null(double dval[],long int n)129 long int array_null( double dval[], long int n )
130
131 {
132 long int i=0;
133
134 for ( i=0; i<n; i++ ) {
135 if ( dval[i]!=0. ) return 0;
136 }
137 return 1;
138
139 }
140
array_outproduct_2D(double a[],double b[])141 void array_outproduct_2D( double a[], double b[] )
142
143 {
144 double vec_tmp[MDIM], three[MDIM], normal[MDIM];
145
146 array_move( a, vec_tmp, 2 ); vec_tmp[2] = 0.;
147 array_set( three, 0., MDIM ); three[2] = 1.;
148 array_outproduct_3D( three, vec_tmp, normal );
149 array_move( normal, b, 2 );
150 }
151
152
array_outproduct_3D(double a[],double b[],double c[])153 void array_outproduct_3D( double a[], double b[], double c[] )
154
155 {
156 c[0] = a[1]*b[2] - a[2]*b[1];
157 c[1] = a[2]*b[0] - a[0]*b[2];
158 c[2] = a[0]*b[1] - a[1]*b[0];
159 }
160
array_subtract(double a[],double b[],double c[],long int n)161 void array_subtract( double a[], double b[], double c[], long int n )
162
163 {
164 register long int i=0;
165
166 for ( i=0; i<n; i++ ) c[i] = a[i] - b[i];
167
168 }
169
array_size(double a[],long int n)170 double array_size( double a[], long int n )
171
172 {
173 double size=0.;
174
175 size = sqrt( scalar_dabs( array_inproduct(a,a,n) ) );
176
177 return size;
178 }
179
equations_factor(long int n,double ** a,long int * p,long int * f)180 long int equations_factor( long int n, double **a, long int *p, long int *f )
181
182 /* factorize matrix
183
184 From Computers and Structures Vol. 52, No. 4, pp. 743-748, 1994
185 O. Hededal and S. Krenk, A Profile Solver in C for Finite Element
186 Equations.
187
188 Adapted to catch large ratio of pivots (Ton van den Boogaard)
189
190 Factors profile matrix [A] defined in the lower triangular
191 part into off-diagonal factors [L] and the diagonal [D]:
192 [A] = [L][D][L]T
193 Rows and columns identified by f=1 are left untouched.
194
195 input:
196 n = Dimension of the matrix a[][].
197 a[i][j] = Lower profile matrix to be factored.
198 i=0 to n-1, j=p[i] to i+1.
199 p[i] = Number of leading zeroes in row i.
200 f[i] = 0: unknown dof, known load.
201 = 1: known dof, unknown load.
202
203 output:
204 a[i][j] = i>j: Profile factor [L] below diagonal.
205 = i=j: Diagonal factor [D] on diagonal.
206 status = No. of TINY diagonal elements
207 */
208
209 {
210
211 #define TINY 1E-20 /* to avoid numerical instability */
212 #define max(x,y) (x>y ? x:y)
213
214 long int i,j,k, status=0;
215 double u=0., maxpiv=0., minpiv=0., abspiv=0.;
216
217 for ( i=0; i<n; i++ ) if (!f[i])
218 {
219 for ( j=p[i]; j<i; j++ ) if (!f[j])
220 for ( k=max(p[i],p[j]); k<j; k++ ) if (!f[k])
221 a[i][j-p[i]] -= a[j][k-p[j]]*a[i][k-p[i]];
222
223 for ( j=p[i]; j<i; j++ ) if (!f[j])
224 {
225 u = a[i][j-p[i]];
226 a[i][j-p[i]] /= a[j][j-p[j]];
227 a[i][i-p[i]] -= a[i][j-p[i]]*u;
228 }
229
230 abspiv = fabs(a[i][i-p[i]]);
231 if ( abspiv < TINY )
232 {
233 a[i][i-p[i]] = TINY;
234 status++;
235 }
236
237 if ( i==0 ) maxpiv = minpiv = abspiv;
238 else if ( abspiv > maxpiv ) maxpiv = abspiv;
239 else if ( abspiv < minpiv ) minpiv = abspiv;
240 }
241
242 if ( minpiv / maxpiv < 1.e-12 ) status = max( status, 1 );
243
244 #undef max
245 #undef TINY
246
247 if ( status==0 ) status = 1;
248 else status = 0;
249 return status;
250 }
251
equations_solve(long int n,double ** a,long int * p,double * x,double * b,long int * f)252 void equations_solve( long int n, double **a, long int *p, double *x,
253 double *b, long int *f )
254
255 /* solve:
256
257 From Computers and Structures Vol. 52, No. 4, pp. 743-748, 1994
258 O. Hededal and S. Krenk, A Profile Solver in C for Finite Element
259 Equations.
260
261 Adapted to catch large ratio of pivots (Ton van den Boogaard)
262
263 Solves a symmetric system of equations, when the profile
264 matrix [A] has been factored in the form
265 [L][D][L]T {x} = {b}
266 [L] is a lower profile matrix and [D] is a diagonal matrix.
267
268 input:
269 n = Dimension of equation system.
270 a[i][j] = i>j: Off-diagonal factor [L].
271 = i=j: Diagonal factor [D].
272 i=0 to n-1, j=p[i]+1 to i.
273 p[i] = Number of leading zeroes in a row.
274 x[i] = System degrees of freedom.
275 b[i] = Load vector.
276 f[i] = 0: unknown dof, known load.
277 = 1: known dof, unknown load.
278
279 output:
280 x[i] = Degrees of freedom.
281 b[i] = Loads.
282 */
283 {
284 long int i,j;
285
286 for ( i=0; i<n; i++ )
287 {
288 if (!f[i])
289 x[i] = b[i];
290 else
291 b[i] = 0;
292 }
293
294 for ( i=0; i<n; i++ )
295 for ( j=p[i]; j<i; j++ )
296 {
297 if (f[i] && !f[j])
298 x[j] -= a[i][j-p[i]]*x[i];
299 else if (f[j] && !f[i] )
300 x[i] -= a[i][j-p[i]]*x[j];
301 }
302
303 for ( i=1; i<n; i++ ) if (!f[i])
304 for ( j=p[i]; j<i; j++ ) if (!f[j])
305 x[i] -= a[i][j-p[i]]*x[j];
306
307 for ( i=0; i<n; i++ ) if (!f[i])
308 x[i] /= a[i][i-p[i]];
309
310 for ( i=n-1; i>0; i-- ) if (!f[i])
311 for ( j=p[i]; j<i; j++ ) if (!f[j])
312 x[j] -= a[i][j-p[i]]*x[i];
313
314 for ( i=0; i<n; i++ )
315 for ( j=p[i]; j<=i; j++ )
316 {
317 if (f[i])
318 b[i] += a[i][j-p[i]]*x[j];
319 if (f[j] && (i!=j) )
320 b[j] += a[i][j-p[i]]*x[i];
321 }
322
323 }
324
fit_polynomial(double points[],long int npoint,double coefficients[],long int ncoefficient)325 long int fit_polynomial( double points[], long int npoint,
326 double coefficients[], long int ncoefficient )
327
328 {
329 long int icoeff=0, jcoeff=0, ipoint=0, return_value=0, length=0, n=0,
330 *p=NULL, *f=NULL;
331 double x=0., y=0., **a=NULL, *b=NULL;
332
333 if ( npoint>0 && ncoefficient<=npoint ) {
334 length = ncoefficient;
335 a = new double * [length];
336 for ( icoeff=0; icoeff<ncoefficient; icoeff++ ) {
337 a[icoeff] = get_new_dbl(length);
338 array_set( a[icoeff], 0., length );
339 }
340 b = get_new_dbl(length);
341 array_set( b, 0., length );
342 f = get_new_int(length);
343 array_set( f, 0, length );
344 p = get_new_int(length);
345 array_set( p, 0, length );
346 for ( ipoint=0; ipoint<npoint; ipoint++ ) {
347 x = points[ipoint*2+0];
348 y = points[ipoint*2+1];
349 for ( icoeff=0; icoeff<ncoefficient; icoeff++ ) {
350 b[icoeff] += scalar_power(x,icoeff)*y;
351 for ( jcoeff=0; jcoeff<ncoefficient; jcoeff++ )
352 a[icoeff][jcoeff] += scalar_power(x,icoeff+jcoeff);
353 }
354 }
355 n = ncoefficient;
356 if ( equations_factor( n, a, p, f ) ) {
357 equations_solve( n, a, p, coefficients, b, f );
358 return_value = 1;
359 }
360 for ( icoeff=0; icoeff<ncoefficient; icoeff++ ) delete[] a[icoeff];
361 delete[] a;
362 delete[] b;
363 delete[] f;
364 delete[] p;
365 }
366
367 return return_value;
368 }
369
itoa(int n,char str[])370 void itoa( int n, char str[] )
371
372 {
373 int i=0, sign=0;
374
375 if ( (sign=n)<0 ) n = -n;
376
377 do {
378 str[i++] = n % 10 + '0';
379 } while ( (n/=10)>0 );
380
381 if ( sign<0 ) str[i++] = '-';
382
383 str[i] = '\0';
384
385 string_reverse(str);
386 }
387
matrix_ab(double * a,double * b,double * c,long int n,long int m,long int k)388 void matrix_ab( double *a, double *b, double *c, long int n, long int m,
389 long int k )
390
391 // c[n][k] = a[n][m] * b[m][k]
392
393 {
394 register long int i=0, j=0, l=0;
395
396 for ( i=0; i<n; i++ ) {
397 for ( j=0; j<k; j++ ) {
398 *( c + i*k + j ) = 0;
399 for ( l=0; l<m; l++ ) {
400 *( c + i*k + j ) += ( *( a + i*m + l ) ) *
401 ( *( b + l*k + j ) );
402 }
403 }
404
405 }
406
407 }
408
matrix_abat(double a[],double b[],double c[],double work[],long int n)409 void matrix_abat( double a[], double b[], double c[],
410 double work[], long int n )
411
412 {
413
414 matrix_ab( a, b, work, n, n, n );
415 matrix_abt( work, a, c, n, n, n );
416
417 }
418
419
420
matrix_abt(double * a,double * b,double * c,long int n,long int m,long int k)421 void matrix_abt( double *a, double *b, double *c, long int n, long int m,
422 long int k )
423
424 // c[n][k] = a[n][m] * b[k][m]Transposed
425
426 {
427 register long int i=0, j=0, l=0;
428
429 for ( i=0; i<n; i++ ) {
430 for ( j=0; j<k; j++ ) {
431 *( c + i*k + j) = 0;
432 for ( l=0; l<m; l++ ) {
433 *( c + i*k + j) += ( *( a + i*m + l ) ) *
434 ( *( b + j*k + l ) );
435 }
436 }
437 }
438
439 }
440
matrix_atb(double * a,double * b,double * c,long int n,long int m,long int k)441 void matrix_atb( double *a, double *b, double *c, long int n, long int m,
442 long int k )
443
444 // c[m][k] = a[n][m]Transposed * b[n][k]
445
446 {
447 register long int i=0, j=0, l=0;
448
449 for ( i=0; i<m; i++ ) {
450 for ( j=0; j<k; j++ ) {
451 *( c + i*k + j ) = 0;
452 for ( l=0; l<n; l++ ) {
453 *( c + i*k + j ) += ( *( a + l*m + i ) ) *
454 ( *( b + l*k + j ) );
455 }
456 }
457 }
458
459 }
460
matrix_atba(double a[],double b[],double c[],double work[],long int n,long int m)461 void matrix_atba( double a[], double b[], double c[],
462 double work[], long int n, long int m )
463
464 // c[m][m] = a[n][m]Transposed * b[n][n] * a[n][m]
465 {
466
467 matrix_atb( a, b, work, n, m, n );
468 matrix_ab( work, a, c, m, n, m );
469 }
470
matrix_a4b(double a[3][3][3][3],double b[],double c[])471 void matrix_a4b( double a[3][3][3][3], double b[], double c[] )
472
473 {
474 register long int i=0, j=0, k=0, l=0;
475
476 for ( i=0; i<3; i++ ) {
477 for ( j=0; j<3; j++ ) {
478 c[i*3+j] = 0.;
479 for ( k=0; k<3; k++ ) {
480 for ( l=0; l<3; l++ ) {
481 c[i*3+j] += a[i][j][k][l] * b[k*3+l];
482 }
483 }
484 }
485 }
486
487 }
488
matrix_determinant(double a[],long int n)489 double matrix_determinant( double a[], long int n )
490
491 {
492 double result=0.;
493
494 if ( n==1 )
495 result = a[0];
496 else if ( n==2 )
497 result = a[0]*a[3] - a[1]*a[2];
498 else {
499 assert( n==3 );
500 result = a[0]*(a[4]*a[8]-a[7]*a[5]) -
501 a[1]*(a[3]*a[8]-a[6]*a[5]) + a[2]*(a[3]*a[7]-a[6]*a[4]);
502 }
503 return result;
504 }
505
matrix_eigenvalues(double mat[],double eigenvalues[])506 void matrix_eigenvalues( double mat[], double eigenvalues[] )
507
508 {
509 double I1=0., I2=0., I3=0., r=0., s=0., t=0., p=0., q=0.,
510 bigR=0., phi=0., y0=0., y1=0., y2=0., tmp=0., inv[3];
511
512 matrix_invariants( mat, inv );
513 I1 = inv[0];
514 I2 = inv[1];
515 I3 = inv[2];
516 r = -I1;
517 s = +I2;
518 t = -I3;
519 p = (3.*s-r*r)/3.;
520 q = 2.*r*r*r/27. - r*s/3. + t;
521 if ( scalar_dabs(q)<EPS_Q ) {
522 y0 = -sqrt(scalar_dabs(p));
523 y1 = +sqrt(scalar_dabs(p));
524 y2 = 0.;
525 }
526 else {
527 bigR = sqrt(scalar_dabs(p)/3.); if ( q<0. ) bigR = -bigR;
528 tmp = q/(2.*bigR*bigR*bigR);
529 if ( tmp<-1. ) tmp = -1.;
530 if ( tmp>+1. ) tmp = +1.;
531 phi = acos(tmp);
532 y0 = -2.*bigR*cos(phi/3.);
533 y1 = -2.*bigR*cos(phi/3.+2.*PIRAD/3.);
534 y2 = -2.*bigR*cos(phi/3.+4.*PIRAD/3.);
535 }
536 eigenvalues[0] = y0 - r/3.;
537 eigenvalues[1] = y1 - r/3.;
538 eigenvalues[2] = y2 - r/3.;
539 }
540
matrix_insert(double a[],long int n,long int m,double b[],long int k,long int l,long int p)541 void matrix_insert( double a[], long int n, long int m,
542 double b[], long int k, long int l, long int p )
543
544 // add matrix a[n,m] into matrix b[*,p] at location k, l
545
546 {
547 long int i=0, j=0, indxi=0, indxj=0;
548
549 for ( i=0; i<n; i++ ) {
550 for ( j=0; j<m; j++ ) {
551 indxi = k + i;
552 indxj = l + j;
553 b[indxi*p+indxj] += a[i*m+j];
554 }
555 }
556 }
557
matrix_invariants(double * mat,double * inv)558 void matrix_invariants( double *mat, double *inv )
559
560 {
561
562 inv[0] = mat[0] + mat[4] + mat[8];
563 inv[1] = mat[0]*mat[4] + mat[4]*mat[8] + mat[8]*mat[0] -
564 mat[1]*mat[3] - mat[5]*mat[7] - mat[6]*mat[2];
565 inv[2] = matrix_determinant( mat, 3 );
566
567 }
568
matrix_inverse(double * mat,double * inv_mat,double & det,long int n)569 long int matrix_inverse( double *mat, double *inv_mat, double &det, long int n )
570
571 {
572 long int i=0;
573 double inv_det=0., a1=0., a2=0., a3=0., norm=0.;
574
575 for ( i=0; i<n*n; i++ ) norm += EPS_DET*mat[i]*mat[i];
576
577 if ( n==1 ) {
578 det = mat[0];
579 if ( scalar_dabs(det)<=norm ) return 0;
580 inv_mat[0] = 1. / mat[0];
581 }
582 else if ( n==2 ) {
583 det = mat[0]*mat[3] - mat[1]*mat[2];
584 if ( scalar_dabs(det)<=norm ) return 0;
585 inv_det=1./det;
586 inv_mat[0] = mat[3]*inv_det;
587 inv_mat[1] = -mat[1]*inv_det;
588 inv_mat[2] = -mat[2]*inv_det;
589 inv_mat[3] = mat[0]*inv_det;
590 }
591 else {
592 assert( n==3 );
593 a1 = mat[4]*mat[8] - mat[7]*mat[5];
594 a2 = mat[7]*mat[2] - mat[1]*mat[8];
595 a3 = mat[1]*mat[5] - mat[4]*mat[2];
596 det = mat[0]*a1+mat[3]*a2+mat[6]*a3;
597 if ( scalar_dabs(det)<=norm ) return 0;
598 inv_det = 1./det;
599 inv_mat[0] = inv_det*a1;
600 inv_mat[1] = inv_det*a2;
601 inv_mat[2] = inv_det*a3;
602 inv_mat[3] = inv_det*(mat[5]*mat[6]-mat[3]*mat[8]);
603 inv_mat[4] = inv_det*(mat[0]*mat[8]-mat[6]*mat[2]);
604 inv_mat[5] = inv_det*(mat[3]*mat[2]-mat[0]*mat[5]);
605 inv_mat[6] = inv_det*(mat[3]*mat[7]-mat[6]*mat[4]);
606 inv_mat[7] = inv_det*(mat[6]*mat[1]-mat[0]*mat[7]);
607 inv_mat[8] = inv_det*(mat[0]*mat[4]-mat[3]*mat[1]);
608 }
609 return 1;
610
611 }
612
613 #define ROTATE(a,i,j,k,l) g=a[i*n+j];h=a[k*n+l];\
614 a[i*n+j]=g-s*(h+g*tau);\
615 a[k*n+l]=h+s*(g-h*tau);
616
matrix_jacobi(double * a,long int n,double d[],double * v,long int * nrot)617 void matrix_jacobi(double *a, long int n, double d[], double *v, long int *nrot)
618
619 // destroys *a!
620
621 {
622 long int j,iq,ip,i;
623 double tresh,theta,tau,t,sm,s,h,g,c,b[100],z[100];
624
625 if ( n>10 ) {
626 pri( "Program error: maximum of n exceeded in jacobi." );
627 exit(TN_EXIT_STATUS);
628 }
629
630 for (ip=0;ip<n;ip++) {
631 for (iq=0;iq<n;iq++) v[ip*n+iq]=0.0;
632 v[ip*n+ip]=1.0;
633 }
634 for (ip=0;ip<n;ip++) {
635 b[ip]=d[ip]=a[ip*n+ip];
636 z[ip]=0.0;
637 }
638 *nrot=0;
639 for (i=1;i<=50;i++) {
640 sm=0.0;
641 for (ip=0;ip<n-1;ip++) {
642 for (iq=ip+1;iq<n;iq++)
643 sm += scalar_dabs(a[ip*n+iq]);
644 }
645 if (sm == 0.0) {
646 return;
647 }
648 if (i < 4)
649 tresh=0.2*sm/(n*n);
650 else
651 tresh=0.0;
652 for (ip=0;ip<n-1;ip++) {
653 for (iq=ip+1;iq<n;iq++) {
654 g=100.0*scalar_dabs(a[ip*n+iq]);
655 if (i > 4 && (double)(scalar_dabs(d[ip])+g) == (double)scalar_dabs(d[ip])
656 && (double)(scalar_dabs(d[iq])+g) == (double)scalar_dabs(d[iq]))
657 a[ip*n+iq]=0.0;
658 else if (scalar_dabs(a[ip*n+iq]) > tresh) {
659 h=d[iq]-d[ip];
660 if ((double)(scalar_dabs(h)+g) == (double)scalar_dabs(h))
661 t=(a[ip*n+iq])/h;
662 else {
663 theta=0.5*h/(a[ip*n+iq]);
664 t=1.0/(scalar_dabs(theta)+sqrt(1.0+theta*theta));
665 if (theta < 0.0) t = -t;
666 }
667 c=1.0/sqrt(1+t*t);
668 s=t*c;
669 tau=s/(1.0+c);
670 h=t*a[ip*n+iq];
671 z[ip] -= h;
672 z[iq] += h;
673 d[ip] -= h;
674 d[iq] += h;
675 a[ip*n+iq]=0.0;
676 for (j=0;j<=ip-1;j++) {
677 ROTATE(a,j,ip,j,iq)
678 }
679 for (j=ip+1;j<=iq-1;j++) {
680 ROTATE(a,ip,j,j,iq)
681 }
682 for (j=iq+1;j<n;j++) {
683 ROTATE(a,ip,j,iq,j)
684 }
685 for (j=0;j<n;j++) {
686 ROTATE(v,j,ip,j,iq)
687 }
688 ++(*nrot);
689 }
690 }
691 }
692 for (ip=0;ip<n;ip++) {
693 b[ip] += z[ip];
694 d[ip]=b[ip];
695 z[ip]=0.0;
696 }
697 }
698 /*
699 pri( "Error: max. number of iterations in JACOBI exceeded." );
700 pri( "Use smaller time steps." );
701 exit(TN_EXIT_STATUS);
702 */
703 }
704
matrix_symmetric(double a[],long int n)705 void matrix_symmetric( double a[], long int n )
706
707 {
708 long int i=0, j=0, indx1=0, indx2=0;
709
710 for ( i=0; i<n; i++ ) {
711 for ( j=0; j<n; j++ ) {
712 indx1 = i*n+j;
713 indx2 = j*n+i;
714 a[indx2] = a[indx1];
715 }
716 }
717 }
718
scalar_dabs(double a)719 double scalar_dabs( double a )
720
721 {
722 double result=0.;
723 if ( a < 0. ) result = -a;
724 else result = a;
725 return result;
726 };
727
scalar_dmax(double a,double b)728 double scalar_dmax( double a, double b )
729
730 {
731 double result=0;
732 if ( a > b ) result = a;
733 else result = b;
734 return result;
735 };
736
scalar_dmin(double a,double b)737 double scalar_dmin( double a, double b )
738
739 {
740 double result=0;
741 if ( a < b ) result = a;
742 else result = b;
743 return result;
744 };
745
scalar_imax(long int a,long int b)746 long int scalar_imax( long int a, long int b )
747
748 {
749 long int result=0;
750 if ( a > b ) result = a;
751 else result = b;
752 return result;
753 };
754
scalar_power(double a,double b)755 double scalar_power( double a, double b )
756
757 {
758 double result=0.;
759
760 if ( b==0. )
761 result = 1.;
762 else
763 result = pow( a, b );
764
765 return result;
766 }
767
scalar_ran_normal(int & idum)768 double scalar_ran_normal( int &idum )
769 /* Return a normal distributed random number
770 with zero mean and unit variance.
771 From: numerical recipes in c gasdev.
772 Set idum to any negative value to initialize or
773 reinitialize the sequence.*/
774 {
775 static int iset=0;
776 static double gset;
777 double fac,r,v1,v2;
778
779 if ( iset==0 ) {
780 do {
781 v1 = 2.0*scalar_ran_uniform(idum) - 1.0;
782 v2 = 2.0*scalar_ran_uniform(idum) - 1.0;
783 r = v1*v1+v2*v2;
784 } while ( r>=1.0 || r==0.0 );
785 fac = sqrt( -2.0*log(r)/r );
786 gset = v1*fac;
787 iset=1;
788 return v2*fac;
789 } else {
790 iset=0;
791 return (double) gset;
792 }
793 }
794
795
scalar_ran_uniform(int & idum)796 double scalar_ran_uniform( int &idum )
797 /* Return a uniform random number between 0 and 1.
798 From: numerical recipes in c */
799
800 #define M1 259200
801 #define IA1 7141
802 #define IC1 54773
803 #define RM1 (1.0/M1)
804 #define M2 134456
805 #define IA2 8121
806 #define IC2 28411
807 #define RM2 (1.0/M2)
808 #define M3 243000
809 #define IA3 4561
810 #define IC3 51349
811
812 {
813 static long ix1,ix2,ix3;
814 static double r[98];
815 double temp;
816 static int iff=0;
817 int j;
818
819 if ( idum<0 || iff==0 ) {
820 iff=1;
821 ix1 = (IC1-(idum)) % M1;
822 ix1 = (IA1*ix1+IC1) % M1;
823 ix2 = ix1 % M2;
824 ix1 = (IA1*ix1+IC1) % M1;
825 ix3 = ix1 % M3;
826 for ( j=1; j<=97; j++ ) {
827 ix1 = (IA1*ix1+IC1) % M1;
828 ix2 = (IA2*ix2+IC2) % M2;
829 r[j] = (ix1+ix2*RM2)*RM1;
830 }
831 idum = 1;
832 }
833 ix1 = (IA1*ix1+IC1) % M1;
834 ix2 = (IA2*ix2+IC2) % M2;
835 ix3 = (IA3*ix3+IC3) % M3;
836 j = (int) 1 + ((97*ix3)/M3);
837 if ( j>97 || j<1 ) {
838 pri( "Error in random generator." );
839 exit(1);
840 }
841 temp = r[j];
842 r[j] = (ix1+ix2*RM2)*RM1;
843 return temp;
844 }
845
846
scalar_sign(double a)847 double scalar_sign( double a )
848
849 {
850 if ( a>0. )
851 return 1.;
852 else
853 return -1.;
854 }
855
scalar_square(double a)856 double scalar_square( double a )
857
858 {
859 return a * a;
860 }
861
sort(double val[],double vec[])862 void sort( double val[], double vec[] )
863
864 {
865 long int idim=0, min_indx=0, middle_indx=0, max_indx=0;
866 double min_val=1.e20, max_val=-1.e20, work_val[MDIM], work_vec[MDIM*MDIM];
867
868 array_move( val, work_val, MDIM );
869 array_move( vec, work_vec, MDIM*MDIM );
870
871 for ( idim=0; idim<MDIM; idim++ ) {
872 if ( work_val[idim]<min_val ) {
873 min_indx = idim;
874 min_val = work_val[idim];
875 }
876 if ( work_val[idim]>max_val ) {
877 max_indx = idim;
878 max_val = work_val[idim];
879 }
880 }
881 assert( max_indx>=0 && max_indx<MDIM );
882 assert( min_indx>=0 && min_indx<MDIM );
883 if ( min_indx!=0 && max_indx!=0 )
884 middle_indx = 0;
885 else if ( min_indx!=1 && max_indx!=1 )
886 middle_indx = 1;
887 else
888 middle_indx = 2;
889
890 val[0] = work_val[max_indx];
891 val[1] = work_val[middle_indx];
892 val[2] = work_val[min_indx];
893 array_move( &work_vec[max_indx*MDIM], &vec[0*MDIM], MDIM );
894 array_move( &work_vec[middle_indx*MDIM], &vec[1*MDIM], MDIM );
895 array_move( &work_vec[min_indx*MDIM], &vec[2*MDIM], MDIM );
896
897 }
898
string_convert_to_lower_case(char str[])899 void string_convert_to_lower_case( char str[] )
900
901 // convert upper case to lower case
902
903 {
904 int i=0, length=0;
905
906 length = strlen(str);
907 for ( i=0; i<length; i++ ) {
908 // convert to lower case
909 str[i] = tolower(str[i]);
910 }
911 }
912
string_isinteger(char name[])913 long int string_isinteger( char name[] )
914
915 // test if string is an integer
916
917 {
918 long int i=0, length=0, result=1;
919
920 length = strlen(name);
921 for ( i=0; i<length; i++ ) {
922 if ( !isdigit(name[i]) ) {
923 if ( name[i]!='-' && name[i]!='+' && name[i]!='e' ) result = 0;
924 }
925 }
926
927 return result;
928 }
929
930
string_isdouble(char name[])931 long int string_isdouble( char name[] )
932
933 // test if string is a double
934
935 {
936 long int i=0, length=0, result=1;
937
938 length = strlen(name);
939 for ( i=0; i<length; i++ ) {
940 if ( !isdigit(name[i]) ) {
941 if ( name[i]!='-' && name[i]!='+' &&
942 name[i]!='e' && name[i]!='.' ) result = 0;
943 }
944 }
945
946 return result;
947 }
948
string_replace(char s[],char from,char to)949 void string_replace( char s[], char from, char to )
950
951 // replace character in string
952
953 {
954 long int i=0, l=0;
955
956 l = strlen(s);
957 for ( i=0; i<l; i++ ) {
958 if ( s[i]==from ) s[i] = to;
959 }
960
961 }
962
963
string_reverse(char s[])964 void string_reverse( char s[] )
965
966 // reverse string
967
968 {
969 long int i=0, l=0;
970 char str[MCHAR];
971
972 l = strlen(s);
973 assert(l<MCHAR);
974
975 for ( i=0; i<l; i++ ) str[i] = s[l-1-i];
976 str[l] = '\0';
977 strcpy(s,str);
978 }
979
string_shorten(char s[],long int length)980 void string_shorten( char s[], long int length )
981
982 // shorten string
983
984 {
985 long int l=0;
986
987 l = strlen(s);
988
989 if ( l>length ) {
990 s[length] = '\0';
991 }
992 }
993
table_xy(double table[],const char * table_name,long int length,double x,double & y)994 long int table_xy( double table[], const char* table_name,
995 long int length, double x, double &y )
996
997 {
998 long int found=0, i=0, n=0, i2=0,i22=0,i21=0,i221=0;
999 double x0=0., x1=0., y0=0., y1=0., dummy=0.;
1000
1001 y = 0.;
1002 if ( length==1 ) {
1003 y = table[0];
1004 return 1;
1005 }
1006 else if ( length==2 ) {
1007 y = table[1];
1008 return 1;
1009 }
1010 else if ( length>=4 ) {
1011 n = length / 2; found = 0; y = 0.;
1012 // order the table using simple sort ofb
1013 for (i = 0;i < n-1; i++ ) {
1014 i2=i*2;i21=i2+1;i22=i21+1;i221=i22+1;
1015 if ( table[i2] > table[i22] ) { // swap
1016 dummy=table[i2];table[i2]=table[i22];table[i22]=dummy; // swap xs
1017 dummy=table[i21];table[i21]=table[i221];table[i221]=dummy; // swap ys
1018 }
1019 }
1020 // end of sorting
1021 for ( i=0; !found && i<n-1; i++ ) {
1022 x0 = table[i*2+0];
1023 y0 = table[i*2+1];
1024 x1 = table[i*2+2];
1025 y1 = table[i*2+3];
1026 if ( x1<=x0 ) {
1027 pri( "Error found in ", table_name );
1028 pri( "The value ", x1 );
1029 pri( "is not larger than ", x0 );
1030 pri( "Total table ", table, length );
1031 exit_tn_on_error();
1032 }
1033 if ( x>=(x0-1.e-10) && x<=x1 ) {
1034 found = 1;
1035 if ( x0==x1 )
1036 y = y0;
1037 else
1038 y = y0 + (y1-y0)*(x-x0)/(x1-x0);
1039 }
1040 }
1041 }
1042
1043 return found;
1044 }
1045
table_xyz(double table[],long int number[],double coord[],double & z)1046 long int table_xyz( double table[], long int number[], double coord[], double &z )
1047
1048 {
1049 long int inol=0, nnol=3, found=0, ix=0, iy=0, i0=0, i1=0, i2=0, i3=0, nx=0, ny=0;
1050 double coord0[MDIM], coord1[MDIM], coord2[MDIM], coord3[MDIM], zcoord[3], weight[3];
1051
1052 if ( number[0]==1 && number[1]==1 ) {
1053 z = table[2];
1054 return 1;
1055 }
1056
1057 nx = number[0];
1058 ny = number[1];
1059 for ( ix=0; !found && ix<nx-1; ix++ ) {
1060 for ( iy=0; !found && iy<ny-1; iy++ ) {
1061 // quad in grid
1062 i0 = (iy+0)*3*nx+((ix+0)*3);
1063 i1 = (iy+0)*3*nx+((ix+1)*3);
1064 i2 = (iy+1)*3*nx+((ix+0)*3);
1065 i3 = (iy+1)*3*nx+((ix+1)*3);
1066 coord0[0] = table[i0+0];
1067 coord0[1] = table[i0+1];
1068 coord0[2] = 0.;
1069 coord1[0] = table[i1+0];
1070 coord1[1] = table[i1+1];
1071 coord1[2] = 0.;
1072 coord2[0] = table[i2+0];
1073 coord2[1] = table[i2+1];
1074 coord2[2] = 0.;
1075 coord3[0] = table[i3+0];
1076 coord3[1] = table[i3+1];
1077 coord3[2] = 0.;
1078 // first triangle in quad
1079 project_point_on_triangle( coord, coord0, coord1, coord2, weight );
1080 found = 1;
1081 for ( inol=0; inol<nnol; inol++ ) {
1082 if ( weight[inol]<-EPS_ISO || weight[inol]>(1.+EPS_ISO) ) found = 0;
1083 }
1084 if ( found ) {
1085 z = 0.;
1086 zcoord[0] = table[i0+2];
1087 zcoord[1] = table[i1+2];
1088 zcoord[2] = table[i2+2];
1089 for ( inol=0; inol<nnol; inol++ ) z += weight[inol] * zcoord[inol];
1090 }
1091 // second triangle in quad
1092 if ( !found ) {
1093 project_point_on_triangle( coord, coord1, coord2, coord3, weight );
1094 found = 1;
1095 for ( inol=0; inol<nnol; inol++ ) {
1096 if ( weight[inol]<-EPS_ISO || weight[inol]>(1.+EPS_ISO) ) found = 0;
1097 }
1098 if ( found ) {
1099 z = 0.;
1100 zcoord[0] = table[i1+2];
1101 zcoord[1] = table[i2+2];
1102 zcoord[2] = table[i3+2];
1103 for ( inol=0; inol<3; inol++ ) z += weight[inol] * zcoord[inol];
1104 }
1105 }
1106 }
1107 }
1108
1109 return found;
1110 }
1111
triangle_area(double c0[],double c1[],double c2[])1112 double triangle_area( double c0[], double c1[], double c2[] )
1113
1114 {
1115 double area=0., vec0[MDIM], vec1[MDIM], vec2[MDIM];
1116
1117 array_set( vec0, 0., MDIM );
1118 array_set( vec1, 0., MDIM );
1119 array_set( vec2, 0., MDIM );
1120
1121 array_subtract( c1, c0, vec0, ndim );
1122 array_subtract( c2, c0, vec1, ndim );
1123 array_outproduct_3D( vec0, vec1, vec2 );
1124 area = array_size( vec2, MDIM ) / 2.;
1125 return area;
1126 }
1127
tetrahedron_volume(double c0[],double c1[],double c2[],double c3[])1128 double tetrahedron_volume( double c0[], double c1[], double c2[], double c3[] )
1129
1130 {
1131 long int nnol=4;
1132 double volume=0., detj=0., p[MDIM*MNOL], d[MDIM*MNOL],
1133 coord[MNOL*MDIM], xj[MDIM*MDIM], xj_inv[MDIM*MDIM];
1134
1135 p[0] = 1.;
1136 p[1] = 0.;
1137 p[2] = 0.;
1138 p[3] = -1.;
1139 p[4] = 0.;
1140 p[5] = 0.;
1141 p[6] = 1.;
1142 p[7] = -1.;
1143 p[8] = 0.;
1144 p[9] = 1.;
1145 p[10] = 0.;
1146 p[11] = -1.;
1147
1148 array_move( c0, &coord[0*MDIM], MDIM );
1149 array_move( c1, &coord[1*MDIM], MDIM );
1150 array_move( c2, &coord[2*MDIM], MDIM );
1151 array_move( c3, &coord[3*MDIM], MDIM );
1152
1153 matrix_ab( p, coord, xj, MDIM, nnol, MDIM );
1154 if ( matrix_inverse( xj, xj_inv, detj, MDIM ) ) {
1155 matrix_ab( xj_inv, p, d, MDIM, MDIM, nnol );
1156 detj = scalar_dabs( detj );
1157 volume = detj / 6.;
1158 }
1159 else volume = 0.;
1160
1161 return volume;
1162
1163 }
1164
1165 /****************************************************************************************
1166 From Triax (D.Masin@city.ac.uk)
1167 ****************************************************************************************/
1168
matrix_inverse_general(double * matr,double * inv,int P)1169 void matrix_inverse_general(double *matr, double *inv, int P) {
1170 int N=int(sqrt((double)P));
1171 register long int i=0, j=0 ;
1172 Matrix matice(N,N);
1173 Matrix img(N,N);
1174 Matrix imginv(N,N);
1175 Matrix invmatrix(N,N);
1176 for(i=1; i<=N; i++) {
1177 for(j=1; j<=N; j++) {
1178 matice(i,j)=matr[N*(i-1)+j-1];
1179 }
1180 }
1181 cinv( matice, img, invmatrix, imginv);
1182 for(i=1; i<=N; i++) {
1183 for(j=1; j<=N; j++) {
1184 inv[N*(i-1)+j-1]=invmatrix(i,j);
1185 }
1186 }
1187 }
1188
make_dev(double tnz[9],double dev[9])1189 void make_dev(double tnz[9], double dev[9]) {
1190 array_set(dev, 0, 9);
1191 double tnzm = ( tnz[0] + tnz[4] + tnz[8] ) / 3.;
1192 array_move( tnz, dev, MDIM*MDIM );
1193 for ( int idim=0; idim<MDIM; idim++ ) dev[idim*MDIM+idim] -= tnzm;
1194 }
1195
1196
matrix4_ab(double a[],double b[],double c[3][3][3][3])1197 void matrix4_ab( double a[], double b[], double c[3][3][3][3] ) {
1198 register long int i=0, j=0, k=0, l=0;
1199
1200 for ( i=0; i<3; i++ ) {
1201 for ( j=0; j<3; j++ ) {
1202 for ( k=0; k<3; k++ ) {
1203 for ( l=0; l<3; l++ ) {
1204 c[i][j][k][l] = a[i*3+j] * b[k*3+l];
1205 }
1206 }
1207 }
1208 }
1209
1210 }
1211
matrix_a_contr_b(double a[],double b[],double & c)1212 void matrix_a_contr_b( double a[], double b[], double &c ) {
1213 register long int i=0, j=0 ;
1214 c=0;
1215
1216 for ( i=0; i<3; i++ ) {
1217 for ( j=0; j<3; j++ ) {
1218 c += a[i*3+j]*b[i*3+j];
1219 }
1220 }
1221
1222 }
1223
matrix_ab4(double a[],double b[3][3][3][3],double c[])1224 void matrix_ab4( double a[], double b[3][3][3][3], double c[] ) {
1225 register long int i=0, j=0, k=0, l=0;
1226
1227 for ( i=0; i<3; i++ ) {
1228 for ( j=0; j<3; j++ ) {
1229 c[i*3+j] = 0.;
1230 for ( k=0; k<3; k++ ) {
1231 for ( l=0; l<3; l++ ) {
1232 c[i*3+j] += a[k*3+l] * b[k][l][i][j];
1233 }
1234 }
1235 }
1236 }
1237 }
1238
calc_IJlode(double geosigma[9],double & I,double & J,double & lode,bool calcderiv,double dIdsig[3][3],double dJdsig[3][3],double dlodedsig[3][3])1239 void calc_IJlode(double geosigma[9], double &I, double &J, double &lode,
1240 bool calcderiv, double dIdsig[3][3], double dJdsig[3][3], double dlodedsig[3][3]) {
1241
1242 I=geosigma[0]+geosigma[4]+geosigma[8]; //geosigma==positive in compression
1243 double sij[3][3];
1244 array_set(*sij, 0, 9);
1245 make_dev(geosigma, *sij);
1246 J=sqrt(array_inproduct(*sij, *sij, 9)/2);
1247 double kron_delta[MDIM][MDIM];
1248 array_set(*kron_delta, 0., MDIM*MDIM);
1249 kron_delta[0][0]=kron_delta[1][1]=kron_delta[2][2]=1;
1250 double S=0;
1251 double sijsjkski=0;
1252 for (int i=0; i<3; i++ ) {
1253 for (int j=0; j<3; j++ ) {
1254 for (int k=0; k<3; k++ ) {
1255 sijsjkski += sij[i][j]*sij[j][k]*sij[k][i];
1256 }
1257 }
1258 }
1259 if(sijsjkski>=0) S=scalar_power(sijsjkski/3, 0.333333333333333333333333333333333333333333);
1260 else S=-scalar_power(-sijsjkski/3, 0.333333333333333333333333333333333333333333);
1261 if(J==0) J=TINY;
1262 double inlodebracket=3.0*sqrt(3.0)*scalar_power(S,3)/(2*scalar_power(J,3));
1263 if(inlodebracket>=1) inlodebracket=1;
1264 if(inlodebracket<=-1) inlodebracket=-1;
1265 lode=asin(inlodebracket)/3;
1266 if (calcderiv){
1267 array_move(*kron_delta, *dIdsig, MDIM*MDIM);
1268 for (int i=0; i<3; i++ ) {
1269 for (int j=0; j<3; j++ ) {
1270 dJdsig[i][j]= sij[i][j]/(2*J);
1271 }
1272 }
1273 double sikskj[3][3];
1274 array_set(*sikskj,0,9);
1275 for (int i=0; i<3; i++ ) {
1276 for (int j=0; j<3; j++ ) {
1277 for (int k=0; k<3; k++ ) {
1278 sikskj[i][j] += sij[i][k]*sij[k][j];
1279 }
1280 }
1281 }
1282 double Sij[3][3];
1283 array_set(*Sij,0,9);
1284 array_set(*dlodedsig,0,9);
1285 if(lode!=PIRAD/6 && lode!=-PIRAD/6) { //at these points grad lode == 0
1286 for (int i=0; i<3; i++ ) {
1287 for (int j=0; j<3; j++ ) {
1288 Sij[i][j]= sqrt(2.0)*(sqrt(3.0)*sikskj[i][j]/(2*J*J)-
1289 sin(3*lode)*sij[i][j]/(2*J)-
1290 kron_delta[i][j]/sqrt(3.0))/cos(3*lode);
1291 }
1292 }
1293 for (int i=0; i<3; i++ ) {
1294 for (int j=0; j<3; j++ ) {
1295 dlodedsig[i][j]=Sij[i][j]/(J*sqrt(2.0));
1296 }
1297 }
1298 }
1299 }
1300 }
1301
1302 /****************************************************************************************
1303 *********from http://www.algarcia.org/nummeth/Programs2E.html******************************
1304 ****************************************************************************************/
1305
1306 // Compute inverse of complex matrix
cinv(Matrix RealA,Matrix ImagA,Matrix & RealAinv,Matrix & ImagAinv)1307 void cinv( Matrix RealA, Matrix ImagA,
1308 Matrix& RealAinv, Matrix& ImagAinv )
1309 // Inputs
1310 // RealA - Real part of matrix A (N by N)
1311 // ImagA - Imaginary part of matrix A (N by N)
1312 // Outputs
1313 // RealAinv - Real part of inverse of matrix A (N by N)
1314 // ImagAinv - Imaginary part of A inverse (N by N)
1315 {
1316
1317 int N = RealA.nRow();
1318 assert( N == RealA.nCol() && N == ImagA.nRow()
1319 && N == ImagA.nCol());
1320 RealAinv = RealA; // Copy matrices to ensure they are same size
1321 ImagAinv = ImagA;
1322
1323 int i, j, k;
1324 Matrix scale(N); // Scale factor
1325 int *index; index = new int [N+1];
1326
1327 //* Matrix B is initialized to the identity matrix
1328 Matrix RealB(N,N), ImagB(N,N);
1329 RealB.set(0.0); ImagB.set(0.0);
1330 for( i=1; i<=N; i++ )
1331 RealB(i,i) = 1.0;
1332
1333 //* Set scale factor, scale(i) = max( |a(i,j)| ), for each row
1334 for( i=1; i<=N; i++ ) {
1335 index[i] = i; // Initialize row index list
1336 double scaleMax = 0.;
1337 for( j=1; j<=N; j++ ) {
1338 double MagA = RealA(i,j)*RealA(i,j) + ImagA(i,j)*ImagA(i,j);
1339 scaleMax = (scaleMax > MagA) ? scaleMax : MagA;
1340 }
1341 scale(i) = scaleMax;
1342 }
1343
1344 //* Loop over rows k = 1, ..., (N-1)
1345 for( k=1; k<=N-1; k++ ) {
1346 //* Select pivot row from max( |a(j,k)/s(j)| )
1347 double ratiomax = 0.0;
1348 int jPivot = k;
1349 for( i=k; i<=N; i++ ) {
1350 double MagA = RealA(index[i],k)*RealA(index[i],k) +
1351 ImagA(index[i],k)*ImagA(index[i],k);
1352 if(scale(index[i])==0) {
1353 cout<<endl<<" Error in matrix inversion ";
1354 exit(0);
1355 }
1356 double ratio = MagA/scale(index[i]);
1357 if( ratio > ratiomax ) {
1358 jPivot=i;
1359 ratiomax = ratio;
1360 }
1361 }
1362 //* Perform pivoting using row index list
1363 int indexJ = index[k];
1364 if( jPivot != k ) { // Pivot
1365 indexJ = index[jPivot];
1366 index[jPivot] = index[k]; // Swap index jPivot and k
1367 index[k] = indexJ;
1368 }
1369 //* Perform forward elimination
1370 for( i=k+1; i<=N; i++ ) {
1371 double denom = RealA(indexJ,k)*RealA(indexJ,k)
1372 + ImagA(indexJ,k)*ImagA(indexJ,k);
1373 if(denom==0) {
1374 cout<<endl<<" Error in matrix inversion ";
1375 exit(0);
1376 }
1377 double RealCoeff = (RealA(index[i],k)*RealA(indexJ,k)
1378 + ImagA(index[i],k)*ImagA(indexJ,k))/denom;
1379 double ImagCoeff = (ImagA(index[i],k)*RealA(indexJ,k)
1380 - RealA(index[i],k)*ImagA(indexJ,k))/denom;
1381 for( j=k+1; j<=N; j++ ) {
1382 RealA(index[i],j) -= RealCoeff*RealA(indexJ,j)
1383 - ImagCoeff*ImagA(indexJ,j);
1384 ImagA(index[i],j) -= RealCoeff*ImagA(indexJ,j)
1385 + ImagCoeff*RealA(indexJ,j);
1386 }
1387 RealA(index[i],k) = RealCoeff;
1388 ImagA(index[i],k) = ImagCoeff;
1389 for( j=1; j<=N; j++ ) {
1390 RealB(index[i],j) -= RealA(index[i],k)*RealB(indexJ,j)
1391 - ImagA(index[i],k)*ImagB(indexJ,j);
1392 ImagB(index[i],j) -= RealA(index[i],k)*ImagB(indexJ,j)
1393 + ImagA(index[i],k)*RealB(indexJ,j);
1394 }
1395 }
1396 }
1397 //* Perform backsubstitution
1398 for( k=1; k<=N; k++ ) {
1399 double denom = RealA(index[N],N)*RealA(index[N],N)
1400 + ImagA(index[N],N)*ImagA(index[N],N);
1401 if(denom==0) {
1402 cout<<endl<<" Error in matrix inversion ";
1403 exit(0);
1404 }
1405 RealAinv(N,k) = (RealB(index[N],k)*RealA(index[N],N)
1406 + ImagB(index[N],k)*ImagA(index[N],N))/denom;
1407 ImagAinv(N,k) = (ImagB(index[N],k)*RealA(index[N],N)
1408 - RealB(index[N],k)*ImagA(index[N],N))/denom;
1409 for( i=N-1; i>=1; i--) {
1410 double RealSum = RealB(index[i],k);
1411 double ImagSum = ImagB(index[i],k);
1412 for( j=i+1; j<=N; j++ ) {
1413 RealSum -= RealA(index[i],j)*RealAinv(j,k)
1414 - ImagA(index[i],j)*ImagAinv(j,k);
1415 ImagSum -= RealA(index[i],j)*ImagAinv(j,k)
1416 + ImagA(index[i],j)*RealAinv(j,k);
1417 }
1418 double denom = RealA(index[i],i)*RealA(index[i],i)
1419 + ImagA(index[i],i)*ImagA(index[i],i);
1420 if(denom==0) {
1421 cout<<endl<<" Error in matrix inversion ";
1422 exit(0);
1423 }
1424 RealAinv(i,k) = (RealSum*RealA(index[i],i)
1425 + ImagSum*ImagA(index[i],i))/denom;
1426 ImagAinv(i,k) = (ImagSum*RealA(index[i],i)
1427 - RealSum*ImagA(index[i],i))/denom;
1428 }
1429 }
1430
1431 delete [] index; // Release allocated memory
1432 }
1433
1434