295 lines
10 KiB
C
295 lines
10 KiB
C
/*
|
|
* The copyright in this software is being made available under the 2-clauses
|
|
* BSD License, included below. This software may be subject to other third
|
|
* party and contributor rights, including patent rights, and no such rights
|
|
* are granted under this license.
|
|
*
|
|
* Copyright (c) 2008, Jerome Fimes, Communications & Systemes <jerome.fimes@c-s.fr>
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
* documentation and/or other materials provided with the distribution.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS `AS IS'
|
|
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
|
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
#include "opj_includes.h"
|
|
|
|
/**
|
|
* LUP decomposition
|
|
*/
|
|
static OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix,
|
|
OPJ_UINT32 * permutations,
|
|
OPJ_FLOAT32 * p_swap_area,
|
|
OPJ_UINT32 nb_compo);
|
|
/**
|
|
* LUP solving
|
|
*/
|
|
static void opj_lupSolve(OPJ_FLOAT32 * pResult,
|
|
OPJ_FLOAT32* pMatrix,
|
|
OPJ_FLOAT32* pVector,
|
|
OPJ_UINT32* pPermutations,
|
|
OPJ_UINT32 nb_compo,
|
|
OPJ_FLOAT32 * p_intermediate_data);
|
|
|
|
/**
|
|
*LUP inversion (call with the result of lupDecompose)
|
|
*/
|
|
static void 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);
|
|
|
|
/*
|
|
==========================================================
|
|
Matric inversion interface
|
|
==========================================================
|
|
*/
|
|
/**
|
|
* Matrix inversion.
|
|
*/
|
|
OPJ_BOOL opj_matrix_inversion_f(OPJ_FLOAT32 * pSrcMatrix,
|
|
OPJ_FLOAT32 * pDestMatrix,
|
|
OPJ_UINT32 nb_compo)
|
|
{
|
|
OPJ_BYTE * l_data = 00;
|
|
OPJ_UINT32 l_permutation_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_UINT32);
|
|
OPJ_UINT32 l_swap_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32);
|
|
OPJ_UINT32 l_total_size = l_permutation_size + 3 * l_swap_size;
|
|
OPJ_UINT32 * lPermutations = 00;
|
|
OPJ_FLOAT32 * l_double_data = 00;
|
|
|
|
l_data = (OPJ_BYTE *) opj_malloc(l_total_size);
|
|
if (l_data == 0) {
|
|
return OPJ_FALSE;
|
|
}
|
|
lPermutations = (OPJ_UINT32 *) l_data;
|
|
l_double_data = (OPJ_FLOAT32 *)(l_data + l_permutation_size);
|
|
memset(lPermutations, 0, l_permutation_size);
|
|
|
|
if (! opj_lupDecompose(pSrcMatrix, lPermutations, l_double_data, nb_compo)) {
|
|
opj_free(l_data);
|
|
return OPJ_FALSE;
|
|
}
|
|
|
|
opj_lupInvert(pSrcMatrix, pDestMatrix, nb_compo, lPermutations, l_double_data,
|
|
l_double_data + nb_compo, l_double_data + 2 * nb_compo);
|
|
opj_free(l_data);
|
|
|
|
return OPJ_TRUE;
|
|
}
|
|
|
|
|
|
/*
|
|
==========================================================
|
|
Local functions
|
|
==========================================================
|
|
*/
|
|
static OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix,
|
|
OPJ_UINT32 * permutations,
|
|
OPJ_FLOAT32 * p_swap_area,
|
|
OPJ_UINT32 nb_compo)
|
|
{
|
|
OPJ_UINT32 * tmpPermutations = permutations;
|
|
OPJ_UINT32 * dstPermutations;
|
|
OPJ_UINT32 k2 = 0, t;
|
|
OPJ_FLOAT32 temp;
|
|
OPJ_UINT32 i, j, k;
|
|
OPJ_FLOAT32 p;
|
|
OPJ_UINT32 lLastColum = nb_compo - 1;
|
|
OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32);
|
|
OPJ_FLOAT32 * lTmpMatrix = matrix;
|
|
OPJ_FLOAT32 * lColumnMatrix, * lDestMatrix;
|
|
OPJ_UINT32 offset = 1;
|
|
OPJ_UINT32 lStride = nb_compo - 1;
|
|
|
|
/*initialize permutations */
|
|
for (i = 0; i < nb_compo; ++i) {
|
|
*tmpPermutations++ = i;
|
|
}
|
|
/* now make a pivot with column switch */
|
|
tmpPermutations = permutations;
|
|
for (k = 0; k < lLastColum; ++k) {
|
|
p = 0.0;
|
|
|
|
/* take the middle element */
|
|
lColumnMatrix = lTmpMatrix + k;
|
|
|
|
/* make permutation with the biggest value in the column */
|
|
for (i = k; i < nb_compo; ++i) {
|
|
temp = ((*lColumnMatrix > 0) ? *lColumnMatrix : -(*lColumnMatrix));
|
|
if (temp > p) {
|
|
p = temp;
|
|
k2 = i;
|
|
}
|
|
/* next line */
|
|
lColumnMatrix += nb_compo;
|
|
}
|
|
|
|
/* a whole rest of 0 -> non singular */
|
|
if (p == 0.0) {
|
|
return OPJ_FALSE;
|
|
}
|
|
|
|
/* should we permute ? */
|
|
if (k2 != k) {
|
|
/*exchange of line */
|
|
/* k2 > k */
|
|
dstPermutations = tmpPermutations + k2 - k;
|
|
/* swap indices */
|
|
t = *tmpPermutations;
|
|
*tmpPermutations = *dstPermutations;
|
|
*dstPermutations = t;
|
|
|
|
/* and swap entire line. */
|
|
lColumnMatrix = lTmpMatrix + (k2 - k) * nb_compo;
|
|
memcpy(p_swap_area, lColumnMatrix, lSwapSize);
|
|
memcpy(lColumnMatrix, lTmpMatrix, lSwapSize);
|
|
memcpy(lTmpMatrix, p_swap_area, lSwapSize);
|
|
}
|
|
|
|
/* now update data in the rest of the line and line after */
|
|
lDestMatrix = lTmpMatrix + k;
|
|
lColumnMatrix = lDestMatrix + nb_compo;
|
|
/* take the middle element */
|
|
temp = *(lDestMatrix++);
|
|
|
|
/* now compute up data (i.e. coeff up of the diagonal). */
|
|
for (i = offset; i < nb_compo; ++i) {
|
|
/*lColumnMatrix; */
|
|
/* divide the lower column elements by the diagonal value */
|
|
|
|
/* matrix[i][k] /= matrix[k][k]; */
|
|
/* p = matrix[i][k] */
|
|
p = *lColumnMatrix / temp;
|
|
*(lColumnMatrix++) = p;
|
|
|
|
for (j = /* k + 1 */ offset; j < nb_compo; ++j) {
|
|
/* matrix[i][j] -= matrix[i][k] * matrix[k][j]; */
|
|
*(lColumnMatrix++) -= p * (*(lDestMatrix++));
|
|
}
|
|
/* come back to the k+1th element */
|
|
lDestMatrix -= lStride;
|
|
/* go to kth element of the next line */
|
|
lColumnMatrix += k;
|
|
}
|
|
|
|
/* offset is now k+2 */
|
|
++offset;
|
|
/* 1 element less for stride */
|
|
--lStride;
|
|
/* next line */
|
|
lTmpMatrix += nb_compo;
|
|
/* next permutation element */
|
|
++tmpPermutations;
|
|
}
|
|
return OPJ_TRUE;
|
|
}
|
|
|
|
static void opj_lupSolve(OPJ_FLOAT32 * pResult,
|
|
OPJ_FLOAT32 * pMatrix,
|
|
OPJ_FLOAT32 * pVector,
|
|
OPJ_UINT32* pPermutations,
|
|
OPJ_UINT32 nb_compo, OPJ_FLOAT32 * p_intermediate_data)
|
|
{
|
|
OPJ_INT32 k;
|
|
OPJ_UINT32 i, j;
|
|
OPJ_FLOAT32 sum;
|
|
OPJ_FLOAT32 u;
|
|
OPJ_UINT32 lStride = nb_compo + 1;
|
|
OPJ_FLOAT32 * lCurrentPtr;
|
|
OPJ_FLOAT32 * lIntermediatePtr;
|
|
OPJ_FLOAT32 * lDestPtr;
|
|
OPJ_FLOAT32 * lTmpMatrix;
|
|
OPJ_FLOAT32 * lLineMatrix = pMatrix;
|
|
OPJ_FLOAT32 * lBeginPtr = pResult + nb_compo - 1;
|
|
OPJ_FLOAT32 * lGeneratedData;
|
|
OPJ_UINT32 * lCurrentPermutationPtr = pPermutations;
|
|
|
|
|
|
lIntermediatePtr = p_intermediate_data;
|
|
lGeneratedData = p_intermediate_data + nb_compo - 1;
|
|
|
|
for (i = 0; i < nb_compo; ++i) {
|
|
sum = 0.0;
|
|
lCurrentPtr = p_intermediate_data;
|
|
lTmpMatrix = lLineMatrix;
|
|
for (j = 1; j <= i; ++j) {
|
|
/* sum += matrix[i][j-1] * y[j-1]; */
|
|
sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++));
|
|
}
|
|
/*y[i] = pVector[pPermutations[i]] - sum; */
|
|
*(lIntermediatePtr++) = pVector[*(lCurrentPermutationPtr++)] - sum;
|
|
lLineMatrix += nb_compo;
|
|
}
|
|
|
|
/* we take the last point of the matrix */
|
|
lLineMatrix = pMatrix + nb_compo * nb_compo - 1;
|
|
|
|
/* and we take after the last point of the destination vector */
|
|
lDestPtr = pResult + nb_compo;
|
|
|
|
|
|
assert(nb_compo != 0);
|
|
for (k = (OPJ_INT32)nb_compo - 1; k != -1 ; --k) {
|
|
sum = 0.0;
|
|
lTmpMatrix = lLineMatrix;
|
|
u = *(lTmpMatrix++);
|
|
lCurrentPtr = lDestPtr--;
|
|
for (j = (OPJ_UINT32)(k + 1); j < nb_compo; ++j) {
|
|
/* sum += matrix[k][j] * x[j] */
|
|
sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++));
|
|
}
|
|
/*x[k] = (y[k] - sum) / u; */
|
|
*(lBeginPtr--) = (*(lGeneratedData--) - sum) / u;
|
|
lLineMatrix -= lStride;
|
|
}
|
|
}
|
|
|
|
|
|
static void 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)
|
|
{
|
|
OPJ_UINT32 j, i;
|
|
OPJ_FLOAT32 * lCurrentPtr;
|
|
OPJ_FLOAT32 * lLineMatrix = pDestMatrix;
|
|
OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32);
|
|
|
|
for (j = 0; j < nb_compo; ++j) {
|
|
lCurrentPtr = lLineMatrix++;
|
|
memset(p_src_temp, 0, lSwapSize);
|
|
p_src_temp[j] = 1.0;
|
|
opj_lupSolve(p_dest_temp, pSrcMatrix, p_src_temp, pPermutations, nb_compo,
|
|
p_swap_area);
|
|
|
|
for (i = 0; i < nb_compo; ++i) {
|
|
*(lCurrentPtr) = p_dest_temp[i];
|
|
lCurrentPtr += nb_compo;
|
|
}
|
|
}
|
|
}
|
|
|