v2/libopenjpeg/invert.c

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