helpfun.c 8.5 KB

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