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