helpfun.c 9.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326
  1. /******************************************************************
  2. iLBC Speech Coder ANSI-C Source Code
  3. helpfun.c
  4. Copyright (C) The Internet Society (2004).
  5. All Rights Reserved.
  6. ******************************************************************/
  7. #include <math.h>
  8. #include "iLBC_define.h"
  9. #include "constants.h"
  10. /*----------------------------------------------------------------*
  11. * calculation of auto correlation
  12. *---------------------------------------------------------------*/
  13. void autocorr(
  14. float *r, /* (o) autocorrelation vector */
  15. const float *x, /* (i) data vector */
  16. int N, /* (i) length of data vector */
  17. int order /* largest lag for calculated
  18. autocorrelations */
  19. ){
  20. int lag, n;
  21. float sum;
  22. for (lag = 0; lag <= order; lag++) {
  23. sum = 0;
  24. for (n = 0; n < N - lag; n++) {
  25. sum += x[n] * x[n+lag];
  26. }
  27. r[lag] = sum;
  28. }
  29. }
  30. /*----------------------------------------------------------------*
  31. * window multiplication
  32. *---------------------------------------------------------------*/
  33. void window(
  34. float *z, /* (o) the windowed data */
  35. const float *x, /* (i) the original data vector */
  36. const float *y, /* (i) the window */
  37. int N /* (i) length of all vectors */
  38. ){
  39. int i;
  40. for (i = 0; i < N; i++) {
  41. z[i] = x[i] * y[i];
  42. }
  43. }
  44. /*----------------------------------------------------------------*
  45. * levinson-durbin solution for lpc coefficients
  46. *---------------------------------------------------------------*/
  47. void levdurb(
  48. float *a, /* (o) lpc coefficient vector starting
  49. with 1.0 */
  50. float *k, /* (o) reflection coefficients */
  51. float *r, /* (i) autocorrelation vector */
  52. int order /* (i) order of lpc filter */
  53. ){
  54. float sum, alpha;
  55. int m, m_h, i;
  56. a[0] = 1.0;
  57. if (r[0] < EPS) { /* if r[0] <= 0, set LPC coeff. to zero */
  58. for (i = 0; i < order; i++) {
  59. k[i] = 0;
  60. a[i+1] = 0;
  61. }
  62. } else {
  63. a[1] = k[0] = -r[1]/r[0];
  64. alpha = r[0] + r[1] * k[0];
  65. for (m = 1; m < order; m++){
  66. sum = r[m + 1];
  67. for (i = 0; i < m; i++){
  68. sum += a[i+1] * r[m - i];
  69. }
  70. k[m] = -sum / alpha;
  71. alpha += k[m] * sum;
  72. m_h = (m + 1) >> 1;
  73. for (i = 0; i < m_h; i++){
  74. sum = a[i+1] + k[m] * a[m - i];
  75. a[m - i] += k[m] * a[i+1];
  76. a[i+1] = sum;
  77. }
  78. a[m+1] = k[m];
  79. }
  80. }
  81. }
  82. /*----------------------------------------------------------------*
  83. * interpolation between vectors
  84. *---------------------------------------------------------------*/
  85. void interpolate(
  86. float *out, /* (o) the interpolated vector */
  87. float *in1, /* (i) the first vector for the
  88. interpolation */
  89. float *in2, /* (i) the second vector for the
  90. interpolation */
  91. float coef, /* (i) interpolation weights */
  92. int length /* (i) length of all vectors */
  93. ){
  94. int i;
  95. float invcoef;
  96. invcoef = (float)1.0 - coef;
  97. for (i = 0; i < length; i++) {
  98. out[i] = coef * in1[i] + invcoef * in2[i];
  99. }
  100. }
  101. /*----------------------------------------------------------------*
  102. * lpc bandwidth expansion
  103. *---------------------------------------------------------------*/
  104. void bwexpand(
  105. float *out, /* (o) the bandwidth expanded lpc
  106. coefficients */
  107. float *in, /* (i) the lpc coefficients before bandwidth
  108. expansion */
  109. float coef, /* (i) the bandwidth expansion factor */
  110. int length /* (i) the length of lpc coefficient vectors */
  111. ){
  112. int i;
  113. float chirp;
  114. chirp = coef;
  115. out[0] = in[0];
  116. for (i = 1; i < length; i++) {
  117. out[i] = chirp * in[i];
  118. chirp *= coef;
  119. }
  120. }
  121. /*----------------------------------------------------------------*
  122. * vector quantization
  123. *---------------------------------------------------------------*/
  124. void vq(
  125. float *Xq, /* (o) the quantized vector */
  126. int *index, /* (o) the quantization index */
  127. const float *CB,/* (i) the vector quantization codebook */
  128. float *X, /* (i) the vector to quantize */
  129. int n_cb, /* (i) the number of vectors in the codebook */
  130. int dim /* (i) the dimension of all vectors */
  131. ){
  132. int i, j;
  133. int pos, minindex;
  134. float dist, tmp, mindist;
  135. pos = 0;
  136. mindist = FLOAT_MAX;
  137. minindex = 0;
  138. for (j = 0; j < n_cb; j++) {
  139. dist = X[0] - CB[pos];
  140. dist *= dist;
  141. for (i = 1; i < dim; i++) {
  142. tmp = X[i] - CB[pos + i];
  143. dist += tmp*tmp;
  144. }
  145. if (dist < mindist) {
  146. mindist = dist;
  147. minindex = j;
  148. }
  149. pos += dim;
  150. }
  151. for (i = 0; i < dim; i++) {
  152. Xq[i] = CB[minindex*dim + i];
  153. }
  154. *index = minindex;
  155. }
  156. /*----------------------------------------------------------------*
  157. * split vector quantization
  158. *---------------------------------------------------------------*/
  159. void SplitVQ(
  160. float *qX, /* (o) the quantized vector */
  161. int *index, /* (o) a vector of indexes for all vector
  162. codebooks in the split */
  163. float *X, /* (i) the vector to quantize */
  164. const float *CB,/* (i) the quantizer codebook */
  165. int nsplit, /* the number of vector splits */
  166. const int *dim, /* the dimension of X and qX */
  167. const int *cbsize /* the number of vectors in the codebook */
  168. ){
  169. int cb_pos, X_pos, i;
  170. cb_pos = 0;
  171. X_pos= 0;
  172. for (i = 0; i < nsplit; i++) {
  173. vq(qX + X_pos, index + i, CB + cb_pos, X + X_pos,
  174. cbsize[i], dim[i]);
  175. X_pos += dim[i];
  176. cb_pos += dim[i] * cbsize[i];
  177. }
  178. }
  179. /*----------------------------------------------------------------*
  180. * scalar quantization
  181. *---------------------------------------------------------------*/
  182. void sort_sq(
  183. float *xq, /* (o) the quantized value */
  184. int *index, /* (o) the quantization index */
  185. float x, /* (i) the value to quantize */
  186. const float *cb,/* (i) the quantization codebook */
  187. int cb_size /* (i) the size of the quantization codebook */
  188. ){
  189. int i;
  190. if (x <= cb[0]) {
  191. *index = 0;
  192. *xq = cb[0];
  193. } else {
  194. i = 0;
  195. while ((x > cb[i]) && i < cb_size - 1) {
  196. i++;
  197. }
  198. if (x > ((cb[i] + cb[i - 1])/2)) {
  199. *index = i;
  200. *xq = cb[i];
  201. } else {
  202. *index = i - 1;
  203. *xq = cb[i - 1];
  204. }
  205. }
  206. }
  207. /*----------------------------------------------------------------*
  208. * check for stability of lsf coefficients
  209. *---------------------------------------------------------------*/
  210. int LSF_check( /* (o) 1 for stable lsf vectors and 0 for
  211. nonstable ones */
  212. float *lsf, /* (i) a table of lsf vectors */
  213. int dim, /* (i) the dimension of each lsf vector */
  214. int NoAn /* (i) the number of lsf vectors in the
  215. table */
  216. ){
  217. int k,n,m, Nit=2, change=0,pos;
  218. //float tmp;
  219. static float eps=(float)0.039; /* 50 Hz */
  220. static float eps2=(float)0.0195;
  221. static float maxlsf=(float)3.14; /* 4000 Hz */
  222. static float minlsf=(float)0.01; /* 0 Hz */
  223. /* LSF separation check*/
  224. for (n=0; n<Nit; n++) { /* Run through a couple of times */
  225. for (m=0; m<NoAn; m++) { /* Number of analyses per frame */
  226. for (k=0; k<(dim-1); k++) {
  227. pos=m*dim+k;
  228. if ((lsf[pos+1]-lsf[pos])<eps) {
  229. if (lsf[pos+1]<lsf[pos]) {
  230. //tmp=lsf[pos+1];
  231. lsf[pos+1]= lsf[pos]+eps2;
  232. lsf[pos]= lsf[pos+1]-eps2;
  233. } else {
  234. lsf[pos]-=eps2;
  235. lsf[pos+1]+=eps2;
  236. }
  237. change=1;
  238. }
  239. if (lsf[pos]<minlsf) {
  240. lsf[pos]=minlsf;
  241. change=1;
  242. }
  243. if (lsf[pos]>maxlsf) {
  244. lsf[pos]=maxlsf;
  245. change=1;
  246. }
  247. }
  248. }
  249. }
  250. return change;
  251. }