| 230 | |
| 231 | |
| 232 | template<typename _Tp> bool |
| 233 | JacobiImpl_( _Tp* A, size_t astep, _Tp* W, _Tp* V, size_t vstep, int n, uchar* buf ) |
| 234 | { |
| 235 | const _Tp eps = std::numeric_limits<_Tp>::epsilon(); |
| 236 | int i, j, k, m; |
| 237 | |
| 238 | astep /= sizeof(A[0]); |
| 239 | if( V ) |
| 240 | { |
| 241 | vstep /= sizeof(V[0]); |
| 242 | for( i = 0; i < n; i++ ) |
| 243 | { |
| 244 | for( j = 0; j < n; j++ ) |
| 245 | V[i*vstep + j] = (_Tp)0; |
| 246 | V[i*vstep + i] = (_Tp)1; |
| 247 | } |
| 248 | } |
| 249 | |
| 250 | int iters, maxIters = n*n*30; |
| 251 | |
| 252 | int* indR = (int*)alignPtr(buf, sizeof(int)); |
| 253 | int* indC = indR + n; |
| 254 | _Tp mv = (_Tp)0; |
| 255 | |
| 256 | for( k = 0; k < n; k++ ) |
| 257 | { |
| 258 | W[k] = A[(astep + 1)*k]; |
| 259 | if( k < n - 1 ) |
| 260 | { |
| 261 | for( m = k+1, mv = std::abs(A[astep*k + m]), i = k+2; i < n; i++ ) |
| 262 | { |
| 263 | _Tp val = std::abs(A[astep*k+i]); |
| 264 | if( mv < val ) |
| 265 | mv = val, m = i; |
| 266 | } |
| 267 | indR[k] = m; |
| 268 | } |
| 269 | if( k > 0 ) |
| 270 | { |
| 271 | for( m = 0, mv = std::abs(A[k]), i = 1; i < k; i++ ) |
| 272 | { |
| 273 | _Tp val = std::abs(A[astep*i+k]); |
| 274 | if( mv < val ) |
| 275 | mv = val, m = i; |
| 276 | } |
| 277 | indC[k] = m; |
| 278 | } |
| 279 | } |
| 280 | |
| 281 | if( n > 1 ) for( iters = 0; iters < maxIters; iters++ ) |
| 282 | { |
| 283 | // find index (k,l) of pivot p |
| 284 | for( k = 0, mv = std::abs(A[indR[0]]), i = 1; i < n-1; i++ ) |
| 285 | { |
| 286 | _Tp val = std::abs(A[astep*i + indR[i]]); |
| 287 | if( mv < val ) |
| 288 | mv = val, k = i; |
| 289 | } |