Team Fortress 2 Source Code as on 22/4/2020
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

548 lines
14 KiB

  1. /* Copyright (C) 2002 Jean-Marc Valin
  2. File: ltp.c
  3. Long-Term Prediction functions
  4. Redistribution and use in source and binary forms, with or without
  5. modification, are permitted provided that the following conditions
  6. are met:
  7. - Redistributions of source code must retain the above copyright
  8. notice, this list of conditions and the following disclaimer.
  9. - Redistributions in binary form must reproduce the above copyright
  10. notice, this list of conditions and the following disclaimer in the
  11. documentation and/or other materials provided with the distribution.
  12. - Neither the name of the Xiph.org Foundation nor the names of its
  13. contributors may be used to endorse or promote products derived from
  14. this software without specific prior written permission.
  15. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  16. ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  17. LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  18. A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR
  19. CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  20. EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  21. PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  22. PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  23. LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  24. NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  25. SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26. */
  27. #include <math.h>
  28. #include "ltp.h"
  29. #include "stack_alloc.h"
  30. #include "filters.h"
  31. #include "speex_bits.h"
  32. #ifdef _USE_SSE
  33. #include "ltp_sse.h"
  34. #else
  35. static float inner_prod(float *x, float *y, int len)
  36. {
  37. int i;
  38. float sum1=0,sum2=0,sum3=0,sum4=0;
  39. for (i=0;i<len;)
  40. {
  41. sum1 += x[i]*y[i];
  42. sum2 += x[i+1]*y[i+1];
  43. sum3 += x[i+2]*y[i+2];
  44. sum4 += x[i+3]*y[i+3];
  45. i+=4;
  46. }
  47. return sum1+sum2+sum3+sum4;
  48. }
  49. #endif
  50. /*Original, non-optimized version*/
  51. /*static float inner_prod(float *x, float *y, int len)
  52. {
  53. int i;
  54. float sum=0;
  55. for (i=0;i<len;i++)
  56. sum += x[i]*y[i];
  57. return sum;
  58. }
  59. */
  60. void open_loop_nbest_pitch(float *sw, int start, int end, int len, int *pitch, float *gain, int N, char *stack)
  61. {
  62. int i,j,k;
  63. /*float corr=0;*/
  64. /*float energy;*/
  65. float *best_score;
  66. float e0;
  67. float *corr, *energy, *score;
  68. best_score = PUSH(stack,N, float);
  69. corr = PUSH(stack,end-start+1, float);
  70. energy = PUSH(stack,end-start+2, float);
  71. score = PUSH(stack,end-start+1, float);
  72. for (i=0;i<N;i++)
  73. {
  74. best_score[i]=-1;
  75. gain[i]=0;
  76. }
  77. energy[0]=inner_prod(sw-start, sw-start, len);
  78. e0=inner_prod(sw, sw, len);
  79. for (i=start;i<=end;i++)
  80. {
  81. /* Update energy for next pitch*/
  82. energy[i-start+1] = energy[i-start] + sw[-i-1]*sw[-i-1] - sw[-i+len-1]*sw[-i+len-1];
  83. }
  84. for (i=start;i<=end;i++)
  85. {
  86. corr[i-start]=0;
  87. score[i-start]=0;
  88. }
  89. for (i=start;i<=end;i++)
  90. {
  91. /* Compute correlation*/
  92. corr[i-start]=inner_prod(sw, sw-i, len);
  93. score[i-start]=corr[i-start]*corr[i-start]/(energy[i-start]+1);
  94. }
  95. for (i=start;i<=end;i++)
  96. {
  97. if (score[i-start]>best_score[N-1])
  98. {
  99. float g1, g;
  100. g1 = corr[i-start]/(energy[i-start]+10);
  101. g = sqrt(g1*corr[i-start]/(e0+10));
  102. if (g>g1)
  103. g=g1;
  104. if (g<0)
  105. g=0;
  106. for (j=0;j<N;j++)
  107. {
  108. if (score[i-start] > best_score[j])
  109. {
  110. for (k=N-1;k>j;k--)
  111. {
  112. best_score[k]=best_score[k-1];
  113. pitch[k]=pitch[k-1];
  114. gain[k] = gain[k-1];
  115. }
  116. best_score[j]=score[i-start];
  117. pitch[j]=i;
  118. gain[j]=g;
  119. break;
  120. }
  121. }
  122. }
  123. }
  124. }
  125. /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
  126. float pitch_gain_search_3tap(
  127. float target[], /* Target vector */
  128. float ak[], /* LPCs for this subframe */
  129. float awk1[], /* Weighted LPCs #1 for this subframe */
  130. float awk2[], /* Weighted LPCs #2 for this subframe */
  131. float exc[], /* Excitation */
  132. void *par,
  133. int pitch, /* Pitch value */
  134. int p, /* Number of LPC coeffs */
  135. int nsf, /* Number of samples in subframe */
  136. SpeexBits *bits,
  137. char *stack,
  138. float *exc2,
  139. float *r,
  140. int *cdbk_index
  141. )
  142. {
  143. int i,j;
  144. float *tmp, *tmp2;
  145. float *x[3];
  146. float *e[3];
  147. float corr[3];
  148. float A[3][3];
  149. float gain[3];
  150. int gain_cdbk_size;
  151. signed char *gain_cdbk;
  152. float err1,err2;
  153. ltp_params *params;
  154. params = (ltp_params*) par;
  155. gain_cdbk=params->gain_cdbk;
  156. gain_cdbk_size=1<<params->gain_bits;
  157. tmp = PUSH(stack, 3*nsf, float);
  158. tmp2 = PUSH(stack, 3*nsf, float);
  159. x[0]=tmp;
  160. x[1]=tmp+nsf;
  161. x[2]=tmp+2*nsf;
  162. e[0]=tmp2;
  163. e[1]=tmp2+nsf;
  164. e[2]=tmp2+2*nsf;
  165. for (i=2;i>=0;i--)
  166. {
  167. int pp=pitch+1-i;
  168. for (j=0;j<nsf;j++)
  169. {
  170. if (j-pp<0)
  171. e[i][j]=exc2[j-pp];
  172. else if (j-pp-pitch<0)
  173. e[i][j]=exc2[j-pp-pitch];
  174. else
  175. e[i][j]=0;
  176. }
  177. if (i==2)
  178. syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p, stack);
  179. else {
  180. for (j=0;j<nsf-1;j++)
  181. x[i][j+1]=x[i+1][j];
  182. x[i][0]=0;
  183. for (j=0;j<nsf;j++)
  184. x[i][j]+=e[i][0]*r[j];
  185. }
  186. }
  187. for (i=0;i<3;i++)
  188. corr[i]=inner_prod(x[i],target,nsf);
  189. for (i=0;i<3;i++)
  190. for (j=0;j<=i;j++)
  191. A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
  192. {
  193. float C[9];
  194. signed char *ptr=gain_cdbk;
  195. int best_cdbk=0;
  196. float best_sum=0;
  197. C[0]=corr[2];
  198. C[1]=corr[1];
  199. C[2]=corr[0];
  200. C[3]=A[1][2];
  201. C[4]=A[0][1];
  202. C[5]=A[0][2];
  203. C[6]=A[2][2];
  204. C[7]=A[1][1];
  205. C[8]=A[0][0];
  206. for (i=0;i<gain_cdbk_size;i++)
  207. {
  208. float sum=0;
  209. float g0,g1,g2;
  210. ptr = gain_cdbk+3*i;
  211. g0=0.015625*ptr[0]+.5;
  212. g1=0.015625*ptr[1]+.5;
  213. g2=0.015625*ptr[2]+.5;
  214. sum += C[0]*g0;
  215. sum += C[1]*g1;
  216. sum += C[2]*g2;
  217. sum -= C[3]*g0*g1;
  218. sum -= C[4]*g2*g1;
  219. sum -= C[5]*g2*g0;
  220. sum -= .5*C[6]*g0*g0;
  221. sum -= .5*C[7]*g1*g1;
  222. sum -= .5*C[8]*g2*g2;
  223. /* If 1, force "safe" pitch values to handle packet loss better */
  224. if (0) {
  225. float tot = fabs(ptr[1]);
  226. if (ptr[0]>0)
  227. tot+=ptr[0];
  228. if (ptr[2]>0)
  229. tot+=ptr[2];
  230. if (tot>1)
  231. continue;
  232. }
  233. if (sum>best_sum || i==0)
  234. {
  235. best_sum=sum;
  236. best_cdbk=i;
  237. }
  238. }
  239. gain[0] = 0.015625*gain_cdbk[best_cdbk*3] + .5;
  240. gain[1] = 0.015625*gain_cdbk[best_cdbk*3+1]+ .5;
  241. gain[2] = 0.015625*gain_cdbk[best_cdbk*3+2]+ .5;
  242. *cdbk_index=best_cdbk;
  243. }
  244. for (i=0;i<nsf;i++)
  245. exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
  246. err1=0;
  247. err2=0;
  248. for (i=0;i<nsf;i++)
  249. err1+=target[i]*target[i];
  250. for (i=0;i<nsf;i++)
  251. err2+=(target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i])
  252. * (target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i]);
  253. return err2;
  254. }
  255. /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
  256. int pitch_search_3tap(
  257. float target[], /* Target vector */
  258. float *sw,
  259. float ak[], /* LPCs for this subframe */
  260. float awk1[], /* Weighted LPCs #1 for this subframe */
  261. float awk2[], /* Weighted LPCs #2 for this subframe */
  262. float exc[], /* Excitation */
  263. void *par,
  264. int start, /* Smallest pitch value allowed */
  265. int end, /* Largest pitch value allowed */
  266. float pitch_coef, /* Voicing (pitch) coefficient */
  267. int p, /* Number of LPC coeffs */
  268. int nsf, /* Number of samples in subframe */
  269. SpeexBits *bits,
  270. char *stack,
  271. float *exc2,
  272. float *r,
  273. int complexity
  274. )
  275. {
  276. int i,j;
  277. int cdbk_index, pitch=0, best_gain_index=0;
  278. float *best_exc;
  279. int best_pitch=0;
  280. float err, best_err=-1;
  281. int N;
  282. ltp_params *params;
  283. int *nbest;
  284. float *gains;
  285. N=complexity;
  286. if (N>10)
  287. N=10;
  288. nbest=PUSH(stack, N, int);
  289. gains = PUSH(stack, N, float);
  290. params = (ltp_params*) par;
  291. if (N==0 || end<start)
  292. {
  293. speex_bits_pack(bits, 0, params->pitch_bits);
  294. speex_bits_pack(bits, 0, params->gain_bits);
  295. for (i=0;i<nsf;i++)
  296. exc[i]=0;
  297. return start;
  298. }
  299. best_exc=PUSH(stack,nsf, float);
  300. if (N>end-start+1)
  301. N=end-start+1;
  302. open_loop_nbest_pitch(sw, start, end, nsf, nbest, gains, N, stack);
  303. for (i=0;i<N;i++)
  304. {
  305. pitch=nbest[i];
  306. for (j=0;j<nsf;j++)
  307. exc[j]=0;
  308. err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf,
  309. bits, stack, exc2, r, &cdbk_index);
  310. if (err<best_err || best_err<0)
  311. {
  312. for (j=0;j<nsf;j++)
  313. best_exc[j]=exc[j];
  314. best_err=err;
  315. best_pitch=pitch;
  316. best_gain_index=cdbk_index;
  317. }
  318. }
  319. /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
  320. speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
  321. speex_bits_pack(bits, best_gain_index, params->gain_bits);
  322. /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
  323. for (i=0;i<nsf;i++)
  324. exc[i]=best_exc[i];
  325. return pitch;
  326. }
  327. void pitch_unquant_3tap(
  328. float exc[], /* Excitation */
  329. int start, /* Smallest pitch value allowed */
  330. int end, /* Largest pitch value allowed */
  331. float pitch_coef, /* Voicing (pitch) coefficient */
  332. void *par,
  333. int nsf, /* Number of samples in subframe */
  334. int *pitch_val,
  335. float *gain_val,
  336. SpeexBits *bits,
  337. char *stack,
  338. int count_lost,
  339. int subframe_offset,
  340. float last_pitch_gain)
  341. {
  342. int i;
  343. int pitch;
  344. int gain_index;
  345. float gain[3];
  346. signed char *gain_cdbk;
  347. ltp_params *params;
  348. params = (ltp_params*) par;
  349. gain_cdbk=params->gain_cdbk;
  350. pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
  351. pitch += start;
  352. gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
  353. /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
  354. gain[0] = 0.015625*gain_cdbk[gain_index*3]+.5;
  355. gain[1] = 0.015625*gain_cdbk[gain_index*3+1]+.5;
  356. gain[2] = 0.015625*gain_cdbk[gain_index*3+2]+.5;
  357. if (count_lost && pitch > subframe_offset)
  358. {
  359. float gain_sum;
  360. if (1) {
  361. float tmp = count_lost < 4 ? last_pitch_gain : 0.4 * last_pitch_gain;
  362. if (tmp>.95)
  363. tmp=.95;
  364. gain_sum = fabs(gain[1]);
  365. if (gain[0]>0)
  366. gain_sum += gain[0];
  367. else
  368. gain_sum -= .5*gain[0];
  369. if (gain[2]>0)
  370. gain_sum += gain[2];
  371. else
  372. gain_sum -= .5*gain[2];
  373. if (gain_sum > tmp) {
  374. float fact = tmp/gain_sum;
  375. for (i=0;i<3;i++)
  376. gain[i]*=fact;
  377. }
  378. }
  379. if (0) {
  380. gain_sum = fabs(gain[0])+fabs(gain[1])+fabs(gain[2]);
  381. if (gain_sum>.95) {
  382. float fact = .95/gain_sum;
  383. for (i=0;i<3;i++)
  384. gain[i]*=fact;
  385. }
  386. }
  387. }
  388. *pitch_val = pitch;
  389. /**gain_val = gain[0]+gain[1]+gain[2];*/
  390. gain_val[0]=gain[0];
  391. gain_val[1]=gain[1];
  392. gain_val[2]=gain[2];
  393. {
  394. float *e[3];
  395. float *tmp2;
  396. tmp2=PUSH(stack, 3*nsf, float);
  397. e[0]=tmp2;
  398. e[1]=tmp2+nsf;
  399. e[2]=tmp2+2*nsf;
  400. for (i=0;i<3;i++)
  401. {
  402. int j;
  403. int pp=pitch+1-i;
  404. #if 0
  405. for (j=0;j<nsf;j++)
  406. {
  407. if (j-pp<0)
  408. e[i][j]=exc[j-pp];
  409. else if (j-pp-pitch<0)
  410. e[i][j]=exc[j-pp-pitch];
  411. else
  412. e[i][j]=0;
  413. }
  414. #else
  415. {
  416. int tmp1, tmp3;
  417. tmp1=nsf;
  418. if (tmp1>pp)
  419. tmp1=pp;
  420. for (j=0;j<tmp1;j++)
  421. e[i][j]=exc[j-pp];
  422. tmp3=nsf;
  423. if (tmp3>pp+pitch)
  424. tmp3=pp+pitch;
  425. for (j=tmp1;j<tmp3;j++)
  426. e[i][j]=exc[j-pp-pitch];
  427. for (j=tmp3;j<nsf;j++)
  428. e[i][j]=0;
  429. }
  430. #endif
  431. }
  432. for (i=0;i<nsf;i++)
  433. exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
  434. }
  435. }
  436. /** Forced pitch delay and gain */
  437. int forced_pitch_quant(
  438. float target[], /* Target vector */
  439. float *sw,
  440. float ak[], /* LPCs for this subframe */
  441. float awk1[], /* Weighted LPCs #1 for this subframe */
  442. float awk2[], /* Weighted LPCs #2 for this subframe */
  443. float exc[], /* Excitation */
  444. void *par,
  445. int start, /* Smallest pitch value allowed */
  446. int end, /* Largest pitch value allowed */
  447. float pitch_coef, /* Voicing (pitch) coefficient */
  448. int p, /* Number of LPC coeffs */
  449. int nsf, /* Number of samples in subframe */
  450. SpeexBits *bits,
  451. char *stack,
  452. float *exc2,
  453. float *r,
  454. int complexity
  455. )
  456. {
  457. int i;
  458. if (pitch_coef>.99)
  459. pitch_coef=.99;
  460. for (i=0;i<nsf;i++)
  461. {
  462. exc[i]=exc[i-start]*pitch_coef;
  463. }
  464. return start;
  465. }
  466. /** Unquantize forced pitch delay and gain */
  467. void forced_pitch_unquant(
  468. float exc[], /* Excitation */
  469. int start, /* Smallest pitch value allowed */
  470. int end, /* Largest pitch value allowed */
  471. float pitch_coef, /* Voicing (pitch) coefficient */
  472. void *par,
  473. int nsf, /* Number of samples in subframe */
  474. int *pitch_val,
  475. float *gain_val,
  476. SpeexBits *bits,
  477. char *stack,
  478. int count_lost,
  479. int subframe_offset,
  480. float last_pitch_gain)
  481. {
  482. int i;
  483. /*pitch_coef=.9;*/
  484. if (pitch_coef>.99)
  485. pitch_coef=.99;
  486. for (i=0;i<nsf;i++)
  487. {
  488. exc[i]=exc[i-start]*pitch_coef;
  489. }
  490. *pitch_val = start;
  491. gain_val[0]=gain_val[2]=0;
  492. gain_val[1] = pitch_coef;
  493. }