trying some ideas for soft-decision DTD based on residual-to-signal ratio
[speexdsp.git] / libspeex / vq.c
1 /* Copyright (C) 2002 Jean-Marc Valin
2    File: vq.c
3    Vector quantization
4
5    Redistribution and use in source and binary forms, with or without
6    modification, are permitted provided that the following conditions
7    are met:
8    
9    - Redistributions of source code must retain the above copyright
10    notice, this list of conditions and the following disclaimer.
11    
12    - Redistributions in binary form must reproduce the above copyright
13    notice, this list of conditions and the following disclaimer in the
14    documentation and/or other materials provided with the distribution.
15    
16    - Neither the name of the Xiph.org Foundation nor the names of its
17    contributors may be used to endorse or promote products derived from
18    this software without specific prior written permission.
19    
20    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
24    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #ifdef HAVE_CONFIG_H
34 #include "config.h"
35 #endif
36
37 #include "vq.h"
38 #include "stack_alloc.h"
39
40 int scal_quant(spx_word16_t in, const spx_word16_t *boundary, int entries)
41 {
42    int i=0;
43    while (i<entries-1 && in>boundary[0])
44    {
45       boundary++;
46       i++;
47    }
48    return i;
49 }
50
51 int scal_quant32(spx_word32_t in, const spx_word32_t *boundary, int entries)
52 {
53    int i=0;
54    while (i<entries-1 && in>boundary[0])
55    {
56       boundary++;
57       i++;
58    }
59    return i;
60 }
61
62 /*Finds the index of the entry in a codebook that best matches the input*/
63 int vq_index(float *in, const float *codebook, int len, int entries)
64 {
65    int i,j;
66    float min_dist=0;
67    int best_index=0;
68    for (i=0;i<entries;i++)
69    {
70       float dist=0;
71       for (j=0;j<len;j++)
72       {
73          float tmp = in[j]-*codebook++;
74          dist += tmp*tmp;
75       }
76       if (i==0 || dist<min_dist)
77       {
78          min_dist=dist;
79          best_index=i;
80       }
81    }
82    return best_index;
83 }
84
85 #ifdef _USE_SSE
86 #include <xmmintrin.h>
87 #include "misc.h"
88 void vq_nbest(spx_word16_t *_in, const __m128 *codebook, int len, int entries, __m128 *E, int N, int *nbest, spx_word32_t *best_dist, char *stack)
89 {
90    int i,j,k,used;
91    VARDECL(float *dist);
92    VARDECL(__m128 *in);
93    __m128 half;
94    used = 0;
95    ALLOC(dist, entries, float);
96    half = _mm_set_ps1(.5f);
97    ALLOC(in, len, __m128);
98    for (i=0;i<len;i++)
99       in[i] = _mm_set_ps1(_in[i]);
100    for (i=0;i<entries>>2;i++)
101    {
102       __m128 d = _mm_mul_ps(E[i], half);
103       for (j=0;j<len;j++)
104          d = _mm_sub_ps(d, _mm_mul_ps(in[j], *codebook++));
105       _mm_storeu_ps(dist+4*i, d);
106    }
107    for (i=0;i<entries;i++)
108    {
109       if (i<N || dist[i]<best_dist[N-1])
110       {
111          for (k=N-1; (k >= 1) && (k > used || dist[i] < best_dist[k-1]); k--)
112          {
113             best_dist[k]=best_dist[k-1];
114             nbest[k] = nbest[k-1];
115          }
116          best_dist[k]=dist[i];
117          nbest[k]=i;
118          used++;
119       }
120    }
121 }
122
123
124 #else
125
126 #if defined(SHORTCUTS) && (defined(ARM4_ASM) || defined(ARM5E_ASM))
127 #include "vq_arm4.h"
128 #else
129
130 /*Finds the indices of the n-best entries in a codebook*/
131 void vq_nbest(spx_word16_t *in, const spx_word16_t *codebook, int len, int entries, spx_word32_t *E, int N, int *nbest, spx_word32_t *best_dist, char *stack)
132 {
133    int i,j,k,used;
134    used = 0;
135    for (i=0;i<entries;i++)
136    {
137       spx_word32_t dist=0;
138       for (j=0;j<len;j++)
139          dist = MAC16_16(dist,in[j],*codebook++);
140 #ifdef FIXED_POINT
141       dist=SUB32(SHR32(E[i],1),dist);
142 #else
143       dist=.5f*E[i]-dist;
144 #endif
145       if (i<N || dist<best_dist[N-1])
146       {
147          for (k=N-1; (k >= 1) && (k > used || dist < best_dist[k-1]); k--)
148          {
149             best_dist[k]=best_dist[k-1];
150             nbest[k] = nbest[k-1];
151          }
152          best_dist[k]=dist;
153          nbest[k]=i;
154          used++;
155       }
156    }
157 }
158 #endif
159
160 #endif
161
162
163
164 #ifdef _USE_SSE
165
166 void vq_nbest_sign(spx_word16_t *_in, const __m128 *codebook, int len, int entries, __m128 *E, int N, int *nbest, spx_word32_t *best_dist, char *stack)
167 {
168    int i,j,k,used;
169    VARDECL(float *dist);
170    VARDECL(__m128 *in);
171    __m128 half;
172    used = 0;
173    ALLOC(dist, entries, float);
174    half = _mm_set_ps1(.5f);
175    ALLOC(in, len, __m128);
176    for (i=0;i<len;i++)
177       in[i] = _mm_set_ps1(_in[i]);
178    for (i=0;i<entries>>2;i++)
179    {
180       __m128 d = _mm_setzero_ps();
181       for (j=0;j<len;j++)
182          d = _mm_add_ps(d, _mm_mul_ps(in[j], *codebook++));
183       _mm_storeu_ps(dist+4*i, d);
184    }
185    for (i=0;i<entries;i++)
186    {
187       int sign;
188       if (dist[i]>0)
189       {
190          sign=0;
191          dist[i]=-dist[i];
192       } else
193       {
194          sign=1;
195       }
196       dist[i] += .5f*((float*)E)[i];
197       if (i<N || dist[i]<best_dist[N-1])
198       {
199          for (k=N-1; (k >= 1) && (k > used || dist[i] < best_dist[k-1]); k--)
200          {
201             best_dist[k]=best_dist[k-1];
202             nbest[k] = nbest[k-1];
203          }
204          best_dist[k]=dist[i];
205          nbest[k]=i;
206          used++;
207          if (sign)
208             nbest[k]+=entries;
209       }
210    }
211 }
212
213 #else
214
215 /*Finds the indices of the n-best entries in a codebook with sign*/
216 void vq_nbest_sign(spx_word16_t *in, const spx_word16_t *codebook, int len, int entries, spx_word32_t *E, int N, int *nbest, spx_word32_t *best_dist, char *stack)
217 {
218    int i,j,k, sign, used;
219    used=0;
220    for (i=0;i<entries;i++)
221    {
222       spx_word32_t dist=0;
223       for (j=0;j<len;j++)
224          dist = MAC16_16(dist,in[j],*codebook++);
225       if (dist>0)
226       {
227          sign=0;
228          dist=-dist;
229       } else
230       {
231          sign=1;
232       }
233 #ifdef FIXED_POINT
234       dist = ADD32(dist,SHR32(E[i],1));
235 #else
236       dist = ADD32(dist,.5f*E[i]);
237 #endif
238       if (i<N || dist<best_dist[N-1])
239       {
240          for (k=N-1; (k >= 1) && (k > used || dist < best_dist[k-1]); k--)
241          {
242             best_dist[k]=best_dist[k-1];
243             nbest[k] = nbest[k-1];
244          }
245          best_dist[k]=dist;
246          nbest[k]=i;
247          used++;
248          if (sign)
249             nbest[k]+=entries;
250       }
251    }
252 }
253 #endif