/* * Copyright (c) 2008, Jerome Fimes, Communications & Systemes * 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 ========================================================== */ 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 colum 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; } 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; } } 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; } } }