1 #ifndef lint 2 static char vcid[] = "$Id: dgefa4.c,v 1.1 1997/06/10 03:18:18 bsmith Exp bsmith $"; 3 #endif 4 /* 5 Inverts 5 by 5 matrix using partial pivoting. 6 */ 7 #include "petsc.h" 8 9 #undef __FUNC__ 10 #define __FUNC__ "Kernel_A_gets_inverse_A_5" 11 int Kernel_A_gets_inverse_A_5(Scalar *a) 12 { 13 int i__2, i__3, kp1, j, k, l,ll,i,ipvt_l[5],*ipvt = ipvt_l-1,kb,k3; 14 int k4,j3; 15 Scalar *aa,*ax,*ay,work_l[25],*work = work_l-1,stmp; 16 double tmp,max; 17 18 /* gaussian elimination with partial pivoting */ 19 20 /* Parameter adjustments */ 21 a -= 6; 22 23 for (k = 1; k <= 4; ++k) { 24 kp1 = k + 1; 25 k3 = 5*k; 26 k4 = k3 + k; 27 /* find l = pivot index */ 28 29 i__2 = 5 - 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,0,"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 = 5 - 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 <= 5; ++j) { 65 j3 = 5*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 = 5 - 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[5] = 5; 80 if (a[30] == 0.) { 81 SETERRQ(3,0,"Zero pivot,final row"); 82 } 83 84 /* 85 Now form the inverse 86 */ 87 88 /* compute inverse(u) */ 89 90 for (k = 1; k <= 5; ++k) { 91 k3 = 5*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 (5 < kp1) continue; 100 ax = aa; 101 for (j = kp1; j <= 5; ++j) { 102 j3 = 5*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 <= 4; ++kb) { 115 k = 5 - kb; 116 k3 = 5*k; 117 kp1 = k + 1; 118 aa = a + k3; 119 for (i = kp1; i <= 5; ++i) { 120 work[i] = aa[i]; 121 aa[i] = 0.0; 122 } 123 for (j = kp1; j <= 5; ++j) { 124 stmp = work[j]; 125 ax = &a[5*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 ay[3] += stmp*ax[3]; 131 ay[4] += stmp*ax[4]; 132 } 133 l = ipvt[k]; 134 if (l != k) { 135 ax = &a[k3 + 1]; 136 ay = &a[5*l + 1]; 137 stmp = ax[0]; ax[0] = ay[0]; ay[0] = stmp; 138 stmp = ax[1]; ax[1] = ay[1]; ay[1] = stmp; 139 stmp = ax[2]; ax[2] = ay[2]; ay[2] = stmp; 140 stmp = ax[3]; ax[3] = ay[3]; ay[3] = stmp; 141 stmp = ax[4]; ax[4] = ay[4]; ay[4] = stmp; 142 } 143 } 144 return 0; 145 } 146 147