| /* | |
| * 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. | |
| * | |
| * Copyright (c) 2008, Jerome Fimes, Communications & Systemes <jerome.fimes@c-s.fr> | |
| * All rights reserved. | |
| * | |
| * Redistribution and use in source and binary forms, with or without | |
| * modification, are permitted provided that the following conditions | |
| * are met: | |
| * 1. Redistributions of source code must retain the above copyright | |
| * notice, this list of conditions and the following disclaimer. | |
| * 2. Redistributions in binary form must reproduce the above copyright | |
| * notice, this list of conditions and the following disclaimer in the | |
| * documentation and/or other materials provided with the distribution. | |
| * | |
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS `AS IS' | |
| * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE | |
| * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | |
| * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE | |
| * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR | |
| * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF | |
| * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS | |
| * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN | |
| * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) | |
| * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | |
| * POSSIBILITY OF SUCH DAMAGE. | |
| */ | |
| #include "opj_includes.h" | |
| /** | |
| * LUP decomposition | |
| */ | |
| static OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix, | |
| 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, | |
| 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); | |
| /* | |
| ========================================================== | |
| Matric inversion interface | |
| ========================================================== | |
| */ | |
| /** | |
| * Matrix inversion. | |
| */ | |
| OPJ_BOOL opj_matrix_inversion_f(OPJ_FLOAT32 * pSrcMatrix, | |
| 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); | |
| return OPJ_TRUE; | |
| } | |
| /* | |
| ========================================================== | |
| Local functions | |
| ========================================================== | |
| */ | |
| 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 */ | |
| 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; | |
| 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; | |
| } | |
| 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) | |
| { | |
| 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++)); | |
| } | |
| /*y[i] = pVector[pPermutations[i]] - sum; */ | |
| *(lIntermediatePtr++) = pVector[*(lCurrentPermutationPtr++)] - sum; | |
| lLineMatrix += nb_compo; | |
| } | |
| /* 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; | |
| assert(nb_compo != 0); | |
| for (k = (OPJ_INT32)nb_compo - 1; k != -1 ; --k) { | |
| sum = 0.0; | |
| lTmpMatrix = lLineMatrix; | |
| u = *(lTmpMatrix++); | |
| 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; */ | |
| *(lBeginPtr--) = (*(lGeneratedData--) - sum) / u; | |
| 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) | |
| { | |
| 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; | |
| } | |
| } | |
| } | |