/src/openjpeg/src/lib/openjp2/invert.c
Line | Count | Source (jump to first uncovered line) |
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 | 0 | { |
74 | 0 | OPJ_BYTE * l_data = 00; |
75 | 0 | OPJ_UINT32 l_permutation_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_UINT32); |
76 | 0 | OPJ_UINT32 l_swap_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); |
77 | 0 | OPJ_UINT32 l_total_size = l_permutation_size + 3 * l_swap_size; |
78 | 0 | OPJ_UINT32 * lPermutations = 00; |
79 | 0 | OPJ_FLOAT32 * l_double_data = 00; |
80 | |
|
81 | 0 | l_data = (OPJ_BYTE *) opj_malloc(l_total_size); |
82 | 0 | if (l_data == 0) { |
83 | 0 | return OPJ_FALSE; |
84 | 0 | } |
85 | 0 | lPermutations = (OPJ_UINT32 *) l_data; |
86 | 0 | l_double_data = (OPJ_FLOAT32 *)(l_data + l_permutation_size); |
87 | 0 | memset(lPermutations, 0, l_permutation_size); |
88 | |
|
89 | 0 | if (! opj_lupDecompose(pSrcMatrix, lPermutations, l_double_data, nb_compo)) { |
90 | 0 | opj_free(l_data); |
91 | 0 | return OPJ_FALSE; |
92 | 0 | } |
93 | | |
94 | 0 | opj_lupInvert(pSrcMatrix, pDestMatrix, nb_compo, lPermutations, l_double_data, |
95 | 0 | l_double_data + nb_compo, l_double_data + 2 * nb_compo); |
96 | 0 | opj_free(l_data); |
97 | |
|
98 | 0 | return OPJ_TRUE; |
99 | 0 | } |
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 | 0 | { |
112 | 0 | OPJ_UINT32 * tmpPermutations = permutations; |
113 | 0 | OPJ_UINT32 * dstPermutations; |
114 | 0 | OPJ_UINT32 k2 = 0, t; |
115 | 0 | OPJ_FLOAT32 temp; |
116 | 0 | OPJ_UINT32 i, j, k; |
117 | 0 | OPJ_FLOAT32 p; |
118 | 0 | OPJ_UINT32 lLastColum = nb_compo - 1; |
119 | 0 | OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); |
120 | 0 | OPJ_FLOAT32 * lTmpMatrix = matrix; |
121 | 0 | OPJ_FLOAT32 * lColumnMatrix, * lDestMatrix; |
122 | 0 | OPJ_UINT32 offset = 1; |
123 | 0 | OPJ_UINT32 lStride = nb_compo - 1; |
124 | | |
125 | | /*initialize permutations */ |
126 | 0 | for (i = 0; i < nb_compo; ++i) { |
127 | 0 | *tmpPermutations++ = i; |
128 | 0 | } |
129 | | /* now make a pivot with column switch */ |
130 | 0 | tmpPermutations = permutations; |
131 | 0 | for (k = 0; k < lLastColum; ++k) { |
132 | 0 | p = 0.0; |
133 | | |
134 | | /* take the middle element */ |
135 | 0 | lColumnMatrix = lTmpMatrix + k; |
136 | | |
137 | | /* make permutation with the biggest value in the column */ |
138 | 0 | for (i = k; i < nb_compo; ++i) { |
139 | 0 | temp = ((*lColumnMatrix > 0) ? *lColumnMatrix : -(*lColumnMatrix)); |
140 | 0 | if (temp > p) { |
141 | 0 | p = temp; |
142 | 0 | k2 = i; |
143 | 0 | } |
144 | | /* next line */ |
145 | 0 | lColumnMatrix += nb_compo; |
146 | 0 | } |
147 | | |
148 | | /* a whole rest of 0 -> non singular */ |
149 | 0 | if (p == 0.0) { |
150 | 0 | return OPJ_FALSE; |
151 | 0 | } |
152 | | |
153 | | /* should we permute ? */ |
154 | 0 | if (k2 != k) { |
155 | | /*exchange of line */ |
156 | | /* k2 > k */ |
157 | 0 | dstPermutations = tmpPermutations + k2 - k; |
158 | | /* swap indices */ |
159 | 0 | t = *tmpPermutations; |
160 | 0 | *tmpPermutations = *dstPermutations; |
161 | 0 | *dstPermutations = t; |
162 | | |
163 | | /* and swap entire line. */ |
164 | 0 | lColumnMatrix = lTmpMatrix + (k2 - k) * nb_compo; |
165 | 0 | memcpy(p_swap_area, lColumnMatrix, lSwapSize); |
166 | 0 | memcpy(lColumnMatrix, lTmpMatrix, lSwapSize); |
167 | 0 | memcpy(lTmpMatrix, p_swap_area, lSwapSize); |
168 | 0 | } |
169 | | |
170 | | /* now update data in the rest of the line and line after */ |
171 | 0 | lDestMatrix = lTmpMatrix + k; |
172 | 0 | lColumnMatrix = lDestMatrix + nb_compo; |
173 | | /* take the middle element */ |
174 | 0 | temp = *(lDestMatrix++); |
175 | | |
176 | | /* now compute up data (i.e. coeff up of the diagonal). */ |
177 | 0 | 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 | 0 | p = *lColumnMatrix / temp; |
184 | 0 | *(lColumnMatrix++) = p; |
185 | |
|
186 | 0 | for (j = /* k + 1 */ offset; j < nb_compo; ++j) { |
187 | | /* matrix[i][j] -= matrix[i][k] * matrix[k][j]; */ |
188 | 0 | *(lColumnMatrix++) -= p * (*(lDestMatrix++)); |
189 | 0 | } |
190 | | /* come back to the k+1th element */ |
191 | 0 | lDestMatrix -= lStride; |
192 | | /* go to kth element of the next line */ |
193 | 0 | lColumnMatrix += k; |
194 | 0 | } |
195 | | |
196 | | /* offset is now k+2 */ |
197 | 0 | ++offset; |
198 | | /* 1 element less for stride */ |
199 | 0 | --lStride; |
200 | | /* next line */ |
201 | 0 | lTmpMatrix += nb_compo; |
202 | | /* next permutation element */ |
203 | 0 | ++tmpPermutations; |
204 | 0 | } |
205 | 0 | return OPJ_TRUE; |
206 | 0 | } |
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 | 0 | { |
214 | 0 | OPJ_INT32 k; |
215 | 0 | OPJ_UINT32 i, j; |
216 | 0 | OPJ_FLOAT32 sum; |
217 | 0 | OPJ_FLOAT32 u; |
218 | 0 | OPJ_UINT32 lStride = nb_compo + 1; |
219 | 0 | OPJ_FLOAT32 * lCurrentPtr; |
220 | 0 | OPJ_FLOAT32 * lIntermediatePtr; |
221 | 0 | OPJ_FLOAT32 * lDestPtr; |
222 | 0 | OPJ_FLOAT32 * lTmpMatrix; |
223 | 0 | OPJ_FLOAT32 * lLineMatrix = pMatrix; |
224 | 0 | OPJ_FLOAT32 * lBeginPtr = pResult + nb_compo - 1; |
225 | 0 | OPJ_FLOAT32 * lGeneratedData; |
226 | 0 | OPJ_UINT32 * lCurrentPermutationPtr = pPermutations; |
227 | | |
228 | |
|
229 | 0 | lIntermediatePtr = p_intermediate_data; |
230 | 0 | lGeneratedData = p_intermediate_data + nb_compo - 1; |
231 | |
|
232 | 0 | for (i = 0; i < nb_compo; ++i) { |
233 | 0 | sum = 0.0; |
234 | 0 | lCurrentPtr = p_intermediate_data; |
235 | 0 | lTmpMatrix = lLineMatrix; |
236 | 0 | for (j = 1; j <= i; ++j) { |
237 | | /* sum += matrix[i][j-1] * y[j-1]; */ |
238 | 0 | sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++)); |
239 | 0 | } |
240 | | /*y[i] = pVector[pPermutations[i]] - sum; */ |
241 | 0 | *(lIntermediatePtr++) = pVector[*(lCurrentPermutationPtr++)] - sum; |
242 | 0 | lLineMatrix += nb_compo; |
243 | 0 | } |
244 | | |
245 | | /* we take the last point of the matrix */ |
246 | 0 | lLineMatrix = pMatrix + nb_compo * nb_compo - 1; |
247 | | |
248 | | /* and we take after the last point of the destination vector */ |
249 | 0 | lDestPtr = pResult + nb_compo; |
250 | | |
251 | |
|
252 | 0 | assert(nb_compo != 0); |
253 | 0 | for (k = (OPJ_INT32)nb_compo - 1; k != -1 ; --k) { |
254 | 0 | sum = 0.0; |
255 | 0 | lTmpMatrix = lLineMatrix; |
256 | 0 | u = *(lTmpMatrix++); |
257 | 0 | lCurrentPtr = lDestPtr--; |
258 | 0 | for (j = (OPJ_UINT32)(k + 1); j < nb_compo; ++j) { |
259 | | /* sum += matrix[k][j] * x[j] */ |
260 | 0 | sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++)); |
261 | 0 | } |
262 | | /*x[k] = (y[k] - sum) / u; */ |
263 | 0 | *(lBeginPtr--) = (*(lGeneratedData--) - sum) / u; |
264 | 0 | lLineMatrix -= lStride; |
265 | 0 | } |
266 | 0 | } |
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 | 0 | { |
277 | 0 | OPJ_UINT32 j, i; |
278 | 0 | OPJ_FLOAT32 * lCurrentPtr; |
279 | 0 | OPJ_FLOAT32 * lLineMatrix = pDestMatrix; |
280 | 0 | OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); |
281 | |
|
282 | 0 | for (j = 0; j < nb_compo; ++j) { |
283 | 0 | lCurrentPtr = lLineMatrix++; |
284 | 0 | memset(p_src_temp, 0, lSwapSize); |
285 | 0 | p_src_temp[j] = 1.0; |
286 | 0 | opj_lupSolve(p_dest_temp, pSrcMatrix, p_src_temp, pPermutations, nb_compo, |
287 | 0 | p_swap_area); |
288 | |
|
289 | 0 | for (i = 0; i < nb_compo; ++i) { |
290 | 0 | *(lCurrentPtr) = p_dest_temp[i]; |
291 | 0 | lCurrentPtr += nb_compo; |
292 | 0 | } |
293 | 0 | } |
294 | 0 | } |
295 | | |