1 /* clunk - cross-platform 3D audio API built on top SDL library
2 * Copyright (C) 2007-2014 Netive Media Group
3 *
4 * This library is free software; you can redistribute it and/or
5 * modify it under the terms of the GNU Lesser General Public
6 * License as published by the Free Software Foundation; either
7 * version 2.1 of the License, or (at your option) any later version.
8
9 * This library is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * Lesser General Public License for more details.
13 *
14 * You should have received a copy of the GNU Lesser General Public
15 * License along with this library; if not, write to the Free Software
16 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
17 */
18
19 #include <clunk/hrtf.h>
20 #include <clunk/buffer.h>
21 #include <clunk/clunk_ex.h>
22 #include <stddef.h>
23 #include <algorithm>
24
25 #include "kemar.h"
26
27 #if defined _MSC_VER || __APPLE__ || __FreeBSD__ || __DragonFly__
28 # define pow10f(x) powf(10.0f, (x))
29 # define log2f(x) (logf(x) / M_LN2)
30 #endif
31
32 namespace clunk
33 {
34
35 clunk_static_assert(Hrtf::WINDOW_BITS > 2);
36
Hrtf()37 Hrtf::Hrtf(): overlap_data()
38 { }
39
idt_iit(const v3f & position,float & idt_offset,float & angle_gr,float & left_to_right_amp)40 void Hrtf::idt_iit(const v3f &position, float &idt_offset, float &angle_gr, float &left_to_right_amp) {
41 float head_r = 0.093f;
42
43 float angle = (float)(M_PI_2 - atan2f(position.y, position.x));
44
45 angle_gr = angle * 180 / float(M_PI);
46 while (angle_gr < 0)
47 angle_gr += 360;
48
49 //LOG_DEBUG(("relative position = (%g,%g,%g), angle = %g (%g)", position.x, position.y, position.z, angle, angle_gr));
50
51 float idt_angle = fmodf(angle, 2 * (float)M_PI);
52
53 if (idt_angle < 0)
54 idt_angle += 2 * (float)M_PI;
55 if (idt_angle >= float(M_PI_2) && idt_angle < (float)M_PI) {
56 idt_angle = float(M_PI) - idt_angle;
57 } else if (idt_angle >= float(M_PI) && idt_angle < 3 * float(M_PI_2)) {
58 idt_angle = (float)M_PI - idt_angle;
59 } else if (idt_angle >= 3 * (float)M_PI_2) {
60 idt_angle -= (float)M_PI * 2;
61 }
62
63 //LOG_DEBUG(("idt_angle = %g (%d)", idt_angle, (int)(idt_angle * 180 / M_PI)));
64 idt_offset = - head_r * (idt_angle + sin(idt_angle)) / 344;
65 left_to_right_amp = pow10f(-sin(idt_angle));
66 //LOG_DEBUG(("idt_offset %g, left_to_right_amp: %g", idt_offset, left_to_right_amp));
67 }
68
get_kemar_data(kemar_ptr & kemar_data,int & elev_n,const v3f & pos)69 void Hrtf::get_kemar_data(kemar_ptr & kemar_data, int & elev_n, const v3f &pos) {
70
71 kemar_data = 0;
72 elev_n = 0;
73 if (pos.is0())
74 return;
75
76 #ifdef _WINDOWS
77 float len = (float)_hypot(pos.x, pos.y);
78 #else
79 float len = (float)hypot(pos.x, pos.y);
80 #endif
81
82 int elev_gr = (int)(180 * atan2f(pos.z, len) / (float)M_PI);
83 //LOG_DEBUG(("elev = %d (%g %g %g)", elev_gr, pos.x, pos.y, pos.z));
84
85 for(size_t i = 0; i < KemarElevationCount; ++i)
86 {
87 const kemar_elevation_data &elev = ::kemar_data[i];
88 if (elev_gr < elev.elevation + KemarElevationStep / 2)
89 {
90 //LOG_DEBUG(("used elevation %d", elev.elevation));
91 kemar_data = elev.data;
92 elev_n = elev.samples;
93 break;
94 }
95 }
96 }
97
process(unsigned sample_rate,clunk::Buffer & dst_buf,unsigned dst_ch,const clunk::Buffer & src_buf,unsigned src_ch,const v3f & delta_position,float fx_volume)98 unsigned Hrtf::process(
99 unsigned sample_rate, clunk::Buffer &dst_buf, unsigned dst_ch,
100 const clunk::Buffer &src_buf, unsigned src_ch,
101 const v3f &delta_position, float fx_volume)
102 {
103 s16 * const dst = static_cast<s16*>(dst_buf.get_ptr());
104 const unsigned dst_n = (unsigned)dst_buf.get_size() / dst_ch / 2;
105
106 const s16 * const src = static_cast<const s16 *>(src_buf.get_ptr());
107 const unsigned src_n = (unsigned)src_buf.get_size() / src_ch / 2;
108 assert(dst_n <= src_n);
109
110 kemar_ptr kemar_data;
111 int angles;
112 get_kemar_data(kemar_data, angles, delta_position);
113
114 if (delta_position.is0() || kemar_data == NULL) {
115 //2d stereo sound!
116 if (src_ch == dst_ch) {
117 memcpy(dst_buf.get_ptr(), src_buf.get_ptr(), dst_buf.get_size());
118 return src_n;
119 }
120 else
121 throw_ex(("unsupported sample conversion"));
122 }
123 assert(dst_ch == 2);
124
125 //LOG_DEBUG(("data: %p, angles: %d", (void *) kemar_data, angles));
126
127 float t_idt, angle_gr, left_to_right_amp;
128 idt_iit(delta_position, t_idt, angle_gr, left_to_right_amp);
129
130 const int kemar_sector_size = 360 / angles;
131 const int kemar_idx[2] = {
132 ((360 - (int)angle_gr + kemar_sector_size / 2) / kemar_sector_size) % angles,
133 ((int)angle_gr + kemar_sector_size / 2) / kemar_sector_size
134 };
135
136 float amp[2] = {
137 left_to_right_amp > 1? 1: 1 / left_to_right_amp,
138 left_to_right_amp > 1? left_to_right_amp: 1
139 };
140 //LOG_DEBUG(("%g (of %d)-> left: %d, right: %d", angle_gr, angles, kemar_idx_left, kemar_idx_right));
141
142 int idt_offset = (int)(t_idt * sample_rate);
143
144 int window = 0;
145 while(sample3d[0].get_size() < dst_n * 2 || sample3d[1].get_size() < dst_n * 2) {
146 size_t offset = window * WINDOW_SIZE / 2;
147 assert(offset + WINDOW_SIZE / 2 <= src_n);
148 for(unsigned c = 0; c < dst_ch; ++c) {
149 sample3d[c].reserve(WINDOW_SIZE);
150 s16 *dst = static_cast<s16 *>(static_cast<void *>((static_cast<u8 *>(sample3d[c].get_ptr()) + sample3d[c].get_size() - WINDOW_SIZE)));
151 hrtf(c, dst, src + offset * src_ch, src_ch, src_n - offset, idt_offset, kemar_data, kemar_idx[c], amp[c]);
152 }
153 ++window;
154 }
155 assert(sample3d[0].get_size() >= dst_n * 2 && sample3d[1].get_size() >= dst_n * 2);
156
157 //LOG_DEBUG(("angle: %g", angle_gr));
158 //LOG_DEBUG(("idt offset %d samples", idt_offset));
159 s16 * src_3d[2] = { static_cast<s16 *>(sample3d[0].get_ptr()), static_cast<s16 *>(sample3d[1].get_ptr()) };
160
161 //LOG_DEBUG(("size1: %u, %u, needed: %u\n%s", (unsigned)sample3d[0].get_size(), (unsigned)sample3d[1].get_size(), dst_n, sample3d[0].dump().c_str()));
162
163 for(unsigned i = 0; i < dst_n; ++i) {
164 for(unsigned c = 0; c < dst_ch; ++c) {
165 dst[i * dst_ch + c] = src_3d[c][i];
166 }
167 }
168 skip(dst_n);
169 return window * WINDOW_SIZE / 2;
170 }
171
skip(unsigned samples)172 void Hrtf::skip(unsigned samples) {
173 for(int i = 0; i < 2; ++i) {
174 Buffer & buf = sample3d[i];
175 buf.pop(samples * 2);
176 }
177 }
178
hrtf(const unsigned channel_idx,s16 * dst,const s16 * src,int src_ch,int src_n,int idt_offset,const kemar_ptr & kemar_data,int kemar_idx,float freq_decay)179 void Hrtf::hrtf(const unsigned channel_idx, s16 *dst, const s16 *src, int src_ch, int src_n, int idt_offset, const kemar_ptr& kemar_data, int kemar_idx, float freq_decay) {
180 assert(channel_idx < 2);
181
182 //LOG_DEBUG(("channel %d: window %d: adding %d, buffer size: %u, decay: %g", channel_idx, window, WINDOW_SIZE, (unsigned)result.get_size(), freq_decay));
183
184 if (channel_idx <= 1) {
185 bool left = channel_idx == 0;
186 if (!left && idt_offset > 0) {
187 idt_offset = 0;
188 } else if (left && idt_offset < 0) {
189 idt_offset = 0;
190 }
191 if (idt_offset < 0)
192 idt_offset = - idt_offset;
193 } else
194 idt_offset = 0;
195
196 assert(std::min(0, idt_offset) >= 0);
197 assert(std::max(0, idt_offset) + WINDOW_SIZE <= src_n);
198
199 for(int i = 0; i < WINDOW_SIZE; ++i) {
200 //-1 0 1 2 3
201 int p = idt_offset + i;
202 assert(p >= 0 && p < src_n);
203 //printf("%d of %d, ", p, src_n);
204 int v = src[p * src_ch];
205 _mdct.data[i] = v / 32768.0f;
206 //fprintf(stderr, "%g ", _mdct.data[i]);
207 }
208
209 _mdct.apply_window();
210 _mdct.mdct();
211 {
212 for(size_t i = 0; i < mdct_type::M; ++i)
213 {
214 const int kemar_sample = i * 257 / mdct_type::M;
215 std::complex<float> fir(kemar_data[kemar_idx][0][kemar_sample][0], kemar_data[kemar_idx][0][kemar_sample][1]);
216 _mdct.data[i] *= std::abs(fir);
217 }
218 }
219
220 //LOG_DEBUG(("kemar angle index: %d\n", kemar_idx));
221 assert(freq_decay >= 1);
222
223 _mdct.imdct();
224 _mdct.apply_window();
225
226 {
227 //stupid msvc
228 int i;
229 for(i = 0; i < WINDOW_SIZE / 2; ++i) {
230 float v = _mdct.data[i] + overlap_data[channel_idx][i];
231
232 if (v < -1) {
233 LOG_DEBUG(("clipping %f", v));
234 v = -1;
235 } else if (v > 1) {
236 LOG_DEBUG(("clipping %f", v));
237 v = 1;
238 }
239 *dst++ = (int)(v * 32767);
240 }
241 for(; i < WINDOW_SIZE; ++i) {
242 overlap_data[channel_idx][i - WINDOW_SIZE / 2] = _mdct.data[i];
243 }
244 }
245 }
246
247
248 }
249