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
