Using a first-order filter for DC rejection
[opus.git] / src / opus_projection_encoder.c
1 /* Copyright (c) 2017 Google Inc.
2    Written by Andrew Allen */
3 /*
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
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14
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 COPYRIGHT OWNER
19    OR 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
28 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 #include "mathops.h"
33 #include "os_support.h"
34 #include "opus_private.h"
35 #include "opus_defines.h"
36 #include "opus_projection.h"
37 #include "opus_multistream.h"
38 #include "stack_alloc.h"
39 #include "mapping_matrix.h"
40
41 #ifdef ENABLE_EXPERIMENTAL_AMBISONICS
42
43 struct OpusProjectionEncoder
44 {
45   opus_int32 mixing_matrix_size_in_bytes;
46   opus_int32 demixing_matrix_size_in_bytes;
47   /* Encoder states go here */
48 };
49
50 #if !defined(DISABLE_FLOAT_API)
51 static void opus_projection_copy_channel_in_float(
52   opus_val16 *dst,
53   int dst_stride,
54   const void *src,
55   int src_stride,
56   int src_channel,
57   int frame_size,
58   void *user_data
59 )
60 {
61   mapping_matrix_multiply_channel_in_float((const MappingMatrix*)user_data,
62     (const float*)src, src_stride, dst, src_channel, dst_stride, frame_size);
63 }
64 #endif
65
66 static void opus_projection_copy_channel_in_short(
67   opus_val16 *dst,
68   int dst_stride,
69   const void *src,
70   int src_stride,
71   int src_channel,
72   int frame_size,
73   void *user_data
74 )
75 {
76   mapping_matrix_multiply_channel_in_short((const MappingMatrix*)user_data,
77     (const opus_int16*)src, src_stride, dst, src_channel, dst_stride, frame_size);
78 }
79
80 static int get_order_plus_one_from_channels(int channels, int *order_plus_one)
81 {
82   int order_plus_one_;
83   int acn_channels;
84   int nondiegetic_channels;
85
86   /* Allowed numbers of channels:
87    * (1 + n)^2 + 2j, for n = 0...14 and j = 0 or 1.
88    */
89   order_plus_one_ = isqrt32(channels);
90   acn_channels = order_plus_one_ * order_plus_one_;
91   nondiegetic_channels = channels - acn_channels;
92   if (order_plus_one)
93     *order_plus_one = order_plus_one_;
94
95   if (order_plus_one_ < 1 || order_plus_one_ > 15 ||
96       (nondiegetic_channels != 0 && nondiegetic_channels != 2))
97     return OPUS_BAD_ARG;
98   return OPUS_OK;
99 }
100
101 static int get_streams_from_channels(int channels, int mapping_family,
102                                      int *streams, int *coupled_streams,
103                                      int *order_plus_one)
104 {
105   if (mapping_family == 253)
106   {
107     if (get_order_plus_one_from_channels(channels, order_plus_one) != OPUS_OK)
108       return OPUS_BAD_ARG;
109     if (streams)
110       *streams = (channels + 1) / 2;
111     if (coupled_streams)
112       *coupled_streams = channels / 2;
113     return OPUS_OK;
114   }
115   return OPUS_BAD_ARG;
116 }
117
118 static MappingMatrix *get_mixing_matrix(OpusProjectionEncoder *st)
119 {
120   return (MappingMatrix *)((char*)st + align(sizeof(OpusProjectionEncoder)));
121 }
122
123 static MappingMatrix *get_demixing_matrix(OpusProjectionEncoder *st)
124 {
125   return (MappingMatrix *)((char*)st + align(sizeof(OpusProjectionEncoder) +
126     st->mixing_matrix_size_in_bytes));
127 }
128
129 static OpusMSEncoder *get_multistream_encoder(OpusProjectionEncoder *st)
130 {
131   return (OpusMSEncoder *)((char*)st + align(sizeof(OpusProjectionEncoder) +
132     st->mixing_matrix_size_in_bytes + st->demixing_matrix_size_in_bytes));
133 }
134
135 opus_int32 opus_projection_ambisonics_encoder_get_size(int channels,
136                                                        int mapping_family)
137 {
138   int nb_streams;
139   int nb_coupled_streams;
140   int order_plus_one;
141   int mixing_matrix_rows, mixing_matrix_cols;
142   int demixing_matrix_rows, demixing_matrix_cols;
143   opus_int32 mixing_matrix_size, demixing_matrix_size;
144   opus_int32 encoder_size;
145   int ret;
146
147   ret = get_streams_from_channels(channels, mapping_family, &nb_streams,
148                                   &nb_coupled_streams, &order_plus_one);
149   if (ret != OPUS_OK)
150     return 0;
151
152   if (order_plus_one == 2)
153   {
154     mixing_matrix_rows = mapping_matrix_foa_mixing.rows;
155     mixing_matrix_cols = mapping_matrix_foa_mixing.cols;
156     demixing_matrix_rows = mapping_matrix_foa_demixing.rows;
157     demixing_matrix_cols = mapping_matrix_foa_demixing.cols;
158   }
159   else if (order_plus_one == 3)
160   {
161     mixing_matrix_rows = mapping_matrix_soa_mixing.rows;
162     mixing_matrix_cols = mapping_matrix_soa_mixing.cols;
163     demixing_matrix_rows = mapping_matrix_soa_demixing.rows;
164     demixing_matrix_cols = mapping_matrix_soa_demixing.cols;
165   }
166   else if (order_plus_one == 4)
167   {
168     mixing_matrix_rows = mapping_matrix_toa_mixing.rows;
169     mixing_matrix_cols = mapping_matrix_toa_mixing.cols;
170     demixing_matrix_rows = mapping_matrix_toa_demixing.rows;
171     demixing_matrix_cols = mapping_matrix_toa_demixing.cols;
172   }
173   else
174     return 0;
175
176   mixing_matrix_size =
177     mapping_matrix_get_size(mixing_matrix_rows, mixing_matrix_cols);
178   if (!mixing_matrix_size)
179     return 0;
180
181   demixing_matrix_size =
182     mapping_matrix_get_size(demixing_matrix_rows, demixing_matrix_cols);
183   if (!demixing_matrix_size)
184     return 0;
185
186   encoder_size =
187       opus_multistream_encoder_get_size(nb_streams, nb_coupled_streams);
188   if (!encoder_size)
189     return 0;
190
191   return align(sizeof(OpusProjectionEncoder)) +
192     mixing_matrix_size + demixing_matrix_size + encoder_size;
193 }
194
195 int opus_projection_ambisonics_encoder_init(OpusProjectionEncoder *st, opus_int32 Fs,
196                                             int channels, int mapping_family,
197                                             int *streams, int *coupled_streams,
198                                             int application)
199 {
200   MappingMatrix *mixing_matrix;
201   MappingMatrix *demixing_matrix;
202   OpusMSEncoder *ms_encoder;
203   int i;
204   int ret;
205   int order_plus_one;
206   unsigned char mapping[255];
207
208   if (streams == NULL || coupled_streams == NULL) {
209     return OPUS_BAD_ARG;
210   }
211
212   if (get_streams_from_channels(channels, mapping_family, streams,
213     coupled_streams, &order_plus_one) != OPUS_OK)
214     return OPUS_BAD_ARG;
215
216   if (mapping_family == 253)
217   {
218     /* Assign mixing matrix based on available pre-computed matrices. */
219     mixing_matrix = get_mixing_matrix(st);
220     if (order_plus_one == 2)
221     {
222       mapping_matrix_init(mixing_matrix, mapping_matrix_foa_mixing.rows,
223         mapping_matrix_foa_mixing.cols, mapping_matrix_foa_mixing.gain,
224         mapping_matrix_foa_mixing_data,
225         sizeof(mapping_matrix_foa_mixing_data));
226     }
227     else if (order_plus_one == 3)
228     {
229       mapping_matrix_init(mixing_matrix, mapping_matrix_soa_mixing.rows,
230         mapping_matrix_soa_mixing.cols, mapping_matrix_soa_mixing.gain,
231         mapping_matrix_soa_mixing_data,
232         sizeof(mapping_matrix_soa_mixing_data));
233     }
234     else if (order_plus_one == 4)
235     {
236       mapping_matrix_init(mixing_matrix, mapping_matrix_toa_mixing.rows,
237         mapping_matrix_toa_mixing.cols, mapping_matrix_toa_mixing.gain,
238         mapping_matrix_toa_mixing_data,
239         sizeof(mapping_matrix_toa_mixing_data));
240     }
241     else
242       return OPUS_BAD_ARG;
243
244     st->mixing_matrix_size_in_bytes = mapping_matrix_get_size(
245       mixing_matrix->rows, mixing_matrix->cols);
246     if (!st->mixing_matrix_size_in_bytes)
247       return OPUS_BAD_ARG;
248
249     /* Assign demixing matrix based on available pre-computed matrices. */
250     demixing_matrix = get_demixing_matrix(st);
251     if (order_plus_one == 2)
252     {
253       mapping_matrix_init(demixing_matrix, mapping_matrix_foa_demixing.rows,
254         mapping_matrix_foa_demixing.cols, mapping_matrix_foa_demixing.gain,
255         mapping_matrix_foa_demixing_data,
256         sizeof(mapping_matrix_foa_demixing_data));
257     }
258     else if (order_plus_one == 3)
259     {
260       mapping_matrix_init(demixing_matrix, mapping_matrix_soa_demixing.rows,
261         mapping_matrix_soa_demixing.cols, mapping_matrix_soa_demixing.gain,
262         mapping_matrix_soa_demixing_data,
263         sizeof(mapping_matrix_soa_demixing_data));
264     }
265     else if (order_plus_one == 4)
266     {
267       mapping_matrix_init(demixing_matrix, mapping_matrix_toa_demixing.rows,
268         mapping_matrix_toa_demixing.cols, mapping_matrix_toa_demixing.gain,
269         mapping_matrix_toa_demixing_data,
270         sizeof(mapping_matrix_toa_demixing_data));
271     }
272     else
273       return OPUS_BAD_ARG;
274
275     st->demixing_matrix_size_in_bytes = mapping_matrix_get_size(
276       demixing_matrix->rows, demixing_matrix->cols);
277     if (!st->demixing_matrix_size_in_bytes)
278       return OPUS_BAD_ARG;
279   }
280   else
281     return OPUS_UNIMPLEMENTED;
282
283   /* Ensure matrices are large enough for desired coding scheme. */
284   if (*streams + *coupled_streams > mixing_matrix->rows ||
285       channels > mixing_matrix->cols ||
286       channels > demixing_matrix->rows ||
287       *streams + *coupled_streams > demixing_matrix->cols)
288     return OPUS_BAD_ARG;
289
290   /* Set trivial mapping so each input channel pairs with a matrix column. */
291   for (i = 0; i < channels; i++)
292     mapping[i] = i;
293
294   /* Initialize multistream encoder with provided settings. */
295   ms_encoder = get_multistream_encoder(st);
296   ret = opus_multistream_encoder_init(ms_encoder, Fs, channels, *streams,
297                                       *coupled_streams, mapping, application);
298   return ret;
299 }
300
301 OpusProjectionEncoder *opus_projection_ambisonics_encoder_create(
302     opus_int32 Fs, int channels, int mapping_family, int *streams,
303     int *coupled_streams, int application, int *error)
304 {
305   int size;
306   int ret;
307   OpusProjectionEncoder *st;
308
309   /* Allocate space for the projection encoder. */
310   size = opus_projection_ambisonics_encoder_get_size(channels, mapping_family);
311   if (!size) {
312     if (error)
313       *error = OPUS_ALLOC_FAIL;
314     return NULL;
315   }
316   st = (OpusProjectionEncoder *)opus_alloc(size);
317   if (!st)
318   {
319     if (error)
320       *error = OPUS_ALLOC_FAIL;
321     return NULL;
322   }
323
324   /* Initialize projection encoder with provided settings. */
325   ret = opus_projection_ambisonics_encoder_init(st, Fs, channels,
326      mapping_family, streams, coupled_streams, application);
327   if (ret != OPUS_OK)
328   {
329     opus_free(st);
330     st = NULL;
331   }
332   if (error)
333     *error = ret;
334   return st;
335 }
336
337 int opus_projection_encode(OpusProjectionEncoder *st, const opus_int16 *pcm,
338                            int frame_size, unsigned char *data,
339                            opus_int32 max_data_bytes)
340 {
341   return opus_multistream_encode_native(get_multistream_encoder(st),
342     opus_projection_copy_channel_in_short, pcm, frame_size, data,
343     max_data_bytes, 16, downmix_int, 0, get_mixing_matrix(st));
344 }
345
346 #ifndef DISABLE_FLOAT_API
347 #ifdef FIXED_POINT
348 int opus_projection_encode_float(OpusProjectionEncoder *st, const float *pcm,
349                                  int frame_size, unsigned char *data,
350                                  opus_int32 max_data_bytes)
351 {
352   return opus_multistream_encode_native(get_multistream_encoder(st),
353     opus_projection_copy_channel_in_float, pcm, frame_size, data,
354     max_data_bytes, 16, downmix_float, 1, get_mixing_matrix(st));
355 }
356 #else
357 int opus_projection_encode_float(OpusProjectionEncoder *st, const float *pcm,
358                                  int frame_size, unsigned char *data,
359                                  opus_int32 max_data_bytes)
360 {
361   return opus_multistream_encode_native(get_multistream_encoder(st),
362     opus_projection_copy_channel_in_float, pcm, frame_size, data,
363     max_data_bytes, 24, downmix_float, 1, get_mixing_matrix(st));
364 }
365 #endif
366 #endif
367
368 void opus_projection_encoder_destroy(OpusProjectionEncoder *st)
369 {
370   opus_free(st);
371 }
372
373 int opus_projection_encoder_ctl(OpusProjectionEncoder *st, int request, ...)
374 {
375   MappingMatrix *demixing_matrix;
376   OpusMSEncoder *ms_encoder;
377   int ret = OPUS_OK;
378
379   ms_encoder = get_multistream_encoder(st);
380   demixing_matrix = get_demixing_matrix(st);
381
382   va_list ap;
383   va_start(ap, request);
384   switch(request)
385   {
386   case OPUS_PROJECTION_GET_DEMIXING_MATRIX_SIZE_REQUEST:
387   {
388     opus_int32 *value = va_arg(ap, opus_int32*);
389     if (!value)
390     {
391       goto bad_arg;
392     }
393     *value =
394       ms_encoder->layout.nb_channels * (ms_encoder->layout.nb_streams
395       + ms_encoder->layout.nb_coupled_streams) * sizeof(opus_int16);
396   }
397   break;
398   case OPUS_PROJECTION_GET_DEMIXING_MATRIX_GAIN_REQUEST:
399   {
400     opus_int32 *value = va_arg(ap, opus_int32*);
401     if (!value)
402     {
403       goto bad_arg;
404     }
405     *value = demixing_matrix->gain;
406   }
407   break;
408   case OPUS_PROJECTION_GET_DEMIXING_MATRIX_REQUEST:
409   {
410     int i, j, k, l;
411     int nb_input_streams;
412     int nb_output_streams;
413     unsigned char *external_char;
414     opus_int16 *internal_short;
415     opus_int32 external_size;
416     opus_int32 internal_size;
417
418     /* (I/O is in relation to the decoder's perspective). */
419     nb_input_streams = ms_encoder->layout.nb_streams +
420       ms_encoder->layout.nb_coupled_streams;
421     nb_output_streams = ms_encoder->layout.nb_channels;
422
423     external_char = va_arg(ap, unsigned char *);
424     external_size = va_arg(ap, opus_int32);
425     if (!external_char)
426     {
427       goto bad_arg;
428     }
429     internal_short = mapping_matrix_get_data(demixing_matrix);
430     internal_size = nb_input_streams * nb_output_streams * sizeof(opus_int16);
431     if (external_size != internal_size)
432     {
433       goto bad_arg;
434     }
435
436     /* Copy demixing matrix subset to output destination. */
437     l = 0;
438     for (i = 0; i < nb_input_streams; i++) {
439       for (j = 0; j < nb_output_streams; j++) {
440         k = demixing_matrix->rows * i + j;
441         external_char[2*l] = (unsigned char)internal_short[k];
442         external_char[2*l+1] = (unsigned char)(internal_short[k] >> 8);
443         l++;
444       }
445     }
446   }
447   break;
448   default:
449   {
450     ret = opus_multistream_encoder_ctl_va_list(ms_encoder, request, ap);
451   }
452   break;
453   }
454   va_end(ap);
455   return ret;
456
457 bad_arg:
458   va_end(ap);
459   return OPUS_BAD_ARG;
460 }
461
462 #endif /* ENABLE_EXPERIMENTAL_AMBISONICS */