| 54 | \****************************************************************************************/ |
| 55 | |
| 56 | template<typename _Tp> static inline int |
| 57 | LUImpl(_Tp* A, size_t astep, int m, _Tp* b, size_t bstep, int n) |
| 58 | { |
| 59 | int i, j, k, p = 1; |
| 60 | astep /= sizeof(A[0]); |
| 61 | bstep /= sizeof(b[0]); |
| 62 | |
| 63 | for( i = 0; i < m; i++ ) |
| 64 | { |
| 65 | k = i; |
| 66 | |
| 67 | for( j = i+1; j < m; j++ ) |
| 68 | if( std::abs(A[j*astep + i]) > std::abs(A[k*astep + i]) ) |
| 69 | k = j; |
| 70 | |
| 71 | if( std::abs(A[k*astep + i]) < std::numeric_limits<_Tp>::epsilon() ) |
| 72 | return 0; |
| 73 | |
| 74 | if( k != i ) |
| 75 | { |
| 76 | for( j = i; j < m; j++ ) |
| 77 | std::swap(A[i*astep + j], A[k*astep + j]); |
| 78 | if( b ) |
| 79 | for( j = 0; j < n; j++ ) |
| 80 | std::swap(b[i*bstep + j], b[k*bstep + j]); |
| 81 | p = -p; |
| 82 | } |
| 83 | |
| 84 | _Tp d = -1/A[i*astep + i]; |
| 85 | |
| 86 | for( j = i+1; j < m; j++ ) |
| 87 | { |
| 88 | _Tp alpha = A[j*astep + i]*d; |
| 89 | |
| 90 | for( k = i+1; k < m; k++ ) |
| 91 | A[j*astep + k] += alpha*A[i*astep + k]; |
| 92 | |
| 93 | if( b ) |
| 94 | for( k = 0; k < n; k++ ) |
| 95 | b[j*bstep + k] += alpha*b[i*bstep + k]; |
| 96 | } |
| 97 | |
| 98 | A[i*astep + i] = -d; |
| 99 | } |
| 100 | |
| 101 | if( b ) |
| 102 | { |
| 103 | for( i = m-1; i >= 0; i-- ) |
| 104 | for( j = 0; j < n; j++ ) |
| 105 | { |
| 106 | _Tp s = b[i*bstep + j]; |
| 107 | for( k = i+1; k < m; k++ ) |
| 108 | s -= A[i*astep + k]*b[k*bstep + j]; |
| 109 | b[i*bstep + j] = s*A[i*astep + i]; |
| 110 | } |
| 111 | } |
| 112 | |
| 113 | return p; |