Check that AltiVec is enabled before using it
[openal-soft.git] / utils / makemhr / loadsofa.cpp
blob3fe4992fdfc5df7ef0fb8d77d8e95fc176147d0e
1 /*
2 * HRTF utility for producing and demonstrating the process of creating an
3 * OpenAL Soft compatible HRIR data set.
5 * Copyright (C) 2018-2019 Christopher Fitzgerald
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2 of the License, or
10 * (at your option) any later version.
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
17 * You should have received a copy of the GNU General Public License along
18 * with this program; if not, write to the Free Software Foundation, Inc.,
19 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
21 * Or visit: http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
24 #include "loadsofa.h"
26 #include <algorithm>
27 #include <atomic>
28 #include <chrono>
29 #include <cmath>
30 #include <cstdio>
31 #include <functional>
32 #include <future>
33 #include <iterator>
34 #include <memory>
35 #include <numeric>
36 #include <optional>
37 #include <string>
38 #include <string_view>
39 #include <thread>
40 #include <vector>
42 #include "alspan.h"
43 #include "alstring.h"
44 #include "alnumeric.h"
45 #include "makemhr.h"
46 #include "polyphase_resampler.h"
47 #include "sofa-support.h"
49 #include "mysofa.h"
52 namespace {
54 using namespace std::string_view_literals;
55 using uint = unsigned int;
57 /* Attempts to produce a compatible layout. Most data sets tend to be
58 * uniform and have the same major axis as used by OpenAL Soft's HRTF model.
59 * This will remove outliers and produce a maximally dense layout when
60 * possible. Those sets that contain purely random measurements or use
61 * different major axes will fail.
63 auto PrepareLayout(const al::span<const float> xyzs, HrirDataT *hData) -> bool
65 fputs("Detecting compatible layout...\n", stdout);
67 auto fds = GetCompatibleLayout(xyzs);
68 if(fds.size() > MAX_FD_COUNT)
70 fputs("Incompatible layout (inumerable radii).\n", stdout);
71 return false;
74 std::array<double,MAX_FD_COUNT> distances{};
75 std::array<uint,MAX_FD_COUNT> evCounts{};
76 auto azCounts = std::vector<std::array<uint,MAX_EV_COUNT>>(MAX_FD_COUNT);
77 for(auto &azs : azCounts) azs.fill(0u);
79 uint fi{0u}, ir_total{0u};
80 for(const auto &field : fds)
82 distances[fi] = field.mDistance;
83 evCounts[fi] = field.mEvCount;
85 for(uint ei{0u};ei < field.mEvStart;ei++)
86 azCounts[fi][ei] = field.mAzCounts[field.mEvCount-ei-1];
87 for(uint ei{field.mEvStart};ei < field.mEvCount;ei++)
89 azCounts[fi][ei] = field.mAzCounts[ei];
90 ir_total += field.mAzCounts[ei];
93 ++fi;
95 printf("Using %u of %zu IRs.\n", ir_total, xyzs.size()/3);
96 const auto azs = al::span{azCounts}.first<MAX_FD_COUNT>();
97 return PrepareHrirData(al::span{distances}.first(fi), evCounts, azs, hData);
100 float GetSampleRate(MYSOFA_HRTF *sofaHrtf)
102 const char *srate_dim{nullptr};
103 const char *srate_units{nullptr};
104 MYSOFA_ARRAY *srate_array{&sofaHrtf->DataSamplingRate};
105 MYSOFA_ATTRIBUTE *srate_attrs{srate_array->attributes};
106 while(srate_attrs)
108 if("DIMENSION_LIST"sv == srate_attrs->name)
110 if(srate_dim)
112 fprintf(stderr, "Duplicate SampleRate.DIMENSION_LIST\n");
113 return 0.0f;
115 srate_dim = srate_attrs->value;
117 else if("Units"sv == srate_attrs->name)
119 if(srate_units)
121 fprintf(stderr, "Duplicate SampleRate.Units\n");
122 return 0.0f;
124 srate_units = srate_attrs->value;
126 else
127 fprintf(stderr, "Unexpected sample rate attribute: %s = %s\n", srate_attrs->name,
128 srate_attrs->value);
129 srate_attrs = srate_attrs->next;
131 if(!srate_dim)
133 fprintf(stderr, "Missing sample rate dimensions\n");
134 return 0.0f;
136 if(srate_dim != "I"sv)
138 fprintf(stderr, "Unsupported sample rate dimensions: %s\n", srate_dim);
139 return 0.0f;
141 if(!srate_units)
143 fprintf(stderr, "Missing sample rate unit type\n");
144 return 0.0f;
146 if(srate_units != "hertz"sv)
148 fprintf(stderr, "Unsupported sample rate unit type: %s\n", srate_units);
149 return 0.0f;
151 /* I dimensions guarantees 1 element, so just extract it. */
152 const auto values = al::span{srate_array->values, sofaHrtf->I};
153 if(values[0] < float{MIN_RATE} || values[0] > float{MAX_RATE})
155 fprintf(stderr, "Sample rate out of range: %f (expected %u to %u)", values[0], MIN_RATE,
156 MAX_RATE);
157 return 0.0f;
159 return values[0];
162 enum class DelayType : uint8_t {
163 None,
164 I_R, /* [1][Channels] */
165 M_R, /* [HRIRs][Channels] */
167 auto PrepareDelay(MYSOFA_HRTF *sofaHrtf) -> std::optional<DelayType>
169 const char *delay_dim{nullptr};
170 MYSOFA_ARRAY *delay_array{&sofaHrtf->DataDelay};
171 MYSOFA_ATTRIBUTE *delay_attrs{delay_array->attributes};
172 while(delay_attrs)
174 if("DIMENSION_LIST"sv == delay_attrs->name)
176 if(delay_dim)
178 fprintf(stderr, "Duplicate Delay.DIMENSION_LIST\n");
179 return std::nullopt;
181 delay_dim = delay_attrs->value;
183 else
184 fprintf(stderr, "Unexpected delay attribute: %s = %s\n", delay_attrs->name,
185 delay_attrs->value ? delay_attrs->value : "<null>");
186 delay_attrs = delay_attrs->next;
188 if(!delay_dim)
190 fprintf(stderr, "Missing delay dimensions\n");
191 return DelayType::None;
193 if(delay_dim == "I,R"sv)
194 return DelayType::I_R;
195 if(delay_dim == "M,R"sv)
196 return DelayType::M_R;
198 fprintf(stderr, "Unsupported delay dimensions: %s\n", delay_dim);
199 return std::nullopt;
202 bool CheckIrData(MYSOFA_HRTF *sofaHrtf)
204 const char *ir_dim{nullptr};
205 MYSOFA_ARRAY *ir_array{&sofaHrtf->DataIR};
206 MYSOFA_ATTRIBUTE *ir_attrs{ir_array->attributes};
207 while(ir_attrs)
209 if("DIMENSION_LIST"sv == ir_attrs->name)
211 if(ir_dim)
213 fprintf(stderr, "Duplicate IR.DIMENSION_LIST\n");
214 return false;
216 ir_dim = ir_attrs->value;
218 else
219 fprintf(stderr, "Unexpected IR attribute: %s = %s\n", ir_attrs->name,
220 ir_attrs->value ? ir_attrs->value : "<null>");
221 ir_attrs = ir_attrs->next;
223 if(!ir_dim)
225 fprintf(stderr, "Missing IR dimensions\n");
226 return false;
228 if(ir_dim != "M,R,N"sv)
230 fprintf(stderr, "Unsupported IR dimensions: %s\n", ir_dim);
231 return false;
233 return true;
237 /* Calculate the onset time of a HRIR. */
238 constexpr int OnsetRateMultiple{10};
239 auto CalcHrirOnset(PPhaseResampler &rs, const uint rate, al::span<double> upsampled,
240 const al::span<const double> hrir) -> double
242 rs.process(hrir, upsampled);
244 auto abs_lt = [](const double lhs, const double rhs) -> bool
245 { return std::abs(lhs) < std::abs(rhs); };
246 auto iter = std::max_element(upsampled.cbegin(), upsampled.cend(), abs_lt);
247 return static_cast<double>(std::distance(upsampled.cbegin(), iter)) /
248 (double{OnsetRateMultiple}*rate);
251 /* Calculate the magnitude response of a HRIR. */
252 void CalcHrirMagnitude(const uint points, al::span<complex_d> h, const al::span<double> hrir)
254 auto iter = std::copy_n(hrir.cbegin(), points, h.begin());
255 std::fill(iter, h.end(), complex_d{0.0, 0.0});
257 forward_fft(h);
258 MagnitudeResponse(h, hrir.first((h.size()/2) + 1));
261 bool LoadResponses(MYSOFA_HRTF *sofaHrtf, HrirDataT *hData, const DelayType delayType,
262 const uint outRate)
264 std::atomic<uint> loaded_count{0u};
266 auto load_proc = [sofaHrtf,hData,delayType,outRate,&loaded_count]() -> bool
268 const uint channels{(hData->mChannelType == CT_STEREO) ? 2u : 1u};
269 hData->mHrirsBase.resize(channels * size_t{hData->mIrCount} * hData->mIrSize, 0.0);
270 const auto hrirs = al::span{hData->mHrirsBase};
272 std::vector<double> restmp;
273 std::optional<PPhaseResampler> resampler;
274 if(outRate && outRate != hData->mIrRate)
276 resampler.emplace().init(hData->mIrRate, outRate);
277 restmp.resize(sofaHrtf->N);
280 const auto srcPosValues = al::span{sofaHrtf->SourcePosition.values, sofaHrtf->M*3_uz};
281 const auto irValues = al::span{sofaHrtf->DataIR.values,
282 size_t{sofaHrtf->M}*sofaHrtf->R*sofaHrtf->N};
283 for(uint si{0u};si < sofaHrtf->M;++si)
285 loaded_count.fetch_add(1u);
287 std::array aer{srcPosValues[3_uz*si], srcPosValues[3_uz*si + 1],
288 srcPosValues[3_uz*si + 2]};
289 mysofa_c2s(aer.data());
291 if(std::abs(aer[1]) >= 89.999f)
292 aer[0] = 0.0f;
293 else
294 aer[0] = std::fmod(360.0f - aer[0], 360.0f);
296 auto field = std::find_if(hData->mFds.cbegin(), hData->mFds.cend(),
297 [&aer](const HrirFdT &fld) -> bool
298 { return (std::abs(aer[2] - fld.mDistance) < 0.001); });
299 if(field == hData->mFds.cend())
300 continue;
302 const double evscale{180.0 / static_cast<double>(field->mEvs.size()-1)};
303 double ef{(90.0 + aer[1]) / evscale};
304 auto ei = static_cast<uint>(std::round(ef));
305 ef = (ef - ei) * evscale;
306 if(std::abs(ef) >= 0.1) continue;
308 const double azscale{360.0 / static_cast<double>(field->mEvs[ei].mAzs.size())};
309 double af{aer[0] / azscale};
310 auto ai = static_cast<uint>(std::round(af));
311 af = (af-ai) * azscale;
312 ai %= static_cast<uint>(field->mEvs[ei].mAzs.size());
313 if(std::abs(af) >= 0.1) continue;
315 HrirAzT &azd = field->mEvs[ei].mAzs[ai];
316 if(!azd.mIrs[0].empty())
318 fprintf(stderr, "\nMultiple measurements near [ a=%f, e=%f, r=%f ].\n",
319 aer[0], aer[1], aer[2]);
320 return false;
323 for(uint ti{0u};ti < channels;++ti)
325 azd.mIrs[ti] = hrirs.subspan(
326 (size_t{hData->mIrCount}*ti + azd.mIndex) * hData->mIrSize, hData->mIrSize);
327 const auto ir = irValues.subspan((size_t{si}*sofaHrtf->R + ti)*sofaHrtf->N,
328 sofaHrtf->N);
329 if(!resampler)
330 std::copy_n(ir.cbegin(), ir.size(), azd.mIrs[ti].begin());
331 else
333 std::copy_n(ir.cbegin(), ir.size(), restmp.begin());
334 resampler->process(restmp, azd.mIrs[ti]);
338 /* Include any per-channel or per-HRIR delays. */
339 if(delayType == DelayType::I_R)
341 const auto delayValues = al::span{sofaHrtf->DataDelay.values,
342 size_t{sofaHrtf->I}*sofaHrtf->R};
343 for(uint ti{0u};ti < channels;++ti)
344 azd.mDelays[ti] = delayValues[ti] / static_cast<float>(hData->mIrRate);
346 else if(delayType == DelayType::M_R)
348 const auto delayValues = al::span{sofaHrtf->DataDelay.values,
349 size_t{sofaHrtf->M}*sofaHrtf->R};
350 for(uint ti{0u};ti < channels;++ti)
351 azd.mDelays[ti] = delayValues[si*sofaHrtf->R + ti] /
352 static_cast<float>(hData->mIrRate);
356 if(outRate && outRate != hData->mIrRate)
358 const double scale{static_cast<double>(outRate) / hData->mIrRate};
359 hData->mIrRate = outRate;
360 hData->mIrPoints = std::min(static_cast<uint>(std::ceil(hData->mIrPoints*scale)),
361 hData->mIrSize);
363 return true;
366 std::future_status load_status{};
367 auto load_future = std::async(std::launch::async, load_proc);
368 do {
369 load_status = load_future.wait_for(std::chrono::milliseconds{50});
370 printf("\rLoading HRIRs... %u of %u", loaded_count.load(), sofaHrtf->M);
371 fflush(stdout);
372 } while(load_status != std::future_status::ready);
373 fputc('\n', stdout);
374 return load_future.get();
378 /* Calculates the frequency magnitudes of the HRIR set. Work is delegated to
379 * this struct, which runs asynchronously on one or more threads (sharing the
380 * same calculator object).
382 struct MagCalculator {
383 const uint mFftSize{};
384 const uint mIrPoints{};
385 std::vector<al::span<double>> mIrs{};
386 std::atomic<size_t> mCurrent{};
387 std::atomic<size_t> mDone{};
389 void Worker()
391 auto htemp = std::vector<complex_d>(mFftSize);
393 while(true)
395 /* Load the current index to process. */
396 size_t idx{mCurrent.load()};
397 do {
398 /* If the index is at the end, we're done. */
399 if(idx >= mIrs.size())
400 return;
401 /* Otherwise, increment the current index atomically so other
402 * threads know to go to the next one. If this call fails, the
403 * current index was just changed by another thread and the new
404 * value is loaded into idx, which we'll recheck.
406 } while(!mCurrent.compare_exchange_weak(idx, idx+1, std::memory_order_relaxed));
408 CalcHrirMagnitude(mIrPoints, htemp, mIrs[idx]);
410 /* Increment the number of IRs done. */
411 mDone.fetch_add(1);
416 } // namespace
418 bool LoadSofaFile(const std::string_view filename, const uint numThreads, const uint fftSize,
419 const uint truncSize, const uint outRate, const ChannelModeT chanMode, HrirDataT *hData)
421 int err;
422 MySofaHrtfPtr sofaHrtf{mysofa_load(std::string{filename}.c_str(), &err)};
423 if(!sofaHrtf)
425 printf("Error: Could not load %.*s: %s (%d)\n", al::sizei(filename), filename.data(),
426 SofaErrorStr(err), err);
427 return false;
430 /* NOTE: Some valid SOFA files are failing this check. */
431 err = mysofa_check(sofaHrtf.get());
432 if(err != MYSOFA_OK)
433 fprintf(stderr, "Warning: Supposedly malformed source file '%.*s': %s (%d)\n",
434 al::sizei(filename), filename.data(), SofaErrorStr(err), err);
436 mysofa_tocartesian(sofaHrtf.get());
438 /* Make sure emitter and receiver counts are sane. */
439 if(sofaHrtf->E != 1)
441 fprintf(stderr, "%u emitters not supported\n", sofaHrtf->E);
442 return false;
444 if(sofaHrtf->R > 2 || sofaHrtf->R < 1)
446 fprintf(stderr, "%u receivers not supported\n", sofaHrtf->R);
447 return false;
449 /* Assume R=2 is a stereo measurement, and R=1 is mono left-ear-only. */
450 if(sofaHrtf->R == 2 && chanMode == CM_AllowStereo)
451 hData->mChannelType = CT_STEREO;
452 else
453 hData->mChannelType = CT_MONO;
455 /* Check and set the FFT and IR size. */
456 if(sofaHrtf->N > fftSize)
458 fprintf(stderr, "Sample points exceeds the FFT size.\n");
459 return false;
461 if(sofaHrtf->N < truncSize)
463 fprintf(stderr, "Sample points is below the truncation size.\n");
464 return false;
466 hData->mIrPoints = sofaHrtf->N;
467 hData->mFftSize = fftSize;
468 hData->mIrSize = std::max(1u + (fftSize/2u), sofaHrtf->N);
470 /* Assume a default head radius of 9cm. */
471 hData->mRadius = 0.09;
473 hData->mIrRate = static_cast<uint>(std::lround(GetSampleRate(sofaHrtf.get())));
474 if(!hData->mIrRate)
475 return false;
477 const auto delayType = PrepareDelay(sofaHrtf.get());
478 if(!delayType)
479 return false;
481 if(!CheckIrData(sofaHrtf.get()))
482 return false;
483 if(!PrepareLayout(al::span{sofaHrtf->SourcePosition.values, sofaHrtf->M*3_uz}, hData))
484 return false;
485 if(!LoadResponses(sofaHrtf.get(), hData, *delayType, outRate))
486 return false;
487 sofaHrtf = nullptr;
489 for(uint fi{0u};fi < hData->mFds.size();fi++)
491 uint ei{0u};
492 for(;ei < hData->mFds[fi].mEvs.size();ei++)
494 uint ai{0u};
495 for(;ai < hData->mFds[fi].mEvs[ei].mAzs.size();ai++)
497 HrirAzT &azd = hData->mFds[fi].mEvs[ei].mAzs[ai];
498 if(!azd.mIrs[0].empty()) break;
500 if(ai < hData->mFds[fi].mEvs[ei].mAzs.size())
501 break;
503 if(ei >= hData->mFds[fi].mEvs.size())
505 fprintf(stderr, "Missing source references [ %d, *, * ].\n", fi);
506 return false;
508 hData->mFds[fi].mEvStart = ei;
509 for(;ei < hData->mFds[fi].mEvs.size();ei++)
511 for(uint ai{0u};ai < hData->mFds[fi].mEvs[ei].mAzs.size();ai++)
513 HrirAzT &azd = hData->mFds[fi].mEvs[ei].mAzs[ai];
514 if(azd.mIrs[0].empty())
516 fprintf(stderr, "Missing source reference [ %d, %d, %d ].\n", fi, ei, ai);
517 return false;
524 size_t hrir_total{0};
525 const uint channels{(hData->mChannelType == CT_STEREO) ? 2u : 1u};
526 const auto hrirs = al::span{hData->mHrirsBase};
527 for(uint fi{0u};fi < hData->mFds.size();fi++)
529 for(uint ei{0u};ei < hData->mFds[fi].mEvStart;ei++)
531 for(uint ai{0u};ai < hData->mFds[fi].mEvs[ei].mAzs.size();ai++)
533 HrirAzT &azd = hData->mFds[fi].mEvs[ei].mAzs[ai];
534 for(size_t ti{0u};ti < channels;ti++)
535 azd.mIrs[ti] = hrirs.subspan((hData->mIrCount*ti + azd.mIndex)*hData->mIrSize,
536 hData->mIrSize);
540 for(uint ei{hData->mFds[fi].mEvStart};ei < hData->mFds[fi].mEvs.size();ei++)
541 hrir_total += hData->mFds[fi].mEvs[ei].mAzs.size() * channels;
544 std::atomic<size_t> hrir_done{0};
545 auto onset_proc = [hData,channels,&hrir_done]() -> bool
547 /* Temporary buffer used to calculate the IR's onset. */
548 auto upsampled = std::vector<double>(size_t{OnsetRateMultiple} * hData->mIrPoints);
549 /* This resampler is used to help detect the response onset. */
550 PPhaseResampler rs;
551 rs.init(hData->mIrRate, OnsetRateMultiple*hData->mIrRate);
553 for(auto &field : hData->mFds)
555 for(auto &elev : field.mEvs.subspan(field.mEvStart))
557 for(auto &azd : elev.mAzs)
559 for(uint ti{0};ti < channels;ti++)
561 hrir_done.fetch_add(1u, std::memory_order_acq_rel);
562 azd.mDelays[ti] += CalcHrirOnset(rs, hData->mIrRate, upsampled,
563 azd.mIrs[ti].first(hData->mIrPoints));
568 return true;
571 std::future_status load_status{};
572 auto load_future = std::async(std::launch::async, onset_proc);
573 do {
574 load_status = load_future.wait_for(std::chrono::milliseconds{50});
575 printf("\rCalculating HRIR onsets... %zu of %zu", hrir_done.load(), hrir_total);
576 fflush(stdout);
577 } while(load_status != std::future_status::ready);
578 fputc('\n', stdout);
579 if(!load_future.get())
580 return false;
582 MagCalculator calculator{hData->mFftSize, hData->mIrPoints};
583 for(auto &field : hData->mFds)
585 for(auto &elev : field.mEvs.subspan(field.mEvStart))
587 for(auto &azd : elev.mAzs)
589 for(uint ti{0};ti < channels;ti++)
590 calculator.mIrs.push_back(azd.mIrs[ti]);
595 std::vector<std::thread> thrds;
596 thrds.reserve(numThreads);
597 for(size_t i{0};i < numThreads;++i)
598 thrds.emplace_back(std::mem_fn(&MagCalculator::Worker), &calculator);
599 size_t count;
600 do {
601 std::this_thread::sleep_for(std::chrono::milliseconds{50});
602 count = calculator.mDone.load();
604 printf("\rCalculating HRIR magnitudes... %zu of %zu", count, calculator.mIrs.size());
605 fflush(stdout);
606 } while(count != calculator.mIrs.size());
607 fputc('\n', stdout);
609 for(auto &thrd : thrds)
611 if(thrd.joinable())
612 thrd.join();
614 return true;