1 2 /* 3 Inverts 2 by 2 matrix using gaussian elimination with partial pivoting. 4 5 Used by the sparse factorization routines in 6 src/mat/impls/baij/seq 7 8 9 This is a combination of the Linpack routines 10 dgefa() and dgedi() specialized for a size of 2. 11 12 */ 13 #include <petscsys.h> 14 15 #undef __FUNCT__ 16 #define __FUNCT__ "PetscKernel_A_gets_inverse_A_2" 17 PETSC_EXTERN PetscErrorCode PetscKernel_A_gets_inverse_A_2(MatScalar *a,PetscReal shift,PetscBool allowzeropivot,PetscBool *zeropivotdetected) 18 { 19 PetscInt i__2,i__3,kp1,j,k,l,ll,i,ipvt[2],k3; 20 PetscInt k4,j3; 21 MatScalar *aa,*ax,*ay,work[4],stmp; 22 MatReal tmp,max; 23 24 PetscFunctionBegin; 25 if (zeropivotdetected) *zeropivotdetected = PETSC_FALSE; 26 shift = .25*shift*(1.e-12 + PetscAbsScalar(a[0]) + PetscAbsScalar(a[3])); 27 28 /* Parameter adjustments */ 29 a -= 3; 30 31 k = 1; 32 kp1 = k + 1; 33 k3 = 2*k; 34 k4 = k3 + k; 35 36 /* find l = pivot index */ 37 i__2 = 3 - k; 38 aa = &a[k4]; 39 max = PetscAbsScalar(aa[0]); 40 l = 1; 41 for (ll=1; ll<i__2; ll++) { 42 tmp = PetscAbsScalar(aa[ll]); 43 if (tmp > max) { max = tmp; l = ll+1;} 44 } 45 l += k - 1; 46 ipvt[k-1] = l; 47 48 if (a[l + k3] == 0.0) { 49 if (shift == 0.0) SETERRQ1(PETSC_COMM_SELF,PETSC_ERR_MAT_LU_ZRPVT,"Zero pivot, row %D",k-1); 50 else { 51 a[l + k3] = shift; 52 } 53 } 54 55 /* interchange if necessary */ 56 if (l != k) { 57 stmp = a[l + k3]; 58 a[l + k3] = a[k4]; 59 a[k4] = stmp; 60 } 61 62 /* compute multipliers */ 63 stmp = -1. / a[k4]; 64 i__2 = 2 - k; 65 aa = &a[1 + k4]; 66 for (ll=0; ll<i__2; ll++) aa[ll] *= stmp; 67 68 /* row elimination with column indexing */ 69 ax = &a[k4+1]; 70 for (j = kp1; j <= 2; ++j) { 71 j3 = 2*j; 72 stmp = a[l + j3]; 73 if (l != k) { 74 a[l + j3] = a[k + j3]; 75 a[k + j3] = stmp; 76 } 77 78 i__3 = 2 - k; 79 ay = &a[1+k+j3]; 80 for (ll=0; ll<i__3; ll++) ay[ll] += stmp*ax[ll]; 81 } 82 83 ipvt[1] = 2; 84 if (a[6] == 0.0) { 85 PetscErrorCode ierr; 86 if (allowzeropivot) { 87 ierr = PetscInfo1(NULL,"Zero pivot, row %D\n",1);CHKERRQ(ierr); 88 *zeropivotdetected = PETSC_TRUE; 89 } else SETERRQ1(PETSC_COMM_SELF,PETSC_ERR_MAT_LU_ZRPVT,"Zero pivot, row %D",1); 90 } 91 92 /* Now form the inverse */ 93 /* compute inverse(u) */ 94 for (k = 1; k <= 2; ++k) { 95 k3 = 2*k; 96 k4 = k3 + k; 97 a[k4] = 1.0 / a[k4]; 98 stmp = -a[k4]; 99 i__2 = k - 1; 100 aa = &a[k3 + 1]; 101 for (ll=0; ll<i__2; ll++) aa[ll] *= stmp; 102 kp1 = k + 1; 103 if (2 < kp1) continue; 104 ax = aa; 105 for (j = kp1; j <= 2; ++j) { 106 j3 = 2*j; 107 stmp = a[k + j3]; 108 a[k + j3] = 0.0; 109 ay = &a[j3 + 1]; 110 for (ll=0; ll<k; ll++) ay[ll] += stmp*ax[ll]; 111 } 112 } 113 114 /* form inverse(u)*inverse(l) */ 115 k = 1; 116 k3 = 2*k; 117 kp1 = k + 1; 118 aa = a + k3; 119 for (i = kp1; i <= 2; ++i) { 120 work[i-1] = aa[i]; 121 aa[i] = 0.0; 122 } 123 for (j = kp1; j <= 2; ++j) { 124 stmp = work[j-1]; 125 ax = &a[2*j + 1]; 126 ay = &a[k3 + 1]; 127 ay[0] += stmp*ax[0]; 128 ay[1] += stmp*ax[1]; 129 } 130 l = ipvt[k-1]; 131 if (l != k) { 132 ax = &a[k3 + 1]; 133 ay = &a[2*l + 1]; 134 stmp = ax[0]; ax[0] = ay[0]; ay[0] = stmp; 135 stmp = ax[1]; ax[1] = ay[1]; ay[1] = stmp; 136 } 137 PetscFunctionReturn(0); 138 } 139 140 #undef __FUNCT__ 141 #define __FUNCT__ "PetscKernel_A_gets_inverse_A_9" 142 PETSC_EXTERN PetscErrorCode PetscKernel_A_gets_inverse_A_9(MatScalar *a,PetscReal shift,PetscBool allowzeropivot,PetscBool *zeropivotdetected) 143 { 144 PetscInt i__2,i__3,kp1,j,k,l,ll,i,ipvt[9],kb,k3; 145 PetscInt k4,j3; 146 MatScalar *aa,*ax,*ay,work[81],stmp; 147 MatReal tmp,max; 148 149 /* gaussian elimination with partial pivoting */ 150 151 PetscFunctionBegin; 152 if (zeropivotdetected) *zeropivotdetected = PETSC_FALSE; 153 154 /* Parameter adjustments */ 155 a -= 10; 156 157 for (k = 1; k <= 8; ++k) { 158 kp1 = k + 1; 159 k3 = 9*k; 160 k4 = k3 + k; 161 /* find l = pivot index */ 162 163 i__2 = 10 - k; 164 aa = &a[k4]; 165 max = PetscAbsScalar(aa[0]); 166 l = 1; 167 for (ll=1; ll<i__2; ll++) { 168 tmp = PetscAbsScalar(aa[ll]); 169 if (tmp > max) { max = tmp; l = ll+1;} 170 } 171 l += k - 1; 172 ipvt[k-1] = l; 173 174 if (a[l + k3] == 0.0) SETERRQ1(PETSC_COMM_SELF,PETSC_ERR_MAT_LU_ZRPVT,"Zero pivot, row %D",k-1); 175 176 /* interchange if necessary */ 177 178 if (l != k) { 179 stmp = a[l + k3]; 180 a[l + k3] = a[k4]; 181 a[k4] = stmp; 182 } 183 184 /* compute multipliers */ 185 186 stmp = -1. / a[k4]; 187 i__2 = 9 - k; 188 aa = &a[1 + k4]; 189 for (ll=0; ll<i__2; ll++) aa[ll] *= stmp; 190 191 /* row elimination with column indexing */ 192 193 ax = &a[k4+1]; 194 for (j = kp1; j <= 9; ++j) { 195 j3 = 9*j; 196 stmp = a[l + j3]; 197 if (l != k) { 198 a[l + j3] = a[k + j3]; 199 a[k + j3] = stmp; 200 } 201 202 i__3 = 9 - k; 203 ay = &a[1+k+j3]; 204 for (ll=0; ll<i__3; ll++) ay[ll] += stmp*ax[ll]; 205 } 206 } 207 ipvt[8] = 9; 208 if (a[90] == 0.0) { 209 PetscErrorCode ierr; 210 if (allowzeropivot) { 211 ierr = PetscInfo1(NULL,"Zero pivot, row %D\n",6);CHKERRQ(ierr); 212 *zeropivotdetected = PETSC_TRUE; 213 } else SETERRQ1(PETSC_COMM_SELF,PETSC_ERR_MAT_LU_ZRPVT,"Zero pivot, row %D",6); 214 } 215 216 /* 217 Now form the inverse 218 */ 219 220 /* compute inverse(u) */ 221 222 for (k = 1; k <= 9; ++k) { 223 k3 = 9*k; 224 k4 = k3 + k; 225 a[k4] = 1.0 / a[k4]; 226 stmp = -a[k4]; 227 i__2 = k - 1; 228 aa = &a[k3 + 1]; 229 for (ll=0; ll<i__2; ll++) aa[ll] *= stmp; 230 kp1 = k + 1; 231 if (9 < kp1) continue; 232 ax = aa; 233 for (j = kp1; j <= 9; ++j) { 234 j3 = 9*j; 235 stmp = a[k + j3]; 236 a[k + j3] = 0.0; 237 ay = &a[j3 + 1]; 238 for (ll=0; ll<k; ll++) ay[ll] += stmp*ax[ll]; 239 } 240 } 241 242 /* form inverse(u)*inverse(l) */ 243 244 for (kb = 1; kb <= 8; ++kb) { 245 k = 9 - kb; 246 k3 = 9*k; 247 kp1 = k + 1; 248 aa = a + k3; 249 for (i = kp1; i <= 9; ++i) { 250 work[i-1] = aa[i]; 251 aa[i] = 0.0; 252 } 253 for (j = kp1; j <= 9; ++j) { 254 stmp = work[j-1]; 255 ax = &a[9*j + 1]; 256 ay = &a[k3 + 1]; 257 ay[0] += stmp*ax[0]; 258 ay[1] += stmp*ax[1]; 259 ay[2] += stmp*ax[2]; 260 ay[3] += stmp*ax[3]; 261 ay[4] += stmp*ax[4]; 262 ay[5] += stmp*ax[5]; 263 ay[6] += stmp*ax[6]; 264 ay[7] += stmp*ax[7]; 265 ay[8] += stmp*ax[8]; 266 } 267 l = ipvt[k-1]; 268 if (l != k) { 269 ax = &a[k3 + 1]; 270 ay = &a[9*l + 1]; 271 stmp = ax[0]; ax[0] = ay[0]; ay[0] = stmp; 272 stmp = ax[1]; ax[1] = ay[1]; ay[1] = stmp; 273 stmp = ax[2]; ax[2] = ay[2]; ay[2] = stmp; 274 stmp = ax[3]; ax[3] = ay[3]; ay[3] = stmp; 275 stmp = ax[4]; ax[4] = ay[4]; ay[4] = stmp; 276 stmp = ax[5]; ax[5] = ay[5]; ay[5] = stmp; 277 stmp = ax[6]; ax[6] = ay[6]; ay[6] = stmp; 278 stmp = ax[7]; ax[7] = ay[7]; ay[7] = stmp; 279 stmp = ax[8]; ax[8] = ay[8]; ay[8] = stmp; 280 } 281 } 282 PetscFunctionReturn(0); 283 } 284 285 /* 286 Inverts 15 by 15 matrix using partial pivoting. 287 288 Used by the sparse factorization routines in 289 src/mat/impls/baij/seq 290 291 This is a combination of the Linpack routines 292 dgefa() and dgedi() specialized for a size of 15. 293 294 */ 295 296 #undef __FUNCT__ 297 #define __FUNCT__ "PetscKernel_A_gets_inverse_A_15" 298 PETSC_EXTERN PetscErrorCode PetscKernel_A_gets_inverse_A_15(MatScalar *a,PetscInt *ipvt,MatScalar *work,PetscReal shift,PetscBool allowzeropivot,PetscBool *zeropivotdetected) 299 { 300 PetscInt i__2,i__3,kp1,j,k,l,ll,i,kb,k3; 301 PetscInt k4,j3; 302 MatScalar *aa,*ax,*ay,stmp; 303 MatReal tmp,max; 304 305 /* gaussian elimination with partial pivoting */ 306 307 PetscFunctionBegin; 308 if (zeropivotdetected) *zeropivotdetected = PETSC_FALSE; 309 310 /* Parameter adjustments */ 311 a -= 16; 312 313 for (k = 1; k <= 14; ++k) { 314 kp1 = k + 1; 315 k3 = 15*k; 316 k4 = k3 + k; 317 /* find l = pivot index */ 318 319 i__2 = 16 - k; 320 aa = &a[k4]; 321 max = PetscAbsScalar(aa[0]); 322 l = 1; 323 for (ll=1; ll<i__2; ll++) { 324 tmp = PetscAbsScalar(aa[ll]); 325 if (tmp > max) { max = tmp; l = ll+1;} 326 } 327 l += k - 1; 328 ipvt[k-1] = l; 329 330 if (a[l + k3] == 0.0) SETERRQ1(PETSC_COMM_SELF,PETSC_ERR_MAT_LU_ZRPVT,"Zero pivot, row %D",k-1); 331 332 /* interchange if necessary */ 333 334 if (l != k) { 335 stmp = a[l + k3]; 336 a[l + k3] = a[k4]; 337 a[k4] = stmp; 338 } 339 340 /* compute multipliers */ 341 342 stmp = -1. / a[k4]; 343 i__2 = 15 - k; 344 aa = &a[1 + k4]; 345 for (ll=0; ll<i__2; ll++) aa[ll] *= stmp; 346 347 /* row elimination with column indexing */ 348 349 ax = &a[k4+1]; 350 for (j = kp1; j <= 15; ++j) { 351 j3 = 15*j; 352 stmp = a[l + j3]; 353 if (l != k) { 354 a[l + j3] = a[k + j3]; 355 a[k + j3] = stmp; 356 } 357 358 i__3 = 15 - k; 359 ay = &a[1+k+j3]; 360 for (ll=0; ll<i__3; ll++) ay[ll] += stmp*ax[ll]; 361 } 362 } 363 ipvt[14] = 15; 364 if (a[240] == 0.0) { 365 PetscErrorCode ierr; 366 if (allowzeropivot) { 367 ierr = PetscInfo1(NULL,"Zero pivot, row %D\n",6);CHKERRQ(ierr); 368 *zeropivotdetected = PETSC_TRUE; 369 } else SETERRQ1(PETSC_COMM_SELF,PETSC_ERR_MAT_LU_ZRPVT,"Zero pivot, row %D",6); 370 } 371 372 /* 373 Now form the inverse 374 */ 375 376 /* compute inverse(u) */ 377 378 for (k = 1; k <= 15; ++k) { 379 k3 = 15*k; 380 k4 = k3 + k; 381 a[k4] = 1.0 / a[k4]; 382 stmp = -a[k4]; 383 i__2 = k - 1; 384 aa = &a[k3 + 1]; 385 for (ll=0; ll<i__2; ll++) aa[ll] *= stmp; 386 kp1 = k + 1; 387 if (15 < kp1) continue; 388 ax = aa; 389 for (j = kp1; j <= 15; ++j) { 390 j3 = 15*j; 391 stmp = a[k + j3]; 392 a[k + j3] = 0.0; 393 ay = &a[j3 + 1]; 394 for (ll=0; ll<k; ll++) ay[ll] += stmp*ax[ll]; 395 } 396 } 397 398 /* form inverse(u)*inverse(l) */ 399 400 for (kb = 1; kb <= 14; ++kb) { 401 k = 15 - kb; 402 k3 = 15*k; 403 kp1 = k + 1; 404 aa = a + k3; 405 for (i = kp1; i <= 15; ++i) { 406 work[i-1] = aa[i]; 407 aa[i] = 0.0; 408 } 409 for (j = kp1; j <= 15; ++j) { 410 stmp = work[j-1]; 411 ax = &a[15*j + 1]; 412 ay = &a[k3 + 1]; 413 ay[0] += stmp*ax[0]; 414 ay[1] += stmp*ax[1]; 415 ay[2] += stmp*ax[2]; 416 ay[3] += stmp*ax[3]; 417 ay[4] += stmp*ax[4]; 418 ay[5] += stmp*ax[5]; 419 ay[6] += stmp*ax[6]; 420 ay[7] += stmp*ax[7]; 421 ay[8] += stmp*ax[8]; 422 ay[9] += stmp*ax[9]; 423 ay[10] += stmp*ax[10]; 424 ay[11] += stmp*ax[11]; 425 ay[12] += stmp*ax[12]; 426 ay[13] += stmp*ax[13]; 427 ay[14] += stmp*ax[14]; 428 } 429 l = ipvt[k-1]; 430 if (l != k) { 431 ax = &a[k3 + 1]; 432 ay = &a[15*l + 1]; 433 stmp = ax[0]; ax[0] = ay[0]; ay[0] = stmp; 434 stmp = ax[1]; ax[1] = ay[1]; ay[1] = stmp; 435 stmp = ax[2]; ax[2] = ay[2]; ay[2] = stmp; 436 stmp = ax[3]; ax[3] = ay[3]; ay[3] = stmp; 437 stmp = ax[4]; ax[4] = ay[4]; ay[4] = stmp; 438 stmp = ax[5]; ax[5] = ay[5]; ay[5] = stmp; 439 stmp = ax[6]; ax[6] = ay[6]; ay[6] = stmp; 440 stmp = ax[7]; ax[7] = ay[7]; ay[7] = stmp; 441 stmp = ax[8]; ax[8] = ay[8]; ay[8] = stmp; 442 stmp = ax[9]; ax[9] = ay[9]; ay[9] = stmp; 443 stmp = ax[10]; ax[10] = ay[10]; ay[10] = stmp; 444 stmp = ax[11]; ax[11] = ay[11]; ay[11] = stmp; 445 stmp = ax[12]; ax[12] = ay[12]; ay[12] = stmp; 446 stmp = ax[13]; ax[13] = ay[13]; ay[13] = stmp; 447 stmp = ax[14]; ax[14] = ay[14]; ay[14] = stmp; 448 } 449 } 450 PetscFunctionReturn(0); 451 } 452