Home | History | Annotate | Download | only in libopenjpeg20
      1 /*
      2  * The copyright in this software is being made available under the 2-clauses
      3  * BSD License, included below. This software may be subject to other third
      4  * party and contributor rights, including patent rights, and no such rights
      5  * are granted under this license.
      6  *
      7  * Copyright (c) 2008, Jerome Fimes, Communications & Systemes <jerome.fimes (at) c-s.fr>
      8  * All rights reserved.
      9  *
     10  * Redistribution and use in source and binary forms, with or without
     11  * modification, are permitted provided that the following conditions
     12  * are met:
     13  * 1. Redistributions of source code must retain the above copyright
     14  *    notice, this list of conditions and the following disclaimer.
     15  * 2. Redistributions in binary form must reproduce the above copyright
     16  *    notice, this list of conditions and the following disclaimer in the
     17  *    documentation and/or other materials provided with the distribution.
     18  *
     19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS `AS IS'
     20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
     21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
     22  * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
     23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
     24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
     25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
     26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
     27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
     28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
     29  * POSSIBILITY OF SUCH DAMAGE.
     30  */
     31 
     32 #include "opj_includes.h"
     33 
     34 /**
     35  * LUP decomposition
     36  */
     37 static OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix,
     38                                  OPJ_UINT32 * permutations,
     39                                  OPJ_FLOAT32 * p_swap_area,
     40                                  OPJ_UINT32 nb_compo);
     41 /**
     42  * LUP solving
     43  */
     44 static void opj_lupSolve(OPJ_FLOAT32 * pResult,
     45                          OPJ_FLOAT32* pMatrix,
     46                          OPJ_FLOAT32* pVector,
     47                          OPJ_UINT32* pPermutations,
     48                          OPJ_UINT32 nb_compo,
     49                          OPJ_FLOAT32 * p_intermediate_data);
     50 
     51 /**
     52  *LUP inversion (call with the result of lupDecompose)
     53  */
     54 static void opj_lupInvert ( OPJ_FLOAT32 * pSrcMatrix,
     55                             OPJ_FLOAT32 * pDestMatrix,
     56                             OPJ_UINT32 nb_compo,
     57                             OPJ_UINT32 * pPermutations,
     58                             OPJ_FLOAT32 * p_src_temp,
     59                             OPJ_FLOAT32 * p_dest_temp,
     60                             OPJ_FLOAT32 * p_swap_area);
     61 
     62 /*
     63 ==========================================================
     64    Matric inversion interface
     65 ==========================================================
     66 */
     67 /**
     68  * Matrix inversion.
     69  */
     70 OPJ_BOOL opj_matrix_inversion_f(OPJ_FLOAT32 * pSrcMatrix,
     71                                 OPJ_FLOAT32 * pDestMatrix,
     72                                 OPJ_UINT32 nb_compo)
     73 {
     74 	OPJ_BYTE * l_data = 00;
     75 	OPJ_UINT32 l_permutation_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_UINT32);
     76 	OPJ_UINT32 l_swap_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32);
     77 	OPJ_UINT32 l_total_size = l_permutation_size + 3 * l_swap_size;
     78 	OPJ_UINT32 * lPermutations = 00;
     79 	OPJ_FLOAT32 * l_double_data = 00;
     80 
     81 	l_data = (OPJ_BYTE *) opj_malloc(l_total_size);
     82 	if (l_data == 0) {
     83 		return OPJ_FALSE;
     84 	}
     85 	lPermutations = (OPJ_UINT32 *) l_data;
     86 	l_double_data = (OPJ_FLOAT32 *) (l_data + l_permutation_size);
     87 	memset(lPermutations,0,l_permutation_size);
     88 
     89 	if(! opj_lupDecompose(pSrcMatrix,lPermutations,l_double_data,nb_compo)) {
     90 		opj_free(l_data);
     91 		return OPJ_FALSE;
     92 	}
     93 
     94     opj_lupInvert(pSrcMatrix,pDestMatrix,nb_compo,lPermutations,l_double_data,l_double_data + nb_compo,l_double_data + 2*nb_compo);
     95 	opj_free(l_data);
     96 
     97     return OPJ_TRUE;
     98 }
     99 
    100 
    101 /*
    102 ==========================================================
    103    Local functions
    104 ==========================================================
    105 */
    106 static OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix,OPJ_UINT32 * permutations,
    107                           OPJ_FLOAT32 * p_swap_area,
    108                           OPJ_UINT32 nb_compo)
    109 {
    110 	OPJ_UINT32 * tmpPermutations = permutations;
    111 	OPJ_UINT32 * dstPermutations;
    112 	OPJ_UINT32 k2=0,t;
    113 	OPJ_FLOAT32 temp;
    114 	OPJ_UINT32 i,j,k;
    115 	OPJ_FLOAT32 p;
    116 	OPJ_UINT32 lLastColum = nb_compo - 1;
    117 	OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32);
    118 	OPJ_FLOAT32 * lTmpMatrix = matrix;
    119 	OPJ_FLOAT32 * lColumnMatrix,* lDestMatrix;
    120 	OPJ_UINT32 offset = 1;
    121 	OPJ_UINT32 lStride = nb_compo-1;
    122 
    123 	/*initialize permutations */
    124 	for (i = 0; i < nb_compo; ++i)
    125 	{
    126     	*tmpPermutations++ = i;
    127 	}
    128 	/* now make a pivot with column switch */
    129 	tmpPermutations = permutations;
    130 	for (k = 0; k < lLastColum; ++k) {
    131 		p = 0.0;
    132 
    133 		/* take the middle element */
    134 		lColumnMatrix = lTmpMatrix + k;
    135 
    136 		/* make permutation with the biggest value in the column */
    137         for (i = k; i < nb_compo; ++i) {
    138 			temp = ((*lColumnMatrix > 0) ? *lColumnMatrix : -(*lColumnMatrix));
    139      		if (temp > p) {
    140      			p = temp;
    141      			k2 = i;
    142      		}
    143 			/* next line */
    144 			lColumnMatrix += nb_compo;
    145      	}
    146 
    147      	/* a whole rest of 0 -> non singular */
    148      	if (p == 0.0) {
    149     		return OPJ_FALSE;
    150 		}
    151 
    152 		/* should we permute ? */
    153 		if (k2 != k) {
    154 			/*exchange of line */
    155      		/* k2 > k */
    156 			dstPermutations = tmpPermutations + k2 - k;
    157 			/* swap indices */
    158 			t = *tmpPermutations;
    159      		*tmpPermutations = *dstPermutations;
    160      		*dstPermutations = t;
    161 
    162 			/* and swap entire line. */
    163 			lColumnMatrix = lTmpMatrix + (k2 - k) * nb_compo;
    164 			memcpy(p_swap_area,lColumnMatrix,lSwapSize);
    165 			memcpy(lColumnMatrix,lTmpMatrix,lSwapSize);
    166 			memcpy(lTmpMatrix,p_swap_area,lSwapSize);
    167 		}
    168 
    169 		/* now update data in the rest of the line and line after */
    170 		lDestMatrix = lTmpMatrix + k;
    171 		lColumnMatrix = lDestMatrix + nb_compo;
    172 		/* take the middle element */
    173 		temp = *(lDestMatrix++);
    174 
    175 		/* now compute up data (i.e. coeff up of the diagonal). */
    176      	for (i = offset; i < nb_compo; ++i)  {
    177 			/*lColumnMatrix; */
    178 			/* divide the lower column elements by the diagonal value */
    179 
    180 			/* matrix[i][k] /= matrix[k][k]; */
    181      		/* p = matrix[i][k] */
    182 			p = *lColumnMatrix / temp;
    183 			*(lColumnMatrix++) = p;
    184 
    185             for (j = /* k + 1 */ offset; j < nb_compo; ++j) {
    186 				/* matrix[i][j] -= matrix[i][k] * matrix[k][j]; */
    187      			*(lColumnMatrix++) -= p * (*(lDestMatrix++));
    188 			}
    189 			/* come back to the k+1th element */
    190 			lDestMatrix -= lStride;
    191 			/* go to kth element of the next line */
    192 			lColumnMatrix += k;
    193      	}
    194 
    195 		/* offset is now k+2 */
    196 		++offset;
    197 		/* 1 element less for stride */
    198 		--lStride;
    199 		/* next line */
    200 		lTmpMatrix+=nb_compo;
    201 		/* next permutation element */
    202 		++tmpPermutations;
    203 	}
    204     return OPJ_TRUE;
    205 }
    206 
    207 static void opj_lupSolve (OPJ_FLOAT32 * pResult,
    208                    OPJ_FLOAT32 * pMatrix,
    209                    OPJ_FLOAT32 * pVector,
    210                    OPJ_UINT32* pPermutations,
    211                    OPJ_UINT32 nb_compo,OPJ_FLOAT32 * p_intermediate_data)
    212 {
    213 	OPJ_INT32 k;
    214     OPJ_UINT32 i,j;
    215 	OPJ_FLOAT32 sum;
    216 	OPJ_FLOAT32 u;
    217     OPJ_UINT32 lStride = nb_compo+1;
    218 	OPJ_FLOAT32 * lCurrentPtr;
    219 	OPJ_FLOAT32 * lIntermediatePtr;
    220 	OPJ_FLOAT32 * lDestPtr;
    221 	OPJ_FLOAT32 * lTmpMatrix;
    222 	OPJ_FLOAT32 * lLineMatrix = pMatrix;
    223 	OPJ_FLOAT32 * lBeginPtr = pResult + nb_compo - 1;
    224 	OPJ_FLOAT32 * lGeneratedData;
    225 	OPJ_UINT32 * lCurrentPermutationPtr = pPermutations;
    226 
    227 
    228 	lIntermediatePtr = p_intermediate_data;
    229 	lGeneratedData = p_intermediate_data + nb_compo - 1;
    230 
    231     for (i = 0; i < nb_compo; ++i) {
    232        	sum = 0.0;
    233 		lCurrentPtr = p_intermediate_data;
    234 		lTmpMatrix = lLineMatrix;
    235         for (j = 1; j <= i; ++j)
    236 		{
    237 			/* sum += matrix[i][j-1] * y[j-1]; */
    238         	sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++));
    239         }
    240 		/*y[i] = pVector[pPermutations[i]] - sum; */
    241         *(lIntermediatePtr++) = pVector[*(lCurrentPermutationPtr++)] - sum;
    242 		lLineMatrix += nb_compo;
    243 	}
    244 
    245 	/* we take the last point of the matrix */
    246 	lLineMatrix = pMatrix + nb_compo*nb_compo - 1;
    247 
    248 	/* and we take after the last point of the destination vector */
    249 	lDestPtr = pResult + nb_compo;
    250 
    251 
    252     assert(nb_compo != 0);
    253 	for (k = (OPJ_INT32)nb_compo - 1; k != -1 ; --k) {
    254 		sum = 0.0;
    255 		lTmpMatrix = lLineMatrix;
    256         u = *(lTmpMatrix++);
    257 		lCurrentPtr = lDestPtr--;
    258         for (j = (OPJ_UINT32)(k + 1); j < nb_compo; ++j) {
    259 			/* sum += matrix[k][j] * x[j] */
    260         	sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++));
    261 		}
    262 		/*x[k] = (y[k] - sum) / u; */
    263         *(lBeginPtr--) = (*(lGeneratedData--) - sum) / u;
    264 		lLineMatrix -= lStride;
    265 	}
    266 }
    267 
    268 
    269 static void opj_lupInvert (OPJ_FLOAT32 * pSrcMatrix,
    270                     OPJ_FLOAT32 * pDestMatrix,
    271                     OPJ_UINT32 nb_compo,
    272                     OPJ_UINT32 * pPermutations,
    273                     OPJ_FLOAT32 * p_src_temp,
    274                     OPJ_FLOAT32 * p_dest_temp,
    275                     OPJ_FLOAT32 * p_swap_area )
    276 {
    277 	OPJ_UINT32 j,i;
    278 	OPJ_FLOAT32 * lCurrentPtr;
    279 	OPJ_FLOAT32 * lLineMatrix = pDestMatrix;
    280 	OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32);
    281 
    282 	for (j = 0; j < nb_compo; ++j) {
    283 		lCurrentPtr = lLineMatrix++;
    284         memset(p_src_temp,0,lSwapSize);
    285     	p_src_temp[j] = 1.0;
    286 		opj_lupSolve(p_dest_temp,pSrcMatrix,p_src_temp, pPermutations, nb_compo , p_swap_area);
    287 
    288 		for (i = 0; i < nb_compo; ++i) {
    289     		*(lCurrentPtr) = p_dest_temp[i];
    290 			lCurrentPtr+=nb_compo;
    291     	}
    292     }
    293 }
    294 
    295