michael@0: /*********************************************************************** michael@0: Copyright (c) 2006-2011, Skype Limited. All rights reserved. michael@0: Redistribution and use in source and binary forms, with or without michael@0: modification, are permitted provided that the following conditions michael@0: are met: michael@0: - Redistributions of source code must retain the above copyright notice, michael@0: this list of conditions and the following disclaimer. michael@0: - Redistributions in binary form must reproduce the above copyright michael@0: notice, this list of conditions and the following disclaimer in the michael@0: documentation and/or other materials provided with the distribution. michael@0: - Neither the name of Internet Society, IETF or IETF Trust, nor the michael@0: names of specific contributors, may be used to endorse or promote michael@0: products derived from this software without specific prior written michael@0: permission. michael@0: THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" michael@0: AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE michael@0: IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE michael@0: ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE michael@0: LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR michael@0: CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF michael@0: SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS michael@0: INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN michael@0: CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) michael@0: ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE michael@0: POSSIBILITY OF SUCH DAMAGE. michael@0: ***********************************************************************/ michael@0: michael@0: #ifdef HAVE_CONFIG_H michael@0: #include "config.h" michael@0: #endif michael@0: michael@0: #include "main_FLP.h" michael@0: #include "tuning_parameters.h" michael@0: michael@0: /********************************************************************** michael@0: * LDL Factorisation. Finds the upper triangular matrix L and the diagonal michael@0: * Matrix D (only the diagonal elements returned in a vector)such that michael@0: * the symmetric matric A is given by A = L*D*L'. michael@0: **********************************************************************/ michael@0: static OPUS_INLINE void silk_LDL_FLP( michael@0: silk_float *A, /* I/O Pointer to Symetric Square Matrix */ michael@0: opus_int M, /* I Size of Matrix */ michael@0: silk_float *L, /* I/O Pointer to Square Upper triangular Matrix */ michael@0: silk_float *Dinv /* I/O Pointer to vector holding the inverse diagonal elements of D */ michael@0: ); michael@0: michael@0: /********************************************************************** michael@0: * Function to solve linear equation Ax = b, when A is a MxM lower michael@0: * triangular matrix, with ones on the diagonal. michael@0: **********************************************************************/ michael@0: static OPUS_INLINE void silk_SolveWithLowerTriangularWdiagOnes_FLP( michael@0: const silk_float *L, /* I Pointer to Lower Triangular Matrix */ michael@0: opus_int M, /* I Dim of Matrix equation */ michael@0: const silk_float *b, /* I b Vector */ michael@0: silk_float *x /* O x Vector */ michael@0: ); michael@0: michael@0: /********************************************************************** michael@0: * Function to solve linear equation (A^T)x = b, when A is a MxM lower michael@0: * triangular, with ones on the diagonal. (ie then A^T is upper triangular) michael@0: **********************************************************************/ michael@0: static OPUS_INLINE void silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP( michael@0: const silk_float *L, /* I Pointer to Lower Triangular Matrix */ michael@0: opus_int M, /* I Dim of Matrix equation */ michael@0: const silk_float *b, /* I b Vector */ michael@0: silk_float *x /* O x Vector */ michael@0: ); michael@0: michael@0: /********************************************************************** michael@0: * Function to solve linear equation Ax = b, when A is a MxM michael@0: * symmetric square matrix - using LDL factorisation michael@0: **********************************************************************/ michael@0: void silk_solve_LDL_FLP( michael@0: silk_float *A, /* I/O Symmetric square matrix, out: reg. */ michael@0: const opus_int M, /* I Size of matrix */ michael@0: const silk_float *b, /* I Pointer to b vector */ michael@0: silk_float *x /* O Pointer to x solution vector */ michael@0: ) michael@0: { michael@0: opus_int i; michael@0: silk_float L[ MAX_MATRIX_SIZE ][ MAX_MATRIX_SIZE ]; michael@0: silk_float T[ MAX_MATRIX_SIZE ]; michael@0: silk_float Dinv[ MAX_MATRIX_SIZE ]; /* inverse diagonal elements of D*/ michael@0: michael@0: silk_assert( M <= MAX_MATRIX_SIZE ); michael@0: michael@0: /*************************************************** michael@0: Factorize A by LDL such that A = L*D*(L^T), michael@0: where L is lower triangular with ones on diagonal michael@0: ****************************************************/ michael@0: silk_LDL_FLP( A, M, &L[ 0 ][ 0 ], Dinv ); michael@0: michael@0: /**************************************************** michael@0: * substitute D*(L^T) = T. ie: michael@0: L*D*(L^T)*x = b => L*T = b <=> T = inv(L)*b michael@0: ******************************************************/ michael@0: silk_SolveWithLowerTriangularWdiagOnes_FLP( &L[ 0 ][ 0 ], M, b, T ); michael@0: michael@0: /**************************************************** michael@0: D*(L^T)*x = T <=> (L^T)*x = inv(D)*T, because D is michael@0: diagonal just multiply with 1/d_i michael@0: ****************************************************/ michael@0: for( i = 0; i < M; i++ ) { michael@0: T[ i ] = T[ i ] * Dinv[ i ]; michael@0: } michael@0: /**************************************************** michael@0: x = inv(L') * inv(D) * T michael@0: *****************************************************/ michael@0: silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP( &L[ 0 ][ 0 ], M, T, x ); michael@0: } michael@0: michael@0: static OPUS_INLINE void silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP( michael@0: const silk_float *L, /* I Pointer to Lower Triangular Matrix */ michael@0: opus_int M, /* I Dim of Matrix equation */ michael@0: const silk_float *b, /* I b Vector */ michael@0: silk_float *x /* O x Vector */ michael@0: ) michael@0: { michael@0: opus_int i, j; michael@0: silk_float temp; michael@0: const silk_float *ptr1; michael@0: michael@0: for( i = M - 1; i >= 0; i-- ) { michael@0: ptr1 = matrix_adr( L, 0, i, M ); michael@0: temp = 0; michael@0: for( j = M - 1; j > i ; j-- ) { michael@0: temp += ptr1[ j * M ] * x[ j ]; michael@0: } michael@0: temp = b[ i ] - temp; michael@0: x[ i ] = temp; michael@0: } michael@0: } michael@0: michael@0: static OPUS_INLINE void silk_SolveWithLowerTriangularWdiagOnes_FLP( michael@0: const silk_float *L, /* I Pointer to Lower Triangular Matrix */ michael@0: opus_int M, /* I Dim of Matrix equation */ michael@0: const silk_float *b, /* I b Vector */ michael@0: silk_float *x /* O x Vector */ michael@0: ) michael@0: { michael@0: opus_int i, j; michael@0: silk_float temp; michael@0: const silk_float *ptr1; michael@0: michael@0: for( i = 0; i < M; i++ ) { michael@0: ptr1 = matrix_adr( L, i, 0, M ); michael@0: temp = 0; michael@0: for( j = 0; j < i; j++ ) { michael@0: temp += ptr1[ j ] * x[ j ]; michael@0: } michael@0: temp = b[ i ] - temp; michael@0: x[ i ] = temp; michael@0: } michael@0: } michael@0: michael@0: static OPUS_INLINE void silk_LDL_FLP( michael@0: silk_float *A, /* I/O Pointer to Symetric Square Matrix */ michael@0: opus_int M, /* I Size of Matrix */ michael@0: silk_float *L, /* I/O Pointer to Square Upper triangular Matrix */ michael@0: silk_float *Dinv /* I/O Pointer to vector holding the inverse diagonal elements of D */ michael@0: ) michael@0: { michael@0: opus_int i, j, k, loop_count, err = 1; michael@0: silk_float *ptr1, *ptr2; michael@0: double temp, diag_min_value; michael@0: silk_float v[ MAX_MATRIX_SIZE ], D[ MAX_MATRIX_SIZE ]; /* temp arrays*/ michael@0: michael@0: silk_assert( M <= MAX_MATRIX_SIZE ); michael@0: michael@0: diag_min_value = FIND_LTP_COND_FAC * 0.5f * ( A[ 0 ] + A[ M * M - 1 ] ); michael@0: for( loop_count = 0; loop_count < M && err == 1; loop_count++ ) { michael@0: err = 0; michael@0: for( j = 0; j < M; j++ ) { michael@0: ptr1 = matrix_adr( L, j, 0, M ); michael@0: temp = matrix_ptr( A, j, j, M ); /* element in row j column j*/ michael@0: for( i = 0; i < j; i++ ) { michael@0: v[ i ] = ptr1[ i ] * D[ i ]; michael@0: temp -= ptr1[ i ] * v[ i ]; michael@0: } michael@0: if( temp < diag_min_value ) { michael@0: /* Badly conditioned matrix: add white noise and run again */ michael@0: temp = ( loop_count + 1 ) * diag_min_value - temp; michael@0: for( i = 0; i < M; i++ ) { michael@0: matrix_ptr( A, i, i, M ) += ( silk_float )temp; michael@0: } michael@0: err = 1; michael@0: break; michael@0: } michael@0: D[ j ] = ( silk_float )temp; michael@0: Dinv[ j ] = ( silk_float )( 1.0f / temp ); michael@0: matrix_ptr( L, j, j, M ) = 1.0f; michael@0: michael@0: ptr1 = matrix_adr( A, j, 0, M ); michael@0: ptr2 = matrix_adr( L, j + 1, 0, M); michael@0: for( i = j + 1; i < M; i++ ) { michael@0: temp = 0.0; michael@0: for( k = 0; k < j; k++ ) { michael@0: temp += ptr2[ k ] * v[ k ]; michael@0: } michael@0: matrix_ptr( L, i, j, M ) = ( silk_float )( ( ptr1[ i ] - temp ) * Dinv[ j ] ); michael@0: ptr2 += M; /* go to next column*/ michael@0: } michael@0: } michael@0: } michael@0: silk_assert( err == 0 ); michael@0: } michael@0: