src/lib/openjp2/invert.c

   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@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