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