Invert matrix according LDU decomposition.
84 size_t i, row, col,
root;
121 for (row = root; ++row != this->
size(); ) {
122 if (fabs((*
this)(row, root)) > fabs(value)) {
123 value = (*this)(row,
root);
129 THROW(JDivisionByZero,
"LDU decomposition zero pivot at " << root);
134 this->
rswap(root, pivot);
136 P.push_back(make_pair(root, pivot));
139 for (row = root + 1; row + 4 <= this->
size(); row += 4) {
141 p0 = (*this)[row + 0] +
root;
142 p1 = (*this)[row + 1] +
root;
143 p2 = (*this)[row + 2] +
root;
144 p3 = (*this)[row + 3] +
root;
146 v0 = (*p0++ /= value);
147 v1 = (*
p1++ /= value);
148 v2 = (*
p2++ /= value);
149 v3 = (*
p3++ /= value);
151 c0 = (*this)[
root] + root + 1;
153 for (i = root + 1; i + 4 <= this->
size(); i += 4, p0 += 4,
p1 += 4,
p2 += 4,
p3 += 4, c0 += 4) {
154 p0[0] -= v0 * c0[0]; p0[1] -= v0 * c0[1]; p0[2] -= v0 * c0[2]; p0[3] -= v0 * c0[3];
155 p1[0] -= v1 * c0[0];
p1[1] -= v1 * c0[1];
p1[2] -= v1 * c0[2];
p1[3] -= v1 * c0[3];
156 p2[0] -= v2 * c0[0];
p2[1] -= v2 * c0[1];
p2[2] -= v2 * c0[2];
p2[3] -= v2 * c0[3];
157 p3[0] -= v3 * c0[0];
p3[1] -= v3 * c0[1];
p3[2] -= v3 * c0[2];
p3[3] -= v3 * c0[3];
160 for ( ; i != this->
size(); ++i, ++p0, ++
p1, ++
p2, ++
p3, ++c0) {
168 for ( ; row != this->
size(); ++row) {
170 p0 = (*this)[row + 0] +
root;
172 v0 = (*p0++ /= value);
174 c0 = (*this)[
root] + root + 1;
176 for (i = root + 1; i + 4 <= this->
size(); i += 4, p0 += 4, c0 += 4) {
177 *p0 -= v0 * c0[0]; p0[1] -= v0 * c0[1]; p0[2] -= v0 * c0[2]; p0[3] -= v0 * c0[3];
180 for ( ; i != this->
size(); ++i, ++p0, ++c0) {
189 for (
size_t i = 0; i != this->
size(); ++i) {
191 double& pivot = (*this)(i,i);
194 THROW(JDivisionByZero,
"Invert D zero pivot at " << i);
208 for (row = 0; row + 4 <= this->
size(); row += 4) {
210 p0 = (*this)[row + 0] + row + 0;
211 p1 = (*this)[row + 1] + row + 1;
212 p2 = (*this)[row + 2] + row + 2;
213 p3 = (*this)[row + 3] + row + 3;
220 for (col = row + 1; col + 4 <= this->
size(); ++col, ++p0, ++
p1, ++
p2, ++
p3) {
227 c0 = (*this)[row + 0] + row + 0 + 1;
228 c1 = (*this)[row + 1] + row + 1 + 1;
229 c2 = (*this)[row + 2] + row + 2 + 1;
230 c3 = (*this)[row + 3] + row + 3 + 1;
232 a0 =
A[col + 0] + row + 0 + 1;
233 a1 =
A[col + 1] + row + 1 + 1;
234 a2 =
A[col + 2] + row + 2 + 1;
235 a3 =
A[col + 3] + row + 3 + 1;
237 for ( ; c0 + 4 < p0; c0 += 4,
c1 += 4, c2 += 4, c3 += 4, a0 += 4, a1 += 4, a2 += 4, a3 += 4) {
238 w0 -= c0[0]*a0[0] + c0[1]*a0[1] + c0[2]*a0[2] + c0[3]*a0[3];
239 w1 -=
c1[0]*a1[0] +
c1[1]*a1[1] +
c1[2]*a1[2] +
c1[3]*a1[3];
240 w2 -= c2[0]*a2[0] + c2[1]*a2[1] + c2[2]*a2[2] + c2[3]*a2[3];
241 w3 -= c3[0]*a3[0] + c3[1]*a3[1] + c3[2]*a3[2] + c3[3]*a3[3];
244 for ( ; c0 != p0; ++c0, ++
c1, ++c2, ++c3, ++a0, ++a1, ++a2, ++a3) {
251 *p0 = w0 * (*this)(col + 0, col + 0);
252 *p1 = w1 * (*this)(col + 1, col + 1);
253 *p2 = w2 * (*this)(col + 2, col + 2);
254 *
p3 = w3 * (*this)(col + 3, col + 3);
263 c0 = (*this)[row + 0] + row + 0 + 1;
264 c1 = (*this)[row + 1] + row + 1 + 1;
265 c2 = (*this)[row + 2] + row + 2 + 1;
267 a0 =
A[col + 0] + row + 0 + 1;
268 a1 =
A[col + 1] + row + 1 + 1;
269 a2 =
A[col + 2] + row + 2 + 1;
271 for ( ; c0 + 4 < p0; c0 += 4,
c1 += 4, c2 += 4, c3 += 4, a0 += 4, a1 += 4, a2 += 4, a3 += 4) {
272 w0 -= c0[0]*a0[0] + c0[1]*a0[1] + c0[2]*a0[2] + c0[3]*a0[3];
273 w1 -=
c1[0]*a1[0] +
c1[1]*a1[1] +
c1[2]*a1[2] +
c1[3]*a1[3];
274 w2 -= c2[0]*a2[0] + c2[1]*a2[1] + c2[2]*a2[2] + c2[3]*a2[3];
277 for ( ; c0 != p0; ++c0, ++
c1, ++c2, ++a0, ++a1, ++a2) {
283 *p0 = w0 * (*this)(col + 0, col + 0);
284 *p1 = w1 * (*this)(col + 1, col + 1);
285 *p2 = w2 * (*this)(col + 2, col + 2);
294 c0 = (*this)[row + 0] + row + 0 + 1;
295 c1 = (*this)[row + 1] + row + 1 + 1;
297 a0 =
A[col + 0] + row + 0 + 1;
298 a1 =
A[col + 1] + row + 1 + 1;
300 for ( ; c0 + 4 < p0; c0 += 4, c1 += 4, c2 += 4, c3 += 4, a0 += 4, a1 += 4, a2 += 4, a3 += 4) {
301 w0 -= c0[0]*a0[0] + c0[1]*a0[1] + c0[2]*a0[2] + c0[3]*a0[3];
302 w1 -= c1[0]*a1[0] + c1[1]*a1[1] + c1[2]*a1[2] + c1[3]*a1[3];
305 for ( ; c0 != p0; ++c0, ++
c1, ++a0, ++a1) {
310 *p0 = w0 * (*this)(col + 0, col + 0);
311 *p1 = w1 * (*this)(col + 1, col + 1);
319 c0 = (*this)[row + 0] + row + 0 + 1;
321 a0 =
A[col + 0] + row + 0 + 1;
323 for ( ; c0 + 4 < p0; c0 += 4, c1 += 4, c2 += 4, c3 += 4, a0 += 4, a1 += 4, a2 += 4, a3 += 4) {
324 w0 -= c0[0]*a0[0] + c0[1]*a0[1] + c0[2]*a0[2] + c0[3]*a0[3];
327 for ( ; c0 != p0; ++c0, ++a0) {
331 *p0 = w0 * (*this)(col + 0, col + 0);
334 for ( ; row != this->
size(); ++row) {
336 p0 = (*this)[row + 0] + row + 0;
340 for (col = row + 1; col != this->
size(); ++col, ++p0) {
346 c0 = (*this)[row + 0] + row + 1;
348 a0 =
A[col + 0] + row + 0 + 1;
350 for ( ; c0 != p0; ++c0, ++a0) {
354 *p0 = w0 * (*this)(col + 0, col + 0);
361 double* buffer =
A.data();
363 for (col = this->
size() - 1; col != 0; --col) {
365 for (i = col; i < this->
size(); ++i) {
367 buffer[i] = (*this)(i, col - 1);
369 (*this)(i, col - 1) = 0.0;
372 for (row = 0; row + 4 < this->
size(); row += 4) {
374 p0 = (*this)[row + 0] + col - 1;
375 p1 = (*this)[row + 1] + col - 1;
376 p2 = (*this)[row + 2] + col - 1;
377 p3 = (*this)[row + 3] + col - 1;
391 for (i = col; i + 4 <= this->
size(); i += 4, c0 += 4, c1 += 4, c2 += 4, c3 += 4, a0 += 4) {
392 w0 += c0[0]*a0[0] + c0[1]*a0[1] + c0[2]*a0[2] + c0[3]*a0[3];
393 w1 += c1[0]*a0[0] + c1[1]*a0[1] + c1[2]*a0[2] + c1[3]*a0[3];
394 w2 += c2[0]*a0[0] + c2[1]*a0[1] + c2[2]*a0[2] + c2[3]*a0[3];
395 w3 += c3[0]*a0[0] + c3[1]*a0[1] + c3[2]*a0[2] + c3[3]*a0[3];
398 for ( ; i != this->
size(); ++i, ++c0, ++
c1, ++c2, ++c3, ++a0) {
411 for ( ; row < this->
size(); ++row) {
413 p0 = (*this)[row] + col - 1;
421 for (i = col; i + 4 <= this->
size(); i += 4, c0 += 4, c1 += 4, c2 += 4, c3 += 4, a0 += 4) {
422 w0 += c0[0]*a0[0] + c0[1]*a0[1] + c0[2]*a0[2] + c0[3]*a0[3];
425 for ( ; i != this->
size(); ++i, ++c0, ++a0) {
434 for (JPermutationMatrix::reverse_iterator p =
P.rbegin(); p !=
P.rend(); ++p) {
435 cswap(p->first, p->second);
#define THROW(JException_t, A)
Marco for throwing exception with std::ostream compatible message.
JMatrixND_t & getInstance()
Get work space.
void rswap(size_t r1, size_t r2)
size_t size() const
Get dimension of matrix.
TCanvas * c1
Global variables to handle mouse events.
do JPlot2D f $WORKDIR detector root
std::vector< std::pair< size_t, size_t > > JPermutationMatrix
Type definition of permutation matrix.
void cswap(size_t c1, size_t c2)
Swap columns.
source $JPP_DIR setenv csh $JPP_DIR &dev null eval JShellParser o a A