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 */
37static 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 */
44static 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 */
54static 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 */
70OPJ_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*/
107static 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
208static 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
269static 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
296