1*4224c193SBarry Smith #ifndef lint 2*4224c193SBarry Smith static char vcid[] = "$Id: dgefa3.c,v 1.8 1997/01/06 20:25:20 balay Exp $"; 3*4224c193SBarry Smith #endif 4*4224c193SBarry Smith /* 5*4224c193SBarry Smith Inverts 3 by 3 matrix using partial pivoting. 6*4224c193SBarry Smith */ 7*4224c193SBarry Smith #include "petsc.h" 8*4224c193SBarry Smith 9*4224c193SBarry Smith #undef __FUNC__ 10*4224c193SBarry Smith #define __FUNC__ "Kernel_A_gets_inverse_A_3" 11*4224c193SBarry Smith int Kernel_A_gets_inverse_A_3(Scalar *a) 12*4224c193SBarry Smith { 13*4224c193SBarry Smith int i__2, i__3, kp1, j, k, l,ll,i,ipvt_l[3],*ipvt = ipvt_l-1,kb,k3; 14*4224c193SBarry Smith int k4,j3; 15*4224c193SBarry Smith Scalar *aa,*ax,*ay,work_l[9],*work = work_l-1,stmp; 16*4224c193SBarry Smith double tmp,max; 17*4224c193SBarry Smith 18*4224c193SBarry Smith /* gaussian elimination with partial pivoting */ 19*4224c193SBarry Smith 20*4224c193SBarry Smith /* Parameter adjustments */ 21*4224c193SBarry Smith a -= 4; 22*4224c193SBarry Smith 23*4224c193SBarry Smith for (k = 1; k <= 2; ++k) { 24*4224c193SBarry Smith kp1 = k + 1; 25*4224c193SBarry Smith k3 = 3*k; 26*4224c193SBarry Smith k4 = k3 + k; 27*4224c193SBarry Smith /* find l = pivot index */ 28*4224c193SBarry Smith 29*4224c193SBarry Smith i__2 = 4 - k; 30*4224c193SBarry Smith aa = &a[k4]; 31*4224c193SBarry Smith max = PetscAbsScalar(aa[0]); 32*4224c193SBarry Smith l = 1; 33*4224c193SBarry Smith for ( ll=1; ll<i__2; ll++ ) { 34*4224c193SBarry Smith tmp = PetscAbsScalar(aa[ll]); 35*4224c193SBarry Smith if (tmp > max) { max = tmp; l = ll+1;} 36*4224c193SBarry Smith } 37*4224c193SBarry Smith l += k - 1; 38*4224c193SBarry Smith ipvt[k] = l; 39*4224c193SBarry Smith 40*4224c193SBarry Smith if (a[l + k3] == 0.) { 41*4224c193SBarry Smith SETERRQ(k,0,"Zero pivot"); 42*4224c193SBarry Smith } 43*4224c193SBarry Smith 44*4224c193SBarry Smith /* interchange if necessary */ 45*4224c193SBarry Smith 46*4224c193SBarry Smith if (l != k) { 47*4224c193SBarry Smith stmp = a[l + k3]; 48*4224c193SBarry Smith a[l + k3] = a[k4]; 49*4224c193SBarry Smith a[k4] = stmp; 50*4224c193SBarry Smith } 51*4224c193SBarry Smith 52*4224c193SBarry Smith /* compute multipliers */ 53*4224c193SBarry Smith 54*4224c193SBarry Smith stmp = -1. / a[k4]; 55*4224c193SBarry Smith i__2 = 3 - k; 56*4224c193SBarry Smith aa = &a[1 + k4]; 57*4224c193SBarry Smith for ( ll=0; ll<i__2; ll++ ) { 58*4224c193SBarry Smith aa[ll] *= stmp; 59*4224c193SBarry Smith } 60*4224c193SBarry Smith 61*4224c193SBarry Smith /* row elimination with column indexing */ 62*4224c193SBarry Smith 63*4224c193SBarry Smith ax = &a[k4+1]; 64*4224c193SBarry Smith for (j = kp1; j <= 3; ++j) { 65*4224c193SBarry Smith j3 = 3*j; 66*4224c193SBarry Smith stmp = a[l + j3]; 67*4224c193SBarry Smith if (l != k) { 68*4224c193SBarry Smith a[l + j3] = a[k + j3]; 69*4224c193SBarry Smith a[k + j3] = stmp; 70*4224c193SBarry Smith } 71*4224c193SBarry Smith 72*4224c193SBarry Smith i__3 = 3 - k; 73*4224c193SBarry Smith ay = &a[1+k+j3]; 74*4224c193SBarry Smith for ( ll=0; ll<i__3; ll++ ) { 75*4224c193SBarry Smith ay[ll] += stmp*ax[ll]; 76*4224c193SBarry Smith } 77*4224c193SBarry Smith } 78*4224c193SBarry Smith } 79*4224c193SBarry Smith ipvt[3] = 3; 80*4224c193SBarry Smith if (a[12] == 0.) { 81*4224c193SBarry Smith SETERRQ(3,0,"Zero pivot,final row"); 82*4224c193SBarry Smith } 83*4224c193SBarry Smith 84*4224c193SBarry Smith /* 85*4224c193SBarry Smith Now form the inverse 86*4224c193SBarry Smith */ 87*4224c193SBarry Smith 88*4224c193SBarry Smith /* compute inverse(u) */ 89*4224c193SBarry Smith 90*4224c193SBarry Smith for (k = 1; k <= 3; ++k) { 91*4224c193SBarry Smith k3 = 3*k; 92*4224c193SBarry Smith k4 = k3 + k; 93*4224c193SBarry Smith a[k4] = 1.0 / a[k4]; 94*4224c193SBarry Smith stmp = -a[k4]; 95*4224c193SBarry Smith i__2 = k - 1; 96*4224c193SBarry Smith aa = &a[k3 + 1]; 97*4224c193SBarry Smith for ( ll=0; ll<i__2; ll++ ) aa[ll] *= stmp; 98*4224c193SBarry Smith kp1 = k + 1; 99*4224c193SBarry Smith if (3 < kp1) continue; 100*4224c193SBarry Smith ax = aa; 101*4224c193SBarry Smith for (j = kp1; j <= 3; ++j) { 102*4224c193SBarry Smith j3 = 3*j; 103*4224c193SBarry Smith stmp = a[k + j3]; 104*4224c193SBarry Smith a[k + j3] = 0.0; 105*4224c193SBarry Smith ay = &a[j3 + 1]; 106*4224c193SBarry Smith for ( ll=0; ll<k; ll++ ) { 107*4224c193SBarry Smith ay[ll] += stmp*ax[ll]; 108*4224c193SBarry Smith } 109*4224c193SBarry Smith } 110*4224c193SBarry Smith } 111*4224c193SBarry Smith 112*4224c193SBarry Smith /* form inverse(u)*inverse(l) */ 113*4224c193SBarry Smith 114*4224c193SBarry Smith for (kb = 1; kb <= 2; ++kb) { 115*4224c193SBarry Smith k = 3 - kb; 116*4224c193SBarry Smith k3 = 3*k; 117*4224c193SBarry Smith kp1 = k + 1; 118*4224c193SBarry Smith aa = a + k3; 119*4224c193SBarry Smith for (i = kp1; i <= 3; ++i) { 120*4224c193SBarry Smith work[i] = aa[i]; 121*4224c193SBarry Smith aa[i] = 0.0; 122*4224c193SBarry Smith } 123*4224c193SBarry Smith for (j = kp1; j <= 3; ++j) { 124*4224c193SBarry Smith stmp = work[j]; 125*4224c193SBarry Smith ax = &a[3*j + 1]; 126*4224c193SBarry Smith ay = &a[k3 + 1]; 127*4224c193SBarry Smith ay[0] += stmp*ax[0]; 128*4224c193SBarry Smith ay[1] += stmp*ax[1]; 129*4224c193SBarry Smith ay[2] += stmp*ax[2]; 130*4224c193SBarry Smith } 131*4224c193SBarry Smith l = ipvt[k]; 132*4224c193SBarry Smith if (l != k) { 133*4224c193SBarry Smith ax = &a[k3 + 1]; 134*4224c193SBarry Smith ay = &a[3*l + 1]; 135*4224c193SBarry Smith stmp = ax[0]; ax[0] = ay[0]; ay[0] = stmp; 136*4224c193SBarry Smith stmp = ax[1]; ax[1] = ay[1]; ay[1] = stmp; 137*4224c193SBarry Smith stmp = ax[2]; ax[2] = ay[2]; ay[2] = stmp; 138*4224c193SBarry Smith } 139*4224c193SBarry Smith } 140*4224c193SBarry Smith return 0; 141*4224c193SBarry Smith } 142*4224c193SBarry Smith 143