Fixes a corner case that was causing silk_A2NLSF() to fail
[opus.git] / silk / A2NLSF.c
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, (subject to the limitations in the disclaimer below)
5 are permitted provided that the following conditions 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 Skype Limited, nor the names of specific
12 contributors, may be used to endorse or promote products derived from
13 this software without specific prior written permission.
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***********************************************************************/
27
28 /* Conversion between prediction filter coefficients and NLSFs  */
29 /* Requires the order to be an even number                      */
30 /* A piecewise linear approximation maps LSF <-> cos(LSF)       */
31 /* Therefore the result is not accurate NLSFs, but the two      */
32 /* functions are accurate inverses of each other                */
33
34 #ifdef HAVE_CONFIG_H
35 #include "config.h"
36 #endif
37
38 #include "SigProc_FIX.h"
39 #include "tables.h"
40
41 /* Number of binary divisions, when not in low complexity mode */
42 #define BIN_DIV_STEPS_A2NLSF_FIX      3 /* must be no higher than 16 - log2( LSF_COS_TAB_SZ_FIX ) */
43 #define QPoly                        16
44 #define MAX_ITERATIONS_A2NLSF_FIX    30
45
46 /* Flag for using 2x as many cosine sampling points, reduces the risk of missing a root */
47 #define OVERSAMPLE_COSINE_TABLE       0
48
49 /* Helper function for A2NLSF(..)                    */
50 /* Transforms polynomials from cos(n*f) to cos(f)^n  */
51 static inline void silk_A2NLSF_trans_poly(
52     opus_int32        *p,    /* I/O    Polynomial                                */
53     const opus_int    dd     /* I      Polynomial order (= filter order / 2 )    */
54 )
55 {
56     opus_int k, n;
57
58     for( k = 2; k <= dd; k++ ) {
59         for( n = dd; n > k; n-- ) {
60             p[ n - 2 ] -= p[ n ];
61         }
62         p[ k - 2 ] -= silk_LSHIFT( p[ k ], 1 );
63     }
64 }
65 /* Helper function for A2NLSF(..)                    */
66 /* Polynomial evaluation                             */
67 static inline opus_int32 silk_A2NLSF_eval_poly(    /* return the polynomial evaluation, in QPoly */
68     opus_int32        *p,    /* I    Polynomial, QPoly        */
69     const opus_int32   x,    /* I    Evaluation point, Q12    */
70     const opus_int    dd     /* I    Order                    */
71 )
72 {
73     opus_int   n;
74     opus_int32 x_Q16, y32;
75
76     y32 = p[ dd ];                                    /* QPoly */
77     x_Q16 = silk_LSHIFT( x, 4 );
78     for( n = dd - 1; n >= 0; n-- ) {
79         y32 = silk_SMLAWW( p[ n ], y32, x_Q16 );       /* QPoly */
80     }
81     return y32;
82 }
83
84 static inline void silk_A2NLSF_init(
85      const opus_int32    *a_Q16,
86      opus_int32          *P,
87      opus_int32          *Q,
88      const opus_int      dd
89 )
90 {
91     opus_int k;
92
93     /* Convert filter coefs to even and odd polynomials */
94     P[dd] = silk_LSHIFT( 1, QPoly );
95     Q[dd] = silk_LSHIFT( 1, QPoly );
96     for( k = 0; k < dd; k++ ) {
97 #if( QPoly < 16 )
98         P[ k ] = silk_RSHIFT_ROUND( -a_Q16[ dd - k - 1 ] - a_Q16[ dd + k ], 16 - QPoly ); /* QPoly */
99         Q[ k ] = silk_RSHIFT_ROUND( -a_Q16[ dd - k - 1 ] + a_Q16[ dd + k ], 16 - QPoly ); /* QPoly */
100 #elif( Qpoly == 16 )
101         P[ k ] = -a_Q16[ dd - k - 1 ] - a_Q16[ dd + k ]; /* QPoly*/
102         Q[ k ] = -a_Q16[ dd - k - 1 ] + a_Q16[ dd + k ]; /* QPoly*/
103 #else
104         P[ k ] = silk_LSHIFT( -a_Q16[ dd - k - 1 ] - a_Q16[ dd + k ], QPoly - 16 ); /* QPoly */
105         Q[ k ] = silk_LSHIFT( -a_Q16[ dd - k - 1 ] + a_Q16[ dd + k ], QPoly - 16 ); /* QPoly */
106 #endif
107     }
108
109     /* Divide out zeros as we have that for even filter orders, */
110     /* z =  1 is always a root in Q, and                        */
111     /* z = -1 is always a root in P                             */
112     for( k = dd; k > 0; k-- ) {
113         P[ k - 1 ] -= P[ k ];
114         Q[ k - 1 ] += Q[ k ];
115     }
116
117     /* Transform polynomials from cos(n*f) to cos(f)^n */
118     silk_A2NLSF_trans_poly( P, dd );
119     silk_A2NLSF_trans_poly( Q, dd );
120 }
121
122 /* Compute Normalized Line Spectral Frequencies (NLSFs) from whitening filter coefficients        */
123 /* If not all roots are found, the a_Q16 coefficients are bandwidth expanded until convergence.    */
124 void silk_A2NLSF(
125     opus_int16        *NLSF,                 /* O    Normalized Line Spectral Frequencies, Q15 (0 - (2^15-1)), [d]    */
126     opus_int32        *a_Q16,                /* I/O  Monic whitening filter coefficients in Q16 [d]                   */
127     const opus_int    d                      /* I    Filter order (must be even)                                      */
128 )
129 {
130     opus_int      i, k, m, dd, root_ix, ffrac;
131     opus_int32 xlo, xhi, xmid;
132     opus_int32 ylo, yhi, ymid, thr;
133     opus_int32 nom, den;
134     opus_int32 P[ SILK_MAX_ORDER_LPC / 2 + 1 ];
135     opus_int32 Q[ SILK_MAX_ORDER_LPC / 2 + 1 ];
136     opus_int32 *PQ[ 2 ];
137     opus_int32 *p;
138
139     /* Store pointers to array */
140     PQ[ 0 ] = P;
141     PQ[ 1 ] = Q;
142
143     dd = silk_RSHIFT( d, 1 );
144
145     silk_A2NLSF_init( a_Q16, P, Q, dd );
146
147     /* Find roots, alternating between P and Q */
148     p = P;    /* Pointer to polynomial */
149
150     xlo = silk_LSFCosTab_FIX_Q12[ 0 ]; /* Q12*/
151     ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
152
153     if( ylo < 0 ) {
154         /* Set the first NLSF to zero and move on to the next */
155         NLSF[ 0 ] = 0;
156         p = Q;                      /* Pointer to polynomial */
157         ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
158         root_ix = 1;                /* Index of current root */
159     } else {
160         root_ix = 0;                /* Index of current root */
161     }
162     k = 1;                          /* Loop counter */
163     i = 0;                          /* Counter for bandwidth expansions applied */
164     thr = 0;
165     while( 1 ) {
166         /* Evaluate polynomial */
167 #if OVERSAMPLE_COSINE_TABLE
168         xhi = silk_LSFCosTab_FIX_Q12[   k       >> 1 ] +
169           ( ( silk_LSFCosTab_FIX_Q12[ ( k + 1 ) >> 1 ] -
170               silk_LSFCosTab_FIX_Q12[   k       >> 1 ] ) >> 1 );    /* Q12 */
171 #else
172         xhi = silk_LSFCosTab_FIX_Q12[ k ]; /* Q12 */
173 #endif
174         yhi = silk_A2NLSF_eval_poly( p, xhi, dd );
175
176         /* Detect zero crossing */
177         if( ( ylo <= 0 && yhi >= thr ) || ( ylo >= 0 && yhi <= -thr ) ) {
178             if( yhi == 0 ) {
179                 /* If the root lies exactly at the end of the current       */
180                 /* interval, look for the next root in the next interval    */
181                 thr = 1;
182             } else {
183                 thr = 0;
184             }
185             /* Binary division */
186 #if OVERSAMPLE_COSINE_TABLE
187             ffrac = -128;
188 #else
189             ffrac = -256;
190 #endif
191             for( m = 0; m < BIN_DIV_STEPS_A2NLSF_FIX; m++ ) {
192                 /* Evaluate polynomial */
193                 xmid = silk_RSHIFT_ROUND( xlo + xhi, 1 );
194                 ymid = silk_A2NLSF_eval_poly( p, xmid, dd );
195
196                 /* Detect zero crossing */
197                 if( ( ylo <= 0 && ymid >= 0 ) || ( ylo >= 0 && ymid <= 0 ) ) {
198                     /* Reduce frequency */
199                     xhi = xmid;
200                     yhi = ymid;
201                 } else {
202                     /* Increase frequency */
203                     xlo = xmid;
204                     ylo = ymid;
205 #if OVERSAMPLE_COSINE_TABLE
206                     ffrac = silk_ADD_RSHIFT( ffrac,  64, m );
207 #else
208                     ffrac = silk_ADD_RSHIFT( ffrac, 128, m );
209 #endif
210                 }
211             }
212
213             /* Interpolate */
214             if( silk_abs( ylo ) < 65536 ) {
215                 /* Avoid dividing by zero */
216                 den = ylo - yhi;
217                 nom = silk_LSHIFT( ylo, 8 - BIN_DIV_STEPS_A2NLSF_FIX ) + silk_RSHIFT( den, 1 );
218                 if( den != 0 ) {
219                     ffrac += silk_DIV32( nom, den );
220                 }
221             } else {
222                 /* No risk of dividing by zero because abs(ylo - yhi) >= abs(ylo) >= 65536 */
223                 ffrac += silk_DIV32( ylo, silk_RSHIFT( ylo - yhi, 8 - BIN_DIV_STEPS_A2NLSF_FIX ) );
224             }
225 #if OVERSAMPLE_COSINE_TABLE
226             NLSF[ root_ix ] = (opus_int16)silk_min_32( silk_LSHIFT( (opus_int32)k, 7 ) + ffrac, silk_int16_MAX );
227 #else
228             NLSF[ root_ix ] = (opus_int16)silk_min_32( silk_LSHIFT( (opus_int32)k, 8 ) + ffrac, silk_int16_MAX );
229 #endif
230
231             silk_assert( NLSF[ root_ix ] >= 0 );
232
233             root_ix++;        /* Next root */
234             if( root_ix >= d ) {
235                 /* Found all roots */
236                 break;
237             }
238             /* Alternate pointer to polynomial */
239             p = PQ[ root_ix & 1 ];
240
241             /* Evaluate polynomial */
242 #if OVERSAMPLE_COSINE_TABLE
243             xlo = silk_LSFCosTab_FIX_Q12[ ( k - 1 ) >> 1 ] +
244               ( ( silk_LSFCosTab_FIX_Q12[   k       >> 1 ] -
245                   silk_LSFCosTab_FIX_Q12[ ( k - 1 ) >> 1 ] ) >> 1 ); /* Q12*/
246 #else
247             xlo = silk_LSFCosTab_FIX_Q12[ k - 1 ]; /* Q12*/
248 #endif
249             ylo = silk_LSHIFT( 1 - ( root_ix & 2 ), 12 );
250         } else {
251             /* Increment loop counter */
252             k++;
253             xlo = xhi;
254             ylo = yhi;
255             thr = 0;
256
257 #if OVERSAMPLE_COSINE_TABLE
258             if( k > 2 * LSF_COS_TAB_SZ_FIX ) {
259 #else
260             if( k > LSF_COS_TAB_SZ_FIX ) {
261 #endif
262                 i++;
263                 if( i > MAX_ITERATIONS_A2NLSF_FIX ) {
264                     /* Set NLSFs to white spectrum and exit */
265                     NLSF[ 0 ] = (opus_int16)silk_DIV32_16( 1 << 15, d + 1 );
266                     for( k = 1; k < d; k++ ) {
267                         NLSF[ k ] = (opus_int16)silk_SMULBB( k + 1, NLSF[ 0 ] );
268                     }
269                     return;
270                 }
271
272                 /* Error: Apply progressively more bandwidth expansion and run again */
273                 silk_bwexpander_32( a_Q16, d, 65536 - silk_SMULBB( 10 + i, i ) ); /* 10_Q16 = 0.00015*/
274
275                 silk_A2NLSF_init( a_Q16, P, Q, dd );
276                 p = P;                            /* Pointer to polynomial */
277                 xlo = silk_LSFCosTab_FIX_Q12[ 0 ]; /* Q12*/
278                 ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
279                 if( ylo < 0 ) {
280                     /* Set the first NLSF to zero and move on to the next */
281                     NLSF[ 0 ] = 0;
282                     p = Q;                        /* Pointer to polynomial */
283                     ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
284                     root_ix = 1;                  /* Index of current root */
285                 } else {
286                     root_ix = 0;                  /* Index of current root */
287                 }
288                 k = 1;                            /* Reset loop counter */
289             }
290         }
291     }
292 }