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