removed prefilter
[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, 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 /* 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 MAX_ITERATIONS_A2NLSF_FIX    16
44
45 /* Helper function for A2NLSF(..)                    */
46 /* Transforms polynomials from cos(n*f) to cos(f)^n  */
47 static OPUS_INLINE void silk_A2NLSF_trans_poly(
48     opus_int32          *p,                     /* I/O    Polynomial                                */
49     const opus_int      dd                      /* I      Polynomial order (= filter order / 2 )    */
50 )
51 {
52     opus_int k, n;
53
54     for( k = 2; k <= dd; k++ ) {
55         for( n = dd; n > k; n-- ) {
56             p[ n - 2 ] -= p[ n ];
57         }
58         p[ k - 2 ] -= silk_LSHIFT( p[ k ], 1 );
59     }
60 }
61 /* Helper function for A2NLSF(..) */
62 /* Polynomial evaluation          */
63 static OPUS_INLINE opus_int32 silk_A2NLSF_eval_poly( /* return the polynomial evaluation, in Q16     */
64     opus_int32          *p,                     /* I    Polynomial, Q16                         */
65     const opus_int32    x,                      /* I    Evaluation point, Q12                   */
66     const opus_int      dd                      /* I    Order                                   */
67 )
68 {
69     opus_int   n;
70     opus_int32 x_Q16, y32;
71
72     y32 = p[ dd ];                                  /* Q16 */
73     x_Q16 = silk_LSHIFT( x, 4 );
74
75     if ( opus_likely( 8 == dd ) )
76     {
77         y32 = silk_SMLAWW( p[ 7 ], y32, x_Q16 );
78         y32 = silk_SMLAWW( p[ 6 ], y32, x_Q16 );
79         y32 = silk_SMLAWW( p[ 5 ], y32, x_Q16 );
80         y32 = silk_SMLAWW( p[ 4 ], y32, x_Q16 );
81         y32 = silk_SMLAWW( p[ 3 ], y32, x_Q16 );
82         y32 = silk_SMLAWW( p[ 2 ], y32, x_Q16 );
83         y32 = silk_SMLAWW( p[ 1 ], y32, x_Q16 );
84         y32 = silk_SMLAWW( p[ 0 ], y32, x_Q16 );
85     }
86     else
87     {
88         for( n = dd - 1; n >= 0; n-- ) {
89             y32 = silk_SMLAWW( p[ n ], y32, x_Q16 );    /* Q16 */
90         }
91     }
92     return y32;
93 }
94
95 static OPUS_INLINE void silk_A2NLSF_init(
96      const opus_int32    *a_Q16,
97      opus_int32          *P,
98      opus_int32          *Q,
99      const opus_int      dd
100 )
101 {
102     opus_int k;
103
104     /* Convert filter coefs to even and odd polynomials */
105     P[dd] = silk_LSHIFT( 1, 16 );
106     Q[dd] = silk_LSHIFT( 1, 16 );
107     for( k = 0; k < dd; k++ ) {
108         P[ k ] = -a_Q16[ dd - k - 1 ] - a_Q16[ dd + k ];    /* Q16 */
109         Q[ k ] = -a_Q16[ dd - k - 1 ] + a_Q16[ dd + k ];    /* Q16 */
110     }
111
112     /* Divide out zeros as we have that for even filter orders, */
113     /* z =  1 is always a root in Q, and                        */
114     /* z = -1 is always a root in P                             */
115     for( k = dd; k > 0; k-- ) {
116         P[ k - 1 ] -= P[ k ];
117         Q[ k - 1 ] += Q[ k ];
118     }
119
120     /* Transform polynomials from cos(n*f) to cos(f)^n */
121     silk_A2NLSF_trans_poly( P, dd );
122     silk_A2NLSF_trans_poly( Q, dd );
123 }
124
125 /* Compute Normalized Line Spectral Frequencies (NLSFs) from whitening filter coefficients      */
126 /* If not all roots are found, the a_Q16 coefficients are bandwidth expanded until convergence. */
127 void silk_A2NLSF(
128     opus_int16                  *NLSF,              /* O    Normalized Line Spectral Frequencies in Q15 (0..2^15-1) [d] */
129     opus_int32                  *a_Q16,             /* I/O  Monic whitening filter coefficients in Q16 [d]              */
130     const opus_int              d                   /* I    Filter order (must be even)                                 */
131 )
132 {
133     opus_int   i, k, m, dd, root_ix, ffrac;
134     opus_int32 xlo, xhi, xmid;
135     opus_int32 ylo, yhi, ymid, thr;
136     opus_int32 nom, den;
137     opus_int32 P[ SILK_MAX_ORDER_LPC / 2 + 1 ];
138     opus_int32 Q[ SILK_MAX_ORDER_LPC / 2 + 1 ];
139     opus_int32 *PQ[ 2 ];
140     opus_int32 *p;
141
142     /* Store pointers to array */
143     PQ[ 0 ] = P;
144     PQ[ 1 ] = Q;
145
146     dd = silk_RSHIFT( d, 1 );
147
148     silk_A2NLSF_init( a_Q16, P, Q, dd );
149
150     /* Find roots, alternating between P and Q */
151     p = P;                          /* Pointer to polynomial */
152
153     xlo = silk_LSFCosTab_FIX_Q12[ 0 ]; /* Q12*/
154     ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
155
156     if( ylo < 0 ) {
157         /* Set the first NLSF to zero and move on to the next */
158         NLSF[ 0 ] = 0;
159         p = Q;                      /* Pointer to polynomial */
160         ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
161         root_ix = 1;                /* Index of current root */
162     } else {
163         root_ix = 0;                /* Index of current root */
164     }
165     k = 1;                          /* Loop counter */
166     i = 0;                          /* Counter for bandwidth expansions applied */
167     thr = 0;
168     while( 1 ) {
169         /* Evaluate polynomial */
170         xhi = silk_LSFCosTab_FIX_Q12[ k ]; /* Q12 */
171         yhi = silk_A2NLSF_eval_poly( p, xhi, dd );
172
173         /* Detect zero crossing */
174         if( ( ylo <= 0 && yhi >= thr ) || ( ylo >= 0 && yhi <= -thr ) ) {
175             if( yhi == 0 ) {
176                 /* If the root lies exactly at the end of the current       */
177                 /* interval, look for the next root in the next interval    */
178                 thr = 1;
179             } else {
180                 thr = 0;
181             }
182             /* Binary division */
183             ffrac = -256;
184             for( m = 0; m < BIN_DIV_STEPS_A2NLSF_FIX; m++ ) {
185                 /* Evaluate polynomial */
186                 xmid = silk_RSHIFT_ROUND( xlo + xhi, 1 );
187                 ymid = silk_A2NLSF_eval_poly( p, xmid, dd );
188
189                 /* Detect zero crossing */
190                 if( ( ylo <= 0 && ymid >= 0 ) || ( ylo >= 0 && ymid <= 0 ) ) {
191                     /* Reduce frequency */
192                     xhi = xmid;
193                     yhi = ymid;
194                 } else {
195                     /* Increase frequency */
196                     xlo = xmid;
197                     ylo = ymid;
198                     ffrac = silk_ADD_RSHIFT( ffrac, 128, m );
199                 }
200             }
201
202             /* Interpolate */
203             if( silk_abs( ylo ) < 65536 ) {
204                 /* Avoid dividing by zero */
205                 den = ylo - yhi;
206                 nom = silk_LSHIFT( ylo, 8 - BIN_DIV_STEPS_A2NLSF_FIX ) + silk_RSHIFT( den, 1 );
207                 if( den != 0 ) {
208                     ffrac += silk_DIV32( nom, den );
209                 }
210             } else {
211                 /* No risk of dividing by zero because abs(ylo - yhi) >= abs(ylo) >= 65536 */
212                 ffrac += silk_DIV32( ylo, silk_RSHIFT( ylo - yhi, 8 - BIN_DIV_STEPS_A2NLSF_FIX ) );
213             }
214             NLSF[ root_ix ] = (opus_int16)silk_min_32( silk_LSHIFT( (opus_int32)k, 8 ) + ffrac, silk_int16_MAX );
215
216             silk_assert( NLSF[ root_ix ] >= 0 );
217
218             root_ix++;        /* Next root */
219             if( root_ix >= d ) {
220                 /* Found all roots */
221                 break;
222             }
223             /* Alternate pointer to polynomial */
224             p = PQ[ root_ix & 1 ];
225
226             /* Evaluate polynomial */
227             xlo = silk_LSFCosTab_FIX_Q12[ k - 1 ]; /* Q12*/
228             ylo = silk_LSHIFT( 1 - ( root_ix & 2 ), 12 );
229         } else {
230             /* Increment loop counter */
231             k++;
232             xlo = xhi;
233             ylo = yhi;
234             thr = 0;
235
236             if( k > LSF_COS_TAB_SZ_FIX ) {
237                 i++;
238                 if( i > MAX_ITERATIONS_A2NLSF_FIX ) {
239                     /* Set NLSFs to white spectrum and exit */
240                     NLSF[ 0 ] = (opus_int16)silk_DIV32_16( 1 << 15, d + 1 );
241                     for( k = 1; k < d; k++ ) {
242                         NLSF[ k ] = (opus_int16)silk_ADD16( NLSF[ k-1 ], NLSF[ 0 ] );
243                     }
244                     return;
245                 }
246
247                 /* Error: Apply progressively more bandwidth expansion and run again */
248                                 silk_bwexpander_32( a_Q16, d, 65536 - silk_LSHIFT( 1, i ) );
249
250                 silk_A2NLSF_init( a_Q16, P, Q, dd );
251                 p = P;                            /* Pointer to polynomial */
252                 xlo = silk_LSFCosTab_FIX_Q12[ 0 ]; /* Q12*/
253                 ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
254                 if( ylo < 0 ) {
255                     /* Set the first NLSF to zero and move on to the next */
256                     NLSF[ 0 ] = 0;
257                     p = Q;                        /* Pointer to polynomial */
258                     ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
259                     root_ix = 1;                  /* Index of current root */
260                 } else {
261                     root_ix = 0;                  /* Index of current root */
262                 }
263                 k = 1;                            /* Reset loop counter */
264             }
265         }
266     }
267 }