src/lib/openjp2/invert.c

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