Invert matrix according LDU decomposition.
76 {
78
79 size_t i, row, col,
root;
80
81 double* p0;
83 double* p2;
84 double* p3;
85
86 const double* c0;
87
88 double v0;
89 double v1;
90 double v2;
91 double v3;
92
93 double w0;
94
95 P.resize(this->
size());
96
97
98
99 for (root = 0;
root != this->
size(); ++root) {
100
103
104 for (row =
root; ++row != this->
size(); ) {
105 if (fabs((*
this)(row,
root)) > fabs(value)) {
106 value = (*this)(row,
root);
107 pivot = row;
108 }
109 }
110
111 if (value == 0.0) {
112 THROW(JDivisionByZero,
"LDU decomposition zero pivot at " <<
root);
113 }
114
116 this->
rswap(root, pivot);
117 }
118
120
121 for (row =
root + 1; row + 4 <= this->
size(); row += 4) {
122
123 p0 = (*this)[row + 0] +
root;
124 p1 = (*this)[row + 1] +
root;
125 p2 = (*this)[row + 2] +
root;
126 p3 = (*this)[row + 3] +
root;
127
128 v0 = (*p0++ /= value);
129 v1 = (*
p1++ /= value);
130 v2 = (*p2++ /= value);
131 v3 = (*p3++ /= value);
132
134
135 for (i =
root + 1; i + 4 <= this->
size(); i += 4, p0 += 4,
p1 += 4, p2 += 4, p3 += 4, c0 += 4) {
136 p0[0] -= v0 * c0[0]; p0[1] -= v0 * c0[1]; p0[2] -= v0 * c0[2]; p0[3] -= v0 * c0[3];
137 p1[0] -= v1 * c0[0];
p1[1] -= v1 * c0[1];
p1[2] -= v1 * c0[2];
p1[3] -= v1 * c0[3];
138 p2[0] -= v2 * c0[0]; p2[1] -= v2 * c0[1]; p2[2] -= v2 * c0[2]; p2[3] -= v2 * c0[3];
139 p3[0] -= v3 * c0[0]; p3[1] -= v3 * c0[1]; p3[2] -= v3 * c0[2]; p3[3] -= v3 * c0[3];
140 }
141
142 for ( ; i != this->
size(); ++i, ++p0, ++
p1, ++p2, ++p3, ++c0) {
143 *p0 -= v0 * (*c0);
145 *p2 -= v2 * (*c0);
146 *p3 -= v3 * (*c0);
147 }
148 }
149
150 for ( ; row != this->
size(); ++row) {
151
152 p0 = (*this)[row + 0] +
root;
153
154 v0 = (*p0++ /= value);
155
157
158 for (i =
root + 1; i + 4 <= this->
size(); i += 4, p0 += 4, c0 += 4) {
159 *p0 -= v0 * c0[0]; p0[1] -= v0 * c0[1]; p0[2] -= v0 * c0[2]; p0[3] -= v0 * c0[3];
160 }
161
162 for ( ; i != this->
size(); ++i, ++p0, ++c0) {
163 *p0 -= v0 * (*c0);
164 }
165 }
166 }
167
168
170
171 A.reset();
172
173 for (i = 0; i != this->
size(); ++i) {
174 A(i,i) = 1.0;
175 }
176
177
178
179
180 col = 0;
181
182 for ( ; col + 4 <= this->
size(); col += 4) {
183
184 for (row = 0; row != this->
size(); ++row) {
185
186 p0 = A[col + 0];
188 p2 = A[col + 2];
189 p3 = A[col + 3];
190
192
193 v0 = p0[i];
195 v2 = p2[i];
196 v3 = p3[i];
197
198 p0[i] = p0[row];
200 p2[i] = p2[row];
201 p3[i] = p3[row];
202
203 for (i = 0, c0 = (*
this)[row]; i != row; ++c0, ++p0, ++
p1, ++p2, ++p3, ++i) {
204 v0 -= (*c0) * (*p0);
205 v1 -= (*c0) * (*p1);
206 v2 -= (*c0) * (*p2);
207 v3 -= (*c0) * (*p3);
208 }
209
210 A[col + 0][row] = v0;
211 A[col + 1][row] = v1;
212 A[col + 2][row] = v2;
213 A[col + 3][row] = v3;
214 }
215 }
216
217 for ( ; col != this->
size(); ++col) {
218
219 for (row = 0; row != this->
size(); ++row) {
220
221 p0 = A[col];
222
224
225 v0 = p0[i];
226
227 p0[i] = p0[row];
228
229 for (i = 0, c0 = (*this)[row] + i; i != row; ++c0, ++p0, ++i) {
230 v0 -= (*c0) * (*p0);
231 }
232
233 A[col][row] = v0;
234 }
235 }
236
237
238
239 col = 0;
240
241 for ( ; col + 4 <= this->
size(); col += 4) {
242
243 for (
int row = this->
size() - 1; row >= 0; --row) {
244
245 p0 = A[col + 0] + row;
246 p1 = A[col + 1] + row;
247 p2 = A[col + 2] + row;
248 p3 = A[col + 3] + row;
249
250 v0 = *p0;
252 v2 = *p2;
253 v3 = *p3;
254
255 ++p0;
257 ++p2;
258 ++p3;
259
260 for (i = row + 1, c0 = (*
this)[row] + i; i != this->
size(); ++c0, ++p0, ++
p1, ++p2, ++p3, ++i) {
261 v0 -= (*c0) * (*p0);
262 v1 -= (*c0) * (*p1);
263 v2 -= (*c0) * (*p2);
264 v3 -= (*c0) * (*p3);
265 }
266
267 w0 = 1.0 / (*this)[row][row];
268
269 A[col + 0][row] = v0 * w0;
270 A[col + 1][row] = v1 * w0;
271 A[col + 2][row] = v2 * w0;
272 A[col + 3][row] = v3 * w0;
273 }
274 }
275
276 for ( ; col != this->
size(); ++col) {
277
278 for (
int row = this->
size() - 1; row >= 0; --row) {
279
280 p0 = A[col] + row;
281
282 v0 = *p0;
283
284 ++p0;
285
286 for (i = row + 1, c0 = (*
this)[row] + i; i != this->
size(); ++c0, ++p0, ++i) {
287 v0 -= (*c0) * (*p0);
288 }
289
290 w0 = 1.0 / (*this)[row][row];
291
292 A[col][row] = v0 * w0;
293 }
294 }
295
296 A.transpose();
297
299 }
#define THROW(JException_t, A)
Marco for throwing exception with std::ostream compatible message.
void swap(JMatrixND_t &A)
Swap matrices.
JMatrixND_t()
Default constructor.
JMatrixND_t & getInstance()
Get work space.
void rswap(size_t r1, size_t r2)
std::vector< int > P
permutation matrix