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