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,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*/
106OPJ_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 colum 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
207void 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
269void 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
295