diff options
Diffstat (limited to 'circuitpython/extmod/ulab/code/numpy/linalg/linalg_tools.c')
| -rw-r--r-- | circuitpython/extmod/ulab/code/numpy/linalg/linalg_tools.c | 171 |
1 files changed, 171 insertions, 0 deletions
diff --git a/circuitpython/extmod/ulab/code/numpy/linalg/linalg_tools.c b/circuitpython/extmod/ulab/code/numpy/linalg/linalg_tools.c new file mode 100644 index 0000000..5e03a50 --- /dev/null +++ b/circuitpython/extmod/ulab/code/numpy/linalg/linalg_tools.c @@ -0,0 +1,171 @@ +/* + * This file is part of the micropython-ulab project, + * + * https://github.com/v923z/micropython-ulab + * + * The MIT License (MIT) + * + * Copyright (c) 2019-2010 Zoltán Vörös +*/ + +#include <math.h> +#include <string.h> +#include "py/runtime.h" + +#include "linalg_tools.h" + +/* + * The following function inverts a matrix, whose entries are given in the input array + * The function has no dependencies beyond micropython itself (for the definition of mp_float_t), + * and can be used independent of ulab. + */ + +bool linalg_invert_matrix(mp_float_t *data, size_t N) { + // returns true, of the inversion was successful, + // false, if the matrix is singular + + // initially, this is the unit matrix: the contents of this matrix is what + // will be returned after all the transformations + mp_float_t *unit = m_new(mp_float_t, N*N); + mp_float_t elem = 1.0; + // initialise the unit matrix + memset(unit, 0, sizeof(mp_float_t)*N*N); + for(size_t m=0; m < N; m++) { + memcpy(&unit[m * (N+1)], &elem, sizeof(mp_float_t)); + } + for(size_t m=0; m < N; m++){ + // this could be faster with ((c < epsilon) && (c > -epsilon)) + if(MICROPY_FLOAT_C_FUN(fabs)(data[m * (N+1)]) < LINALG_EPSILON) { + //look for a line to swap + size_t m1 = m + 1; + for(; m1 < N; m1++) { + if(!(MICROPY_FLOAT_C_FUN(fabs)(data[m1*N + m]) < LINALG_EPSILON)) { + for(size_t m2=0; m2 < N; m2++) { + mp_float_t swapVal = data[m*N+m2]; + data[m*N+m2] = data[m1*N+m2]; + data[m1*N+m2] = swapVal; + swapVal = unit[m*N+m2]; + unit[m*N+m2] = unit[m1*N+m2]; + unit[m1*N+m2] = swapVal; + } + break; + } + } + if (m1 >= N) { + m_del(mp_float_t, unit, N*N); + return false; + } + } + for(size_t n=0; n < N; n++) { + if(m != n){ + elem = data[N * n + m] / data[m * (N+1)]; + for(size_t k=0; k < N; k++) { + data[N * n + k] -= elem * data[N * m + k]; + unit[N * n + k] -= elem * unit[N * m + k]; + } + } + } + } + for(size_t m=0; m < N; m++) { + elem = data[m * (N+1)]; + for(size_t n=0; n < N; n++) { + data[N * m + n] /= elem; + unit[N * m + n] /= elem; + } + } + memcpy(data, unit, sizeof(mp_float_t)*N*N); + m_del(mp_float_t, unit, N * N); + return true; +} + +/* + * The following function calculates the eigenvalues and eigenvectors of a symmetric + * real matrix, whose entries are given in the input array. + * The function has no dependencies beyond micropython itself (for the definition of mp_float_t), + * and can be used independent of ulab. + */ + +size_t linalg_jacobi_rotations(mp_float_t *array, mp_float_t *eigvectors, size_t S) { + // eigvectors should be a 0-array; start out with the unit matrix + for(size_t m=0; m < S; m++) { + eigvectors[m * (S+1)] = 1.0; + } + mp_float_t largest, w, t, c, s, tau, aMk, aNk, vm, vn; + size_t M, N; + size_t iterations = JACOBI_MAX * S * S; + do { + iterations--; + // find the pivot here + M = 0; + N = 0; + largest = 0.0; + for(size_t m=0; m < S-1; m++) { // -1: no need to inspect last row + for(size_t n=m+1; n < S; n++) { + w = MICROPY_FLOAT_C_FUN(fabs)(array[m * S + n]); + if((largest < w) && (LINALG_EPSILON < w)) { + M = m; + N = n; + largest = w; + } + } + } + if(M + N == 0) { // all entries are smaller than epsilon, there is not much we can do... + break; + } + // at this point, we have the pivot, and it is the entry (M, N) + // now we have to find the rotation angle + w = (array[N * S + N] - array[M * S + M]) / (MICROPY_FLOAT_CONST(2.0)*array[M * S + N]); + // The following if/else chooses the smaller absolute value for the tangent + // of the rotation angle. Going with the smaller should be numerically stabler. + if(w > 0) { + t = MICROPY_FLOAT_C_FUN(sqrt)(w*w + MICROPY_FLOAT_CONST(1.0)) - w; + } else { + t = MICROPY_FLOAT_CONST(-1.0)*(MICROPY_FLOAT_C_FUN(sqrt)(w*w + MICROPY_FLOAT_CONST(1.0)) + w); + } + s = t / MICROPY_FLOAT_C_FUN(sqrt)(t*t + MICROPY_FLOAT_CONST(1.0)); // the sine of the rotation angle + c = MICROPY_FLOAT_CONST(1.0) / MICROPY_FLOAT_C_FUN(sqrt)(t*t + MICROPY_FLOAT_CONST(1.0)); // the cosine of the rotation angle + tau = (MICROPY_FLOAT_CONST(1.0)-c)/s; // this is equal to the tangent of the half of the rotation angle + + // at this point, we have the rotation angles, so we can transform the matrix + // first the two diagonal elements + // a(M, M) = a(M, M) - t*a(M, N) + array[M * S + M] = array[M * S + M] - t * array[M * S + N]; + // a(N, N) = a(N, N) + t*a(M, N) + array[N * S + N] = array[N * S + N] + t * array[M * S + N]; + // after the rotation, the a(M, N), and a(N, M) entries should become zero + array[M * S + N] = array[N * S + M] = MICROPY_FLOAT_CONST(0.0); + // then all other elements in the column + for(size_t k=0; k < S; k++) { + if((k == M) || (k == N)) { + continue; + } + aMk = array[M * S + k]; + aNk = array[N * S + k]; + // a(M, k) = a(M, k) - s*(a(N, k) + tau*a(M, k)) + array[M * S + k] -= s * (aNk + tau * aMk); + // a(N, k) = a(N, k) + s*(a(M, k) - tau*a(N, k)) + array[N * S + k] += s * (aMk - tau * aNk); + // a(k, M) = a(M, k) + array[k * S + M] = array[M * S + k]; + // a(k, N) = a(N, k) + array[k * S + N] = array[N * S + k]; + } + // now we have to update the eigenvectors + // the rotation matrix, R, multiplies from the right + // R is the unit matrix, except for the + // R(M,M) = R(N, N) = c + // R(N, M) = s + // (M, N) = -s + // entries. This means that only the Mth, and Nth columns will change + for(size_t m=0; m < S; m++) { + vm = eigvectors[m * S + M]; + vn = eigvectors[m * S + N]; + // the new value of eigvectors(m, M) + eigvectors[m * S + M] = c * vm - s * vn; + // the new value of eigvectors(m, N) + eigvectors[m * S + N] = s * vm + c * vn; + } + } while(iterations > 0); + + return iterations; +} |