Cosmetic changes in fsdk_define.h
[pdfium.git] / third_party / libopenjpeg20 / 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