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, 95 l_double_data + nb_compo, l_double_data + 2 * nb_compo); 96 opj_free(l_data); 97 98 return OPJ_TRUE; 99 } 100 101 102 /* 103 ========================================================== 104 Local functions 105 ========================================================== 106 */ 107 static OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix, 108 OPJ_UINT32 * permutations, 109 OPJ_FLOAT32 * p_swap_area, 110 OPJ_UINT32 nb_compo) 111 { 112 OPJ_UINT32 * tmpPermutations = permutations; 113 OPJ_UINT32 * dstPermutations; 114 OPJ_UINT32 k2 = 0, t; 115 OPJ_FLOAT32 temp; 116 OPJ_UINT32 i, j, k; 117 OPJ_FLOAT32 p; 118 OPJ_UINT32 lLastColum = nb_compo - 1; 119 OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); 120 OPJ_FLOAT32 * lTmpMatrix = matrix; 121 OPJ_FLOAT32 * lColumnMatrix, * lDestMatrix; 122 OPJ_UINT32 offset = 1; 123 OPJ_UINT32 lStride = nb_compo - 1; 124 125 /*initialize permutations */ 126 for (i = 0; i < nb_compo; ++i) { 127 *tmpPermutations++ = i; 128 } 129 /* now make a pivot with column switch */ 130 tmpPermutations = permutations; 131 for (k = 0; k < lLastColum; ++k) { 132 p = 0.0; 133 134 /* take the middle element */ 135 lColumnMatrix = lTmpMatrix + k; 136 137 /* make permutation with the biggest value in the column */ 138 for (i = k; i < nb_compo; ++i) { 139 temp = ((*lColumnMatrix > 0) ? *lColumnMatrix : -(*lColumnMatrix)); 140 if (temp > p) { 141 p = temp; 142 k2 = i; 143 } 144 /* next line */ 145 lColumnMatrix += nb_compo; 146 } 147 148 /* a whole rest of 0 -> non singular */ 149 if (p == 0.0) { 150 return OPJ_FALSE; 151 } 152 153 /* should we permute ? */ 154 if (k2 != k) { 155 /*exchange of line */ 156 /* k2 > k */ 157 dstPermutations = tmpPermutations + k2 - k; 158 /* swap indices */ 159 t = *tmpPermutations; 160 *tmpPermutations = *dstPermutations; 161 *dstPermutations = t; 162 163 /* and swap entire line. */ 164 lColumnMatrix = lTmpMatrix + (k2 - k) * nb_compo; 165 memcpy(p_swap_area, lColumnMatrix, lSwapSize); 166 memcpy(lColumnMatrix, lTmpMatrix, lSwapSize); 167 memcpy(lTmpMatrix, p_swap_area, lSwapSize); 168 } 169 170 /* now update data in the rest of the line and line after */ 171 lDestMatrix = lTmpMatrix + k; 172 lColumnMatrix = lDestMatrix + nb_compo; 173 /* take the middle element */ 174 temp = *(lDestMatrix++); 175 176 /* now compute up data (i.e. coeff up of the diagonal). */ 177 for (i = offset; i < nb_compo; ++i) { 178 /*lColumnMatrix; */ 179 /* divide the lower column elements by the diagonal value */ 180 181 /* matrix[i][k] /= matrix[k][k]; */ 182 /* p = matrix[i][k] */ 183 p = *lColumnMatrix / temp; 184 *(lColumnMatrix++) = p; 185 186 for (j = /* k + 1 */ offset; j < nb_compo; ++j) { 187 /* matrix[i][j] -= matrix[i][k] * matrix[k][j]; */ 188 *(lColumnMatrix++) -= p * (*(lDestMatrix++)); 189 } 190 /* come back to the k+1th element */ 191 lDestMatrix -= lStride; 192 /* go to kth element of the next line */ 193 lColumnMatrix += k; 194 } 195 196 /* offset is now k+2 */ 197 ++offset; 198 /* 1 element less for stride */ 199 --lStride; 200 /* next line */ 201 lTmpMatrix += nb_compo; 202 /* next permutation element */ 203 ++tmpPermutations; 204 } 205 return OPJ_TRUE; 206 } 207 208 static void opj_lupSolve(OPJ_FLOAT32 * pResult, 209 OPJ_FLOAT32 * pMatrix, 210 OPJ_FLOAT32 * pVector, 211 OPJ_UINT32* pPermutations, 212 OPJ_UINT32 nb_compo, OPJ_FLOAT32 * p_intermediate_data) 213 { 214 OPJ_INT32 k; 215 OPJ_UINT32 i, j; 216 OPJ_FLOAT32 sum; 217 OPJ_FLOAT32 u; 218 OPJ_UINT32 lStride = nb_compo + 1; 219 OPJ_FLOAT32 * lCurrentPtr; 220 OPJ_FLOAT32 * lIntermediatePtr; 221 OPJ_FLOAT32 * lDestPtr; 222 OPJ_FLOAT32 * lTmpMatrix; 223 OPJ_FLOAT32 * lLineMatrix = pMatrix; 224 OPJ_FLOAT32 * lBeginPtr = pResult + nb_compo - 1; 225 OPJ_FLOAT32 * lGeneratedData; 226 OPJ_UINT32 * lCurrentPermutationPtr = pPermutations; 227 228 229 lIntermediatePtr = p_intermediate_data; 230 lGeneratedData = p_intermediate_data + nb_compo - 1; 231 232 for (i = 0; i < nb_compo; ++i) { 233 sum = 0.0; 234 lCurrentPtr = p_intermediate_data; 235 lTmpMatrix = lLineMatrix; 236 for (j = 1; j <= i; ++j) { 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, 287 p_swap_area); 288 289 for (i = 0; i < nb_compo; ++i) { 290 *(lCurrentPtr) = p_dest_temp[i]; 291 lCurrentPtr += nb_compo; 292 } 293 } 294 } 295 296
