Fix memory issues in Projection API.
[opus.git] / src / mapping_matrix.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 "arch.h"
33 #include "float_cast.h"
34 #include "opus_private.h"
35 #include "opus_defines.h"
36 #include "mapping_matrix.h"
37
38 #ifdef ENABLE_EXPERIMENTAL_AMBISONICS
39
40 #define MATRIX_INDEX(nb_rows, row, col) (nb_rows * col + row)
41
42 int mapping_matrix_get_size(int rows, int cols)
43 {
44   return align(sizeof(MappingMatrix)) + align(rows * cols * sizeof(opus_int16));
45 }
46
47 opus_int16 *mapping_matrix_get_data(const MappingMatrix *matrix)
48 {
49   return (opus_int16*)((char*)matrix + align(sizeof(MappingMatrix)));
50 }
51
52 void mapping_matrix_init(MappingMatrix * const matrix,
53   int rows, int cols, int gain, const opus_int16 *data, opus_int32 data_size)
54 {
55   int i;
56   opus_int16 *ptr;
57
58 #if !defined(ENABLE_ASSERTIONS)
59   (void)data_size;
60 #endif
61   celt_assert(align(data_size) == align(rows * cols * sizeof(opus_int16)));
62
63   matrix->rows = rows;
64   matrix->cols = cols;
65   matrix->gain = gain;
66   ptr = mapping_matrix_get_data(matrix);
67   for (i = 0; i < rows * cols; i++)
68   {
69      ptr[i] = data[i];
70   }
71 }
72
73 #ifndef DISABLE_FLOAT_API
74 void mapping_matrix_multiply_channel_in_float(
75     const MappingMatrix *matrix,
76     const float *input,
77     int input_rows,
78     opus_val16 *output,
79     int output_row,
80     int output_rows,
81     int frame_size)
82 {
83   /* Matrix data is ordered col-wise. */
84   opus_int16* matrix_data;
85   int i, col;
86
87   celt_assert(input_rows <= matrix->cols && output_rows <= matrix->rows);
88
89   matrix_data = mapping_matrix_get_data(matrix);
90
91   for (i = 0; i < frame_size; i++)
92   {
93     float tmp = 0;
94     for (col = 0; col < input_rows; col++)
95     {
96       tmp +=
97         matrix_data[MATRIX_INDEX(matrix->rows, output_row, col)] *
98         input[MATRIX_INDEX(input_rows, col, i)];
99     }
100 #if defined(FIXED_POINT)
101     output[output_rows * i] = FLOAT2INT16((1/32768.f)*tmp);
102 #else
103     output[output_rows * i] = (1/32768.f)*tmp;
104 #endif
105   }
106 }
107
108 void mapping_matrix_multiply_channel_out_float(
109     const MappingMatrix *matrix,
110     const opus_val16 *input,
111     int input_row,
112     int input_rows,
113     float *output,
114     int output_rows,
115     int frame_size
116 )
117 {
118   /* Matrix data is ordered col-wise. */
119   opus_int16* matrix_data;
120   int i, row;
121   float input_sample;
122
123   celt_assert(input_rows <= matrix->cols && output_rows <= matrix->rows);
124
125   matrix_data = mapping_matrix_get_data(matrix);
126
127   for (i = 0; i < frame_size; i++)
128   {
129 #if defined(FIXED_POINT)
130     input_sample = (1/32768.f)*input[input_rows * i];
131 #else
132     input_sample = input[input_rows * i];
133 #endif
134     for (row = 0; row < output_rows; row++)
135     {
136       float tmp =
137         (1/32768.f)*matrix_data[MATRIX_INDEX(matrix->rows, row, input_row)] *
138         input_sample;
139       output[MATRIX_INDEX(output_rows, row, i)] += tmp;
140     }
141   }
142 }
143 #endif /* DISABLE_FLOAT_API */
144
145 void mapping_matrix_multiply_channel_in_short(
146     const MappingMatrix *matrix,
147     const opus_int16 *input,
148     int input_rows,
149     opus_val16 *output,
150     int output_row,
151     int output_rows,
152     int frame_size)
153 {
154   /* Matrix data is ordered col-wise. */
155   opus_int16* matrix_data;
156   int i, col;
157
158   celt_assert(input_rows <= matrix->cols && output_rows <= matrix->rows);
159
160   matrix_data = mapping_matrix_get_data(matrix);
161
162   for (i = 0; i < frame_size; i++)
163   {
164     opus_val32 tmp = 0;
165     for (col = 0; col < input_rows; col++)
166     {
167 #if defined(FIXED_POINT)
168       tmp +=
169         ((opus_int32)matrix_data[MATRIX_INDEX(matrix->rows, output_row, col)] *
170         (opus_int32)input[MATRIX_INDEX(input_rows, col, i)]) >> 8;
171 #else
172       tmp +=
173         matrix_data[MATRIX_INDEX(matrix->rows, output_row, col)] *
174         input[MATRIX_INDEX(input_rows, col, i)];
175 #endif
176     }
177 #if defined(FIXED_POINT)
178     output[output_rows * i] = (opus_int16)((tmp + 64) >> 7);
179 #else
180     output[output_rows * i] = (1/(32768.f*32768.f))*tmp;
181 #endif
182   }
183 }
184
185 void mapping_matrix_multiply_channel_out_short(
186     const MappingMatrix *matrix,
187     const opus_val16 *input,
188     int input_row,
189     int input_rows,
190     opus_int16 *output,
191     int output_rows,
192     int frame_size)
193 {
194   /* Matrix data is ordered col-wise. */
195   opus_int16* matrix_data;
196   int i, row;
197   opus_int32 input_sample;
198
199   celt_assert(input_rows <= matrix->cols && output_rows <= matrix->rows);
200
201   matrix_data = mapping_matrix_get_data(matrix);
202
203   for (i = 0; i < frame_size; i++)
204   {
205 #if defined(FIXED_POINT)
206     input_sample = (opus_int32)input[input_rows * i];
207 #else
208     input_sample = (opus_int32)FLOAT2INT16(input[input_rows * i]);
209 #endif
210     for (row = 0; row < output_rows; row++)
211     {
212       opus_int32 tmp =
213         (opus_int32)matrix_data[MATRIX_INDEX(matrix->rows, row, input_row)] *
214         input_sample;
215       output[MATRIX_INDEX(output_rows, row, i)] += (tmp + 16384) >> 15;
216     }
217   }
218 }
219
220 const MappingMatrix mapping_matrix_foa_mixing = { 6, 6, 0 };
221 const opus_int16 mapping_matrix_foa_mixing_data[36] = {
222      16384,      0, -16384,  23170,      0,      0,  16384,  23170,
223      16384,      0,      0,      0,  16384,      0, -16384, -23170,
224          0,      0,  16384, -23170,  16384,      0,      0,      0,
225          0,      0,      0,      0,  32767,      0,      0,      0,
226          0,      0,      0,  32767
227 };
228
229 const MappingMatrix mapping_matrix_soa_mixing = { 11, 11, 0 };
230 const opus_int16 mapping_matrix_soa_mixing_data[121] = {
231      10923,   7723,  13377, -13377,  11585,   9459,   7723, -16384,
232      -6689,      0,      0,  10923,   7723,  13377,  13377, -11585,
233       9459,   7723,  16384,  -6689,      0,      0,  10923, -15447,
234      13377,      0,      0, -18919,   7723,      0,  13377,      0,
235          0,  10923,   7723, -13377, -13377,  11585,  -9459,   7723,
236      16384,  -6689,      0,      0,  10923,  -7723,      0,  13377,
237     -16384,      0, -15447,      0,   9459,      0,      0,  10923,
238      -7723,      0, -13377,  16384,      0, -15447,      0,   9459,
239          0,      0,  10923,  15447,      0,      0,      0,      0,
240     -15447,      0, -18919,      0,      0,  10923,   7723, -13377,
241      13377, -11585,  -9459,   7723, -16384,  -6689,      0,      0,
242      10923, -15447, -13377,      0,      0,  18919,   7723,      0,
243      13377,      0,      0,      0,      0,      0,      0,      0,
244          0,      0,      0,      0,  32767,      0,      0,      0,
245          0,      0,      0,      0,      0,      0,      0,      0,
246      32767
247 };
248
249 const MappingMatrix mapping_matrix_toa_mixing = { 18, 18, 0 };
250 const opus_int16 mapping_matrix_toa_mixing_data[324] = {
251       8208,      0,   -881,  14369,      0,      0,  -8192,  -4163,
252      13218,      0,      0,      0,  11095,  -8836,  -6218,  14833,
253          0,      0,   8208, -10161,    881,  10161, -13218,  -2944,
254      -8192,   2944,      0, -10488,  -6218,   6248, -11095,  -6248,
255          0, -10488,      0,      0,   8208,  10161,    881, -10161,
256     -13218,   2944,  -8192,  -2944,      0,  10488,  -6218,  -6248,
257     -11095,   6248,      0,  10488,      0,      0,   8176,   5566,
258     -11552,   5566,   9681, -11205,   8192, -11205,      0,   4920,
259     -15158,   9756,  -3334,   9756,      0,  -4920,      0,      0,
260       8176,   7871,  11552,      0,      0,  15846,   8192,      0,
261      -9681,  -6958,      0,  13797,   3334,      0, -15158,      0,
262          0,      0,   8176,      0,  11552,   7871,      0,      0,
263       8192,  15846,   9681,      0,      0,      0,   3334,  13797,
264      15158,   6958,      0,      0,   8176,   5566, -11552,  -5566,
265      -9681, -11205,   8192,  11205,      0,   4920,  15158,   9756,
266      -3334,  -9756,      0,   4920,      0,      0,   8208,  14369,
267       -881,      0,      0,  -4163,  -8192,      0, -13218, -14833,
268          0,  -8836,  11095,      0,   6218,      0,      0,      0,
269       8208,  10161,    881,  10161,  13218,   2944,  -8192,   2944,
270          0,  10488,   6218,  -6248, -11095,  -6248,      0, -10488,
271          0,      0,   8208, -14369,   -881,      0,      0,   4163,
272      -8192,      0, -13218,  14833,      0,   8836,  11095,      0,
273       6218,      0,      0,      0,   8208,      0,   -881, -14369,
274          0,      0,  -8192,   4163,  13218,      0,      0,      0,
275      11095,   8836,  -6218, -14833,      0,      0,   8176,  -5566,
276     -11552,   5566,  -9681,  11205,   8192, -11205,      0,  -4920,
277      15158,  -9756,  -3334,   9756,      0,  -4920,      0,      0,
278       8176,      0,  11552,  -7871,      0,      0,   8192, -15846,
279       9681,      0,      0,      0,   3334, -13797,  15158,  -6958,
280          0,      0,   8176,  -7871,  11552,      0,      0, -15846,
281       8192,      0,  -9681,   6958,      0, -13797,   3334,      0,
282     -15158,      0,      0,      0,   8176,  -5566, -11552,  -5566,
283       9681,  11205,   8192,  11205,      0,  -4920, -15158,  -9756,
284      -3334,  -9756,      0,   4920,      0,      0,   8208, -10161,
285        881, -10161,  13218,  -2944,  -8192,  -2944,      0, -10488,
286       6218,   6248, -11095,   6248,      0,  10488,      0,      0,
287          0,      0,      0,      0,      0,      0,      0,      0,
288          0,      0,      0,      0,      0,      0,      0,      0,
289      32767,      0,      0,      0,      0,      0,      0,      0,
290          0,      0,      0,      0,      0,      0,      0,      0,
291          0,      0,      0,  32767
292 };
293
294 const MappingMatrix mapping_matrix_foa_demixing = { 6, 6, 0 };
295 const opus_int16 mapping_matrix_foa_demixing_data[36] = {
296      16384,  16384,  16384,  16384,      0,      0,      0,  23170,
297          0, -23170,      0,      0, -16384,  16384, -16384,  16384,
298          0,      0,  23170,      0, -23170,      0,      0,      0,
299          0,      0,      0,      0,  32767,      0,      0,      0,
300          0,      0,      0,  32767
301 };
302
303 const MappingMatrix mapping_matrix_soa_demixing = { 11, 11, 3050 };
304 const opus_int16 mapping_matrix_soa_demixing_data[121] = {
305       2771,   2771,   2771,   2771,   2771,   2771,   2771,   2771,
306       2771,      0,      0,  10033,  10033, -20066,  10033,  14189,
307      14189, -28378,  10033, -20066,      0,      0,   3393,   3393,
308       3393,  -3393,      0,      0,      0,  -3393,  -3393,      0,
309          0, -17378,  17378,      0, -17378, -24576,  24576,      0,
310      17378,      0,      0,      0, -14189,  14189,      0, -14189,
311     -28378,  28378,      0,  14189,      0,      0,      0,   2399,
312       2399,  -4799,  -2399,      0,      0,      0,  -2399,   4799,
313          0,      0,   1959,   1959,   1959,   1959,  -3918,  -3918,
314      -3918,   1959,   1959,      0,      0,  -4156,   4156,      0,
315       4156,      0,      0,      0,  -4156,      0,      0,      0,
316       8192,   8192, -16384,   8192,  16384,  16384, -32768,   8192,
317     -16384,      0,      0,      0,      0,      0,      0,      0,
318          0,      0,      0,      0,   8312,      0,      0,      0,
319          0,      0,      0,      0,      0,      0,      0,      0,
320       8312
321 };
322
323 const MappingMatrix mapping_matrix_toa_demixing = { 18, 18, 0 };
324 const opus_int16 mapping_matrix_toa_demixing_data[324] = {
325       8192,   8192,   8192,   8192,   8192,   8192,   8192,   8192,
326       8192,   8192,   8192,   8192,   8192,   8192,   8192,   8192,
327          0,      0,      0,  -9779,   9779,   6263,   8857,      0,
328       6263,  13829,   9779, -13829,      0,  -6263,      0,  -8857,
329      -6263,  -9779,      0,      0,  -3413,   3413,   3413, -11359,
330      11359,  11359, -11359,  -3413,   3413,  -3413,  -3413, -11359,
331      11359,  11359, -11359,   3413,      0,      0,  13829,   9779,
332      -9779,   6263,      0,   8857,  -6263,      0,   9779,      0,
333     -13829,   6263,  -8857,      0,  -6263,  -9779,      0,      0,
334          0, -15617, -15617,   6406,      0,      0,  -6406,      0,
335      15617,      0,      0,  -6406,      0,      0,   6406,  15617,
336          0,      0,      0,  -5003,   5003, -10664,  15081,      0,
337     -10664,  -7075,   5003,   7075,      0,  10664,      0, -15081,
338      10664,  -5003,      0,      0,  -8176,  -8176,  -8176,   8208,
339       8208,   8208,   8208,  -8176,  -8176,  -8176,  -8176,   8208,
340       8208,   8208,   8208,  -8176,      0,      0,  -7075,   5003,
341      -5003, -10664,      0,  15081,  10664,      0,   5003,      0,
342       7075, -10664, -15081,      0,  10664,  -5003,      0,      0,
343      15617,      0,      0,      0,  -6406,   6406,      0, -15617,
344          0, -15617,  15617,      0,   6406,  -6406,      0,      0,
345          0,      0,      0, -11393,  11393,   2993,  -4233,      0,
346       2993, -16112,  11393,  16112,      0,  -2993,      0,   4233,
347      -2993, -11393,      0,      0,      0,  -9974,  -9974, -13617,
348          0,      0,  13617,      0,   9974,      0,      0,  13617,
349          0,      0, -13617,   9974,      0,      0,      0,   5579,
350      -5579,  10185,  14403,      0,  10185,  -7890,  -5579,   7890,
351          0, -10185,      0, -14403, -10185,   5579,      0,      0,
352      11826, -11826, -11826,   -901,    901,    901,   -901,  11826,
353     -11826,  11826,  11826,   -901,    901,    901,   -901, -11826,
354          0,      0,  -7890,  -5579,   5579,  10185,      0,  14403,
355     -10185,      0,  -5579,      0,   7890,  10185, -14403,      0,
356     -10185,   5579,      0,      0,  -9974,      0,      0,      0,
357     -13617,  13617,      0,   9974,      0,   9974,  -9974,      0,
358      13617, -13617,      0,      0,      0,      0,  16112, -11393,
359      11393,  -2993,      0,   4233,   2993,      0, -11393,      0,
360     -16112,  -2993,  -4233,      0,   2993,  11393,      0,      0,
361          0,      0,      0,      0,      0,      0,      0,      0,
362          0,      0,      0,      0,      0,      0,      0,      0,
363      32767,      0,      0,      0,      0,      0,      0,      0,
364          0,      0,      0,      0,      0,      0,      0,      0,
365          0,      0,      0,  32767
366 };
367
368 #endif /* ENABLE_EXPERIMENTAL_AMBISONICS */