Tristan Matthews | 0a329cc | 2013-07-17 13:20:14 -0400 | [diff] [blame] | 1 | |
| 2 | /****************************************************************** |
| 3 | |
| 4 | iLBC Speech Coder ANSI-C Source Code |
| 5 | |
| 6 | lsf.c |
| 7 | |
| 8 | Copyright (C) The Internet Society (2004). |
| 9 | All Rights Reserved. |
| 10 | |
| 11 | ******************************************************************/ |
| 12 | |
| 13 | #include <string.h> |
| 14 | |
| 15 | |
| 16 | |
| 17 | |
| 18 | |
| 19 | #include <math.h> |
| 20 | |
| 21 | #include "iLBC_define.h" |
| 22 | |
| 23 | /*----------------------------------------------------------------* |
| 24 | * conversion from lpc coefficients to lsf coefficients |
| 25 | *---------------------------------------------------------------*/ |
| 26 | |
| 27 | void a2lsf( |
| 28 | float *freq,/* (o) lsf coefficients */ |
| 29 | float *a /* (i) lpc coefficients */ |
| 30 | ){ |
| 31 | float steps[LSF_NUMBER_OF_STEPS] = |
| 32 | {(float)0.00635, (float)0.003175, (float)0.0015875, |
| 33 | (float)0.00079375}; |
| 34 | float step; |
| 35 | int step_idx; |
| 36 | int lsp_index; |
| 37 | float p[LPC_HALFORDER]; |
| 38 | float q[LPC_HALFORDER]; |
| 39 | float p_pre[LPC_HALFORDER]; |
| 40 | float q_pre[LPC_HALFORDER]; |
| 41 | float old_p, old_q, *old; |
| 42 | float *pq_coef; |
| 43 | float omega, old_omega; |
| 44 | int i; |
| 45 | float hlp, hlp1, hlp2, hlp3, hlp4, hlp5; |
| 46 | |
| 47 | for (i=0; i<LPC_HALFORDER; i++) { |
| 48 | p[i] = (float)-1.0 * (a[i + 1] + a[LPC_FILTERORDER - i]); |
| 49 | q[i] = a[LPC_FILTERORDER - i] - a[i + 1]; |
| 50 | } |
| 51 | |
| 52 | p_pre[0] = (float)-1.0 - p[0]; |
| 53 | p_pre[1] = - p_pre[0] - p[1]; |
| 54 | p_pre[2] = - p_pre[1] - p[2]; |
| 55 | p_pre[3] = - p_pre[2] - p[3]; |
| 56 | p_pre[4] = - p_pre[3] - p[4]; |
| 57 | p_pre[4] = p_pre[4] / 2; |
| 58 | |
| 59 | q_pre[0] = (float)1.0 - q[0]; |
| 60 | q_pre[1] = q_pre[0] - q[1]; |
| 61 | q_pre[2] = q_pre[1] - q[2]; |
| 62 | q_pre[3] = q_pre[2] - q[3]; |
| 63 | q_pre[4] = q_pre[3] - q[4]; |
| 64 | q_pre[4] = q_pre[4] / 2; |
| 65 | |
| 66 | omega = 0.0; |
| 67 | |
| 68 | |
| 69 | |
| 70 | |
| 71 | |
| 72 | old_omega = 0.0; |
| 73 | |
| 74 | old_p = FLOAT_MAX; |
| 75 | old_q = FLOAT_MAX; |
| 76 | |
| 77 | /* Here we loop through lsp_index to find all the |
| 78 | LPC_FILTERORDER roots for omega. */ |
| 79 | |
| 80 | for (lsp_index = 0; lsp_index<LPC_FILTERORDER; lsp_index++) { |
| 81 | |
| 82 | /* Depending on lsp_index being even or odd, we |
| 83 | alternatively solve the roots for the two LSP equations. */ |
| 84 | |
| 85 | |
| 86 | if ((lsp_index & 0x1) == 0) { |
| 87 | pq_coef = p_pre; |
| 88 | old = &old_p; |
| 89 | } else { |
| 90 | pq_coef = q_pre; |
| 91 | old = &old_q; |
| 92 | } |
| 93 | |
| 94 | /* Start with low resolution grid */ |
| 95 | |
| 96 | for (step_idx = 0, step = steps[step_idx]; |
| 97 | step_idx < LSF_NUMBER_OF_STEPS;){ |
| 98 | |
| 99 | /* cos(10piw) + pq(0)cos(8piw) + pq(1)cos(6piw) + |
| 100 | pq(2)cos(4piw) + pq(3)cod(2piw) + pq(4) */ |
| 101 | |
| 102 | hlp = (float)cos(omega * TWO_PI); |
| 103 | hlp1 = (float)2.0 * hlp + pq_coef[0]; |
| 104 | hlp2 = (float)2.0 * hlp * hlp1 - (float)1.0 + |
| 105 | pq_coef[1]; |
| 106 | hlp3 = (float)2.0 * hlp * hlp2 - hlp1 + pq_coef[2]; |
| 107 | hlp4 = (float)2.0 * hlp * hlp3 - hlp2 + pq_coef[3]; |
| 108 | hlp5 = hlp * hlp4 - hlp3 + pq_coef[4]; |
| 109 | |
| 110 | |
| 111 | if (((hlp5 * (*old)) <= 0.0) || (omega >= 0.5)){ |
| 112 | |
| 113 | if (step_idx == (LSF_NUMBER_OF_STEPS - 1)){ |
| 114 | |
| 115 | if (fabs(hlp5) >= fabs(*old)) { |
| 116 | freq[lsp_index] = omega - step; |
| 117 | } else { |
| 118 | freq[lsp_index] = omega; |
| 119 | } |
| 120 | |
| 121 | |
| 122 | |
| 123 | |
| 124 | |
| 125 | |
| 126 | |
| 127 | if ((*old) >= 0.0){ |
| 128 | *old = (float)-1.0 * FLOAT_MAX; |
| 129 | } else { |
| 130 | *old = FLOAT_MAX; |
| 131 | } |
| 132 | |
| 133 | omega = old_omega; |
| 134 | step_idx = 0; |
| 135 | |
| 136 | step_idx = LSF_NUMBER_OF_STEPS; |
| 137 | } else { |
| 138 | |
| 139 | if (step_idx == 0) { |
| 140 | old_omega = omega; |
| 141 | } |
| 142 | |
| 143 | step_idx++; |
| 144 | omega -= steps[step_idx]; |
| 145 | |
| 146 | /* Go back one grid step */ |
| 147 | |
| 148 | step = steps[step_idx]; |
| 149 | } |
| 150 | } else { |
| 151 | |
| 152 | /* increment omega until they are of different sign, |
| 153 | and we know there is at least one root between omega |
| 154 | and old_omega */ |
| 155 | *old = hlp5; |
| 156 | omega += step; |
| 157 | } |
| 158 | } |
| 159 | } |
| 160 | |
| 161 | for (i = 0; i<LPC_FILTERORDER; i++) { |
| 162 | freq[i] = freq[i] * TWO_PI; |
| 163 | } |
| 164 | } |
| 165 | |
| 166 | /*----------------------------------------------------------------* |
| 167 | * conversion from lsf coefficients to lpc coefficients |
| 168 | *---------------------------------------------------------------*/ |
| 169 | |
| 170 | void lsf2a( |
| 171 | float *a_coef, /* (o) lpc coefficients */ |
| 172 | float *freq /* (i) lsf coefficients */ |
| 173 | |
| 174 | |
| 175 | |
| 176 | |
| 177 | |
| 178 | ){ |
| 179 | int i, j; |
| 180 | float hlp; |
| 181 | float p[LPC_HALFORDER], q[LPC_HALFORDER]; |
| 182 | float a[LPC_HALFORDER + 1], a1[LPC_HALFORDER], |
| 183 | a2[LPC_HALFORDER]; |
| 184 | float b[LPC_HALFORDER + 1], b1[LPC_HALFORDER], |
| 185 | b2[LPC_HALFORDER]; |
| 186 | |
| 187 | for (i=0; i<LPC_FILTERORDER; i++) { |
| 188 | freq[i] = freq[i] * PI2; |
| 189 | } |
| 190 | |
| 191 | /* Check input for ill-conditioned cases. This part is not |
| 192 | found in the TIA standard. It involves the following 2 IF |
| 193 | blocks. If "freq" is judged ill-conditioned, then we first |
| 194 | modify freq[0] and freq[LPC_HALFORDER-1] (normally |
| 195 | LPC_HALFORDER = 10 for LPC applications), then we adjust |
| 196 | the other "freq" values slightly */ |
| 197 | |
| 198 | |
| 199 | if ((freq[0] <= 0.0) || (freq[LPC_FILTERORDER - 1] >= 0.5)){ |
| 200 | |
| 201 | |
| 202 | if (freq[0] <= 0.0) { |
| 203 | freq[0] = (float)0.022; |
| 204 | } |
| 205 | |
| 206 | |
| 207 | if (freq[LPC_FILTERORDER - 1] >= 0.5) { |
| 208 | freq[LPC_FILTERORDER - 1] = (float)0.499; |
| 209 | } |
| 210 | |
| 211 | hlp = (freq[LPC_FILTERORDER - 1] - freq[0]) / |
| 212 | (float) (LPC_FILTERORDER - 1); |
| 213 | |
| 214 | for (i=1; i<LPC_FILTERORDER; i++) { |
| 215 | freq[i] = freq[i - 1] + hlp; |
| 216 | } |
| 217 | } |
| 218 | |
| 219 | memset(a1, 0, LPC_HALFORDER*sizeof(float)); |
| 220 | memset(a2, 0, LPC_HALFORDER*sizeof(float)); |
| 221 | memset(b1, 0, LPC_HALFORDER*sizeof(float)); |
| 222 | memset(b2, 0, LPC_HALFORDER*sizeof(float)); |
| 223 | memset(a, 0, (LPC_HALFORDER+1)*sizeof(float)); |
| 224 | memset(b, 0, (LPC_HALFORDER+1)*sizeof(float)); |
| 225 | |
| 226 | |
| 227 | |
| 228 | |
| 229 | |
| 230 | |
| 231 | /* p[i] and q[i] compute cos(2*pi*omega_{2j}) and |
| 232 | cos(2*pi*omega_{2j-1} in eqs. 4.2.2.2-1 and 4.2.2.2-2. |
| 233 | Note that for this code p[i] specifies the coefficients |
| 234 | used in .Q_A(z) while q[i] specifies the coefficients used |
| 235 | in .P_A(z) */ |
| 236 | |
| 237 | for (i=0; i<LPC_HALFORDER; i++) { |
| 238 | p[i] = (float)cos(TWO_PI * freq[2 * i]); |
| 239 | q[i] = (float)cos(TWO_PI * freq[2 * i + 1]); |
| 240 | } |
| 241 | |
| 242 | a[0] = 0.25; |
| 243 | b[0] = 0.25; |
| 244 | |
| 245 | for (i= 0; i<LPC_HALFORDER; i++) { |
| 246 | a[i + 1] = a[i] - 2 * p[i] * a1[i] + a2[i]; |
| 247 | b[i + 1] = b[i] - 2 * q[i] * b1[i] + b2[i]; |
| 248 | a2[i] = a1[i]; |
| 249 | a1[i] = a[i]; |
| 250 | b2[i] = b1[i]; |
| 251 | b1[i] = b[i]; |
| 252 | } |
| 253 | |
| 254 | for (j=0; j<LPC_FILTERORDER; j++) { |
| 255 | |
| 256 | if (j == 0) { |
| 257 | a[0] = 0.25; |
| 258 | b[0] = -0.25; |
| 259 | } else { |
| 260 | a[0] = b[0] = 0.0; |
| 261 | } |
| 262 | |
| 263 | for (i=0; i<LPC_HALFORDER; i++) { |
| 264 | a[i + 1] = a[i] - 2 * p[i] * a1[i] + a2[i]; |
| 265 | b[i + 1] = b[i] - 2 * q[i] * b1[i] + b2[i]; |
| 266 | a2[i] = a1[i]; |
| 267 | a1[i] = a[i]; |
| 268 | b2[i] = b1[i]; |
| 269 | b1[i] = b[i]; |
| 270 | } |
| 271 | |
| 272 | a_coef[j + 1] = 2 * (a[LPC_HALFORDER] + b[LPC_HALFORDER]); |
| 273 | } |
| 274 | |
| 275 | a_coef[0] = 1.0; |
| 276 | } |
| 277 | |
| 278 | |
| 279 | |
| 280 | |
| 281 | |
| 282 | |
| 283 | |