Changeset 4068 for trunk/sources/HeuristicLab.ExtLibs/HeuristicLab.ALGLIB/2.5.0/ALGLIB-2.5.0/ssolve.cs
- Timestamp:
- 07/22/10 00:44:01 (14 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/sources/HeuristicLab.ExtLibs/HeuristicLab.ALGLIB/2.5.0/ALGLIB-2.5.0/ssolve.cs
r3839 r4068 25 25 *************************************************************************/ 26 26 27 using System; 28 29 namespace alglib 30 { 31 public class ssolve 32 { 33 /************************************************************************* 34 Solving a system of linear equations with a system matrix given by its 35 LDLT decomposition 36 37 The algorithm solves systems with a square matrix only. 38 39 Input parameters: 40 A - LDLT decomposition of the matrix (the result of the 41 SMatrixLDLT subroutine). 42 Pivots - row permutation table (the result of the SMatrixLDLT subroutine). 43 B - right side of a system. 44 Array whose index ranges within [0..N-1]. 45 N - size of matrix A. 46 IsUpper - points to the triangle of matrix A in which the LDLT 47 decomposition is stored. 48 If IsUpper=True, the decomposition has the form of U*D*U', 49 matrix U is stored in the upper triangle of matrix A (in 50 that case, the lower triangle isn't used and isn't changed 51 by the subroutine). 52 Similarly, if IsUpper=False, the decomposition has the form 53 of L*D*L' and the lower triangle stores matrix L. 54 55 Output parameters: 56 X - solution of a system. 57 Array whose index ranges within [0..N-1]. 58 59 Result: 60 True, if the matrix is not singular. X contains the solution. 61 False, if the matrix is singular (the determinant of matrix D is equal 62 to 0). In this case, X doesn't contain a solution. 63 *************************************************************************/ 64 public static bool smatrixldltsolve(ref double[,] a, 65 ref int[] pivots, 66 double[] b, 67 int n, 68 bool isupper, 69 ref double[] x) 70 { 71 bool result = new bool(); 72 int i = 0; 73 int k = 0; 74 int kp = 0; 75 double ak = 0; 76 double akm1 = 0; 77 double akm1k = 0; 78 double bk = 0; 79 double bkm1 = 0; 80 double denom = 0; 81 double v = 0; 82 int i_ = 0; 83 84 b = (double[])b.Clone(); 85 86 87 // 88 // Quick return if possible 89 // 90 result = true; 91 if( n==0 ) 92 { 93 return result; 94 } 95 96 // 97 // Check that the diagonal matrix D is nonsingular 98 // 99 if( isupper ) 100 { 101 102 // 103 // Upper triangular storage: examine D from bottom to top 104 // 105 for(i=n-1; i>=0; i--) 106 { 107 if( pivots[i]>=0 & (double)(a[i,i])==(double)(0) ) 108 { 109 result = false; 110 return result; 111 } 112 } 113 } 114 else 115 { 116 117 // 118 // Lower triangular storage: examine D from top to bottom. 119 // 120 for(i=0; i<=n-1; i++) 121 { 122 if( pivots[i]>=0 & (double)(a[i,i])==(double)(0) ) 123 { 124 result = false; 125 return result; 126 } 127 } 128 } 129 130 // 131 // Solve Ax = b 132 // 133 if( isupper ) 134 { 135 136 // 137 // Solve A*X = B, where A = U*D*U'. 138 // 139 // First solve U*D*X = B, overwriting B with X. 140 // 141 // K+1 is the main loop index, decreasing from N to 1 in steps of 142 // 1 or 2, depending on the size of the diagonal blocks. 143 // 144 k = n-1; 145 while( k>=0 ) 146 { 147 if( pivots[k]>=0 ) 148 { 149 150 // 151 // 1 x 1 diagonal block 152 // 153 // Interchange rows K+1 and IPIV(K+1). 154 // 155 kp = pivots[k]; 156 if( kp!=k ) 157 { 158 v = b[k]; 159 b[k] = b[kp]; 160 b[kp] = v; 161 } 162 163 // 164 // Multiply by inv(U(K+1)), where U(K+1) is the transformation 165 // stored in column K+1 of A. 166 // 167 v = b[k]; 168 for(i_=0; i_<=k-1;i_++) 169 { 170 b[i_] = b[i_] - v*a[i_,k]; 171 } 172 173 // 174 // Multiply by the inverse of the diagonal block. 175 // 176 b[k] = b[k]/a[k,k]; 177 k = k-1; 178 } 179 else 180 { 181 182 // 183 // 2 x 2 diagonal block 184 // 185 // Interchange rows K+1-1 and -IPIV(K+1). 186 // 187 kp = pivots[k]+n; 188 if( kp!=k-1 ) 189 { 190 v = b[k-1]; 191 b[k-1] = b[kp]; 192 b[kp] = v; 193 } 194 195 // 196 // Multiply by inv(U(K+1)), where U(K+1) is the transformation 197 // stored in columns K+1-1 and K+1 of A. 198 // 199 v = b[k]; 200 for(i_=0; i_<=k-2;i_++) 201 { 202 b[i_] = b[i_] - v*a[i_,k]; 203 } 204 v = b[k-1]; 205 for(i_=0; i_<=k-2;i_++) 206 { 207 b[i_] = b[i_] - v*a[i_,k-1]; 208 } 209 210 // 211 // Multiply by the inverse of the diagonal block. 212 // 213 akm1k = a[k-1,k]; 214 akm1 = a[k-1,k-1]/akm1k; 215 ak = a[k,k]/akm1k; 216 denom = akm1*ak-1; 217 bkm1 = b[k-1]/akm1k; 218 bk = b[k]/akm1k; 219 b[k-1] = (ak*bkm1-bk)/denom; 220 b[k] = (akm1*bk-bkm1)/denom; 221 k = k-2; 222 } 223 } 224 225 // 226 // Next solve U'*X = B, overwriting B with X. 227 // 228 // K+1 is the main loop index, increasing from 1 to N in steps of 229 // 1 or 2, depending on the size of the diagonal blocks. 230 // 231 k = 0; 232 while( k<=n-1 ) 233 { 234 if( pivots[k]>=0 ) 235 { 236 237 // 238 // 1 x 1 diagonal block 239 // 240 // Multiply by inv(U'(K+1)), where U(K+1) is the transformation 241 // stored in column K+1 of A. 242 // 243 v = 0.0; 244 for(i_=0; i_<=k-1;i_++) 245 { 246 v += b[i_]*a[i_,k]; 247 } 248 b[k] = b[k]-v; 249 250 // 251 // Interchange rows K+1 and IPIV(K+1). 252 // 253 kp = pivots[k]; 254 if( kp!=k ) 255 { 256 v = b[k]; 257 b[k] = b[kp]; 258 b[kp] = v; 259 } 260 k = k+1; 261 } 262 else 263 { 264 265 // 266 // 2 x 2 diagonal block 267 // 268 // Multiply by inv(U'(K+1+1)), where U(K+1+1) is the transformation 269 // stored in columns K+1 and K+1+1 of A. 270 // 271 v = 0.0; 272 for(i_=0; i_<=k-1;i_++) 273 { 274 v += b[i_]*a[i_,k]; 275 } 276 b[k] = b[k]-v; 277 v = 0.0; 278 for(i_=0; i_<=k-1;i_++) 279 { 280 v += b[i_]*a[i_,k+1]; 281 } 282 b[k+1] = b[k+1]-v; 283 284 // 285 // Interchange rows K+1 and -IPIV(K+1). 286 // 287 kp = pivots[k]+n; 288 if( kp!=k ) 289 { 290 v = b[k]; 291 b[k] = b[kp]; 292 b[kp] = v; 293 } 294 k = k+2; 295 } 296 } 297 } 298 else 299 { 300 301 // 302 // Solve A*X = B, where A = L*D*L'. 303 // 304 // First solve L*D*X = B, overwriting B with X. 305 // 306 // K+1 is the main loop index, increasing from 1 to N in steps of 307 // 1 or 2, depending on the size of the diagonal blocks. 308 // 309 k = 0; 310 while( k<=n-1 ) 311 { 312 if( pivots[k]>=0 ) 313 { 314 315 // 316 // 1 x 1 diagonal block 317 // 318 // Interchange rows K+1 and IPIV(K+1). 319 // 320 kp = pivots[k]; 321 if( kp!=k ) 322 { 323 v = b[k]; 324 b[k] = b[kp]; 325 b[kp] = v; 326 } 327 328 // 329 // Multiply by inv(L(K+1)), where L(K+1) is the transformation 330 // stored in column K+1 of A. 331 // 332 if( k+1<n ) 333 { 334 v = b[k]; 335 for(i_=k+1; i_<=n-1;i_++) 336 { 337 b[i_] = b[i_] - v*a[i_,k]; 338 } 339 } 340 341 // 342 // Multiply by the inverse of the diagonal block. 343 // 344 b[k] = b[k]/a[k,k]; 345 k = k+1; 346 } 347 else 348 { 349 350 // 351 // 2 x 2 diagonal block 352 // 353 // Interchange rows K+1+1 and -IPIV(K+1). 354 // 355 kp = pivots[k]+n; 356 if( kp!=k+1 ) 357 { 358 v = b[k+1]; 359 b[k+1] = b[kp]; 360 b[kp] = v; 361 } 362 363 // 364 // Multiply by inv(L(K+1)), where L(K+1) is the transformation 365 // stored in columns K+1 and K+1+1 of A. 366 // 367 if( k+1<n-1 ) 368 { 369 v = b[k]; 370 for(i_=k+2; i_<=n-1;i_++) 371 { 372 b[i_] = b[i_] - v*a[i_,k]; 373 } 374 v = b[k+1]; 375 for(i_=k+2; i_<=n-1;i_++) 376 { 377 b[i_] = b[i_] - v*a[i_,k+1]; 378 } 379 } 380 381 // 382 // Multiply by the inverse of the diagonal block. 383 // 384 akm1k = a[k+1,k]; 385 akm1 = a[k,k]/akm1k; 386 ak = a[k+1,k+1]/akm1k; 387 denom = akm1*ak-1; 388 bkm1 = b[k]/akm1k; 389 bk = b[k+1]/akm1k; 390 b[k] = (ak*bkm1-bk)/denom; 391 b[k+1] = (akm1*bk-bkm1)/denom; 392 k = k+2; 393 } 394 } 395 396 // 397 // Next solve L'*X = B, overwriting B with X. 398 // 399 // K+1 is the main loop index, decreasing from N to 1 in steps of 400 // 1 or 2, depending on the size of the diagonal blocks. 401 // 402 k = n-1; 403 while( k>=0 ) 404 { 405 if( pivots[k]>=0 ) 406 { 407 408 // 409 // 1 x 1 diagonal block 410 // 411 // Multiply by inv(L'(K+1)), where L(K+1) is the transformation 412 // stored in column K+1 of A. 413 // 414 if( k+1<n ) 415 { 416 v = 0.0; 417 for(i_=k+1; i_<=n-1;i_++) 418 { 419 v += b[i_]*a[i_,k]; 420 } 421 b[k] = b[k]-v; 422 } 423 424 // 425 // Interchange rows K+1 and IPIV(K+1). 426 // 427 kp = pivots[k]; 428 if( kp!=k ) 429 { 430 v = b[k]; 431 b[k] = b[kp]; 432 b[kp] = v; 433 } 434 k = k-1; 435 } 436 else 437 { 438 439 // 440 // 2 x 2 diagonal block 441 // 442 // Multiply by inv(L'(K+1-1)), where L(K+1-1) is the transformation 443 // stored in columns K+1-1 and K+1 of A. 444 // 445 if( k+1<n ) 446 { 447 v = 0.0; 448 for(i_=k+1; i_<=n-1;i_++) 449 { 450 v += b[i_]*a[i_,k]; 451 } 452 b[k] = b[k]-v; 453 v = 0.0; 454 for(i_=k+1; i_<=n-1;i_++) 455 { 456 v += b[i_]*a[i_,k-1]; 457 } 458 b[k-1] = b[k-1]-v; 459 } 460 461 // 462 // Interchange rows K+1 and -IPIV(K+1). 463 // 464 kp = pivots[k]+n; 465 if( kp!=k ) 466 { 467 v = b[k]; 468 b[k] = b[kp]; 469 b[kp] = v; 470 } 471 k = k-2; 472 } 473 } 474 } 475 x = new double[n-1+1]; 476 for(i_=0; i_<=n-1;i_++) 477 { 478 x[i_] = b[i_]; 479 } 27 28 namespace alglib { 29 public class ssolve { 30 /************************************************************************* 31 Solving a system of linear equations with a system matrix given by its 32 LDLT decomposition 33 34 The algorithm solves systems with a square matrix only. 35 36 Input parameters: 37 A - LDLT decomposition of the matrix (the result of the 38 SMatrixLDLT subroutine). 39 Pivots - row permutation table (the result of the SMatrixLDLT subroutine). 40 B - right side of a system. 41 Array whose index ranges within [0..N-1]. 42 N - size of matrix A. 43 IsUpper - points to the triangle of matrix A in which the LDLT 44 decomposition is stored. 45 If IsUpper=True, the decomposition has the form of U*D*U', 46 matrix U is stored in the upper triangle of matrix A (in 47 that case, the lower triangle isn't used and isn't changed 48 by the subroutine). 49 Similarly, if IsUpper=False, the decomposition has the form 50 of L*D*L' and the lower triangle stores matrix L. 51 52 Output parameters: 53 X - solution of a system. 54 Array whose index ranges within [0..N-1]. 55 56 Result: 57 True, if the matrix is not singular. X contains the solution. 58 False, if the matrix is singular (the determinant of matrix D is equal 59 to 0). In this case, X doesn't contain a solution. 60 *************************************************************************/ 61 public static bool smatrixldltsolve(ref double[,] a, 62 ref int[] pivots, 63 double[] b, 64 int n, 65 bool isupper, 66 ref double[] x) { 67 bool result = new bool(); 68 int i = 0; 69 int k = 0; 70 int kp = 0; 71 double ak = 0; 72 double akm1 = 0; 73 double akm1k = 0; 74 double bk = 0; 75 double bkm1 = 0; 76 double denom = 0; 77 double v = 0; 78 int i_ = 0; 79 80 b = (double[])b.Clone(); 81 82 83 // 84 // Quick return if possible 85 // 86 result = true; 87 if (n == 0) { 88 return result; 89 } 90 91 // 92 // Check that the diagonal matrix D is nonsingular 93 // 94 if (isupper) { 95 96 // 97 // Upper triangular storage: examine D from bottom to top 98 // 99 for (i = n - 1; i >= 0; i--) { 100 if (pivots[i] >= 0 & (double)(a[i, i]) == (double)(0)) { 101 result = false; 480 102 return result; 481 } 482 483 484 /************************************************************************* 485 Solving a system of linear equations with a symmetric system matrix 486 487 Input parameters: 488 A - system matrix (upper or lower triangle). 489 Array whose indexes range within [0..N-1, 0..N-1]. 490 B - right side of a system. 491 Array whose index ranges within [0..N-1]. 492 N - size of matrix A. 493 IsUpper - If IsUpper = True, A contains the upper triangle, 494 otherwise A contains the lower triangle. 495 496 Output parameters: 497 X - solution of a system. 498 Array whose index ranges within [0..N-1]. 499 500 Result: 501 True, if the matrix is not singular. X contains the solution. 502 False, if the matrix is singular (the determinant of the matrix is equal 503 to 0). In this case, X doesn't contain a solution. 504 505 -- ALGLIB -- 506 Copyright 2005 by Bochkanov Sergey 507 *************************************************************************/ 508 public static bool smatrixsolve(double[,] a, 509 ref double[] b, 510 int n, 511 bool isupper, 512 ref double[] x) 513 { 514 bool result = new bool(); 515 int[] pivots = new int[0]; 516 517 a = (double[,])a.Clone(); 518 519 ldlt.smatrixldlt(ref a, n, isupper, ref pivots); 520 result = smatrixldltsolve(ref a, ref pivots, b, n, isupper, ref x); 103 } 104 } 105 } else { 106 107 // 108 // Lower triangular storage: examine D from top to bottom. 109 // 110 for (i = 0; i <= n - 1; i++) { 111 if (pivots[i] >= 0 & (double)(a[i, i]) == (double)(0)) { 112 result = false; 521 113 return result; 522 } 523 524 525 public static bool solvesystemldlt(ref double[,] a, 526 ref int[] pivots, 527 double[] b, 528 int n, 529 bool isupper, 530 ref double[] x) 531 { 532 bool result = new bool(); 533 int i = 0; 534 int k = 0; 535 int kp = 0; 536 int km1 = 0; 537 int km2 = 0; 538 int kp1 = 0; 539 int kp2 = 0; 540 double ak = 0; 541 double akm1 = 0; 542 double akm1k = 0; 543 double bk = 0; 544 double bkm1 = 0; 545 double denom = 0; 546 double v = 0; 547 int i_ = 0; 548 549 b = (double[])b.Clone(); 550 551 552 // 553 // Quick return if possible 554 // 555 result = true; 556 if( n==0 ) 557 { 558 return result; 559 } 560 561 // 562 // Check that the diagonal matrix D is nonsingular 563 // 564 if( isupper ) 565 { 566 567 // 568 // Upper triangular storage: examine D from bottom to top 569 // 570 for(i=n; i>=1; i--) 571 { 572 if( pivots[i]>0 & (double)(a[i,i])==(double)(0) ) 573 { 574 result = false; 575 return result; 576 } 577 } 578 } 579 else 580 { 581 582 // 583 // Lower triangular storage: examine D from top to bottom. 584 // 585 for(i=1; i<=n; i++) 586 { 587 if( pivots[i]>0 & (double)(a[i,i])==(double)(0) ) 588 { 589 result = false; 590 return result; 591 } 592 } 593 } 594 595 // 596 // Solve Ax = b 597 // 598 if( isupper ) 599 { 600 601 // 602 // Solve A*X = B, where A = U*D*U'. 603 // 604 // First solve U*D*X = B, overwriting B with X. 605 // 606 // K is the main loop index, decreasing from N to 1 in steps of 607 // 1 or 2, depending on the size of the diagonal blocks. 608 // 609 k = n; 610 while( k>=1 ) 611 { 612 if( pivots[k]>0 ) 613 { 614 615 // 616 // 1 x 1 diagonal block 617 // 618 // Interchange rows K and IPIV(K). 619 // 620 kp = pivots[k]; 621 if( kp!=k ) 622 { 623 v = b[k]; 624 b[k] = b[kp]; 625 b[kp] = v; 626 } 627 628 // 629 // Multiply by inv(U(K)), where U(K) is the transformation 630 // stored in column K of A. 631 // 632 km1 = k-1; 633 v = b[k]; 634 for(i_=1; i_<=km1;i_++) 635 { 636 b[i_] = b[i_] - v*a[i_,k]; 637 } 638 639 // 640 // Multiply by the inverse of the diagonal block. 641 // 642 b[k] = b[k]/a[k,k]; 643 k = k-1; 644 } 645 else 646 { 647 648 // 649 // 2 x 2 diagonal block 650 // 651 // Interchange rows K-1 and -IPIV(K). 652 // 653 kp = -pivots[k]; 654 if( kp!=k-1 ) 655 { 656 v = b[k-1]; 657 b[k-1] = b[kp]; 658 b[kp] = v; 659 } 660 661 // 662 // Multiply by inv(U(K)), where U(K) is the transformation 663 // stored in columns K-1 and K of A. 664 // 665 km2 = k-2; 666 km1 = k-1; 667 v = b[k]; 668 for(i_=1; i_<=km2;i_++) 669 { 670 b[i_] = b[i_] - v*a[i_,k]; 671 } 672 v = b[k-1]; 673 for(i_=1; i_<=km2;i_++) 674 { 675 b[i_] = b[i_] - v*a[i_,km1]; 676 } 677 678 // 679 // Multiply by the inverse of the diagonal block. 680 // 681 akm1k = a[k-1,k]; 682 akm1 = a[k-1,k-1]/akm1k; 683 ak = a[k,k]/akm1k; 684 denom = akm1*ak-1; 685 bkm1 = b[k-1]/akm1k; 686 bk = b[k]/akm1k; 687 b[k-1] = (ak*bkm1-bk)/denom; 688 b[k] = (akm1*bk-bkm1)/denom; 689 k = k-2; 690 } 691 } 692 693 // 694 // Next solve U'*X = B, overwriting B with X. 695 // 696 // K is the main loop index, increasing from 1 to N in steps of 697 // 1 or 2, depending on the size of the diagonal blocks. 698 // 699 k = 1; 700 while( k<=n ) 701 { 702 if( pivots[k]>0 ) 703 { 704 705 // 706 // 1 x 1 diagonal block 707 // 708 // Multiply by inv(U'(K)), where U(K) is the transformation 709 // stored in column K of A. 710 // 711 km1 = k-1; 712 v = 0.0; 713 for(i_=1; i_<=km1;i_++) 714 { 715 v += b[i_]*a[i_,k]; 716 } 717 b[k] = b[k]-v; 718 719 // 720 // Interchange rows K and IPIV(K). 721 // 722 kp = pivots[k]; 723 if( kp!=k ) 724 { 725 v = b[k]; 726 b[k] = b[kp]; 727 b[kp] = v; 728 } 729 k = k+1; 730 } 731 else 732 { 733 734 // 735 // 2 x 2 diagonal block 736 // 737 // Multiply by inv(U'(K+1)), where U(K+1) is the transformation 738 // stored in columns K and K+1 of A. 739 // 740 km1 = k-1; 741 kp1 = k+1; 742 v = 0.0; 743 for(i_=1; i_<=km1;i_++) 744 { 745 v += b[i_]*a[i_,k]; 746 } 747 b[k] = b[k]-v; 748 v = 0.0; 749 for(i_=1; i_<=km1;i_++) 750 { 751 v += b[i_]*a[i_,kp1]; 752 } 753 b[k+1] = b[k+1]-v; 754 755 // 756 // Interchange rows K and -IPIV(K). 757 // 758 kp = -pivots[k]; 759 if( kp!=k ) 760 { 761 v = b[k]; 762 b[k] = b[kp]; 763 b[kp] = v; 764 } 765 k = k+2; 766 } 767 } 768 } 769 else 770 { 771 772 // 773 // Solve A*X = B, where A = L*D*L'. 774 // 775 // First solve L*D*X = B, overwriting B with X. 776 // 777 // K is the main loop index, increasing from 1 to N in steps of 778 // 1 or 2, depending on the size of the diagonal blocks. 779 // 780 k = 1; 781 while( k<=n ) 782 { 783 if( pivots[k]>0 ) 784 { 785 786 // 787 // 1 x 1 diagonal block 788 // 789 // Interchange rows K and IPIV(K). 790 // 791 kp = pivots[k]; 792 if( kp!=k ) 793 { 794 v = b[k]; 795 b[k] = b[kp]; 796 b[kp] = v; 797 } 798 799 // 800 // Multiply by inv(L(K)), where L(K) is the transformation 801 // stored in column K of A. 802 // 803 if( k<n ) 804 { 805 kp1 = k+1; 806 v = b[k]; 807 for(i_=kp1; i_<=n;i_++) 808 { 809 b[i_] = b[i_] - v*a[i_,k]; 810 } 811 } 812 813 // 814 // Multiply by the inverse of the diagonal block. 815 // 816 b[k] = b[k]/a[k,k]; 817 k = k+1; 818 } 819 else 820 { 821 822 // 823 // 2 x 2 diagonal block 824 // 825 // Interchange rows K+1 and -IPIV(K). 826 // 827 kp = -pivots[k]; 828 if( kp!=k+1 ) 829 { 830 v = b[k+1]; 831 b[k+1] = b[kp]; 832 b[kp] = v; 833 } 834 835 // 836 // Multiply by inv(L(K)), where L(K) is the transformation 837 // stored in columns K and K+1 of A. 838 // 839 if( k<n-1 ) 840 { 841 kp1 = k+1; 842 kp2 = k+2; 843 v = b[k]; 844 for(i_=kp2; i_<=n;i_++) 845 { 846 b[i_] = b[i_] - v*a[i_,k]; 847 } 848 v = b[k+1]; 849 for(i_=kp2; i_<=n;i_++) 850 { 851 b[i_] = b[i_] - v*a[i_,kp1]; 852 } 853 } 854 855 // 856 // Multiply by the inverse of the diagonal block. 857 // 858 akm1k = a[k+1,k]; 859 akm1 = a[k,k]/akm1k; 860 ak = a[k+1,k+1]/akm1k; 861 denom = akm1*ak-1; 862 bkm1 = b[k]/akm1k; 863 bk = b[k+1]/akm1k; 864 b[k] = (ak*bkm1-bk)/denom; 865 b[k+1] = (akm1*bk-bkm1)/denom; 866 k = k+2; 867 } 868 } 869 870 // 871 // Next solve L'*X = B, overwriting B with X. 872 // 873 // K is the main loop index, decreasing from N to 1 in steps of 874 // 1 or 2, depending on the size of the diagonal blocks. 875 // 876 k = n; 877 while( k>=1 ) 878 { 879 if( pivots[k]>0 ) 880 { 881 882 // 883 // 1 x 1 diagonal block 884 // 885 // Multiply by inv(L'(K)), where L(K) is the transformation 886 // stored in column K of A. 887 // 888 if( k<n ) 889 { 890 kp1 = k+1; 891 v = 0.0; 892 for(i_=kp1; i_<=n;i_++) 893 { 894 v += b[i_]*a[i_,k]; 895 } 896 b[k] = b[k]-v; 897 } 898 899 // 900 // Interchange rows K and IPIV(K). 901 // 902 kp = pivots[k]; 903 if( kp!=k ) 904 { 905 v = b[k]; 906 b[k] = b[kp]; 907 b[kp] = v; 908 } 909 k = k-1; 910 } 911 else 912 { 913 914 // 915 // 2 x 2 diagonal block 916 // 917 // Multiply by inv(L'(K-1)), where L(K-1) is the transformation 918 // stored in columns K-1 and K of A. 919 // 920 if( k<n ) 921 { 922 kp1 = k+1; 923 km1 = k-1; 924 v = 0.0; 925 for(i_=kp1; i_<=n;i_++) 926 { 927 v += b[i_]*a[i_,k]; 928 } 929 b[k] = b[k]-v; 930 v = 0.0; 931 for(i_=kp1; i_<=n;i_++) 932 { 933 v += b[i_]*a[i_,km1]; 934 } 935 b[k-1] = b[k-1]-v; 936 } 937 938 // 939 // Interchange rows K and -IPIV(K). 940 // 941 kp = -pivots[k]; 942 if( kp!=k ) 943 { 944 v = b[k]; 945 b[k] = b[kp]; 946 b[kp] = v; 947 } 948 k = k-2; 949 } 950 } 951 } 952 x = new double[n+1]; 953 for(i_=1; i_<=n;i_++) 954 { 955 x[i_] = b[i_]; 956 } 114 } 115 } 116 } 117 118 // 119 // Solve Ax = b 120 // 121 if (isupper) { 122 123 // 124 // Solve A*X = B, where A = U*D*U'. 125 // 126 // First solve U*D*X = B, overwriting B with X. 127 // 128 // K+1 is the main loop index, decreasing from N to 1 in steps of 129 // 1 or 2, depending on the size of the diagonal blocks. 130 // 131 k = n - 1; 132 while (k >= 0) { 133 if (pivots[k] >= 0) { 134 135 // 136 // 1 x 1 diagonal block 137 // 138 // Interchange rows K+1 and IPIV(K+1). 139 // 140 kp = pivots[k]; 141 if (kp != k) { 142 v = b[k]; 143 b[k] = b[kp]; 144 b[kp] = v; 145 } 146 147 // 148 // Multiply by inv(U(K+1)), where U(K+1) is the transformation 149 // stored in column K+1 of A. 150 // 151 v = b[k]; 152 for (i_ = 0; i_ <= k - 1; i_++) { 153 b[i_] = b[i_] - v * a[i_, k]; 154 } 155 156 // 157 // Multiply by the inverse of the diagonal block. 158 // 159 b[k] = b[k] / a[k, k]; 160 k = k - 1; 161 } else { 162 163 // 164 // 2 x 2 diagonal block 165 // 166 // Interchange rows K+1-1 and -IPIV(K+1). 167 // 168 kp = pivots[k] + n; 169 if (kp != k - 1) { 170 v = b[k - 1]; 171 b[k - 1] = b[kp]; 172 b[kp] = v; 173 } 174 175 // 176 // Multiply by inv(U(K+1)), where U(K+1) is the transformation 177 // stored in columns K+1-1 and K+1 of A. 178 // 179 v = b[k]; 180 for (i_ = 0; i_ <= k - 2; i_++) { 181 b[i_] = b[i_] - v * a[i_, k]; 182 } 183 v = b[k - 1]; 184 for (i_ = 0; i_ <= k - 2; i_++) { 185 b[i_] = b[i_] - v * a[i_, k - 1]; 186 } 187 188 // 189 // Multiply by the inverse of the diagonal block. 190 // 191 akm1k = a[k - 1, k]; 192 akm1 = a[k - 1, k - 1] / akm1k; 193 ak = a[k, k] / akm1k; 194 denom = akm1 * ak - 1; 195 bkm1 = b[k - 1] / akm1k; 196 bk = b[k] / akm1k; 197 b[k - 1] = (ak * bkm1 - bk) / denom; 198 b[k] = (akm1 * bk - bkm1) / denom; 199 k = k - 2; 200 } 201 } 202 203 // 204 // Next solve U'*X = B, overwriting B with X. 205 // 206 // K+1 is the main loop index, increasing from 1 to N in steps of 207 // 1 or 2, depending on the size of the diagonal blocks. 208 // 209 k = 0; 210 while (k <= n - 1) { 211 if (pivots[k] >= 0) { 212 213 // 214 // 1 x 1 diagonal block 215 // 216 // Multiply by inv(U'(K+1)), where U(K+1) is the transformation 217 // stored in column K+1 of A. 218 // 219 v = 0.0; 220 for (i_ = 0; i_ <= k - 1; i_++) { 221 v += b[i_] * a[i_, k]; 222 } 223 b[k] = b[k] - v; 224 225 // 226 // Interchange rows K+1 and IPIV(K+1). 227 // 228 kp = pivots[k]; 229 if (kp != k) { 230 v = b[k]; 231 b[k] = b[kp]; 232 b[kp] = v; 233 } 234 k = k + 1; 235 } else { 236 237 // 238 // 2 x 2 diagonal block 239 // 240 // Multiply by inv(U'(K+1+1)), where U(K+1+1) is the transformation 241 // stored in columns K+1 and K+1+1 of A. 242 // 243 v = 0.0; 244 for (i_ = 0; i_ <= k - 1; i_++) { 245 v += b[i_] * a[i_, k]; 246 } 247 b[k] = b[k] - v; 248 v = 0.0; 249 for (i_ = 0; i_ <= k - 1; i_++) { 250 v += b[i_] * a[i_, k + 1]; 251 } 252 b[k + 1] = b[k + 1] - v; 253 254 // 255 // Interchange rows K+1 and -IPIV(K+1). 256 // 257 kp = pivots[k] + n; 258 if (kp != k) { 259 v = b[k]; 260 b[k] = b[kp]; 261 b[kp] = v; 262 } 263 k = k + 2; 264 } 265 } 266 } else { 267 268 // 269 // Solve A*X = B, where A = L*D*L'. 270 // 271 // First solve L*D*X = B, overwriting B with X. 272 // 273 // K+1 is the main loop index, increasing from 1 to N in steps of 274 // 1 or 2, depending on the size of the diagonal blocks. 275 // 276 k = 0; 277 while (k <= n - 1) { 278 if (pivots[k] >= 0) { 279 280 // 281 // 1 x 1 diagonal block 282 // 283 // Interchange rows K+1 and IPIV(K+1). 284 // 285 kp = pivots[k]; 286 if (kp != k) { 287 v = b[k]; 288 b[k] = b[kp]; 289 b[kp] = v; 290 } 291 292 // 293 // Multiply by inv(L(K+1)), where L(K+1) is the transformation 294 // stored in column K+1 of A. 295 // 296 if (k + 1 < n) { 297 v = b[k]; 298 for (i_ = k + 1; i_ <= n - 1; i_++) { 299 b[i_] = b[i_] - v * a[i_, k]; 300 } 301 } 302 303 // 304 // Multiply by the inverse of the diagonal block. 305 // 306 b[k] = b[k] / a[k, k]; 307 k = k + 1; 308 } else { 309 310 // 311 // 2 x 2 diagonal block 312 // 313 // Interchange rows K+1+1 and -IPIV(K+1). 314 // 315 kp = pivots[k] + n; 316 if (kp != k + 1) { 317 v = b[k + 1]; 318 b[k + 1] = b[kp]; 319 b[kp] = v; 320 } 321 322 // 323 // Multiply by inv(L(K+1)), where L(K+1) is the transformation 324 // stored in columns K+1 and K+1+1 of A. 325 // 326 if (k + 1 < n - 1) { 327 v = b[k]; 328 for (i_ = k + 2; i_ <= n - 1; i_++) { 329 b[i_] = b[i_] - v * a[i_, k]; 330 } 331 v = b[k + 1]; 332 for (i_ = k + 2; i_ <= n - 1; i_++) { 333 b[i_] = b[i_] - v * a[i_, k + 1]; 334 } 335 } 336 337 // 338 // Multiply by the inverse of the diagonal block. 339 // 340 akm1k = a[k + 1, k]; 341 akm1 = a[k, k] / akm1k; 342 ak = a[k + 1, k + 1] / akm1k; 343 denom = akm1 * ak - 1; 344 bkm1 = b[k] / akm1k; 345 bk = b[k + 1] / akm1k; 346 b[k] = (ak * bkm1 - bk) / denom; 347 b[k + 1] = (akm1 * bk - bkm1) / denom; 348 k = k + 2; 349 } 350 } 351 352 // 353 // Next solve L'*X = B, overwriting B with X. 354 // 355 // K+1 is the main loop index, decreasing from N to 1 in steps of 356 // 1 or 2, depending on the size of the diagonal blocks. 357 // 358 k = n - 1; 359 while (k >= 0) { 360 if (pivots[k] >= 0) { 361 362 // 363 // 1 x 1 diagonal block 364 // 365 // Multiply by inv(L'(K+1)), where L(K+1) is the transformation 366 // stored in column K+1 of A. 367 // 368 if (k + 1 < n) { 369 v = 0.0; 370 for (i_ = k + 1; i_ <= n - 1; i_++) { 371 v += b[i_] * a[i_, k]; 372 } 373 b[k] = b[k] - v; 374 } 375 376 // 377 // Interchange rows K+1 and IPIV(K+1). 378 // 379 kp = pivots[k]; 380 if (kp != k) { 381 v = b[k]; 382 b[k] = b[kp]; 383 b[kp] = v; 384 } 385 k = k - 1; 386 } else { 387 388 // 389 // 2 x 2 diagonal block 390 // 391 // Multiply by inv(L'(K+1-1)), where L(K+1-1) is the transformation 392 // stored in columns K+1-1 and K+1 of A. 393 // 394 if (k + 1 < n) { 395 v = 0.0; 396 for (i_ = k + 1; i_ <= n - 1; i_++) { 397 v += b[i_] * a[i_, k]; 398 } 399 b[k] = b[k] - v; 400 v = 0.0; 401 for (i_ = k + 1; i_ <= n - 1; i_++) { 402 v += b[i_] * a[i_, k - 1]; 403 } 404 b[k - 1] = b[k - 1] - v; 405 } 406 407 // 408 // Interchange rows K+1 and -IPIV(K+1). 409 // 410 kp = pivots[k] + n; 411 if (kp != k) { 412 v = b[k]; 413 b[k] = b[kp]; 414 b[kp] = v; 415 } 416 k = k - 2; 417 } 418 } 419 } 420 x = new double[n - 1 + 1]; 421 for (i_ = 0; i_ <= n - 1; i_++) { 422 x[i_] = b[i_]; 423 } 424 return result; 425 } 426 427 428 /************************************************************************* 429 Solving a system of linear equations with a symmetric system matrix 430 431 Input parameters: 432 A - system matrix (upper or lower triangle). 433 Array whose indexes range within [0..N-1, 0..N-1]. 434 B - right side of a system. 435 Array whose index ranges within [0..N-1]. 436 N - size of matrix A. 437 IsUpper - If IsUpper = True, A contains the upper triangle, 438 otherwise A contains the lower triangle. 439 440 Output parameters: 441 X - solution of a system. 442 Array whose index ranges within [0..N-1]. 443 444 Result: 445 True, if the matrix is not singular. X contains the solution. 446 False, if the matrix is singular (the determinant of the matrix is equal 447 to 0). In this case, X doesn't contain a solution. 448 449 -- ALGLIB -- 450 Copyright 2005 by Bochkanov Sergey 451 *************************************************************************/ 452 public static bool smatrixsolve(double[,] a, 453 ref double[] b, 454 int n, 455 bool isupper, 456 ref double[] x) { 457 bool result = new bool(); 458 int[] pivots = new int[0]; 459 460 a = (double[,])a.Clone(); 461 462 ldlt.smatrixldlt(ref a, n, isupper, ref pivots); 463 result = smatrixldltsolve(ref a, ref pivots, b, n, isupper, ref x); 464 return result; 465 } 466 467 468 public static bool solvesystemldlt(ref double[,] a, 469 ref int[] pivots, 470 double[] b, 471 int n, 472 bool isupper, 473 ref double[] x) { 474 bool result = new bool(); 475 int i = 0; 476 int k = 0; 477 int kp = 0; 478 int km1 = 0; 479 int km2 = 0; 480 int kp1 = 0; 481 int kp2 = 0; 482 double ak = 0; 483 double akm1 = 0; 484 double akm1k = 0; 485 double bk = 0; 486 double bkm1 = 0; 487 double denom = 0; 488 double v = 0; 489 int i_ = 0; 490 491 b = (double[])b.Clone(); 492 493 494 // 495 // Quick return if possible 496 // 497 result = true; 498 if (n == 0) { 499 return result; 500 } 501 502 // 503 // Check that the diagonal matrix D is nonsingular 504 // 505 if (isupper) { 506 507 // 508 // Upper triangular storage: examine D from bottom to top 509 // 510 for (i = n; i >= 1; i--) { 511 if (pivots[i] > 0 & (double)(a[i, i]) == (double)(0)) { 512 result = false; 957 513 return result; 958 } 959 960 961 public static bool solvesymmetricsystem(double[,] a, 962 double[] b, 963 int n, 964 bool isupper, 965 ref double[] x) 966 { 967 bool result = new bool(); 968 int[] pivots = new int[0]; 969 970 a = (double[,])a.Clone(); 971 b = (double[])b.Clone(); 972 973 ldlt.ldltdecomposition(ref a, n, isupper, ref pivots); 974 result = solvesystemldlt(ref a, ref pivots, b, n, isupper, ref x); 514 } 515 } 516 } else { 517 518 // 519 // Lower triangular storage: examine D from top to bottom. 520 // 521 for (i = 1; i <= n; i++) { 522 if (pivots[i] > 0 & (double)(a[i, i]) == (double)(0)) { 523 result = false; 975 524 return result; 976 } 525 } 526 } 527 } 528 529 // 530 // Solve Ax = b 531 // 532 if (isupper) { 533 534 // 535 // Solve A*X = B, where A = U*D*U'. 536 // 537 // First solve U*D*X = B, overwriting B with X. 538 // 539 // K is the main loop index, decreasing from N to 1 in steps of 540 // 1 or 2, depending on the size of the diagonal blocks. 541 // 542 k = n; 543 while (k >= 1) { 544 if (pivots[k] > 0) { 545 546 // 547 // 1 x 1 diagonal block 548 // 549 // Interchange rows K and IPIV(K). 550 // 551 kp = pivots[k]; 552 if (kp != k) { 553 v = b[k]; 554 b[k] = b[kp]; 555 b[kp] = v; 556 } 557 558 // 559 // Multiply by inv(U(K)), where U(K) is the transformation 560 // stored in column K of A. 561 // 562 km1 = k - 1; 563 v = b[k]; 564 for (i_ = 1; i_ <= km1; i_++) { 565 b[i_] = b[i_] - v * a[i_, k]; 566 } 567 568 // 569 // Multiply by the inverse of the diagonal block. 570 // 571 b[k] = b[k] / a[k, k]; 572 k = k - 1; 573 } else { 574 575 // 576 // 2 x 2 diagonal block 577 // 578 // Interchange rows K-1 and -IPIV(K). 579 // 580 kp = -pivots[k]; 581 if (kp != k - 1) { 582 v = b[k - 1]; 583 b[k - 1] = b[kp]; 584 b[kp] = v; 585 } 586 587 // 588 // Multiply by inv(U(K)), where U(K) is the transformation 589 // stored in columns K-1 and K of A. 590 // 591 km2 = k - 2; 592 km1 = k - 1; 593 v = b[k]; 594 for (i_ = 1; i_ <= km2; i_++) { 595 b[i_] = b[i_] - v * a[i_, k]; 596 } 597 v = b[k - 1]; 598 for (i_ = 1; i_ <= km2; i_++) { 599 b[i_] = b[i_] - v * a[i_, km1]; 600 } 601 602 // 603 // Multiply by the inverse of the diagonal block. 604 // 605 akm1k = a[k - 1, k]; 606 akm1 = a[k - 1, k - 1] / akm1k; 607 ak = a[k, k] / akm1k; 608 denom = akm1 * ak - 1; 609 bkm1 = b[k - 1] / akm1k; 610 bk = b[k] / akm1k; 611 b[k - 1] = (ak * bkm1 - bk) / denom; 612 b[k] = (akm1 * bk - bkm1) / denom; 613 k = k - 2; 614 } 615 } 616 617 // 618 // Next solve U'*X = B, overwriting B with X. 619 // 620 // K is the main loop index, increasing from 1 to N in steps of 621 // 1 or 2, depending on the size of the diagonal blocks. 622 // 623 k = 1; 624 while (k <= n) { 625 if (pivots[k] > 0) { 626 627 // 628 // 1 x 1 diagonal block 629 // 630 // Multiply by inv(U'(K)), where U(K) is the transformation 631 // stored in column K of A. 632 // 633 km1 = k - 1; 634 v = 0.0; 635 for (i_ = 1; i_ <= km1; i_++) { 636 v += b[i_] * a[i_, k]; 637 } 638 b[k] = b[k] - v; 639 640 // 641 // Interchange rows K and IPIV(K). 642 // 643 kp = pivots[k]; 644 if (kp != k) { 645 v = b[k]; 646 b[k] = b[kp]; 647 b[kp] = v; 648 } 649 k = k + 1; 650 } else { 651 652 // 653 // 2 x 2 diagonal block 654 // 655 // Multiply by inv(U'(K+1)), where U(K+1) is the transformation 656 // stored in columns K and K+1 of A. 657 // 658 km1 = k - 1; 659 kp1 = k + 1; 660 v = 0.0; 661 for (i_ = 1; i_ <= km1; i_++) { 662 v += b[i_] * a[i_, k]; 663 } 664 b[k] = b[k] - v; 665 v = 0.0; 666 for (i_ = 1; i_ <= km1; i_++) { 667 v += b[i_] * a[i_, kp1]; 668 } 669 b[k + 1] = b[k + 1] - v; 670 671 // 672 // Interchange rows K and -IPIV(K). 673 // 674 kp = -pivots[k]; 675 if (kp != k) { 676 v = b[k]; 677 b[k] = b[kp]; 678 b[kp] = v; 679 } 680 k = k + 2; 681 } 682 } 683 } else { 684 685 // 686 // Solve A*X = B, where A = L*D*L'. 687 // 688 // First solve L*D*X = B, overwriting B with X. 689 // 690 // K is the main loop index, increasing from 1 to N in steps of 691 // 1 or 2, depending on the size of the diagonal blocks. 692 // 693 k = 1; 694 while (k <= n) { 695 if (pivots[k] > 0) { 696 697 // 698 // 1 x 1 diagonal block 699 // 700 // Interchange rows K and IPIV(K). 701 // 702 kp = pivots[k]; 703 if (kp != k) { 704 v = b[k]; 705 b[k] = b[kp]; 706 b[kp] = v; 707 } 708 709 // 710 // Multiply by inv(L(K)), where L(K) is the transformation 711 // stored in column K of A. 712 // 713 if (k < n) { 714 kp1 = k + 1; 715 v = b[k]; 716 for (i_ = kp1; i_ <= n; i_++) { 717 b[i_] = b[i_] - v * a[i_, k]; 718 } 719 } 720 721 // 722 // Multiply by the inverse of the diagonal block. 723 // 724 b[k] = b[k] / a[k, k]; 725 k = k + 1; 726 } else { 727 728 // 729 // 2 x 2 diagonal block 730 // 731 // Interchange rows K+1 and -IPIV(K). 732 // 733 kp = -pivots[k]; 734 if (kp != k + 1) { 735 v = b[k + 1]; 736 b[k + 1] = b[kp]; 737 b[kp] = v; 738 } 739 740 // 741 // Multiply by inv(L(K)), where L(K) is the transformation 742 // stored in columns K and K+1 of A. 743 // 744 if (k < n - 1) { 745 kp1 = k + 1; 746 kp2 = k + 2; 747 v = b[k]; 748 for (i_ = kp2; i_ <= n; i_++) { 749 b[i_] = b[i_] - v * a[i_, k]; 750 } 751 v = b[k + 1]; 752 for (i_ = kp2; i_ <= n; i_++) { 753 b[i_] = b[i_] - v * a[i_, kp1]; 754 } 755 } 756 757 // 758 // Multiply by the inverse of the diagonal block. 759 // 760 akm1k = a[k + 1, k]; 761 akm1 = a[k, k] / akm1k; 762 ak = a[k + 1, k + 1] / akm1k; 763 denom = akm1 * ak - 1; 764 bkm1 = b[k] / akm1k; 765 bk = b[k + 1] / akm1k; 766 b[k] = (ak * bkm1 - bk) / denom; 767 b[k + 1] = (akm1 * bk - bkm1) / denom; 768 k = k + 2; 769 } 770 } 771 772 // 773 // Next solve L'*X = B, overwriting B with X. 774 // 775 // K is the main loop index, decreasing from N to 1 in steps of 776 // 1 or 2, depending on the size of the diagonal blocks. 777 // 778 k = n; 779 while (k >= 1) { 780 if (pivots[k] > 0) { 781 782 // 783 // 1 x 1 diagonal block 784 // 785 // Multiply by inv(L'(K)), where L(K) is the transformation 786 // stored in column K of A. 787 // 788 if (k < n) { 789 kp1 = k + 1; 790 v = 0.0; 791 for (i_ = kp1; i_ <= n; i_++) { 792 v += b[i_] * a[i_, k]; 793 } 794 b[k] = b[k] - v; 795 } 796 797 // 798 // Interchange rows K and IPIV(K). 799 // 800 kp = pivots[k]; 801 if (kp != k) { 802 v = b[k]; 803 b[k] = b[kp]; 804 b[kp] = v; 805 } 806 k = k - 1; 807 } else { 808 809 // 810 // 2 x 2 diagonal block 811 // 812 // Multiply by inv(L'(K-1)), where L(K-1) is the transformation 813 // stored in columns K-1 and K of A. 814 // 815 if (k < n) { 816 kp1 = k + 1; 817 km1 = k - 1; 818 v = 0.0; 819 for (i_ = kp1; i_ <= n; i_++) { 820 v += b[i_] * a[i_, k]; 821 } 822 b[k] = b[k] - v; 823 v = 0.0; 824 for (i_ = kp1; i_ <= n; i_++) { 825 v += b[i_] * a[i_, km1]; 826 } 827 b[k - 1] = b[k - 1] - v; 828 } 829 830 // 831 // Interchange rows K and -IPIV(K). 832 // 833 kp = -pivots[k]; 834 if (kp != k) { 835 v = b[k]; 836 b[k] = b[kp]; 837 b[kp] = v; 838 } 839 k = k - 2; 840 } 841 } 842 } 843 x = new double[n + 1]; 844 for (i_ = 1; i_ <= n; i_++) { 845 x[i_] = b[i_]; 846 } 847 return result; 977 848 } 849 850 851 public static bool solvesymmetricsystem(double[,] a, 852 double[] b, 853 int n, 854 bool isupper, 855 ref double[] x) { 856 bool result = new bool(); 857 int[] pivots = new int[0]; 858 859 a = (double[,])a.Clone(); 860 b = (double[])b.Clone(); 861 862 ldlt.ldltdecomposition(ref a, n, isupper, ref pivots); 863 result = solvesystemldlt(ref a, ref pivots, b, n, isupper, ref x); 864 return result; 865 } 866 } 978 867 }
Note: See TracChangeset
for help on using the changeset viewer.
