1 /*
2 Inverts 6 by 6 matrix using gaussian elimination with partial pivoting.
3
4 Used by the sparse factorization routines in
5 src/mat/impls/baij/seq
6
7 This is a combination of the Linpack routines
8 dgefa() and dgedi() specialized for a size of 6.
9
10 */
11 #include <petscsys.h>
12 #include <petsc/private/kernels/blockinvert.h>
13
PetscKernel_A_gets_inverse_A_6(MatScalar * a,PetscReal shift,PetscBool allowzeropivot,PetscBool * zeropivotdetected)14 PetscErrorCode PetscKernel_A_gets_inverse_A_6(MatScalar *a, PetscReal shift, PetscBool allowzeropivot, PetscBool *zeropivotdetected)
15 {
16 PetscInt i__2, i__3, kp1, j, k, l, ll, i, ipvt[6], kb, k3;
17 PetscInt k4, j3;
18 MatScalar *aa, *ax, *ay, work[36], stmp;
19 MatReal tmp, max;
20
21 PetscFunctionBegin;
22 if (zeropivotdetected) *zeropivotdetected = PETSC_FALSE;
23 shift = .25 * shift * (1.e-12 + PetscAbsScalar(a[0]) + PetscAbsScalar(a[7]) + PetscAbsScalar(a[14]) + PetscAbsScalar(a[21]) + PetscAbsScalar(a[28]) + PetscAbsScalar(a[35]));
24
25 /* Parameter adjustments */
26 a -= 7;
27
28 for (k = 1; k <= 5; ++k) {
29 kp1 = k + 1;
30 k3 = 6 * k;
31 k4 = k3 + k;
32
33 /* find l = pivot index */
34 i__2 = 7 - k;
35 aa = &a[k4];
36 max = PetscAbsScalar(aa[0]);
37 l = 1;
38 for (ll = 1; ll < i__2; ll++) {
39 tmp = PetscAbsScalar(aa[ll]);
40 if (tmp > max) {
41 max = tmp;
42 l = ll + 1;
43 }
44 }
45 l += k - 1;
46 ipvt[k - 1] = l;
47
48 if (a[l + k3] == 0.0) {
49 if (shift == 0.0) {
50 PetscCheck(allowzeropivot, PETSC_COMM_SELF, PETSC_ERR_MAT_LU_ZRPVT, "Zero pivot, row %" PetscInt_FMT, k - 1);
51 PetscCall(PetscInfo(NULL, "Zero pivot, row %" PetscInt_FMT "\n", k - 1));
52 if (zeropivotdetected) *zeropivotdetected = PETSC_TRUE;
53 } else {
54 /* SHIFT is applied to SINGLE diagonal entry; does this make any sense? */
55 a[l + k3] = shift;
56 }
57 }
58
59 /* interchange if necessary */
60 if (l != k) {
61 stmp = a[l + k3];
62 a[l + k3] = a[k4];
63 a[k4] = stmp;
64 }
65
66 /* compute multipliers */
67 stmp = -1. / a[k4];
68 i__2 = 6 - k;
69 aa = &a[1 + k4];
70 for (ll = 0; ll < i__2; ll++) aa[ll] *= stmp;
71
72 /* row elimination with column indexing */
73 ax = &a[k4 + 1];
74 for (j = kp1; j <= 6; ++j) {
75 j3 = 6 * j;
76 stmp = a[l + j3];
77 if (l != k) {
78 a[l + j3] = a[k + j3];
79 a[k + j3] = stmp;
80 }
81
82 i__3 = 6 - k;
83 ay = &a[1 + k + j3];
84 for (ll = 0; ll < i__3; ll++) ay[ll] += stmp * ax[ll];
85 }
86 }
87 ipvt[5] = 6;
88 if (a[42] == 0.0) {
89 PetscCheck(PetscLikely(allowzeropivot), PETSC_COMM_SELF, PETSC_ERR_MAT_LU_ZRPVT, "Zero pivot, row 5");
90 PetscCall(PetscInfo(NULL, "Zero pivot, row 5\n"));
91 if (zeropivotdetected) *zeropivotdetected = PETSC_TRUE;
92 }
93
94 /* Now form the inverse */
95 /* compute inverse(u) */
96 for (k = 1; k <= 6; ++k) {
97 k3 = 6 * k;
98 k4 = k3 + k;
99 a[k4] = 1.0 / a[k4];
100 stmp = -a[k4];
101 i__2 = k - 1;
102 aa = &a[k3 + 1];
103 for (ll = 0; ll < i__2; ll++) aa[ll] *= stmp;
104 kp1 = k + 1;
105 if (6 < kp1) continue;
106 ax = aa;
107 for (j = kp1; j <= 6; ++j) {
108 j3 = 6 * j;
109 stmp = a[k + j3];
110 a[k + j3] = 0.0;
111 ay = &a[j3 + 1];
112 for (ll = 0; ll < k; ll++) ay[ll] += stmp * ax[ll];
113 }
114 }
115
116 /* form inverse(u)*inverse(l) */
117 for (kb = 1; kb <= 5; ++kb) {
118 k = 6 - kb;
119 k3 = 6 * k;
120 kp1 = k + 1;
121 aa = a + k3;
122 for (i = kp1; i <= 6; ++i) {
123 work[i - 1] = aa[i];
124 aa[i] = 0.0;
125 }
126 for (j = kp1; j <= 6; ++j) {
127 stmp = work[j - 1];
128 ax = &a[6 * j + 1];
129 ay = &a[k3 + 1];
130 ay[0] += stmp * ax[0];
131 ay[1] += stmp * ax[1];
132 ay[2] += stmp * ax[2];
133 ay[3] += stmp * ax[3];
134 ay[4] += stmp * ax[4];
135 ay[5] += stmp * ax[5];
136 }
137 l = ipvt[k - 1];
138 if (l != k) {
139 ax = &a[k3 + 1];
140 ay = &a[6 * l + 1];
141 stmp = ax[0];
142 ax[0] = ay[0];
143 ay[0] = stmp;
144 stmp = ax[1];
145 ax[1] = ay[1];
146 ay[1] = stmp;
147 stmp = ax[2];
148 ax[2] = ay[2];
149 ay[2] = stmp;
150 stmp = ax[3];
151 ax[3] = ay[3];
152 ay[3] = stmp;
153 stmp = ax[4];
154 ax[4] = ay[4];
155 ay[4] = stmp;
156 stmp = ax[5];
157 ax[5] = ay[5];
158 ay[5] = stmp;
159 }
160 }
161 PetscFunctionReturn(PETSC_SUCCESS);
162 }
163