OLD | NEW |
| (Empty) |
1 /*********************************************************************** | |
2 Copyright (c) 2006-2011, Skype Limited. All rights reserved. | |
3 Redistribution and use in source and binary forms, with or without | |
4 modification, are permitted provided that the following conditions | |
5 are met: | |
6 - Redistributions of source code must retain the above copyright notice, | |
7 this list of conditions and the following disclaimer. | |
8 - Redistributions in binary form must reproduce the above copyright | |
9 notice, this list of conditions and the following disclaimer in the | |
10 documentation and/or other materials provided with the distribution. | |
11 - Neither the name of Internet Society, IETF or IETF Trust, nor the | |
12 names of specific contributors, may be used to endorse or promote | |
13 products derived from this software without specific prior written | |
14 permission. | |
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" | |
16 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE | |
17 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | |
18 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE | |
19 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR | |
20 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF | |
21 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS | |
22 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN | |
23 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) | |
24 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | |
25 POSSIBILITY OF SUCH DAMAGE. | |
26 ***********************************************************************/ | |
27 | |
28 #ifdef HAVE_CONFIG_H | |
29 #include "config.h" | |
30 #endif | |
31 | |
32 #include "main_FLP.h" | |
33 #include "tuning_parameters.h" | |
34 | |
35 /********************************************************************** | |
36 * LDL Factorisation. Finds the upper triangular matrix L and the diagonal | |
37 * Matrix D (only the diagonal elements returned in a vector)such that | |
38 * the symmetric matric A is given by A = L*D*L'. | |
39 **********************************************************************/ | |
40 static OPUS_INLINE void silk_LDL_FLP( | |
41 silk_float *A, /* I/O Pointer to Symetric Square Matrix
*/ | |
42 opus_int M, /* I Size of Matrix
*/ | |
43 silk_float *L, /* I/O Pointer to Square Upper triangular M
atrix */ | |
44 silk_float *Dinv /* I/O Pointer to vector holding the invers
e diagonal elements of D */ | |
45 ); | |
46 | |
47 /********************************************************************** | |
48 * Function to solve linear equation Ax = b, when A is a MxM lower | |
49 * triangular matrix, with ones on the diagonal. | |
50 **********************************************************************/ | |
51 static OPUS_INLINE void silk_SolveWithLowerTriangularWdiagOnes_FLP( | |
52 const silk_float *L, /* I Pointer to Lower Triangular Matrix
*/ | |
53 opus_int M, /* I Dim of Matrix equation
*/ | |
54 const silk_float *b, /* I b Vector
*/ | |
55 silk_float *x /* O x Vector
*/ | |
56 ); | |
57 | |
58 /********************************************************************** | |
59 * Function to solve linear equation (A^T)x = b, when A is a MxM lower | |
60 * triangular, with ones on the diagonal. (ie then A^T is upper triangular) | |
61 **********************************************************************/ | |
62 static OPUS_INLINE void silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP( | |
63 const silk_float *L, /* I Pointer to Lower Triangular Matrix
*/ | |
64 opus_int M, /* I Dim of Matrix equation
*/ | |
65 const silk_float *b, /* I b Vector
*/ | |
66 silk_float *x /* O x Vector
*/ | |
67 ); | |
68 | |
69 /********************************************************************** | |
70 * Function to solve linear equation Ax = b, when A is a MxM | |
71 * symmetric square matrix - using LDL factorisation | |
72 **********************************************************************/ | |
73 void silk_solve_LDL_FLP( | |
74 silk_float *A, /* I/O
Symmetric square matrix, out: reg. */ | |
75 const opus_int M, /* I
Size of matrix */ | |
76 const silk_float *b, /* I
Pointer to b vector */ | |
77 silk_float *x /* O
Pointer to x solution vector */ | |
78 ) | |
79 { | |
80 opus_int i; | |
81 silk_float L[ MAX_MATRIX_SIZE ][ MAX_MATRIX_SIZE ]; | |
82 silk_float T[ MAX_MATRIX_SIZE ]; | |
83 silk_float Dinv[ MAX_MATRIX_SIZE ]; /* inverse diagonal elements of D*/ | |
84 | |
85 silk_assert( M <= MAX_MATRIX_SIZE ); | |
86 | |
87 /*************************************************** | |
88 Factorize A by LDL such that A = L*D*(L^T), | |
89 where L is lower triangular with ones on diagonal | |
90 ****************************************************/ | |
91 silk_LDL_FLP( A, M, &L[ 0 ][ 0 ], Dinv ); | |
92 | |
93 /**************************************************** | |
94 * substitute D*(L^T) = T. ie: | |
95 L*D*(L^T)*x = b => L*T = b <=> T = inv(L)*b | |
96 ******************************************************/ | |
97 silk_SolveWithLowerTriangularWdiagOnes_FLP( &L[ 0 ][ 0 ], M, b, T ); | |
98 | |
99 /**************************************************** | |
100 D*(L^T)*x = T <=> (L^T)*x = inv(D)*T, because D is | |
101 diagonal just multiply with 1/d_i | |
102 ****************************************************/ | |
103 for( i = 0; i < M; i++ ) { | |
104 T[ i ] = T[ i ] * Dinv[ i ]; | |
105 } | |
106 /**************************************************** | |
107 x = inv(L') * inv(D) * T | |
108 *****************************************************/ | |
109 silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP( &L[ 0 ][ 0 ], M, T, x )
; | |
110 } | |
111 | |
112 static OPUS_INLINE void silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP( | |
113 const silk_float *L, /* I Pointer to Lower Triangular Matrix
*/ | |
114 opus_int M, /* I Dim of Matrix equation
*/ | |
115 const silk_float *b, /* I b Vector
*/ | |
116 silk_float *x /* O x Vector
*/ | |
117 ) | |
118 { | |
119 opus_int i, j; | |
120 silk_float temp; | |
121 const silk_float *ptr1; | |
122 | |
123 for( i = M - 1; i >= 0; i-- ) { | |
124 ptr1 = matrix_adr( L, 0, i, M ); | |
125 temp = 0; | |
126 for( j = M - 1; j > i ; j-- ) { | |
127 temp += ptr1[ j * M ] * x[ j ]; | |
128 } | |
129 temp = b[ i ] - temp; | |
130 x[ i ] = temp; | |
131 } | |
132 } | |
133 | |
134 static OPUS_INLINE void silk_SolveWithLowerTriangularWdiagOnes_FLP( | |
135 const silk_float *L, /* I Pointer to Lower Triangular Matrix
*/ | |
136 opus_int M, /* I Dim of Matrix equation
*/ | |
137 const silk_float *b, /* I b Vector
*/ | |
138 silk_float *x /* O x Vector
*/ | |
139 ) | |
140 { | |
141 opus_int i, j; | |
142 silk_float temp; | |
143 const silk_float *ptr1; | |
144 | |
145 for( i = 0; i < M; i++ ) { | |
146 ptr1 = matrix_adr( L, i, 0, M ); | |
147 temp = 0; | |
148 for( j = 0; j < i; j++ ) { | |
149 temp += ptr1[ j ] * x[ j ]; | |
150 } | |
151 temp = b[ i ] - temp; | |
152 x[ i ] = temp; | |
153 } | |
154 } | |
155 | |
156 static OPUS_INLINE void silk_LDL_FLP( | |
157 silk_float *A, /* I/O Pointer to Symetric Square Matrix
*/ | |
158 opus_int M, /* I Size of Matrix
*/ | |
159 silk_float *L, /* I/O Pointer to Square Upper triangular M
atrix */ | |
160 silk_float *Dinv /* I/O Pointer to vector holding the invers
e diagonal elements of D */ | |
161 ) | |
162 { | |
163 opus_int i, j, k, loop_count, err = 1; | |
164 silk_float *ptr1, *ptr2; | |
165 double temp, diag_min_value; | |
166 silk_float v[ MAX_MATRIX_SIZE ], D[ MAX_MATRIX_SIZE ]; /* temp arrays*/ | |
167 | |
168 silk_assert( M <= MAX_MATRIX_SIZE ); | |
169 | |
170 diag_min_value = FIND_LTP_COND_FAC * 0.5f * ( A[ 0 ] + A[ M * M - 1 ] ); | |
171 for( loop_count = 0; loop_count < M && err == 1; loop_count++ ) { | |
172 err = 0; | |
173 for( j = 0; j < M; j++ ) { | |
174 ptr1 = matrix_adr( L, j, 0, M ); | |
175 temp = matrix_ptr( A, j, j, M ); /* element in row j column j*/ | |
176 for( i = 0; i < j; i++ ) { | |
177 v[ i ] = ptr1[ i ] * D[ i ]; | |
178 temp -= ptr1[ i ] * v[ i ]; | |
179 } | |
180 if( temp < diag_min_value ) { | |
181 /* Badly conditioned matrix: add white noise and run again */ | |
182 temp = ( loop_count + 1 ) * diag_min_value - temp; | |
183 for( i = 0; i < M; i++ ) { | |
184 matrix_ptr( A, i, i, M ) += ( silk_float )temp; | |
185 } | |
186 err = 1; | |
187 break; | |
188 } | |
189 D[ j ] = ( silk_float )temp; | |
190 Dinv[ j ] = ( silk_float )( 1.0f / temp ); | |
191 matrix_ptr( L, j, j, M ) = 1.0f; | |
192 | |
193 ptr1 = matrix_adr( A, j, 0, M ); | |
194 ptr2 = matrix_adr( L, j + 1, 0, M); | |
195 for( i = j + 1; i < M; i++ ) { | |
196 temp = 0.0; | |
197 for( k = 0; k < j; k++ ) { | |
198 temp += ptr2[ k ] * v[ k ]; | |
199 } | |
200 matrix_ptr( L, i, j, M ) = ( silk_float )( ( ptr1[ i ] - temp )
* Dinv[ j ] ); | |
201 ptr2 += M; /* go to next column*/ | |
202 } | |
203 } | |
204 } | |
205 silk_assert( err == 0 ); | |
206 } | |
207 | |
OLD | NEW |