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