diff options
Diffstat (limited to 'third_party/libopenjpeg20/invert.c')
| -rw-r--r-- | third_party/libopenjpeg20/invert.c | 417 |
1 files changed, 209 insertions, 208 deletions
diff --git a/third_party/libopenjpeg20/invert.c b/third_party/libopenjpeg20/invert.c index 7efaf6eca2..89f607155f 100644 --- a/third_party/libopenjpeg20/invert.c +++ b/third_party/libopenjpeg20/invert.c @@ -1,6 +1,6 @@ /* - * The copyright in this software is being made available under the 2-clauses - * BSD License, included below. This software may be subject to other third + * The copyright in this software is being made available under the 2-clauses + * BSD License, included below. This software may be subject to other third * party and contributor rights, including patent rights, and no such rights * are granted under this license. * @@ -31,33 +31,33 @@ #include "opj_includes.h" -/** +/** * LUP decomposition */ static OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix, - OPJ_UINT32 * permutations, + OPJ_UINT32 * permutations, OPJ_FLOAT32 * p_swap_area, OPJ_UINT32 nb_compo); -/** +/** * LUP solving */ -static void opj_lupSolve(OPJ_FLOAT32 * pResult, - OPJ_FLOAT32* pMatrix, - OPJ_FLOAT32* pVector, - OPJ_UINT32* pPermutations, +static void opj_lupSolve(OPJ_FLOAT32 * pResult, + OPJ_FLOAT32* pMatrix, + OPJ_FLOAT32* pVector, + OPJ_UINT32* pPermutations, OPJ_UINT32 nb_compo, OPJ_FLOAT32 * p_intermediate_data); -/** +/** *LUP inversion (call with the result of lupDecompose) */ -static void opj_lupInvert ( OPJ_FLOAT32 * pSrcMatrix, - OPJ_FLOAT32 * pDestMatrix, - OPJ_UINT32 nb_compo, - OPJ_UINT32 * pPermutations, - OPJ_FLOAT32 * p_src_temp, - OPJ_FLOAT32 * p_dest_temp, - OPJ_FLOAT32 * p_swap_area); +static void opj_lupInvert(OPJ_FLOAT32 * pSrcMatrix, + OPJ_FLOAT32 * pDestMatrix, + OPJ_UINT32 nb_compo, + OPJ_UINT32 * pPermutations, + OPJ_FLOAT32 * p_src_temp, + OPJ_FLOAT32 * p_dest_temp, + OPJ_FLOAT32 * p_swap_area); /* ========================================================== @@ -68,32 +68,33 @@ static void opj_lupInvert ( OPJ_FLOAT32 * pSrcMatrix, * Matrix inversion. */ OPJ_BOOL opj_matrix_inversion_f(OPJ_FLOAT32 * pSrcMatrix, - OPJ_FLOAT32 * pDestMatrix, + OPJ_FLOAT32 * pDestMatrix, OPJ_UINT32 nb_compo) { - OPJ_BYTE * l_data = 00; - OPJ_UINT32 l_permutation_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_UINT32); - OPJ_UINT32 l_swap_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); - OPJ_UINT32 l_total_size = l_permutation_size + 3 * l_swap_size; - OPJ_UINT32 * lPermutations = 00; - OPJ_FLOAT32 * l_double_data = 00; - - l_data = (OPJ_BYTE *) opj_malloc(l_total_size); - if (l_data == 0) { - return OPJ_FALSE; - } - lPermutations = (OPJ_UINT32 *) l_data; - l_double_data = (OPJ_FLOAT32 *) (l_data + l_permutation_size); - memset(lPermutations,0,l_permutation_size); - - if(! opj_lupDecompose(pSrcMatrix,lPermutations,l_double_data,nb_compo)) { - opj_free(l_data); - return OPJ_FALSE; - } - - opj_lupInvert(pSrcMatrix,pDestMatrix,nb_compo,lPermutations,l_double_data,l_double_data + nb_compo,l_double_data + 2*nb_compo); - opj_free(l_data); - + OPJ_BYTE * l_data = 00; + OPJ_UINT32 l_permutation_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_UINT32); + OPJ_UINT32 l_swap_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); + OPJ_UINT32 l_total_size = l_permutation_size + 3 * l_swap_size; + OPJ_UINT32 * lPermutations = 00; + OPJ_FLOAT32 * l_double_data = 00; + + l_data = (OPJ_BYTE *) opj_malloc(l_total_size); + if (l_data == 0) { + return OPJ_FALSE; + } + lPermutations = (OPJ_UINT32 *) l_data; + l_double_data = (OPJ_FLOAT32 *)(l_data + l_permutation_size); + memset(lPermutations, 0, l_permutation_size); + + if (! opj_lupDecompose(pSrcMatrix, lPermutations, l_double_data, nb_compo)) { + opj_free(l_data); + return OPJ_FALSE; + } + + opj_lupInvert(pSrcMatrix, pDestMatrix, nb_compo, lPermutations, l_double_data, + l_double_data + nb_compo, l_double_data + 2 * nb_compo); + opj_free(l_data); + return OPJ_TRUE; } @@ -103,192 +104,192 @@ OPJ_BOOL opj_matrix_inversion_f(OPJ_FLOAT32 * pSrcMatrix, Local functions ========================================================== */ -static OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix,OPJ_UINT32 * permutations, - OPJ_FLOAT32 * p_swap_area, - OPJ_UINT32 nb_compo) +static OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix, + OPJ_UINT32 * permutations, + OPJ_FLOAT32 * p_swap_area, + OPJ_UINT32 nb_compo) { - OPJ_UINT32 * tmpPermutations = permutations; - OPJ_UINT32 * dstPermutations; - OPJ_UINT32 k2=0,t; - OPJ_FLOAT32 temp; - OPJ_UINT32 i,j,k; - OPJ_FLOAT32 p; - OPJ_UINT32 lLastColum = nb_compo - 1; - OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); - OPJ_FLOAT32 * lTmpMatrix = matrix; - OPJ_FLOAT32 * lColumnMatrix,* lDestMatrix; - OPJ_UINT32 offset = 1; - OPJ_UINT32 lStride = nb_compo-1; - - /*initialize permutations */ - for (i = 0; i < nb_compo; ++i) - { - *tmpPermutations++ = i; - } - /* now make a pivot with column switch */ - tmpPermutations = permutations; - for (k = 0; k < lLastColum; ++k) { - p = 0.0; - - /* take the middle element */ - lColumnMatrix = lTmpMatrix + k; - - /* make permutation with the biggest value in the column */ + OPJ_UINT32 * tmpPermutations = permutations; + OPJ_UINT32 * dstPermutations; + OPJ_UINT32 k2 = 0, t; + OPJ_FLOAT32 temp; + OPJ_UINT32 i, j, k; + OPJ_FLOAT32 p; + OPJ_UINT32 lLastColum = nb_compo - 1; + OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); + OPJ_FLOAT32 * lTmpMatrix = matrix; + OPJ_FLOAT32 * lColumnMatrix, * lDestMatrix; + OPJ_UINT32 offset = 1; + OPJ_UINT32 lStride = nb_compo - 1; + + /*initialize permutations */ + for (i = 0; i < nb_compo; ++i) { + *tmpPermutations++ = i; + } + /* now make a pivot with column switch */ + tmpPermutations = permutations; + for (k = 0; k < lLastColum; ++k) { + p = 0.0; + + /* take the middle element */ + lColumnMatrix = lTmpMatrix + k; + + /* make permutation with the biggest value in the column */ for (i = k; i < nb_compo; ++i) { - temp = ((*lColumnMatrix > 0) ? *lColumnMatrix : -(*lColumnMatrix)); - if (temp > p) { - p = temp; - k2 = i; - } - /* next line */ - lColumnMatrix += nb_compo; - } - - /* a whole rest of 0 -> non singular */ - if (p == 0.0) { - return OPJ_FALSE; - } - - /* should we permute ? */ - if (k2 != k) { - /*exchange of line */ - /* k2 > k */ - dstPermutations = tmpPermutations + k2 - k; - /* swap indices */ - t = *tmpPermutations; - *tmpPermutations = *dstPermutations; - *dstPermutations = t; - - /* and swap entire line. */ - lColumnMatrix = lTmpMatrix + (k2 - k) * nb_compo; - memcpy(p_swap_area,lColumnMatrix,lSwapSize); - memcpy(lColumnMatrix,lTmpMatrix,lSwapSize); - memcpy(lTmpMatrix,p_swap_area,lSwapSize); - } - - /* now update data in the rest of the line and line after */ - lDestMatrix = lTmpMatrix + k; - lColumnMatrix = lDestMatrix + nb_compo; - /* take the middle element */ - temp = *(lDestMatrix++); - - /* now compute up data (i.e. coeff up of the diagonal). */ - for (i = offset; i < nb_compo; ++i) { - /*lColumnMatrix; */ - /* divide the lower column elements by the diagonal value */ - - /* matrix[i][k] /= matrix[k][k]; */ - /* p = matrix[i][k] */ - p = *lColumnMatrix / temp; - *(lColumnMatrix++) = p; - + temp = ((*lColumnMatrix > 0) ? *lColumnMatrix : -(*lColumnMatrix)); + if (temp > p) { + p = temp; + k2 = i; + } + /* next line */ + lColumnMatrix += nb_compo; + } + + /* a whole rest of 0 -> non singular */ + if (p == 0.0) { + return OPJ_FALSE; + } + + /* should we permute ? */ + if (k2 != k) { + /*exchange of line */ + /* k2 > k */ + dstPermutations = tmpPermutations + k2 - k; + /* swap indices */ + t = *tmpPermutations; + *tmpPermutations = *dstPermutations; + *dstPermutations = t; + + /* and swap entire line. */ + lColumnMatrix = lTmpMatrix + (k2 - k) * nb_compo; + memcpy(p_swap_area, lColumnMatrix, lSwapSize); + memcpy(lColumnMatrix, lTmpMatrix, lSwapSize); + memcpy(lTmpMatrix, p_swap_area, lSwapSize); + } + + /* now update data in the rest of the line and line after */ + lDestMatrix = lTmpMatrix + k; + lColumnMatrix = lDestMatrix + nb_compo; + /* take the middle element */ + temp = *(lDestMatrix++); + + /* now compute up data (i.e. coeff up of the diagonal). */ + for (i = offset; i < nb_compo; ++i) { + /*lColumnMatrix; */ + /* divide the lower column elements by the diagonal value */ + + /* matrix[i][k] /= matrix[k][k]; */ + /* p = matrix[i][k] */ + p = *lColumnMatrix / temp; + *(lColumnMatrix++) = p; + for (j = /* k + 1 */ offset; j < nb_compo; ++j) { - /* matrix[i][j] -= matrix[i][k] * matrix[k][j]; */ - *(lColumnMatrix++) -= p * (*(lDestMatrix++)); - } - /* come back to the k+1th element */ - lDestMatrix -= lStride; - /* go to kth element of the next line */ - lColumnMatrix += k; - } - - /* offset is now k+2 */ - ++offset; - /* 1 element less for stride */ - --lStride; - /* next line */ - lTmpMatrix+=nb_compo; - /* next permutation element */ - ++tmpPermutations; - } + /* matrix[i][j] -= matrix[i][k] * matrix[k][j]; */ + *(lColumnMatrix++) -= p * (*(lDestMatrix++)); + } + /* come back to the k+1th element */ + lDestMatrix -= lStride; + /* go to kth element of the next line */ + lColumnMatrix += k; + } + + /* offset is now k+2 */ + ++offset; + /* 1 element less for stride */ + --lStride; + /* next line */ + lTmpMatrix += nb_compo; + /* next permutation element */ + ++tmpPermutations; + } return OPJ_TRUE; } - -static void opj_lupSolve (OPJ_FLOAT32 * pResult, - OPJ_FLOAT32 * pMatrix, - OPJ_FLOAT32 * pVector, - OPJ_UINT32* pPermutations, - OPJ_UINT32 nb_compo,OPJ_FLOAT32 * p_intermediate_data) + +static void opj_lupSolve(OPJ_FLOAT32 * pResult, + OPJ_FLOAT32 * pMatrix, + OPJ_FLOAT32 * pVector, + OPJ_UINT32* pPermutations, + OPJ_UINT32 nb_compo, OPJ_FLOAT32 * p_intermediate_data) { - OPJ_INT32 k; - OPJ_UINT32 i,j; - OPJ_FLOAT32 sum; - OPJ_FLOAT32 u; - OPJ_UINT32 lStride = nb_compo+1; - OPJ_FLOAT32 * lCurrentPtr; - OPJ_FLOAT32 * lIntermediatePtr; - OPJ_FLOAT32 * lDestPtr; - OPJ_FLOAT32 * lTmpMatrix; - OPJ_FLOAT32 * lLineMatrix = pMatrix; - OPJ_FLOAT32 * lBeginPtr = pResult + nb_compo - 1; - OPJ_FLOAT32 * lGeneratedData; - OPJ_UINT32 * lCurrentPermutationPtr = pPermutations; - - - lIntermediatePtr = p_intermediate_data; - lGeneratedData = p_intermediate_data + nb_compo - 1; - + OPJ_INT32 k; + OPJ_UINT32 i, j; + OPJ_FLOAT32 sum; + OPJ_FLOAT32 u; + OPJ_UINT32 lStride = nb_compo + 1; + OPJ_FLOAT32 * lCurrentPtr; + OPJ_FLOAT32 * lIntermediatePtr; + OPJ_FLOAT32 * lDestPtr; + OPJ_FLOAT32 * lTmpMatrix; + OPJ_FLOAT32 * lLineMatrix = pMatrix; + OPJ_FLOAT32 * lBeginPtr = pResult + nb_compo - 1; + OPJ_FLOAT32 * lGeneratedData; + OPJ_UINT32 * lCurrentPermutationPtr = pPermutations; + + + lIntermediatePtr = p_intermediate_data; + lGeneratedData = p_intermediate_data + nb_compo - 1; + for (i = 0; i < nb_compo; ++i) { - sum = 0.0; - lCurrentPtr = p_intermediate_data; - lTmpMatrix = lLineMatrix; - for (j = 1; j <= i; ++j) - { - /* sum += matrix[i][j-1] * y[j-1]; */ - sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++)); + sum = 0.0; + lCurrentPtr = p_intermediate_data; + lTmpMatrix = lLineMatrix; + for (j = 1; j <= i; ++j) { + /* sum += matrix[i][j-1] * y[j-1]; */ + sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++)); } - /*y[i] = pVector[pPermutations[i]] - sum; */ + /*y[i] = pVector[pPermutations[i]] - sum; */ *(lIntermediatePtr++) = pVector[*(lCurrentPermutationPtr++)] - sum; - lLineMatrix += nb_compo; - } + lLineMatrix += nb_compo; + } - /* we take the last point of the matrix */ - lLineMatrix = pMatrix + nb_compo*nb_compo - 1; + /* we take the last point of the matrix */ + lLineMatrix = pMatrix + nb_compo * nb_compo - 1; - /* and we take after the last point of the destination vector */ - lDestPtr = pResult + nb_compo; + /* and we take after the last point of the destination vector */ + lDestPtr = pResult + nb_compo; assert(nb_compo != 0); - for (k = (OPJ_INT32)nb_compo - 1; k != -1 ; --k) { - sum = 0.0; - lTmpMatrix = lLineMatrix; + for (k = (OPJ_INT32)nb_compo - 1; k != -1 ; --k) { + sum = 0.0; + lTmpMatrix = lLineMatrix; u = *(lTmpMatrix++); - lCurrentPtr = lDestPtr--; + lCurrentPtr = lDestPtr--; for (j = (OPJ_UINT32)(k + 1); j < nb_compo; ++j) { - /* sum += matrix[k][j] * x[j] */ - sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++)); - } - /*x[k] = (y[k] - sum) / u; */ + /* sum += matrix[k][j] * x[j] */ + sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++)); + } + /*x[k] = (y[k] - sum) / u; */ *(lBeginPtr--) = (*(lGeneratedData--) - sum) / u; - lLineMatrix -= lStride; - } + lLineMatrix -= lStride; + } } - - -static void opj_lupInvert (OPJ_FLOAT32 * pSrcMatrix, - OPJ_FLOAT32 * pDestMatrix, - OPJ_UINT32 nb_compo, - OPJ_UINT32 * pPermutations, - OPJ_FLOAT32 * p_src_temp, - OPJ_FLOAT32 * p_dest_temp, - OPJ_FLOAT32 * p_swap_area ) + + +static void opj_lupInvert(OPJ_FLOAT32 * pSrcMatrix, + OPJ_FLOAT32 * pDestMatrix, + OPJ_UINT32 nb_compo, + OPJ_UINT32 * pPermutations, + OPJ_FLOAT32 * p_src_temp, + OPJ_FLOAT32 * p_dest_temp, + OPJ_FLOAT32 * p_swap_area) { - OPJ_UINT32 j,i; - OPJ_FLOAT32 * lCurrentPtr; - OPJ_FLOAT32 * lLineMatrix = pDestMatrix; - OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); - - for (j = 0; j < nb_compo; ++j) { - lCurrentPtr = lLineMatrix++; - memset(p_src_temp,0,lSwapSize); - p_src_temp[j] = 1.0; - opj_lupSolve(p_dest_temp,pSrcMatrix,p_src_temp, pPermutations, nb_compo , p_swap_area); - - for (i = 0; i < nb_compo; ++i) { - *(lCurrentPtr) = p_dest_temp[i]; - lCurrentPtr+=nb_compo; - } + OPJ_UINT32 j, i; + OPJ_FLOAT32 * lCurrentPtr; + OPJ_FLOAT32 * lLineMatrix = pDestMatrix; + OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); + + for (j = 0; j < nb_compo; ++j) { + lCurrentPtr = lLineMatrix++; + memset(p_src_temp, 0, lSwapSize); + p_src_temp[j] = 1.0; + opj_lupSolve(p_dest_temp, pSrcMatrix, p_src_temp, pPermutations, nb_compo, + p_swap_area); + + for (i = 0; i < nb_compo; ++i) { + *(lCurrentPtr) = p_dest_temp[i]; + lCurrentPtr += nb_compo; + } } } |