1 | |
2 | |
3 | |
4 | |
5 | |
6 | |
7 | |
8 | |
9 | |
10 | |
11 | |
12 | |
13 | |
14 | |
15 | |
16 | |
17 | |
18 | |
19 | |
20 | |
21 | |
22 | |
23 | |
24 | |
25 | |
26 | |
27 | |
28 | #ifdef HAVE_CONFIG_H1 |
29 | #include "config.h" |
30 | #endif |
31 | |
32 | #include "main_FLP.h" |
33 | #include "tuning_parameters.h" |
34 | |
35 | |
36 | |
37 | |
38 | |
39 | |
40 | static OPUS_INLINEinline void silk_LDL_FLP( |
41 | silk_floatfloat *A, |
42 | opus_intint M, |
43 | silk_floatfloat *L, |
44 | silk_floatfloat *Dinv |
45 | ); |
46 | |
47 | |
48 | |
49 | |
50 | |
51 | static OPUS_INLINEinline void silk_SolveWithLowerTriangularWdiagOnes_FLP( |
52 | const silk_floatfloat *L, |
53 | opus_intint M, |
54 | const silk_floatfloat *b, |
55 | silk_floatfloat *x |
56 | ); |
57 | |
58 | |
59 | |
60 | |
61 | |
62 | static OPUS_INLINEinline void silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP( |
63 | const silk_floatfloat *L, |
64 | opus_intint M, |
65 | const silk_floatfloat *b, |
66 | silk_floatfloat *x |
67 | ); |
68 | |
69 | |
70 | |
71 | |
72 | |
73 | void silk_solve_LDL_FLP( |
74 | silk_floatfloat *A, |
75 | const opus_intint M, |
76 | const silk_floatfloat *b, |
77 | silk_floatfloat *x |
78 | ) |
79 | { |
80 | opus_intint i; |
81 | silk_floatfloat L[ MAX_MATRIX_SIZE16 ][ MAX_MATRIX_SIZE16 ]; |
82 | silk_floatfloat T[ MAX_MATRIX_SIZE16 ]; |
83 | silk_floatfloat Dinv[ MAX_MATRIX_SIZE16 ]; |
84 | |
85 | silk_assert( M <= MAX_MATRIX_SIZE ); |
86 | |
87 | |
88 | |
89 | |
90 | |
91 | silk_LDL_FLP( A, M, &L[ 0 ][ 0 ], Dinv ); |
| |
| 3 | | Returning from 'silk_LDL_FLP' | |
|
92 | |
93 | |
94 | |
95 | |
96 | |
97 | silk_SolveWithLowerTriangularWdiagOnes_FLP( &L[ 0 ][ 0 ], M, b, T ); |
98 | |
99 | |
100 | |
101 | |
102 | |
103 | for( i = 0; i < M; i++ ) { |
| 4 | | Loop condition is false. Execution continues on line 109 | |
|
104 | T[ i ] = T[ i ] * Dinv[ i ]; |
105 | } |
106 | |
107 | |
108 | |
109 | silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP( &L[ 0 ][ 0 ], M, T, x ); |
| 5 | | Calling 'silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP' | |
|
110 | } |
111 | |
112 | static OPUS_INLINEinline void silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP( |
113 | const silk_floatfloat *L, |
114 | opus_intint M, |
115 | const silk_floatfloat *b, |
116 | silk_floatfloat *x |
117 | ) |
118 | { |
119 | opus_intint i, j; |
120 | silk_floatfloat temp; |
121 | const silk_floatfloat *ptr1; |
122 | |
123 | for( i = M - 1; i >= 0; i-- ) { |
| |
| 7 | | Loop condition is true. Entering loop body | |
|
| 9 | | Loop condition is true. Entering loop body | |
|
124 | ptr1 = matrix_adr( L, 0, i, M )((L) + ((0)*(M)+(i))); |
125 | temp = 0; |
126 | for( j = M - 1; j > i ; j-- ) { |
| 8 | | Loop condition is false. Execution continues on line 129 | |
|
| 10 | | Loop condition is true. Entering loop body | |
|
127 | temp += ptr1[ j * M ] * x[ j ]; |
| 11 | | The left operand of '*' is a garbage value |
|
128 | } |
129 | temp = b[ i ] - temp; |
130 | x[ i ] = temp; |
131 | } |
132 | } |
133 | |
134 | static OPUS_INLINEinline void silk_SolveWithLowerTriangularWdiagOnes_FLP( |
135 | const silk_floatfloat *L, |
136 | opus_intint M, |
137 | const silk_floatfloat *b, |
138 | silk_floatfloat *x |
139 | ) |
140 | { |
141 | opus_intint i, j; |
142 | silk_floatfloat temp; |
143 | const silk_floatfloat *ptr1; |
144 | |
145 | for( i = 0; i < M; i++ ) { |
146 | ptr1 = matrix_adr( L, i, 0, M )((L) + ((i)*(M)+(0))); |
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_INLINEinline void silk_LDL_FLP( |
157 | silk_floatfloat *A, |
158 | opus_intint M, |
159 | silk_floatfloat *L, |
160 | silk_floatfloat *Dinv |
161 | ) |
162 | { |
163 | opus_intint i, j, k, loop_count, err = 1; |
164 | silk_floatfloat *ptr1, *ptr2; |
165 | double temp, diag_min_value; |
166 | silk_floatfloat v[ MAX_MATRIX_SIZE16 ], D[ MAX_MATRIX_SIZE16 ]; |
167 | |
168 | silk_assert( M <= MAX_MATRIX_SIZE ); |
169 | |
170 | diag_min_value = FIND_LTP_COND_FAC1e-5f * 0.5f * ( A[ 0 ] + A[ M * M - 1 ] ); |
171 | for( loop_count = 0; loop_count < M && err == 1; loop_count++ ) { |
| 2 | | Assuming 'loop_count' is >= 'M' | |
|
172 | err = 0; |
173 | for( j = 0; j < M; j++ ) { |
174 | ptr1 = matrix_adr( L, j, 0, M )((L) + ((j)*(M)+(0))); |
175 | temp = matrix_ptr( A, j, j, M )(*((A) + ((j)*(M)+(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 | |
182 | temp = ( loop_count + 1 ) * diag_min_value - temp; |
183 | for( i = 0; i < M; i++ ) { |
184 | matrix_ptr( A, i, i, M )(*((A) + ((i)*(M)+(i)))) += ( silk_floatfloat )temp; |
185 | } |
186 | err = 1; |
187 | break; |
188 | } |
189 | D[ j ] = ( silk_floatfloat )temp; |
190 | Dinv[ j ] = ( silk_floatfloat )( 1.0f / temp ); |
191 | matrix_ptr( L, j, j, M )(*((L) + ((j)*(M)+(j)))) = 1.0f; |
192 | |
193 | ptr1 = matrix_adr( A, j, 0, M )((A) + ((j)*(M)+(0))); |
194 | ptr2 = matrix_adr( L, j + 1, 0, M)((L) + ((j + 1)*(M)+(0))); |
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 )(*((L) + ((i)*(M)+(j)))) = ( silk_floatfloat )( ( ptr1[ i ] - temp ) * Dinv[ j ] ); |
201 | ptr2 += M; |
202 | } |
203 | } |
204 | } |
205 | silk_assert( err == 0 ); |
206 | } |
207 | |