pvq: use small LUT for integer sqrt((n+3)/2) sqrt((n+2)/2)
[daala.git] / src / pvq.c
1 /*Daala video codec
2 Copyright (c) 2012 Daala project contributors.  All rights reserved.
3
4 Redistribution and use in source and binary forms, with or without
5 modification, are permitted provided that the following conditions are met:
6
7 - Redistributions of source code must retain the above copyright notice, this
8   list of conditions and the following disclaimer.
9
10 - Redistributions in binary form must reproduce the above copyright notice,
11   this list of conditions and the following disclaimer in the documentation
12   and/or other materials provided with the distribution.
13
14 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
17 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
18 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
20 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
21 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
22 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
23 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/
24
25 #ifdef HAVE_CONFIG_H
26 # include "config.h"
27 #endif
28
29 #include "odintrin.h"
30 #include "pvq.h"
31 #include "partition.h"
32 #include <stdlib.h>
33 #include <stdio.h>
34 #include <math.h>
35 #include <string.h>
36 #include "filter.h"
37
38 /*These tables were generated using compute_basis.c. If OD_FILT_SIZE is
39    changed, they have to be regenerated.*/
40 static const double MAG4[] = {
41   0.870774, 0.872037, 0.949493, 0.947936
42 };
43 static const double MAG8[] = {
44   0.936496, 0.892830, 0.938452, 0.970087,
45   0.974272, 0.967954, 0.974035, 0.990480
46 };
47 static const double MAG16[] = {
48   0.968807, 0.940969, 0.947977, 0.957741,
49   0.969762, 0.978644, 0.984885, 0.988009,
50   0.987424, 0.985569, 0.984215, 0.984462,
51   0.987205, 0.991415, 0.994985, 0.998237
52 };
53 static const double MAG32[] = {
54   0.985068, 0.970006, 0.969893, 0.973192,
55   0.973444, 0.975881, 0.979601, 0.981070,
56   0.984989, 0.987520, 0.988830, 0.990983,
57   0.992376, 0.992884, 0.993447, 0.993381,
58   0.993712, 0.994060, 0.993294, 0.992392,
59   0.991338, 0.992410, 0.992051, 0.993874,
60   0.993488, 0.994162, 0.995318, 0.995925,
61   0.997475, 0.999027, 0.998303, 1.001413,
62 };
63 static const double MAG64[] = {
64   0.992453, 0.984930, 0.985137, 0.985029,
65   0.985514, 0.985784, 0.986269, 0.986854,
66   0.989932, 0.987780, 0.988269, 0.989175,
67   0.989951, 0.990466, 0.991145, 0.991839,
68   0.990773, 0.993191, 0.993618, 0.994221,
69   0.994662, 0.995259, 0.995826, 0.995996,
70   0.999070, 0.996624, 0.996835, 0.996948,
71   0.997022, 0.996973, 0.996993, 0.996996,
72   0.996871, 0.996828, 0.996598, 0.996688,
73   0.996845, 0.996407, 0.996327, 0.996435,
74   0.999173, 0.996216, 0.995981, 0.996173,
75   0.996595, 0.996334, 0.996512, 0.996627,
76   0.994976, 0.997113, 0.997248, 0.997548,
77   0.997943, 0.998121, 0.998291, 0.998687,
78   1.001696, 0.999133, 0.999315, 0.999621,
79   0.999745, 0.999905, 0.999936, 1.000075
80 };
81
82 static const double MAG4_CHROMA_420[] = {
83   0.870774, 0.872037, 0.949493, 0.947936
84 };
85 static const double MAG8_CHROMA_420[] = {
86   0.936496, 0.892830, 0.938452, 0.970087,
87   0.974272, 0.967954, 0.974035, 0.990480
88 };
89 static const double MAG16_CHROMA_420[] = {
90   0.968807, 0.940969, 0.947977, 0.957741,
91   0.969762, 0.978644, 0.984885, 0.988009,
92   0.987424, 0.985569, 0.984215, 0.984462,
93   0.987205, 0.991415, 0.994985, 0.998237
94 };
95 static const double MAG32_CHROMA_420[] = {
96   0.985068, 0.970006, 0.969893, 0.973192,
97   0.973444, 0.975881, 0.979601, 0.981070,
98   0.984989, 0.987520, 0.988830, 0.990983,
99   0.992376, 0.992884, 0.993447, 0.993381,
100   0.993712, 0.994060, 0.993294, 0.992392,
101   0.991338, 0.992410, 0.992051, 0.993874,
102   0.993488, 0.994162, 0.995318, 0.995925,
103   0.997475, 0.999027, 0.998303, 1.001413
104 };
105 static const double MAG64_CHROMA_420[] = {
106   0.992453, 0.984930, 0.985137, 0.985029,
107   0.985514, 0.985784, 0.986269, 0.986854,
108   0.989932, 0.987780, 0.988269, 0.989175,
109   0.989951, 0.990466, 0.991145, 0.991839,
110   0.990773, 0.993191, 0.993618, 0.994221,
111   0.994662, 0.995259, 0.995826, 0.995996,
112   0.999070, 0.996624, 0.996835, 0.996948,
113   0.997022, 0.996973, 0.996993, 0.996996,
114   0.996871, 0.996828, 0.996598, 0.996688,
115   0.996845, 0.996407, 0.996327, 0.996435,
116   0.999173, 0.996216, 0.995981, 0.996173,
117   0.996595, 0.996334, 0.996512, 0.996627,
118   0.994976, 0.997113, 0.997248, 0.997548,
119   0.997943, 0.998121, 0.998291, 0.998687,
120   1.001696, 0.999133, 0.999315, 0.999621,
121   0.999745, 0.999905, 0.999936, 1.000075
122 };
123
124 const double *OD_BASIS_MAG[2][OD_NBSIZES + 1] = {
125   {
126     MAG4, MAG8, MAG16, MAG32, MAG64
127   },
128   {
129     MAG4_CHROMA_420, MAG8_CHROMA_420, MAG16_CHROMA_420, MAG32_CHROMA_420,
130     MAG64_CHROMA_420
131   }
132 };
133
134 /* Quantization matrices for 8x8. For other block sizes, we currently just do
135    resampling. */
136 /* Flat quantization, i.e. optimize for PSNR. */
137 const int OD_QM8_Q4_FLAT[] = {
138   16, 16, 16, 16, 16, 16, 16, 16,
139   16, 16, 16, 16, 16, 16, 16, 16,
140   16, 16, 16, 16, 16, 16, 16, 16,
141   16, 16, 16, 16, 16, 16, 16, 16,
142   16, 16, 16, 16, 16, 16, 16, 16,
143   16, 16, 16, 16, 16, 16, 16, 16,
144   16, 16, 16, 16, 16, 16, 16, 16,
145   16, 16, 16, 16, 16, 16, 16, 16
146 };
147 # if 0
148 /* M1: MPEG2 matrix for inter (which has a dead zone). */
149 const int OD_QM8_Q4[] = {
150   16, 17, 18, 19, 20, 21, 22, 23,
151   17, 18, 19, 20, 21, 22, 23, 24,
152   18, 19, 20, 21, 22, 23, 24, 25,
153   19, 20, 21, 22, 23, 24, 26, 27,
154   20, 21, 22, 23, 25, 26, 27, 28,
155   21, 22, 23, 24, 26, 27, 28, 30,
156   22, 23, 24, 26, 27, 28, 30, 31,
157   23, 24, 25, 27, 28, 30, 31, 33};
158 # endif
159 # if 0
160 /* M2: MPEG2 matrix for intra (no dead zone). */
161 const int OD_QM8_Q4[] = {
162   16, 16, 19, 22, 22, 26, 26, 27,
163   16, 16, 22, 22, 26, 27, 27, 29,
164   19, 22, 26, 26, 27, 29, 29, 35,
165   22, 24, 27, 27, 29, 32, 34, 38,
166   26, 27, 29, 29, 32, 35, 38, 46,
167   27, 29, 34, 34, 35, 40, 46, 56,
168   29, 34, 34, 37, 40, 48, 56, 69,
169   34, 37, 38, 40, 48, 58, 69, 83
170 };
171 # endif
172 # if 0
173 /* M3: Taken from dump_psnrhvs. */
174 const int OD_QM8_Q4[] = {
175   16, 16, 17, 20, 24, 29, 36, 42,
176   16, 17, 17, 19, 22, 26, 31, 37,
177   17, 17, 21, 23, 26, 30, 34, 40,
178   20, 19, 23, 28, 31, 35, 39, 45,
179   24, 22, 26, 31, 36, 41, 46, 51,
180   29, 26, 30, 35, 41, 47, 52, 58,
181   36, 31, 34, 39, 46, 52, 59, 66,
182   42, 37, 40, 45, 51, 58, 66, 73
183 };
184 # endif
185 # if 1
186 /* M4: a compromise equal to .5*(M3 + .5*(M2+transpose(M2))) */
187 const int OD_QM8_Q4_HVS[] = {
188   16, 16, 18, 21, 24, 28, 32, 36,
189   16, 17, 20, 21, 24, 27, 31, 35,
190   18, 20, 24, 25, 27, 31, 33, 38,
191   21, 21, 25, 28, 30, 34, 37, 42,
192   24, 24, 27, 30, 34, 38, 43, 49,
193   28, 27, 31, 34, 38, 44, 50, 58,
194   32, 31, 33, 37, 43, 50, 58, 68,
195   36, 35, 38, 42, 49, 58, 68, 78
196 };
197 #endif
198
199 /* Constants for the beta parameter, which controls how activity masking is
200    used.
201    beta = 1 / (1 - alpha), so when beta is 1, alpha is 0 and activity
202    masking is disabled. When beta is 1.5, activity masking is used. Note that
203    activity masking is neither used for 4x4 blocks nor for chroma. */
204 #define OD_BETA(b) OD_QCONST32(b, OD_BETA_SHIFT)
205 static const od_val16 OD_PVQ_BETA4_LUMA[1] = {OD_BETA(1.)};
206 static const od_val16 OD_PVQ_BETA8_LUMA[4] = {OD_BETA(1.), OD_BETA(1.),
207  OD_BETA(1.), OD_BETA(1.)};
208 static const od_val16 OD_PVQ_BETA16_LUMA[7] = {OD_BETA(1.), OD_BETA(1.),
209  OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.)};
210 static const od_val16 OD_PVQ_BETA32_LUMA[10] = {OD_BETA(1.), OD_BETA(1.),
211  OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.),
212  OD_BETA(1.), OD_BETA(1.)};
213 static const od_val16 OD_PVQ_BETA64_LUMA[13] = {OD_BETA(1.), OD_BETA(1.),
214  OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.),
215  OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.)};
216
217 static const od_val16 OD_PVQ_BETA4_LUMA_MASKING[1] = {OD_BETA(1.)};
218 static const od_val16 OD_PVQ_BETA8_LUMA_MASKING[4] = {OD_BETA(1.5),
219  OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5)};
220 static const od_val16 OD_PVQ_BETA16_LUMA_MASKING[7] = {OD_BETA(1.5),
221  OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5),
222  OD_BETA(1.5)};
223 static const od_val16 OD_PVQ_BETA32_LUMA_MASKING[10] = {OD_BETA(1.5),
224  OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5),
225  OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5)};
226 static const od_val16 OD_PVQ_BETA64_LUMA_MASKING[13] = {OD_BETA(1.5),
227  OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5),
228  OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5), OD_BETA(1.5),
229  OD_BETA(1.5), OD_BETA(1.5)};
230
231 static const od_val16 OD_PVQ_BETA4_CHROMA[1] = {OD_BETA(1.)};
232 static const od_val16 OD_PVQ_BETA8_CHROMA[4] = {OD_BETA(1.), OD_BETA(1.),
233  OD_BETA(1.), OD_BETA(1.)};
234 static const od_val16 OD_PVQ_BETA16_CHROMA[7] = {OD_BETA(1.), OD_BETA(1.),
235  OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.)};
236 static const od_val16 OD_PVQ_BETA32_CHROMA[10] = {OD_BETA(1.), OD_BETA(1.),
237  OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.),
238  OD_BETA(1.), OD_BETA(1.)};
239 static const od_val16 OD_PVQ_BETA64_CHROMA[13] = {OD_BETA(1.), OD_BETA(1.),
240  OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.),
241  OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.), OD_BETA(1.)};
242
243 const od_val16 *const OD_PVQ_BETA[2][OD_NPLANES_MAX][OD_NBSIZES + 1] = {
244  {{OD_PVQ_BETA4_LUMA, OD_PVQ_BETA8_LUMA,
245    OD_PVQ_BETA16_LUMA, OD_PVQ_BETA32_LUMA,
246    OD_PVQ_BETA64_LUMA},
247   {OD_PVQ_BETA4_CHROMA, OD_PVQ_BETA8_CHROMA,
248    OD_PVQ_BETA16_CHROMA, OD_PVQ_BETA32_CHROMA,
249    OD_PVQ_BETA64_CHROMA},
250   {OD_PVQ_BETA4_CHROMA, OD_PVQ_BETA8_CHROMA,
251    OD_PVQ_BETA16_CHROMA, OD_PVQ_BETA32_CHROMA,
252    OD_PVQ_BETA64_CHROMA},
253   {OD_PVQ_BETA4_CHROMA, OD_PVQ_BETA8_CHROMA,
254    OD_PVQ_BETA16_CHROMA, OD_PVQ_BETA32_CHROMA,
255    OD_PVQ_BETA64_CHROMA}},
256  {{OD_PVQ_BETA4_LUMA_MASKING, OD_PVQ_BETA8_LUMA_MASKING,
257    OD_PVQ_BETA16_LUMA_MASKING, OD_PVQ_BETA32_LUMA_MASKING,
258    OD_PVQ_BETA64_LUMA_MASKING},
259   {OD_PVQ_BETA4_CHROMA, OD_PVQ_BETA8_CHROMA,
260    OD_PVQ_BETA16_CHROMA, OD_PVQ_BETA32_CHROMA,
261    OD_PVQ_BETA64_CHROMA},
262   {OD_PVQ_BETA4_CHROMA, OD_PVQ_BETA8_CHROMA,
263    OD_PVQ_BETA16_CHROMA, OD_PVQ_BETA32_CHROMA,
264    OD_PVQ_BETA64_CHROMA},
265   {OD_PVQ_BETA4_CHROMA, OD_PVQ_BETA8_CHROMA,
266    OD_PVQ_BETA16_CHROMA, OD_PVQ_BETA32_CHROMA,
267    OD_PVQ_BETA64_CHROMA}}
268 };
269
270 void od_adapt_pvq_ctx_reset(od_pvq_adapt_ctx *state, int is_keyframe) {
271   od_pvq_codeword_ctx *ctx;
272   int i;
273   int pli;
274   int bs;
275   ctx = &state->pvq_codeword_ctx;
276   generic_model_init(&state->pvq_param_model[0]);
277   generic_model_init(&state->pvq_param_model[1]);
278   generic_model_init(&state->pvq_param_model[2]);
279   for (i = 0; i < 2*OD_NBSIZES; i++) {
280     ctx->pvq_adapt[4*i + OD_ADAPT_K_Q8] = 384;
281     ctx->pvq_adapt[4*i + OD_ADAPT_SUM_EX_Q8] = 256;
282     ctx->pvq_adapt[4*i + OD_ADAPT_COUNT_Q8] = 104;
283     ctx->pvq_adapt[4*i + OD_ADAPT_COUNT_EX_Q8] = 128;
284   }
285   ctx->pvq_k1_increment = 128;
286   OD_CDFS_INIT(ctx->pvq_k1_cdf, ctx->pvq_k1_increment);
287   for (pli = 0; pli < OD_NPLANES_MAX; pli++) {
288     for (bs = 0; bs < OD_NBSIZES; bs++)
289     for (i = 0; i < PVQ_MAX_PARTITIONS; i++) {
290       state->pvq_exg[pli][bs][i] = 2 << 16;
291     }
292   }
293   for (i = 0; i < OD_NBSIZES*PVQ_MAX_PARTITIONS; i++) {
294     state->pvq_ext[i] = is_keyframe ? 24576 : 2 << 16;
295   }
296   state->pvq_gaintheta_increment = 128;
297   OD_CDFS_INIT(state->pvq_gaintheta_cdf, state->pvq_gaintheta_increment >> 2);
298   state->pvq_skip_dir_increment = 128;
299   OD_CDFS_INIT(state->pvq_skip_dir_cdf, state->pvq_skip_dir_increment >> 2);
300   ctx->pvq_split_increment = 128;
301   OD_CDFS_INIT(ctx->pvq_split_cdf, ctx->pvq_split_increment >> 1);
302 }
303
304 /* QMs are arranged from smallest to largest blocksizes, first for
305    blocks with decimation=0, followed by blocks with decimation=1.*/
306 int od_qm_offset(int bs, int xydec)
307 {
308     return xydec*OD_QM_STRIDE + OD_QM_OFFSET(bs);
309 }
310
311 /* Initialize the quantization matrix with the magnitude compensation applied.
312    We need to compensate for the magnitude because lapping causes some basis
313    functions to be smaller, so they would end up being quantized too finely
314    (the same error in the quantized domain would result in a smaller pixel
315    domain error). */
316 void od_init_qm(int16_t *x, int16_t *x_inv, const int *qm) {
317   int i;
318   int j;
319   int16_t y[OD_BSIZE_MAX*OD_BSIZE_MAX];
320   int16_t y_inv[OD_BSIZE_MAX*OD_BSIZE_MAX];
321   int16_t *x1;
322   int16_t *x1_inv;
323   int off;
324   int bs;
325   int xydec;
326   for (bs = 0; bs < OD_NBSIZES; bs++) {
327     for (xydec = 0; xydec < 2; xydec++) {
328       off = od_qm_offset(bs, xydec);
329       x1 = x + off;
330       x1_inv = x_inv + off;
331       for (i = 0; i < 4 << bs; i++) {
332         for (j = 0; j < 4 << bs; j++) {
333           double mag;
334 #if OD_DEBLOCKING || OD_DISABLE_FILTER
335           mag = 1.0;
336 #else
337           mag = OD_BASIS_MAG[xydec][bs][i]*OD_BASIS_MAG[xydec][bs][j];
338 #endif
339           if (i == 0 && j == 0) {
340             mag = 1.0;
341           }
342           else {
343             mag /= 0.0625*qm[(i << 1 >> bs)*8 + (j << 1 >> bs)];
344             OD_ASSERT(mag > 0.0);
345           }
346           /*Convert to fit in 16 bits.*/
347           y[i*(4 << bs) + j] = (int16_t)OD_MINI(OD_QM_SCALE_MAX,
348            (int32_t)floor(.5 + mag*OD_QM_SCALE));
349           y_inv[i*(4 << bs) + j] = (int16_t)floor(.5
350            + OD_QM_SCALE*OD_QM_INV_SCALE/(double)y[i*(4 << bs) + j]);
351         }
352       }
353       od_raster_to_coding_order_16(x1, 4 << bs, y, 4 << bs);
354       od_raster_to_coding_order_16(x1_inv, 4 << bs, y_inv, 4 << bs);
355     }
356   }
357 }
358
359 /* Maps each possible size (n) in the split k-tokenizer to a different value.
360    Possible values of n are:
361    2, 3, 4, 7, 8, 14, 15, 16, 31, 32, 63, 64, 127, 128
362    Since we don't care about the order (even in the bit-stream) the simplest
363    ordering (implemented here) is:
364    14, 2, 3, 4, 7, 8, 15, 16, 31, 32, 63, 64, 127, 128 */
365 int od_pvq_size_ctx(int n) {
366   int logn;
367   int odd;
368   logn = OD_ILOG(n - 1);
369   odd = n & 1;
370   return 2*logn - 1 - odd - 7*(n == 14);
371 }
372
373 /* Maps a length n to a context for the (k=1, n<=16) coder, with a special
374    case when n is the original length (orig_length=1) of the vector (i.e. we
375    haven't split it yet). For orig_length=0, we use the same mapping as
376    od_pvq_size_ctx() up to n=16. When orig_length=1, we map lengths
377    7, 8, 14, 15 to contexts 8 to 11. */
378 int od_pvq_k1_ctx(int n, int orig_length) {
379   if (orig_length) return 8 + 2*(n > 8) + (n & 1);
380   else return od_pvq_size_ctx(n);
381 }
382
383 /* Indexing for the packed quantization matrices. */
384 int od_qm_get_index(int bs, int band) {
385   /* The -band/3 term is due to the fact that we force corresponding horizontal
386      and vertical bands to have the same quantization. */
387   OD_ASSERT(bs >= 0 && bs < OD_NBSIZES);
388   return bs*(bs + 1) + band - band/3;
389 }
390
391 #if !defined(OD_FLOAT_PVQ)
392 /*See celt/mathops.c in Opus and tools/cos_search.c.*/
393 static int16_t od_pvq_cos_pi_2(int16_t x)
394 {
395   int16_t x2;
396   x2 = OD_MULT16_16_Q15(x, x);
397   return OD_MINI(32767, (1073758164 - x*x + x2*(-7654 + OD_MULT16_16_Q16(x2,
398    16573 + OD_MULT16_16_Q16(-2529, x2)))) >> 15);
399 }
400 #endif
401
402 /*Approximates cos(x) for -pi < x < pi.
403   Input is in OD_THETA_SCALE.*/
404 od_val16 od_pvq_cos(od_val32 x) {
405 #if defined(OD_FLOAT_PVQ)
406   return cos(x);
407 #else
408   /*Wrap x around by masking, since cos is periodic.*/
409   x = x & 0x0001ffff;
410   if (x > (1 << 16)) {
411     x = (1 << 17) - x;
412   }
413   if (x & 0x00007fff) {
414     if (x < (1 << 15)) {
415        return od_pvq_cos_pi_2((int16_t)x);
416     }
417     else {
418       return -od_pvq_cos_pi_2((int16_t)(65536 - x));
419     }
420   }
421   else {
422     if (x & 0x0000ffff) {
423       return 0;
424     }
425     else if (x & 0x0001ffff) {
426       return -32767;
427     }
428     else {
429       return 32767;
430     }
431   }
432 #endif
433 }
434
435 /*Approximates sin(x) for 0 <= x < pi.
436   Input is in OD_THETA_SCALE.*/
437 od_val16 od_pvq_sin(od_val32 x) {
438 #if defined(OD_FLOAT_PVQ)
439   return sin(x);
440 #else
441   return od_pvq_cos(32768 - x);
442 #endif
443 }
444
445 #if !defined(OD_FLOAT_PVQ)
446 /* Computes an upper-bound on the number of bits required to store the L2 norm
447    of a vector (excluding sign). */
448 int od_vector_log_mag(const od_coeff *x, int n) {
449   int i;
450   int32_t sum;
451   sum = 0;
452   for (i = 0; i < n; i++) {
453     int16_t tmp;
454     tmp = x[i] >> 8;
455     sum += tmp*(int32_t)tmp;
456   }
457   /* We add one full bit (instead of rounding OD_ILOG() up) for safety because
458      the >> 8 above causes the sum to be slightly underestimated. */
459   return 8 + 1 + OD_ILOG(n + sum)/2;
460 }
461 #endif
462
463 /** Computes Householder reflection that aligns the reference r to the
464  *  dimension in r with the greatest absolute value. The reflection
465  *  vector is returned in r.
466  *
467  * @param [in,out]  r      reference vector to be reflected, reflection
468  *                         also returned in r
469  * @param [in]      n      number of dimensions in r
470  * @param [in]      gr     gain of reference vector
471  * @param [out]     sign   sign of reflection
472  * @return                 dimension number to which reflection aligns
473  **/
474 int od_compute_householder(od_val16 *r, int n, od_val32 gr, int *sign,
475  int shift) {
476   int m;
477   int i;
478   int s;
479   od_val16 maxr;
480   OD_UNUSED(shift);
481   /* Pick component with largest magnitude. Not strictly
482    * necessary, but it helps numerical stability */
483   m = 0;
484   maxr = 0;
485   for (i = 0; i < n; i++) {
486     if (OD_ABS(r[i]) > maxr) {
487       maxr = OD_ABS(r[i]);
488       m = i;
489     }
490   }
491   s = r[m] > 0 ? 1 : -1;
492   /* This turns r into a Householder reflection vector that would reflect
493    * the original r[] to e_m */
494   r[m] += OD_SHR_ROUND(gr*s, shift);
495   *sign = s;
496   return m;
497 }
498
499 #if !defined(OD_FLOAT_PVQ)
500 #define OD_RCP_INSHIFT 15
501 #define OD_RCP_OUTSHIFT 14
502 static od_val16 od_rcp(od_val16 x)
503 {
504   int i;
505   od_val16 n;
506   od_val16 r;
507   i = OD_ILOG(x) - 1;
508   /*n is Q15 with range [0,1).*/
509   n = OD_VSHR_ROUND(x, i - OD_RCP_INSHIFT) - (1 << OD_RCP_INSHIFT);
510   /*Start with a linear approximation:
511     r = 1.8823529411764706-0.9411764705882353*n.
512     The coefficients and the result are Q14 in the range [15420,30840].*/
513   r = 30840 + OD_MULT16_16_Q15(-15420, n);
514   /*Perform two Newton iterations:
515     r -= r*((r*n)-1.Q15)
516        = r*((r*n)+(r-1.Q15)).*/
517   r = r - OD_MULT16_16_Q15(r, (OD_MULT16_16_Q15(r, n) + r - 32768));
518   /*We subtract an extra 1 in the second iteration to avoid overflow; it also
519      neatly compensates for truncation error in the rest of the process.*/
520   r = r - (1 + OD_MULT16_16_Q15(r, OD_MULT16_16_Q15(r, n) + r - 32768));
521   /*r is now the Q15 solution to 2/(n+1), with a maximum relative error
522      of 7.05346E-5, a (relative) RMSE of 2.14418E-5, and a peak absolute
523      error of 1.24665/32768.*/
524   return OD_VSHR_ROUND(r, i - OD_RCP_OUTSHIFT);
525 }
526 #endif
527
528 /** Applies Householder reflection from compute_householder(). The
529  * reflection is its own inverse.
530  *
531  * @param [out]     out    reflected vector
532  * @param [in]      x      vector to be reflected
533  * @param [in]      r      reflection
534  * @param [in]      n      number of dimensions in x,r
535  */
536 void od_apply_householder(od_val16 *out, const od_val16 *x, const od_val16 *r,
537  int n) {
538   int i;
539   od_val32 proj;
540   od_val16 proj_1;
541   od_val32 l2r;
542 #if !defined(OD_FLOAT_PVQ)
543   od_val16 proj_norm;
544   od_val16 l2r_norm;
545   od_val16 rcp;
546   int proj_shift;
547   int l2r_shift;
548   int outshift;
549 #endif
550   /*FIXME: Can we get l2r and/or l2r_shift from an earlier computation?*/
551   l2r = 0;
552   for (i = 0; i < n; i++) {
553     l2r += OD_MULT16_16(r[i], r[i]);
554   }
555   /* Apply Householder reflection */
556   proj = 0;
557   for (i = 0; i < n; i++) {
558     proj += OD_MULT16_16(r[i], x[i]);
559   }
560 #if defined(OD_FLOAT_PVQ)
561   proj_1 = proj*2./(1e-100 + l2r);
562   for (i = 0; i < n; i++) {
563     out[i] = x[i] - r[i]*proj_1;
564   }
565 #else
566   /*l2r_norm is [0.5, 1.0[ in Q15.*/
567   l2r_shift = (OD_ILOG(l2r) - 1) - 14;
568   l2r_norm = OD_VSHR_ROUND(l2r, l2r_shift);
569   rcp = od_rcp(l2r_norm);
570   proj_shift = (OD_ILOG(abs(proj)) - 1) - 14;
571   /*proj_norm is [0.5, 1.0[ in Q15.*/
572   proj_norm = OD_VSHR_ROUND(proj, proj_shift);
573   proj_1 = OD_MULT16_16_Q15(proj_norm, rcp);
574   /*The proj*2. in the float code becomes -1 in the final outshift.
575     The sign of l2r_shift is positive since we're taking the reciprocal of
576      l2r_norm and this is a right shift.*/
577   outshift = OD_MINI(30, OD_RCP_OUTSHIFT - proj_shift - 1 + l2r_shift);
578   if (outshift >= 0) {
579     for (i = 0; i < n; i++) {
580       int32_t tmp;
581       tmp = OD_MULT16_16(r[i], proj_1);
582       tmp = OD_SHR_ROUND(tmp, outshift);
583       out[i] = x[i] - tmp;
584     }
585   }
586   else {
587     /*FIXME: Can we make this case impossible?
588       Right now, if r[] is all zeros except for 1, 2, or 3 ones, and
589        if x[] is all zeros except for large values at the same position as the
590        ones in r[], then we can end up with a shift of -1.*/
591     for (i = 0; i < n; i++) {
592       int32_t tmp;
593       tmp = OD_MULT16_16(r[i], proj_1);
594       tmp = OD_SHL(tmp, -outshift);
595       out[i] = x[i] - tmp;
596     }
597   }
598 #endif
599 }
600
601 #if !defined(OD_FLOAT_PVQ)
602 static od_val16 od_beta_rcp(od_val16 beta){
603   if (beta == OD_BETA(1.))
604     return OD_BETA(1.);
605   else if (beta == OD_BETA(1.5))
606     return OD_BETA(1./1.5);
607   else {
608     od_val16 rcp_beta;
609     /*Shift by 1 less, transposing beta to range [.5, .75] and thus < 32768.*/
610     rcp_beta = od_rcp(beta << (OD_RCP_INSHIFT - 1 - OD_BETA_SHIFT));
611     return OD_SHR_ROUND(rcp_beta, OD_RCP_OUTSHIFT + 1 - OD_BETA_SHIFT);
612   }
613 }
614
615 #define OD_EXP2_INSHIFT 15
616 #define OD_EXP2_FRACSHIFT 15
617 #define OD_EXP2_OUTSHIFT 15
618 static const int32_t OD_EXP2_C[5] = {32768, 22709, 7913, 1704, 443};
619 /*Output is [1.0, 2.0) in Q(OD_EXP2_FRACSHIFT).
620   It does not include the integer offset, which is added in od_exp2 after the
621    final shift).*/
622 static int32_t od_exp2_frac(int32_t x)
623 {
624   return OD_MULT16_16_Q15(x, (OD_EXP2_C[1] + OD_MULT16_16_Q15(x,
625    (OD_EXP2_C[2] + OD_MULT16_16_Q15(x, (OD_EXP2_C[3]
626    + OD_MULT16_16_Q15(x, OD_EXP2_C[4])))))));
627 }
628
629 /** Base-2 exponential approximation (2^x) with Q15 input and output.*/
630 static int32_t od_exp2(int32_t x)
631 {
632   int integer;
633   int32_t frac;
634   integer = x >> OD_EXP2_INSHIFT;
635   if (integer > 14)
636     return 0x7f000000;
637   else if (integer < -15)
638     return 0;
639   frac = od_exp2_frac(x - OD_SHL(integer, OD_EXP2_INSHIFT));
640   return OD_VSHR_ROUND(OD_EXP2_C[0] + frac, -integer) + 1;
641 }
642
643 #define OD_LOG2_INSHIFT 15
644 #define OD_LOG2_OUTSHIFT 15
645 #define OD_LOG2_INSCALE_1 (1./(1 << OD_LOG2_INSHIFT))
646 #define OD_LOG2_OUTSCALE (1 << OD_LOG2_OUTSHIFT)
647 static int16_t od_log2(int16_t x)
648 {
649   return x + OD_MULT16_16_Q15(x, (14482 + OD_MULT16_16_Q15(x, (-23234
650    + OD_MULT16_16_Q15(x, (13643 + OD_MULT16_16_Q15(x, (-6403
651    + OD_MULT16_16_Q15(x, 1515)))))))));
652 }
653
654 static int32_t od_pow(int32_t x, od_val16 beta)
655 {
656   int16_t t;
657   int xshift;
658   int log2_x;
659   od_val32 logr;
660   /*FIXME: this conditional is to avoid doing log2(0).*/
661   if (x == 0)
662     return 0;
663   log2_x = (OD_ILOG(x) - 1);
664   xshift = log2_x - OD_LOG2_INSHIFT;
665   /*t should be in range [0.0, 1.0[ in Q(OD_LOG2_INSHIFT).*/
666   t = OD_VSHR(x, xshift) - (1 << OD_LOG2_INSHIFT);
667   /*log2(g/OD_COMPAND_SCALE) = log2(x) - OD_COMPAND_SHIFT in
668      Q(OD_LOG2_OUTSHIFT).*/
669   logr = od_log2(t) + (log2_x - OD_COMPAND_SHIFT)*OD_LOG2_OUTSCALE;
670   logr = OD_MULT16_32_QBETA(beta, logr);
671   return od_exp2(logr);
672 }
673 #endif
674
675 /** Gain companding: raises gain to the power 1/beta for activity masking.
676  *
677  * @param [in]  g     real (uncompanded) gain
678  * @param [in]  q0    uncompanded quality parameter
679  * @param [in]  beta  activity masking beta param (exponent)
680  * @return            g^(1/beta)
681  */
682 static od_val32 od_gain_compand(od_val32 g, int q0, od_val16 beta) {
683 #if defined(OD_FLOAT_PVQ)
684   if (beta == 1) return OD_ROUND32(OD_CGAIN_SCALE*g/(double)q0);
685   else {
686     return OD_ROUND32(OD_CGAIN_SCALE*OD_COMPAND_SCALE*pow(g*OD_COMPAND_SCALE_1,
687      1./beta)/(double)q0);
688   }
689 #else
690   if (beta == OD_BETA(1)) return (OD_CGAIN_SCALE*g + (q0 >> 1))/q0;
691   else {
692     int32_t expr;
693     expr = od_pow(g, od_beta_rcp(beta));
694     expr <<= OD_CGAIN_SHIFT + OD_COMPAND_SHIFT - OD_EXP2_OUTSHIFT;
695     return (expr + (q0 >> 1))/q0;
696   }
697 #endif
698 }
699
700 #if !defined(OD_FLOAT_PVQ)
701 #define OD_SQRT_INSHIFT 16
702 #define OD_SQRT_OUTSHIFT 15
703 static int16_t od_rsqrt_norm(int16_t x);
704
705 static int16_t od_sqrt_norm(int32_t x)
706 {
707   OD_ASSERT(x < 65536);
708   return OD_MINI(OD_SHR_ROUND(x*od_rsqrt_norm(x), OD_SQRT_OUTSHIFT), 32767);
709 }
710
711 static int16_t od_sqrt(int32_t x, int *sqrt_shift)
712 {
713   int k;
714   int s;
715   int32_t t;
716   if (x == 0) {
717     *sqrt_shift = 0;
718      return 0;
719   }
720   OD_ASSERT(x < (1 << 30));
721   k = ((OD_ILOG(x) - 1) >> 1);
722   /*t is x in the range [0.25, 1) in QINSHIFT, or x*2^(-s).
723     Shift by log2(x) - log2(0.25*(1 << INSHIFT)) to ensure 0.25 lower bound.*/
724   s = 2*k - (OD_SQRT_INSHIFT - 2);
725   t = OD_VSHR(x, s);
726   /*We want to express od_sqrt() in terms of od_sqrt_norm(), which is
727      defined as (2^OUTSHIFT)*sqrt(t*(2^-INSHIFT)) with t=x*(2^-s).
728     This simplifies to 2^(OUTSHIFT-(INSHIFT/2)-(s/2))*sqrt(x), so the caller
729      needs to shift right by OUTSHIFT - INSHIFT/2 - s/2.*/
730   *sqrt_shift = OD_SQRT_OUTSHIFT - ((s + OD_SQRT_INSHIFT) >> 1);
731   return od_sqrt_norm(t);
732 }
733 #endif
734
735 /** Gain expanding: raises gain to the power beta for activity masking.
736  *
737  * @param [in]  cg    companded gain
738  * @param [in]  q0    uncompanded quality parameter
739  * @param [in]  beta  activity masking beta param (exponent)
740  * @return            g^beta
741  */
742 od_val32 od_gain_expand(od_val32 cg0, int q0, od_val16 beta) {
743   if (beta == OD_BETA(1)) {
744     /*The multiply fits into 28 bits because the expanded gain has a range from
745        0 to 2^20.*/
746     return OD_SHR_ROUND(cg0*q0, OD_CGAIN_SHIFT);
747   }
748   else if (beta == OD_BETA(1.5)) {
749 #if defined(OD_FLOAT_PVQ)
750     double cg;
751     cg = cg0*OD_CGAIN_SCALE_1;
752     cg *= q0*OD_COMPAND_SCALE_1;
753     return OD_ROUND32(OD_COMPAND_SCALE*cg*sqrt(cg));
754 #else
755     int32_t irt;
756     int64_t tmp;
757     int sqrt_inshift;
758     int sqrt_outshift;
759     /*cg0 is in Q(OD_CGAIN_SHIFT) and we need to divide it by
760        2^OD_COMPAND_SHIFT.*/
761     irt = od_sqrt(cg0*q0, &sqrt_outshift);
762     sqrt_inshift = (OD_CGAIN_SHIFT + OD_COMPAND_SHIFT) >> 1;
763     /*tmp is in Q(OD_CGAIN_SHIFT + OD_COMPAND_SHIFT).*/
764     tmp = cg0*q0*(int64_t)irt;
765     /*Expanded gain must be in Q(OD_COMPAND_SHIFT), thus OD_COMPAND_SHIFT is
766        not included here.*/
767     return OD_VSHR_ROUND(tmp, OD_CGAIN_SHIFT + sqrt_outshift + sqrt_inshift);
768 #endif
769   }
770   else {
771 #if defined(OD_FLOAT_PVQ)
772     /*Expanded gain must be in Q(OD_COMPAND_SHIFT), hence the multiply by
773        OD_COMPAND_SCALE.*/
774     double cg;
775     cg = cg0*OD_CGAIN_SCALE_1;
776     return OD_ROUND32(OD_COMPAND_SCALE*pow(cg*q0*OD_COMPAND_SCALE_1, beta));
777 #else
778     int32_t expr;
779     int32_t cg;
780     cg = OD_SHR_ROUND(cg0*q0, OD_CGAIN_SHIFT);
781     expr = od_pow(cg, beta);
782     /*Expanded gain must be in Q(OD_COMPAND_SHIFT), hence the subtraction by
783        OD_COMPAND_SHIFT.*/
784     return OD_SHR_ROUND(expr, OD_EXP2_OUTSHIFT - OD_COMPAND_SHIFT);
785 #endif
786   }
787 }
788
789 /** Computes the raw and quantized/companded gain of a given input
790  * vector
791  *
792  * @param [in]      x      vector of input data
793  * @param [in]      n      number of elements in vector x
794  * @param [in]      q0     quantizer
795  * @param [out]     g      raw gain
796  * @param [in]      beta   activity masking beta param
797  * @param [in]      bshift shift to be applied to raw gain
798  * @return                 quantized/companded gain
799  */
800 od_val32 od_pvq_compute_gain(const od_val16 *x, int n, int q0, od_val32 *g,
801  od_val16 beta, int bshift) {
802   int i;
803   od_val32 acc;
804 #if !defined(OD_FLOAT_PVQ)
805   od_val32 irt;
806   int sqrt_shift;
807 #else
808   OD_UNUSED(bshift);
809 #endif
810   acc = 0;
811   for (i = 0; i < n; i++) {
812     acc += x[i]*(od_val32)x[i];
813   }
814 #if defined(OD_FLOAT_PVQ)
815   *g = sqrt(acc);
816 #else
817   irt = od_sqrt(acc, &sqrt_shift);
818   *g = OD_VSHR_ROUND(irt, sqrt_shift - bshift);
819 #endif
820   /* Normalize gain by quantization step size and apply companding
821      (if ACTIVITY != 1). */
822   return od_gain_compand(*g, q0, beta);
823 }
824
825 /** Compute theta quantization range from quantized/companded gain
826  *
827  * @param [in]      qcg    quantized companded gain value
828  * @param [in]      beta   activity masking beta param
829  * @return                 max theta value
830  */
831 int od_pvq_compute_max_theta(od_val32 qcg, od_val16 beta){
832   /* Set angular resolution (in ra) to match the encoded gain */
833 #if defined(OD_FLOAT_PVQ)
834   int ts = (int)floor(.5 + qcg*OD_CGAIN_SCALE_1*M_PI/(2*beta));
835 #else
836   int ts = OD_SHR_ROUND(qcg*OD_MULT16_16_QBETA(OD_QCONST32(M_PI/2,
837    OD_CGAIN_SHIFT), od_beta_rcp(beta)), OD_CGAIN_SHIFT*2);
838 #endif
839   /* Special case for low gains -- will need to be tuned anyway */
840   if (qcg < OD_QCONST32(1.4, OD_CGAIN_SHIFT)) ts = 1;
841   return ts;
842 }
843
844 /** Decode quantized theta value from coded value
845  *
846  * @param [in]      t          quantized companded gain value
847  * @param [in]      max_theta  maximum theta value
848  * @return                     decoded theta value
849  */
850 od_val32 od_pvq_compute_theta(int t, int max_theta) {
851   if (max_theta != 0) {
852 #if defined(OD_FLOAT_PVQ)
853     return OD_MINI(t, max_theta - 1)*.5*M_PI/max_theta;
854 #else
855     return (OD_MAX_THETA_SCALE*OD_MINI(t, max_theta - 1)
856      + (max_theta >> 1))/max_theta;
857 #endif
858   }
859   else return 0;
860 }
861
862 #define OD_SQRT_TBL_SHIFT (10)
863
864 #define OD_ITHETA_SHIFT 15
865 /** Compute the number of pulses used for PVQ encoding a vector from
866  * available metrics (encode and decode side)
867  *
868  * @param [in]      qcg        quantized companded gain value
869  * @param [in]      itheta     quantized PVQ error angle theta
870  * @param [in]      theta      PVQ error angle theta
871  * @param [in]      noref      indicates present or lack of reference
872  *                             (prediction)
873  * @param [in]      n          number of elements to be coded
874  * @param [in]      beta       activity masking beta param
875  * @param [in]      nodesync   do not use info that depends on the reference
876  * @return                     number of pulses to use for coding
877  */
878 int od_pvq_compute_k(od_val32 qcg, int itheta, od_val32 theta, int noref, int n,
879  od_val16 beta, int nodesync) {
880 #if !defined(OD_FLOAT_PVQ)
881   /*Lookup table for sqrt(n+3/2) and sqrt(n+2/2) in Q10.
882     Real max values are 32792 and 32784, but clamped to stay within 16 bits.
883     Update with tools/gen_sqrt_tbl if needed.*/
884   static const od_val16 od_sqrt_table[2][13] = {
885    {0, 0, 0, 0, 2290, 2985, 4222, 0, 8256, 0, 16416, 0, 32767},
886    {0, 0, 0, 0, 2401, 3072, 4284, 0, 8287, 0, 16432, 0, 32767}};
887 #endif
888   if (noref) {
889     if (qcg == 0) return 0;
890     if (n == 15 && qcg == OD_CGAIN_SCALE && beta > OD_BETA(1.25)) {
891       return 1;
892     }
893     else {
894 #if defined(OD_FLOAT_PVQ)
895       return OD_MAXI(1, (int)floor(.5 + (qcg*OD_CGAIN_SCALE_1 - .2)*
896        sqrt((n + 3)/2)/beta));
897 #else
898       od_val16 rt;
899       OD_ASSERT(OD_ILOG(n + 1) < 13);
900       rt = od_sqrt_table[1][OD_ILOG(n + 1)];
901       /*FIXME: get rid of 64-bit mul.*/
902       return OD_MAXI(1, OD_SHR_ROUND((int64_t)((qcg
903        - (int64_t)OD_QCONST32(.2, OD_CGAIN_SHIFT))*
904        OD_MULT16_16_QBETA(od_beta_rcp(beta), rt)), OD_CGAIN_SHIFT
905        + OD_SQRT_TBL_SHIFT));
906 #endif
907     }
908   }
909   else {
910     if (itheta == 0) return 0;
911     /* Sets K according to gain and theta, based on the high-rate
912        PVQ distortion curves (see PVQ document). Low-rate will have to be
913        perceptually tuned anyway. We subtract 0.2 from the radius as an
914        approximation for the fact that the coefficients aren't identically
915        distributed within a band so at low gain the number of dimensions that
916        are likely to have a pulse is less than n. */
917     if (nodesync) {
918 #if defined(OD_FLOAT_PVQ)
919       return OD_MAXI(1, (int)floor(.5 + (itheta - .2)*sqrt((n + 2)/2)));
920 #else
921       od_val16 rt;
922       OD_ASSERT(OD_ILOG(n + 1) < 13);
923       rt = od_sqrt_table[0][OD_ILOG(n + 1)];
924       /*FIXME: get rid of 64-bit mul.*/
925       return OD_MAXI(1, OD_VSHR_ROUND(((OD_SHL(itheta, OD_ITHETA_SHIFT)
926        - OD_QCONST32(.2, OD_ITHETA_SHIFT)))*(int64_t)rt,
927        OD_SQRT_TBL_SHIFT + OD_ITHETA_SHIFT));
928 #endif
929     }
930     else {
931       return OD_MAXI(1, (int)floor(.5 + (qcg*OD_CGAIN_SCALE_1*
932        od_pvq_sin(theta)*OD_TRIG_SCALE_1 - .2)*sqrt((n
933        + 2)/2)/(beta*OD_BETA_SCALE_1)));
934     }
935   }
936 }
937
938 #if !defined(OD_FLOAT_PVQ)
939 #define OD_RSQRT_INSHIFT 16
940 #define OD_RSQRT_OUTSHIFT 14
941 /** Reciprocal sqrt approximation where the input is in the range [0.25,1) in
942      Q16 and the output is in the range (1.0, 2.0] in Q14).
943     Error is always within +/1 of round(1/sqrt(t))*/
944 static int16_t od_rsqrt_norm(int16_t t)
945 {
946   int16_t n;
947   int32_t r;
948   int32_t r2;
949   int32_t ry;
950   int32_t y;
951   int32_t ret;
952   /* Range of n is [-16384,32767] ([-0.5,1) in Q15).*/
953   n = t - 32768;
954   OD_ASSERT(n >= -16384);
955   /*Get a rough initial guess for the root.
956     The optimal minimax quadratic approximation (using relative error) is
957      r = 1.437799046117536+n*(-0.823394375837328+n*0.4096419668459485).
958     Coefficients here, and the final result r, are Q14.*/
959   r = (23565 + OD_MULT16_16_Q15(n, (-13481 + OD_MULT16_16_Q15(n, 6711))));
960   /*We want y = t*r*r-1 in Q15, but t is 32-bit Q16 and r is Q14.
961     We can compute the result from n and r using Q15 multiplies with some
962      adjustment, carefully done to avoid overflow.*/
963   r2 = r*r;
964   y = (((r2 >> 15)*n + r2) >> 12) - 131077;
965   ry = r*y;
966   /*Apply a 2nd-order Householder iteration: r += r*y*(y*0.375-0.5).
967     This yields the Q14 reciprocal square root of the Q16 t, with a maximum
968      relative error of 1.04956E-4, a (relative) RMSE of 2.80979E-5, and a peak
969      absolute error of 2.26591/16384.*/
970   ret = r + ((((ry >> 16)*(3*y) >> 3) - ry) >> 18);
971   OD_ASSERT(ret >= 16384 && ret < 32768);
972   return (int16_t)ret;
973 }
974
975 static int16_t od_rsqrt(int32_t x, int *rsqrt_shift)
976 {
977    int k;
978    int s;
979    int16_t t;
980    k = (OD_ILOG(x) - 1) >> 1;
981   /*t is x in the range [0.25, 1) in QINSHIFT, or x*2^(-s).
982     Shift by log2(x) - log2(0.25*(1 << INSHIFT)) to ensure 0.25 lower bound.*/
983    s = 2*k - (OD_RSQRT_INSHIFT - 2);
984    t = OD_VSHR(x, s);
985    /*We want to express od_rsqrt() in terms of od_rsqrt_norm(), which is
986       defined as (2^OUTSHIFT)/sqrt(t*(2^-INSHIFT)) with t=x*(2^-s).
987      This simplifies to 2^(OUTSHIFT+(INSHIFT/2)+(s/2))/sqrt(x), so the caller
988       needs to shift right by OUTSHIFT + INSHIFT/2 + s/2.*/
989    *rsqrt_shift = OD_RSQRT_OUTSHIFT + ((s + OD_RSQRT_INSHIFT) >> 1);
990    return od_rsqrt_norm(t);
991 }
992 #endif
993
994 /** Synthesizes one parition of coefficient values from a PVQ-encoded
995  * vector.  This 'partial' version is called by the encode loop where
996  * the Householder reflection has already been computed and there's no
997  * need to recompute it.
998  *
999  * @param [out]     xcoeff  output coefficient partition (x in math doc)
1000  * @param [in]      ypulse  PVQ-encoded values (y in the math doc); in
1001  *                          the noref case, this vector has n entries,
1002  *                          in the reference case it contains n-1 entries
1003  *                          (the m-th entry is not included)
1004  * @param [in]      r       reference vector (prediction)
1005  * @param [in]      n       number of elements in this partition
1006  * @param [in]      noref   indicates presence or lack of prediction
1007  * @param [in]      g       decoded quantized vector gain
1008  * @param [in]      theta   decoded theta (prediction error)
1009  * @param [in]      m       alignment dimension of Householder reflection
1010  * @param [in]      s       sign of Householder reflection
1011  * @param [in]      qm_inv  inverse of the QM with magnitude compensation
1012  */
1013 void od_pvq_synthesis_partial(od_coeff *xcoeff, const od_coeff *ypulse,
1014  const od_val16 *r16, int n, int noref, od_val32 g, od_val32 theta, int m, int s,
1015  const int16_t *qm_inv) {
1016   int i;
1017   int yy;
1018   od_val32 scale;
1019   int nn;
1020   int gshift;
1021   int qshift;
1022   OD_ASSERT(g != 0);
1023   nn = n-(!noref); /* when noref==0, vector in is sized n-1 */
1024   yy = 0;
1025   for (i = 0; i < nn; i++)
1026     yy += ypulse[i]*(int32_t)ypulse[i];
1027   /* Shift required for the magnitude of the pre-qm synthesis to be guaranteed
1028      to fit in 16 bits. In practice, the range will be 8192-16384 after scaling
1029      most of the time. */
1030   gshift = OD_MAXI(0, OD_ILOG(g) - 14);
1031   /*scale is g/sqrt(yy) in Q(16-gshift) so that x[]*scale has a norm that fits
1032      in 16 bits.*/
1033   if (yy == 0) scale = 0;
1034 #if defined(OD_FLOAT_PVQ)
1035   else {
1036     scale = g/sqrt(yy);
1037   }
1038   OD_UNUSED(gshift);
1039   OD_UNUSED(qshift);
1040 #else
1041   else {
1042     int rsqrt_shift;
1043     int16_t rsqrt;
1044     /*FIXME: should be < int64_t*/
1045     int64_t tmp;
1046     rsqrt = od_rsqrt(yy, &rsqrt_shift);
1047     tmp = rsqrt*(int64_t)g;
1048     scale = OD_VSHR_ROUND(tmp, rsqrt_shift + gshift - 16);
1049   }
1050   /* Shift to apply after multiplying by the inverse QM, taking into account
1051      gshift. */
1052   qshift = OD_QM_INV_SHIFT - gshift;
1053 #endif
1054   if (noref) {
1055     for (i = 0; i < n; i++) {
1056       od_val32 x;
1057       /* This multiply doesn't round, so it introduces some bias.
1058          It would be nice (but not critical) to fix this. */
1059       x = OD_MULT16_32_Q16(ypulse[i], scale);
1060 #if defined(OD_FLOAT_PVQ)
1061       xcoeff[i] = (od_coeff)floor(.5
1062        + x*(qm_inv[i]*OD_QM_INV_SCALE_1));
1063 #else
1064       xcoeff[i] = OD_SHR_ROUND(x*qm_inv[i], qshift);
1065 #endif
1066     }
1067   }
1068   else{
1069     od_val16 x[MAXN];
1070     scale = OD_ROUND32(scale*OD_TRIG_SCALE_1*od_pvq_sin(theta));
1071     /* The following multiply doesn't round, but it's probably OK since
1072        the Householder reflection is likely to undo most of the resulting
1073        bias. */
1074     for (i = 0; i < m; i++)
1075       x[i] = OD_MULT16_32_Q16(ypulse[i], scale);
1076     x[m] = OD_ROUND16(-s*(OD_SHR_ROUND(g, gshift))*OD_TRIG_SCALE_1*
1077      od_pvq_cos(theta));
1078     for (i = m; i < nn; i++)
1079       x[i+1] = OD_MULT16_32_Q16(ypulse[i], scale);
1080     od_apply_householder(x, x, r16, n);
1081     for (i = 0; i < n; i++) {
1082 #if defined(OD_FLOAT_PVQ)
1083       xcoeff[i] = (od_coeff)floor(.5 + (x[i]*(qm_inv[i]*OD_QM_INV_SCALE_1)));
1084 #else
1085       xcoeff[i] = OD_SHR_ROUND(x[i]*qm_inv[i], qshift);
1086 #endif
1087     }
1088   }
1089 }
1090