From fd842db26f5abb5e398aabc55fa68d66035dd258 Mon Sep 17 00:00:00 2001 From: TheM14 Date: Sun, 1 Nov 2026 02:54:55 +0800 Subject: [PATCH] Render SOFA HRIRs directly and place bed channels by their labels Two changes that belong to the same reproduction model: a bed channel has one position, defined by its label, and a SOFA set keeps the impulse responses it was measured with. CBA bed channels: the position comes from the channel's label in ETSI TS 103 420 Table B.10, not from the layout the decoder is asked to render to. The per-layout anchor table displaced bed channels onto the nearest target speaker, which silently moved content whenever the target layout changed. - Drop the anchor table and the geometry lookup; bed_channel_q15 is the only source of bed positions, so stream and task no longer need a layout to ask for one - Keep the 17-bit non-standard bed mask in channel order, as Table 13 / TS 103 190-2 Table 65 transmit it - Exclude LFE channels from the object list while keeping them in the coverage mask - Cover the Table B.10 coordinates, the q15 encoding, the 10-bit and 17-bit masks and the LFE split in tests SOFA rendering: compiling a SOFA set into a fifth-order directional field threw away the measured impulse response - the SH projection, the 64-QMF/77-hybrid round trip and the synthetic room replaced it. The full IRs are now kept and rendered as such, with the reproduction defined by a configuration that names a personalized model rather than by the renderer itself. - Add SphericalMesh: incremental convex hull over the measurement directions, barycentric weights inside the containing triangle and spherical Voronoi areas for the centre spectrum - Add FullHrirField: rfft of the selected shell, 200-1500 Hz phase fit for the delay, delay-aligned spectra, log-magnitude/phase or MCA interpolation, rigid-sphere radial ratio, recording origin and the common FFT/2 latency - Add Uniform/MultiSourcePartitionedConvolver: block 128, numpy FFT conventions, zero-fade filter rebuild from the blocks in flight - Move the Rosella reference arithmetic (profile geometry, direction basis, direct gains and room send, table-A room) out of the renderer into rosella_reference so both renderers share it - Add ReferenceEnvironment and SofaReferenceRuntime: JSON configuration with model hash and distance-profile mapping, level calibration over equal-area directions, the reference room, 1536-sample batching and a single latency removal - Keep .jochrtf an explicit input and the SOFA compile step behind --hrtf-cache-policy/--hrtf-cache-dir; add --reference-config and --sofa-interpolation, and make --hrtf-radius-m optional for a single shell - Cover the mesh weights and Voronoi areas, the sphere series and the partitioned convolver with reference values in tests - Document both rules in the READMEs and the binaural notes Rosella renders stay byte-identical (d5877e89...) and so do renders of the legacy compiled path (6f0a75d2...). Direct SOFA renders match the Python reference to 1e-4 in level; the residual comes from the barycentric solve (analytic inverse here, LAPACK there) on near-degenerate triangles. --- CMakeLists.txt | 6 + README.en.md | 10 +- README.md | 6 +- docs/binaural.en.md | 86 ++- docs/binaural.md | 71 +- include/joc_core.h | 24 +- src/binaural/binaural_reference.cpp | 393 +++++++++++ src/binaural/binaural_reference.h | 97 +++ src/binaural/sofa_reference_runtime.cpp | 571 ++++++++++++++++ src/binaural/sofa_reference_runtime.h | 155 +++++ src/cli/joc_cli.cpp | 44 +- src/hrtf/full_hrir_field.cpp | 774 +++++++++++++++++++++ src/hrtf/full_hrir_field.h | 96 +++ src/hrtf/partitioned_convolution.cpp | 336 +++++++++ src/hrtf/partitioned_convolution.h | 118 ++++ src/hrtf/rosella_reference.cpp | 854 +++++++++++++++++++++++ src/hrtf/rosella_reference.h | 107 +++ src/hrtf/rosella_renderer.cpp | 866 +----------------------- src/hrtf/sofa_field.cpp | 8 + src/hrtf/sofa_field.h | 3 + src/hrtf/spherical_mesh.cpp | 531 +++++++++++++++ src/hrtf/spherical_mesh.h | 50 ++ src/joc_bitstream/bed_layout_map.cpp | 115 +--- src/joc_bitstream/bed_layout_map.h | 6 +- src/stream/stream.cpp | 4 +- src/task/task.cpp | 204 ++++-- tests/test_core.cpp | 276 ++++++++ 27 files changed, 4746 insertions(+), 1065 deletions(-) create mode 100644 src/binaural/binaural_reference.cpp create mode 100644 src/binaural/binaural_reference.h create mode 100644 src/binaural/sofa_reference_runtime.cpp create mode 100644 src/binaural/sofa_reference_runtime.h create mode 100644 src/hrtf/full_hrir_field.cpp create mode 100644 src/hrtf/full_hrir_field.h create mode 100644 src/hrtf/partitioned_convolution.cpp create mode 100644 src/hrtf/partitioned_convolution.h create mode 100644 src/hrtf/rosella_reference.cpp create mode 100644 src/hrtf/rosella_reference.h create mode 100644 src/hrtf/spherical_mesh.cpp create mode 100644 src/hrtf/spherical_mesh.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 4782523..1e33f10 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -92,7 +92,9 @@ set(JOC_REUSED_SOURCES set(JOC_INTERNAL_SOURCES src/adm/adm_metadata.cpp src/adm/adm_tracks.cpp + src/binaural/binaural_reference.cpp src/binaural/binaural_runtime.cpp + src/binaural/sofa_reference_runtime.cpp src/binaural/sofa_binaural_renderer.cpp src/eac3_transport/eac3_reader.cpp src/emdf/emdf_parser.cpp @@ -108,10 +110,14 @@ set(JOC_INTERNAL_SOURCES src/hrtf/rosella_filterbank.cpp src/hrtf/rosella_model.cpp src/hrtf/rosella_renderer.cpp + src/hrtf/rosella_reference.cpp src/hrtf/kernel_tables.cpp + src/hrtf/full_hrir_field.cpp + src/hrtf/partitioned_convolution.cpp src/hrtf/sofa.cpp src/hrtf/sofa_cache.cpp src/hrtf/sofa_field.cpp + src/hrtf/spherical_mesh.cpp src/io/adm_writer.cpp src/io/hdf5.cpp src/io/inflate.cpp diff --git a/README.en.md b/README.en.md index 7ae9eaa..442cad5 100644 --- a/README.en.md +++ b/README.en.md @@ -6,8 +6,8 @@ The C++ implementation of JustOneCacophony: an execution core for E-AC-3 JOC bitstream parsing, object reconstruction and rendering. It extracts EMDF, ID14 JOC parameters and ID11 OAMD metadata from E-AC-3 syncframes, combines them with the core 5.1 PCM decoded by FFmpeg to rebuild the LFE and 15 object signals, and writes -ADM BWF, a WAV for a chosen speaker layout, or a binaural WAV using a compiled HRTF -directional field. +ADM BWF, a WAV for a chosen speaker layout, or a binaural WAV rendered directly from +full SOFA HRIRs (or from a compiled `.jochrtf` directional field). This is research code, not a complete, standard-conformant or production JOC decoder. It covers the bitstream forms it implements and reports an explicit error @@ -224,7 +224,11 @@ The extra pair's **identity** never reaches the DSP (section 10.1): when the dep renders, with the bed mapping following the channel codes the `chanmap` declares. The bed layout the OAMD declares may hold more channels than the core substream codes (a 9.1.6 bed with a 5.1 core, for example): only a core channel missing from that layout counts as a -mismatch. When one audio frame holds several EMDF containers carrying ID11/ID14, they +mismatch. For bed-only content the 15 JOC object slots are the bed channels, each rendered as +a point source at the fixed position its channel label defines (ETSI TS 103 420 table B.10); +speaker, ADM and binaural output share those source coordinates and never re-place them per +layout, while LFE keeps its separate path. When one audio frame holds several EMDF +containers carrying ID11/ID14, they are scored by the association rule of section 8.2 and its Table 56: a container whose OAMD and JOC payloads share a group id and whose ID14 parses completely wins, then one whose ID14 merely parses; only a unique best score is used and the others are recorded as a diagnostic diff --git a/README.md b/README.md index b193faf..e6b5ca4 100644 --- a/README.md +++ b/README.md @@ -4,8 +4,8 @@ JustOneCacophony 的 C++ 实现:E-AC-3 JOC 码流解析、对象重建与渲染的执行内核。它从 E-AC-3 同步帧中提取 EMDF、ID14 JOC 参数与 ID11 OAMD 元数据,结合 FFmpeg 解码出的核心 PCM(N=5 或 7) -重建 LFE 与 15 路对象 PCM,并输出 ADM BWF、指定扬声器布局的 WAV,或用编译好的 HRTF 方向场 -直接输出双耳 WAV。 +重建 LFE 与 15 路对象 PCM,并输出 ADM BWF、指定扬声器布局的 WAV,或按完整 HRIR 直接 +渲染双耳 WAV(SOFA),也可以读入编译好的 `.jochrtf` 方向场渲染。 这是研究代码,不是完整、标准兼容或生产级的 JOC 解码器。它只覆盖已实现的码流形态,遇到未知 变体时明确报错,而不是假装一切都很和谐。 @@ -191,6 +191,8 @@ ffmpeg 侧只在 $N=5$ 档位传 `-ac 6`;$N=7$ 档位不传 `-ac`,保留解 拒绝。额外那一对的**身份**只进元数据、不进 DSP(§10.1):依赖子流 `chanmap` 与 $k$ 不一致、或 OAMD 声明的 bed 布局可以比核心子流实际编码的声道多(例如 9.1.6 bed 只编 5.1 核心),只有核心 声道在 bed 布局里找不到位置才记 diagnostics 并照常解码,床映射按 `chanmap` 的通道码走。 +bed-only 内容的 15 路 JOC 对象就是 bed 声道:每一路按其声道标签的固定方位(ETSI TS 103 420 +Table B.10)作为点声源渲染,扬声器、ADM 与双耳共用同一组源坐标,不随输出布局改变;LFE 独立处理。 同一音频帧里出现多个含 ID11/ID14 的 EMDF 容器时,按规格 §8.2 + Table 56 的关联规则打分: OAMD 与 JOC 载荷 groupid 相同且 ID14 能完整解析的优先,其次只看 ID14 能否解析;唯一最高分才用, 其余记入 diagnostics(`emdf/extra_container_ignored`)。额外那一对在 10 槽 ADM 骨架上的落位 diff --git a/docs/binaural.en.md b/docs/binaural.en.md index a6f43b1..0817396 100644 --- a/docs/binaural.en.md +++ b/docs/binaural.en.md @@ -5,28 +5,70 @@ JustOneCacophony's binaural backend supports three HRTF sources: `SimpleFreeFieldHRIR` SOFA, the Rosella `.personalized_headphone` model exported by Dolby's official personalization scan (its JSON parsing is implemented by -this project and invokes no Dolby software), and the `.jochrtf` cache compiled -from SOFA. SOFA is compiled into an in-memory directional field when the model -is loaded. A `.jochrtf` file is only a disposable, reproducible JOC compiled -HRTF cache; it is neither an interchange format nor a prerequisite for using -SOFA. +this project and invokes no Dolby software), and `.jochrtf` (a JOC compiled HRTF +cache). + +SOFA keeps its full HRIRs and renders directly through a configured reference +environment; it is no longer compiled into a directional field: ```text -SOFA FIR - -> CanonicalHrtf - -> 48 kHz / one radius shell / delay-phase policy - -> 64-QMF / 77-hybrid projection - -> fifth-order ACN/N3D real-SH field - -> per-object direct + early reflections - -> shared unitary-FDN late room - -> float64 stereo +SOFA full HRIR [M,2,N] + -> CanonicalHrtf (48 kHz / one radius shell) + -> delay-aligned complex spectra (200-1500 Hz phase fit, Data.Delay included) + -> spherical-triangle barycentric interpolation (lf_logmag_phase or lf_mca_zero) + -> rigid-sphere radial correction (reference-environment distance) + -> per-object partitioned convolution (block 128, FFT = max(2048, >=2*support)) + -> reference-environment recursive room model + -> float64 stereo (one common latency removed) ``` +`.jochrtf` stays an explicit input (`--compiled-hrtf-cache`) and keeps the +real-SH field renderer; passing `--hrtf-cache-policy` / `--hrtf-cache-dir` +selects the legacy "SOFA -> `.jochrtf`" compile path. + +## Reference environment + +Direct SOFA rendering needs a reference-environment configuration +(`--reference-config`, default `/local/binaural_reference.json`). It names +the Rosella `.personalized_headphone` model together with its SHA-256, the +`profile_index` used as the level anchor (2 = far, 3 = mid), the room impulse +slots (4096 by default) and the number of level-calibration directions (128). + +Every SOFA direction is first mapped into the reference reproduction through the +model's distance profile: + +```text +ADM position -> int16 metadata grid -> distance profile + -> unit direction + distance scale + -> direct gain a = 1/sqrt(1+coupling*(h0*d)^2) (1 for near) + -> room send = h0*d*a (0 for near) + -> near-centre regularization: ear distance = max(d, minimum_distance) +``` + +`near|mid|far` (`--binaural-mode`) selects one of the model's three distance +profiles; level calibration always uses the configured `profile_index` as its +anchor, so all three modes share one level. + +The direct path is the interpolated, delay-aligned full HRIR; the room send only +feeds the reference room model (the LFE channel never does). Their latencies come +from the larger of the field (FFT/2 plus the recording origin) and the bank +(961), and are removed once before the output. + ## Inputs -The binaural backend consumes a compiled directional field (a JOC compiled HRTF cache, -`.jochrtf`) plus the shared filterbank tables. Both are read-only inputs: this library -performs no parsing or conversion of measurement data formats. +A SOFA input renders directly from its full HRIRs and only needs the SOFA file and +the reference-environment configuration: + +```powershell +joc_cli input.m4a --binaural ` + --sofa-hrtf HRTF\subject.sofa ` + --reference-config local\binaural_reference.json ` + --binaural-mode mid --sofa-interpolation lf_logmag_phase +``` + +A `.jochrtf` input (and `.personalized_headphone`) also needs the shared filterbank +tables. Both are read-only inputs; parsing and coordinate validation happen while the +model is loaded, and nothing is converted again during rendering. ```powershell joc_cli input.m4a --binaural ` @@ -35,8 +77,14 @@ joc_cli input.m4a --binaural ` --binaural-mode mid --binaural-tail-seconds 5.0 ``` +A multi-radius SOFA requires `--hrtf-radius-m`; a single shell is selected +automatically. `--sofa-interpolation` picks the local interpolation of the full HRIR: +`lf_logmag_phase` (default, log magnitude plus phase) or `lf_mca_zero` (auditory-band +magnitude correction). + The library exposes the same fields: `joc_task_config` for a file task, -`joc_stream_config` for streaming, where `hrtf_path`, `kernels_path`, +`joc_stream_config` for streaming, where `hrtf_sofa_path` (or `hrtf_path`), +`reference_config_path`, `sofa_interpolation`, `hrtf_compile`, `kernels_path`, `binaural_mode` and `binaural_tail_seconds` configure the binaural path. The `.jochrtf` file is an **input**, not a product of this library: compiling it from @@ -110,6 +158,10 @@ The compiler recognizes three mutually exclusive representations: No path may add a second ear delay or phase-group delay. +The direct SOFA path does not use this compile-time policy: it keeps the full IRs and +estimates the arrival time with a 200-1500 Hz phase fit, so `Data.Delay` and the fitted +delay are each applied once (the delay is removed first and restored afterwards). + ## Public filterbank and directional field The SOFA and `.jochrtf` runtime is fixed at: diff --git a/docs/binaural.md b/docs/binaural.md index 66f27bc..8e8a5d1 100644 --- a/docs/binaural.md +++ b/docs/binaural.md @@ -4,25 +4,60 @@ JustOneCacophony 的双耳后端支持三种 HRTF 来源:`SimpleFreeFieldHRIR` SOFA、 杜比官方软件个性化扫描导出的 Rosella `.personalized_headphone`(JSON 解析由本项目 -自行实现,不调用杜比软件),以及从 SOFA 编译出的 `.jochrtf` 缓存。SOFA 在模型加载 -时编译成内存方向场;`.jochrtf` 只是可删除、可重建的 JOC compiled HRTF cache, -不是交换格式,也不是使用 SOFA 的前置步骤。 +自行实现,不调用杜比软件),以及 `.jochrtf`(JOC compiled HRTF cache)。 + +SOFA 默认保留完整 HRIR,按“参考环境”直连渲染,不再编译成方向场: ```text -SOFA FIR - -> CanonicalHrtf - -> 48 kHz / 单 radius shell / delay-phase policy - -> 64-QMF / 77-hybrid projection - -> 五阶 ACN/N3D 实球谐场 - -> 逐对象 direct + early reflections - -> shared unitary-FDN late room - -> stereo float64 +SOFA 完整 HRIR [M,2,N] + -> CanonicalHrtf(48 kHz / 单 radius shell) + -> 延迟对齐的复数频谱(200-1500 Hz 相位拟合,含 Data.Delay) + -> 球面三角形重心插值(lf_logmag_phase 或 lf_mca_zero) + -> 刚性球径向修正(按参考环境距离) + -> 逐对象 partitioned convolution(block 128,FFT = max(2048, ≥2×support)) + -> 参考环境的递归房间模型 + -> stereo float64(统一扣除 latency) ``` +`.jochrtf` 仍是显式输入(`--compiled-hrtf-cache`),走球谐方向场渲染;显式给出 +`--hrtf-cache-policy` / `--hrtf-cache-dir` 会回到旧的“SOFA → `.jochrtf`”编译路径。 + +## 参考环境 + +SOFA 直连渲染需要一个参考环境配置(`--reference-config`,默认 +`/local/binaural_reference.json`)。配置给出 Rosella `.personalized_headphone` +模型的路径与 SHA-256、`profile_index`(2=far、3=mid,作为 level 校准锚点)、 +`room_impulse_slots`(默认 4096)与 `level_calibration_directions`(默认 128)。 + +渲染时每个 SOFA 方向先经该模型的 distance profile 映射成参考环境中的位置: + +```text +ADM position -> int16 metadata grid -> distance profile + -> direction(单位向量)+ 距离 scale + -> direct gain a = 1/sqrt(1+coupling·(h0·d)^2)(near 恒为 1) + -> room send = h0·d·a(near 恒为 0) + -> 近中心正则化:ear distance 取 max(d, minimum_distance) +``` + +`near|mid|far` 选择该模型的三个 distance profile 之一(`--binaural-mode`),level 校准 +始终用配置里的 `profile_index` 作为锚点,因此三种模式共享同一个 level。 + +指向同一声道的直达声是延迟对齐后的完整 HRIR 插值结果,房间 send 只进参考环境的 +房间模型(LFE 声道不进房间)。两者的 latency 由 field(FFT/2 与 recording origin) +和 bank(961)中的较大者决定,输出前统一扣除。 + ## 输入接口 -双耳后端消费一个已编译的方向场(JOC compiled HRTF cache,`.jochrtf`)与共享滤波器组表; -两者都是只读输入,本库不做任何测量数据格式的解析或转换: +SOFA 输入直接按完整 HRIR 渲染,只需要 SOFA 与参考环境配置: + +```powershell +joc_cli input.m4a --binaural ` + --sofa-hrtf HRTF\subject.sofa ` + --reference-config local\binaural_reference.json ` + --binaural-mode mid --sofa-interpolation lf_logmag_phase +``` + +`.jochrtf` 输入(以及 `.personalized_headphone`)需要共享滤波器组表,两者都是只读输入: ```powershell joc_cli input.m4a --binaural ` @@ -31,8 +66,13 @@ joc_cli input.m4a --binaural ` --binaural-mode mid --binaural-tail-seconds 5.0 ``` +多球壳 SOFA 必须用 `--hrtf-radius-m` 指定球壳(单球壳自动选择);`--hrtf-radius-m` +对已编译的 `.jochrtf` 不再生效。`--sofa-interpolation` 选择完整 HRIR 的局部插值: +`lf_logmag_phase`(默认,对数幅度 + 相位)或 `lf_mca_zero`(听觉带幅度修正)。 + 库接口使用同一组字段:文件任务用 `joc_task_config`,流式用 `joc_stream_config`, -其中 `hrtf_path`、`kernels_path`、`binaural_mode`、`binaural_tail_seconds` 决定双耳通路。 +其中 `hrtf_sofa_path`(或 `hrtf_path`)、`reference_config_path`、`sofa_interpolation`、 +`hrtf_compile`、`kernels_path`、`binaural_mode`、`binaural_tail_seconds` 决定双耳通路。 `.jochrtf` 是**输入**而不是本库的产物:从 SOFA 或测量数据编译该缓存属于工具链的职责, 与本仓库解耦。缓存加载时会校验成员集合、dtype 与形状、C 连续性、CRC-32 以及按成员重算的 @@ -96,6 +136,9 @@ convention adapter,不能只通过 reshape 进入核心 importer。 任何路径都不能再叠加第二套 ear delay 或 phase-group delay。 +SOFA 直连路径不走这套编译期语义:它保留完整 IR,用 200-1500 Hz 相位拟合估计到达时间, +`Data.Delay` 与拟合出的延迟各应用一次(先去掉 `Data.Delay`,再用插值后的延迟复原)。 + ## 公开 filterbank 与方向场 运行时固定为: diff --git a/include/joc_core.h b/include/joc_core.h index 1369317..ee28f69 100644 --- a/include/joc_core.h +++ b/include/joc_core.h @@ -358,8 +358,14 @@ typedef enum joc_clip_action { JOC_CLIP_ABORT = 3 /* fail the task */ } joc_clip_action; -/* Compiled-HRTF cache policy for a SOFA input; the .jochrtf itself is an - * internal artifact of the compile step. */ +/* Local interpolation of the full-HRIR SOFA field (config version 3). */ +typedef enum joc_sofa_interpolation { + JOC_SOFA_INTERPOLATION_LOGMAG_PHASE = 0, /* delay-aligned log-magnitude/phase */ + JOC_SOFA_INTERPOLATION_MCA_ZERO = 1 /* auditory-band magnitude correction */ +} joc_sofa_interpolation; + +/* Compiled-HRTF cache policy for the legacy SOFA compile step; the .jochrtf + * itself stays an explicit input (hrtf_path). */ typedef enum joc_hrtf_cache_policy { JOC_HRTF_CACHE_NONE = 0, /* compile and discard */ JOC_HRTF_CACHE_MEMORY = 1, /* compile and keep in this process (default) */ @@ -424,16 +430,22 @@ typedef struct joc_task_config { joc_cancel_token* cancel; /* optional */ - /* Binaural HRTF input (config version 2): when hrtf_sofa_path is set the - * library compiles it with hrtf_cache_policy / hrtf_cache_dir / hrtf_radius_m + /* Binaural HRTF input: when hrtf_sofa_path is set the SOFA renders directly + * from its full HRIRs through the reference environment (config version 3) * and hrtf_path is unused. hrtf_path stays the advanced override that reads - * a .jochrtf directly. */ + * a .jochrtf directly with the compiled-field renderer. */ const char* hrtf_sofa_path; /* SOFA SimpleFreeFieldHRIR input */ const char* personalized_headphone_path; /* Rosella .personalized_headphone input */ const char* hrtf_cache_dir; /* disk policy directory */ uint32_t hrtf_cache_policy; /* joc_hrtf_cache_policy */ uint32_t rosella_filterbank; /* joc_rosella_filterbank; 0 = PUBLIC */ - double hrtf_radius_m; /* SOFA measurement-radius shell */ + double hrtf_radius_m; /* SOFA shell radius; <= 0 = automatic */ + /* Reference environment for the direct SOFA path (config version 3). */ + const char* reference_config_path; /* NULL = local/binaural_reference.json */ + uint32_t sofa_interpolation; /* joc_sofa_interpolation */ + /* Nonzero keeps the legacy SOFA compile step (hrtf_cache_policy/hrtf_cache_dir + * decide where the .jochrtf lands); zero renders the SOFA directly. */ + uint32_t hrtf_compile; } joc_task_config; typedef enum joc_task_status { diff --git a/src/binaural/binaural_reference.cpp b/src/binaural/binaural_reference.cpp new file mode 100644 index 0000000..f617eda --- /dev/null +++ b/src/binaural/binaural_reference.cpp @@ -0,0 +1,393 @@ + +#include "binaural/binaural_reference.h" + +#include +#include +#include +#include +#include +#include + +#include "foundation/fft.h" +#include "foundation/fs_utf8.h" +#include "foundation/mini_json.h" +#include "foundation/sha256.h" +#include "hrtf/sofa_field.h" + +namespace joc::binaural { +namespace { + +constexpr double kPi = 3.141592653589793238462643383279502884; +constexpr double kMaximumExtension = 50.1; + +Status reference_error(const char* message) { + return Status::fail(JOC_ERR_INVALID_CONFIG, joc::stage::kRender, message); +} + +bool read_text_file(const std::string& path, std::string* out) { + std::ifstream stream(fs_utf8::to_path(path), std::ios::binary); + if (!stream) { + return false; + } + std::ostringstream buffer; + buffer << stream.rdbuf(); + *out = buffer.str(); + return true; +} + +bool read_binary_file(const std::string& path, std::string* out) { + return read_text_file(path, out); +} + +std::string directory_of(const std::string& path) { + const std::size_t slash = path.find_last_of("/\\"); + return slash == std::string::npos ? std::string() : path.substr(0, slash + 1); +} + +std::string join_path(const std::string& directory, const std::string& name) { + if (directory.empty()) { + return name; + } + const char last = directory.back(); + return (last == '/' || last == '\\' ? directory : directory + "/") + name; +} + +std::vector parse_number_list(const std::string& raw) { + std::vector values; + std::size_t index = 0; + while (index < raw.size()) { + if (raw[index] == '-' || raw[index] == '+' || raw[index] == '.' || + (raw[index] >= '0' && raw[index] <= '9')) { + std::size_t end = index; + while (end < raw.size() && + (raw[end] == '-' || raw[end] == '+' || raw[end] == '.' || + raw[end] == 'e' || raw[end] == 'E' || + (raw[end] >= '0' && raw[end] <= '9'))) { + ++end; + } + values.push_back(std::strtod(raw.substr(index, end - index).c_str(), nullptr)); + index = end; + continue; + } + ++index; + } + return values; +} + +} // namespace + +std::string ReferenceEnvironment::default_reference_config_path() { + return std::string("local/binaural_reference.json"); +} + +Status ReferenceEnvironment::load(const std::string& config_path, int mode_index, + bool mode_specified) { + config_path_ = config_path.empty() ? default_reference_config_path() : config_path; + std::string text; + if (!read_text_file(config_path_, &text)) { + return Status::fail(JOC_ERR_HRTF_NOT_FOUND, joc::stage::kRender, + "reference configuration not found: " + config_path_); + } + configuration_sha256_ = joc::crypto::sha256_hex(text.data(), text.size()); + + std::vector members; + std::string error; + if (!json::parse_object(text, &members, &error)) { + return reference_error("reference configuration is not a JSON object"); + } + std::string schema; + const json::Member* schema_member = json::find(members, "schema"); + if (schema_member == nullptr || !json::as_string(*schema_member, &schema) || + schema != "binaural-reference-v1") { + return reference_error("unsupported reference configuration schema"); + } + std::string model_path; + std::string expected_sha; + const json::Member* model_member = json::find(members, "model_path"); + const json::Member* sha_member = json::find(members, "model_sha256"); + if (model_member == nullptr || sha_member == nullptr || + !json::as_string(*model_member, &model_path) || + !json::as_string(*sha_member, &expected_sha)) { + return reference_error("reference configuration needs model_path and model_sha256"); + } + long long profile_index = 0; + const json::Member* profile_member = json::find(members, "profile_index"); + if (profile_member == nullptr || !json::as_integer(*profile_member, &profile_index)) { + return reference_error("reference configuration needs profile_index"); + } + calibration_profile_index_ = static_cast(profile_index); + if (calibration_profile_index_ != kProfileFar && + calibration_profile_index_ != kProfileMid) { + return reference_error("unsupported reference profile"); + } + const json::Member* profile_id_member = json::find(members, "profile_id"); + if (profile_id_member != nullptr) { + json::as_string(*profile_id_member, &profile_id_); + } + long long slots = 4096; + if (const json::Member* member = json::find(members, "room_impulse_slots")) { + json::as_integer(*member, &slots); + } + long long directions = 128; + if (const json::Member* member = json::find(members, "level_calibration_directions")) { + json::as_integer(*member, &directions); + } + if (slots < 128 || slots > 16384 || directions < 16 || directions > 2048) { + return reference_error("reference configuration exceeds supported bounds"); + } + room_impulse_slots_ = static_cast(slots); + calibration_directions_ = static_cast(directions); + + // The model path is relative to the configuration file unless absolute. + std::string resolved = model_path; + if (!(model_path.size() > 1 && (model_path[1] == ':' || model_path[0] == '/' || + model_path[0] == '\\'))) { + resolved = join_path(directory_of(config_path_), model_path); + } + std::string model_bytes; + if (!read_binary_file(resolved, &model_bytes)) { + return Status::fail(JOC_ERR_HRTF_NOT_FOUND, joc::stage::kRender, + "reference model not found: " + resolved); + } + model_sha256_ = joc::crypto::sha256_hex(model_bytes.data(), model_bytes.size()); + std::string lower_expected = expected_sha; + std::transform(lower_expected.begin(), lower_expected.end(), lower_expected.begin(), + [](unsigned char value) { return static_cast(std::tolower(value)); }); + std::string lower_actual = model_sha256_; + std::transform(lower_actual.begin(), lower_actual.end(), lower_actual.begin(), + [](unsigned char value) { return static_cast(std::tolower(value)); }); + if (lower_actual != lower_expected) { + return reference_error("reference data hash mismatch"); + } + const Status status = hrtf::load_personalized_headphone(resolved, &model_); + if (!status.ok()) { + return status; + } + if (model_.sample_rate != 48000) { + return reference_error("unsupported reference profile"); + } + // Cartesian reference configurations only. + if (model_.header_integer_fields[0] != 0) { + return reference_error("reference-v1 requires a Cartesian mapping"); + } + + mode_name_ = mode_specified + ? (mode_index == kProfileNear ? "near" + : (mode_index == kProfileFar ? "far" : "mid")) + : (calibration_profile_index_ == kProfileFar ? "far" : "mid"); + profile_index_ = mode_specified ? mode_index + : (calibration_profile_index_ == kProfileFar ? kProfileFar + : kProfileMid); + if (profile_index_ != kProfileNear && profile_index_ != kProfileFar && + profile_index_ != kProfileMid) { + return reference_error("reference mode must be near, mid, or far"); + } + + header_constant_ = model_.header_float_scalars[0]; + coupling_ = model_.header_float_scalars[1]; + if (!(header_constant_ > 0.0) || !(coupling_ > 0.0)) { + return reference_error("invalid reference energy coupling"); + } + const hrtf::RosellaDistanceProfile& profile = + model_.profiles[calibration_profile_index_]; + minimum_distance_m_ = static_cast(profile.minimum_normalized_radius) * + static_cast(profile.distance_scale_m); + const double origin[3] = {0.0, 1.0, 0.0}; + ReferencePosition position; + const Status mapped = map_position(origin, &position); + if (!mapped.ok()) { + return mapped; + } + reference_distance_m_ = position.distance_m; + + if (const json::Member* member = json::find(members, "compiled_receiver_positions_m")) { + compiled_receivers_m_ = parse_number_list(member->raw); + if (compiled_receivers_m_.size() != 6u) { + compiled_receivers_m_.clear(); + } + } + return Status::success(); +} + +Status ReferenceEnvironment::map_position(const double position[3], + ReferencePosition* out) const { + return map_position(position, profile_index_, out); +} + +Status ReferenceEnvironment::map_position(const double position[3], int profile_index, + ReferencePosition* out) const { + if (out == nullptr) { + return reference_error("reference position output is null"); + } + for (int axis = 0; axis < 3; ++axis) { + if (!std::isfinite(position[axis])) { + return reference_error("reference position must contain three finite values"); + } + } + double maximum = 0.0; + for (int axis = 0; axis < 3; ++axis) { + maximum = std::max(maximum, std::fabs(position[axis])); + } + const double extension = std::max(1.0, maximum); + if (extension > kMaximumExtension + 1e-9) { + return reference_error("position exceeds the finite metadata domain"); + } + const double scaled[3] = {position[0] / extension, position[1] / extension, + position[2] / extension}; + const int profile = profile_index < 0 ? profile_index_ : profile_index; + hrtf::ProfileGeometry geometry; + Status status = hrtf::profile_geometry(model_, scaled, profile, &geometry); + if (!status.ok()) { + return status; + } + const hrtf::RosellaDistanceProfile& distance_profile = model_.profiles[profile]; + const double scale = static_cast(distance_profile.distance_scale_m); + const double distance = scale * geometry.radius * extension; + const double minimum = + static_cast(distance_profile.minimum_normalized_radius) * scale; + const double regularized = std::max(distance, minimum); + const double alpha = regularized > 0.0 ? distance / regularized : 0.0; + const double effective = header_constant_ * distance; + double gain = 1.0; + double send = 0.0; + if (profile != kProfileNear) { + gain = 1.0 / std::sqrt(1.0 + coupling_ * effective * effective); + send = effective * gain; + } + for (int axis = 0; axis < 3; ++axis) { + out->direction[axis] = geometry.direction[axis]; + } + out->distance_m = distance; + out->ear_distance_m = regularized; + out->centre_weight = alpha; + out->direct_gain = gain; + out->room_send = send; + return Status::success(); +} + +Status ReferenceEnvironment::create_room(std::unique_ptr* out) const { + if (out == nullptr) { + return reference_error("room output is null"); + } + std::unique_ptr room = std::make_unique(); + Status status = room->configure(model_, room_impulse_slots_); + if (!status.ok()) { + return status; + } + *out = std::move(room); + return Status::success(); +} + +std::unique_ptr ReferenceEnvironment::create_analysis_bank( + std::size_t channels) const { + return hrtf::RosellaBank::create(hrtf::RosellaBankKind::Public, channels); +} + +std::unique_ptr ReferenceEnvironment::create_synthesis_bank() const { + return hrtf::RosellaBank::create(hrtf::RosellaBankKind::Public, 2u); +} + +Status ReferenceEnvironment::calibrate(std::size_t fft_size, + const std::vector& frequencies, + const SpectrumAt& spectrum_at, double* level) const { + if (level == nullptr || frequencies.empty()) { + return reference_error("calibration needs a spectrum callback and frequencies"); + } + const std::vector dictionary = + hrtf::hybrid_gain_synthesis_dictionary_for_check(fft_size); + if (dictionary.size() != fft_size * 154u) { + return reference_error("hybrid gain dictionary has the wrong shape"); + } + const std::vector field_left = hrtf::rosella_logical_field(model_.field_left_padded); + const std::vector field_right = hrtf::rosella_logical_field(model_.field_right_padded); + + // Fibonacci sphere: equal-area calibration directions. + const int count = calibration_directions_; + std::vector band_bins; + std::vector weights; + for (std::size_t bin = 0; bin < frequencies.size(); ++bin) { + if (frequencies[bin] >= 100.0 && frequencies[bin] <= 16000.0) { + band_bins.push_back(bin); + weights.push_back(1.0 / frequencies[bin]); + } + } + if (band_bins.empty()) { + return reference_error("calibration band is empty"); + } + const std::size_t bins = frequencies.size(); + double reference_energy = 0.0; + double new_energy = 0.0; + std::vector> reference(2 * bins); + std::vector> target; + std::vector parameters(2 * 154); + for (int index = 0; index < count; ++index) { + const double z = 1.0 - 2.0 * (static_cast(index) + 0.5) / count; + const double phi = static_cast(index) * kPi * (3.0 - std::sqrt(5.0)); + const double radial = std::sqrt(std::max(0.0, 1.0 - z * z)); + const double direction[3] = {-radial * std::sin(phi), radial * std::cos(phi), z}; + const double adm[3] = {-direction[1], direction[0], direction[2]}; + ReferencePosition state; + Status status = map_position(adm, calibration_profile_index_, &state); + if (!status.ok()) { + return status; + } + hrtf::DirectResult result; + status = hrtf::direct_and_room_send(model_, field_left, field_right, adm, + calibration_profile_index_, &result); + if (!status.ok()) { + return status; + } + for (int ear = 0; ear < 2; ++ear) { + for (int band = 0; band < 77; ++band) { + parameters[static_cast(ear * 154 + 2 * band)] = + result.gains[ear][band].real(); + parameters[static_cast(ear * 154 + 2 * band + 1)] = + result.gains[ear][band].imag(); + } + } + for (int ear = 0; ear < 2; ++ear) { + std::vector time(fft_size, 0.0); + for (std::size_t tap = 0; tap < fft_size; ++tap) { + double sum = 0.0; + for (int parameter = 0; parameter < 154; ++parameter) { + sum += dictionary[tap * 154u + static_cast(parameter)] * + parameters[static_cast(ear * 154 + parameter)]; + } + time[tap] = sum; + } + std::vector> buffer(fft_size); + for (std::size_t tap = 0; tap < fft_size; ++tap) { + buffer[tap] = std::complex(time[tap], 0.0); + } + dsp::fft_radix2(&buffer, false); + for (std::size_t bin = 0; bin < bins; ++bin) { + reference[static_cast(ear) * bins + bin] = buffer[bin]; + } + } + status = spectrum_at(state, &target); + if (!status.ok()) { + return status; + } + if (target.size() != 2 * bins) { + return reference_error("calibration spectrum has the wrong shape"); + } + for (std::size_t index_band = 0; index_band < band_bins.size(); ++index_band) { + const std::size_t bin = band_bins[index_band]; + const double weight = weights[index_band]; + for (int ear = 0; ear < 2; ++ear) { + const std::size_t offset = static_cast(ear) * bins + bin; + reference_energy += std::norm(reference[offset]) * weight; + new_energy += + std::norm(target[offset] * state.direct_gain) * weight; + } + } + } + if (!(reference_energy > 1e-24) || !(new_energy > 1e-24) || + !std::isfinite(new_energy)) { + return reference_error("reference level calibration has no finite signal energy"); + } + *level = std::sqrt(reference_energy / new_energy); + return Status::success(); +} + +} // namespace joc::binaural diff --git a/src/binaural/binaural_reference.h b/src/binaural/binaural_reference.h new file mode 100644 index 0000000..e52607e --- /dev/null +++ b/src/binaural/binaural_reference.h @@ -0,0 +1,97 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "foundation/status.h" +#include "hrtf/full_hrir_field.h" +#include "hrtf/rosella_bank.h" +#include "hrtf/rosella_model.h" +#include "hrtf/rosella_reference.h" + +// Configured reference environment for the SOFA renderer, ported from the +// reference binaural_reference.py: the JSON configuration names the personalized +// model that defines the reference reproduction (room, bank and level anchor), +// and every SOFA direction is mapped through that model's distance profile. +namespace joc::binaural { + +// Profile indices, as in the reference rosella_direct module. +inline constexpr int kProfileNear = 1; +inline constexpr int kProfileFar = 2; +inline constexpr int kProfileMid = 3; + +struct ReferencePosition { + double direction[3] = {1.0, 0.0, 0.0}; + double distance_m = 0.0; // physical distance implied by the metadata + double ear_distance_m = 0.0; // distance floored at the near-centre minimum + double centre_weight = 1.0; // direction vs centre blend + double direct_gain = 1.0; + double room_send = 0.0; +}; + +class ReferenceEnvironment { +public: + // Loads `config_path` (JSON) and the model it names, verifying its SHA-256. + // An empty path selects `default_reference_config_path()`. + Status load(const std::string& config_path, int mode_index, bool mode_specified); + + int profile_index() const { return profile_index_; } + int calibration_profile_index() const { return calibration_profile_index_; } + int minimum_profile_index() const { return calibration_profile_index_; } + const std::string& mode_name() const { return mode_name_; } + const hrtf::RosellaModel& model() const { return model_; } + const std::string& profile_id() const { return profile_id_; } + const std::string& model_sha256() const { return model_sha256_; } + const std::string& configuration_sha256() const { return configuration_sha256_; } + double reference_distance_m() const { return reference_distance_m_; } + double minimum_distance_m() const { return minimum_distance_m_; } + int room_impulse_slots() const { return room_impulse_slots_; } + int calibration_directions() const { return calibration_directions_; } + const std::vector& compiled_receivers_m() const { return compiled_receivers_m_; } + + Status map_position(const double position[3], ReferencePosition* out) const; + // `profile_index` selects the distance profile; negative keeps the mode's. + Status map_position(const double position[3], int profile_index, + ReferencePosition* out) const; + + Status create_room(std::unique_ptr* out) const; + + // The public bank is the reference bank; the analysis side carries the source + // channels, the synthesis side the two ears. + std::unique_ptr create_analysis_bank(std::size_t channels) const; + std::unique_ptr create_synthesis_bank() const; + + // Raises the SOFA field to the reference level: one scalar over the equal-area + // calibration directions, shared by all three modes. + using SpectrumAt = std::function>*)>; + Status calibrate(std::size_t fft_size, const std::vector& frequencies, + const SpectrumAt& spectrum_at, double* level) const; + + static std::string default_reference_config_path(); + +private: + hrtf::RosellaModel model_; + std::string config_path_; + std::string profile_id_; + std::string model_sha256_; + std::string configuration_sha256_; + std::string mode_name_; + int calibration_profile_index_ = kProfileMid; + int profile_index_ = kProfileMid; + int room_impulse_slots_ = 4096; + int calibration_directions_ = 128; + double header_constant_ = 1.0; // header_float_scalars[0] + double coupling_ = 0.0; // header_float_scalars[1] + double minimum_distance_m_ = 0.0; + double reference_distance_m_ = 0.0; + std::vector compiled_receivers_m_; // [2][3], when the config supplies them +}; + +} // namespace joc::binaural diff --git a/src/binaural/sofa_reference_runtime.cpp b/src/binaural/sofa_reference_runtime.cpp new file mode 100644 index 0000000..9b3e849 --- /dev/null +++ b/src/binaural/sofa_reference_runtime.cpp @@ -0,0 +1,571 @@ + +#include "binaural/sofa_reference_runtime.h" + +#include +#include +#include +#include + +#include "foundation/fft.h" +#include "hrtf/sofa.h" +#include "hrtf/sofa_field.h" + +namespace joc::binaural { +namespace { + +constexpr double kPi = 3.141592653589793238462643383279502884; + +Status runtime_error(const char* message) { + return Status::fail(JOC_ERR_INVALID_ARGUMENT, joc::stage::kRender, message); +} + +Status render_fail(const char* message) { + return Status::fail(JOC_ERR_HRTF_FORMAT, joc::stage::kDsp, message); +} + +// Inverse real transform: Hermitian half -> n real samples (numpy irfft). +void irfft_to_real(const std::vector>& half, std::size_t count, + std::vector* out) { + std::vector> buffer(count); + const std::size_t bins = count / 2 + 1; + for (std::size_t bin = 0; bin < bins; ++bin) { + buffer[bin] = half[bin]; + } + for (std::size_t bin = 1; bin + 1 < bins; ++bin) { + buffer[count - bin] = std::conj(half[bin]); + } + dsp::fft_radix2(&buffer, true); + out->resize(count); + for (std::size_t index = 0; index < count; ++index) { + (*out)[index] = buffer[index].real(); + } +} + +// numpy.roll along the sample axis. +void roll_forward(const std::vector& source, std::size_t shift, + std::vector* out) { + const std::size_t count = source.size(); + std::vector copy(source); + out->resize(count); + if (count == 0) { + return; + } + const std::size_t offset = shift % count; + for (std::size_t index = 0; index < count; ++index) { + (*out)[index] = copy[(index + count - offset) % count]; + } +} + +// scipy.signal.firwin(129, 150, fs=48000): windowed-sinc, unit DC gain. +std::vector lowpass_150(double sample_rate) { + constexpr int kTaps = 129; + const double cutoff = 150.0 / (sample_rate * 0.5); + std::vector taps(kTaps, 0.0); + double total = 0.0; + for (int index = 0; index < kTaps; ++index) { + const double position = static_cast(index) - 0.5 * (kTaps - 1); + double value = 0.0; + if (std::fabs(position) < 1e-12) { + value = cutoff; + } else { + value = cutoff * std::sin(kPi * cutoff * position) / (kPi * position); + } + const double window = 0.54 - 0.46 * std::cos(2.0 * kPi * index / (kTaps - 1)); + taps[static_cast(index)] = value * window; + total += value * window; + } + for (double& value : taps) { + value /= total; + } + return taps; +} + +void stereo_delay(const std::vector& input, std::size_t delay, std::vector* state, + std::vector* out) { + const std::size_t samples = input.size() / 2; + out->assign(input.size(), 0.0); + const std::size_t capacity = delay * 2; + std::vector joined(capacity + input.size(), 0.0); + for (std::size_t index = 0; index < capacity; ++index) { + joined[index] = (*state)[index]; + } + std::copy(input.begin(), input.end(), joined.begin() + static_cast(capacity)); + for (std::size_t index = 0; index < input.size(); ++index) { + (*out)[index] = joined[index]; + } + state->assign(capacity, 0.0); + for (std::size_t index = 0; index < capacity; ++index) { + (*state)[index] = joined[input.size() + index]; + } +} + +} // namespace + +SofaReferenceRuntime::SofaReferenceRuntime() = default; +SofaReferenceRuntime::~SofaReferenceRuntime() = default; + +void SofaReferenceRuntime::rebuild_latency() { + latency_samples_ = std::max(implementation_latency_, bank_latency_) + + std::max(origin_samples_, 0); + room_delay_samples_ = latency_samples_ - bank_latency_; + direct_delay_samples_ = std::max( + 0, latency_samples_ - implementation_latency_ - std::max(origin_samples_, 0)); + latency_remaining_ = latency_samples_; + room_delay_state_.assign(static_cast(room_delay_samples_) * 2, 0.0); + direct_delay_state_.assign(static_cast(direct_delay_samples_) * 2, 0.0); +} + +Status SofaReferenceRuntime::open(const std::string& sofa_path, + const SofaReferenceOptions& options) { + output_gain_ = options.output_gain; + Status status = environment_.load(options.reference_config, options.mode_index, + options.mode_specified); + if (!status.ok()) { + return status; + } + hrtf::SofaHrir sofa; + status = hrtf::load_sofa(sofa_path, &sofa); + if (!status.ok()) { + return status; + } + hrtf::CanonicalHrtf canonical; + status = hrtf::canonicalize_sofa(sofa, &canonical); + if (!status.ok()) { + return status; + } + hrtf::FullHrirOptions field_options; + field_options.method = options.interpolation; + field_options.has_shell_radius = options.has_shell_radius; + field_options.shell_radius_m = options.shell_radius_m; + status = field_.build(canonical, field_options); + if (!status.ok()) { + return status; + } + fft_size_ = field_.fft_size(); + bins_ = field_.frequency_bins(); + frequencies_ = field_.frequencies(); + implementation_latency_ = field_.common_latency_samples(); + origin_samples_ = field_.recording_origin_samples(); + if (origin_samples_ < 0) { + return runtime_error("negative recording origin requires an explicit time base"); + } + bank_latency_ = static_cast(hrtf::rosella_bank_latency(hrtf::RosellaBankKind::Public)); + if (environment_.minimum_distance_m() <= field_.head_radius_m()) { + return runtime_error("reference near-centre regularization lies inside the head"); + } + rebuild_latency(); + + const auto callback = [this](const ReferencePosition& state, + std::vector>* out) { + return spectrum_at(state, out); + }; + status = environment_.calibrate(fft_size_, frequencies_, callback, &level_calibration_); + if (!status.ok()) { + return status; + } + analysis_bank_ = environment_.create_analysis_bank(kSofaReferenceSources); + synthesis_bank_ = environment_.create_synthesis_bank(); + status = environment_.create_room(&room_); + if (!status.ok()) { + return status; + } + convolver_.configure(kSofaReferenceSources, kSofaReferenceBlock, fft_size_, 2); + hop_block_.assign(kSofaReferenceBlock * kSofaReferenceSources, 0.0); + staging_.assign(kSofaReferenceBatch * kSofaReferenceSources, 0.0); + direct_.assign(kSofaReferenceBlock * 2, 0.0); + qmf_.assign(2 * kSofaReferenceSources * 64, std::complex(0.0, 0.0)); + hybrid_.assign(2 * kSofaReferenceSources * 77, std::complex(0.0, 0.0)); + room_in_.assign(2 * 77, std::complex(0.0, 0.0)); + room_out_.assign(2 * 2 * 77, std::complex(0.0, 0.0)); + synth_.assign(2 * 2 * 64, std::complex(0.0, 0.0)); + synth_time_.assign(2 * 2 * 64, 0.0); + wet_.assign(kSofaReferenceBlock * 2, 0.0); + sends_.assign(kSofaReferenceSources, 0.0); + send_targets_.assign(kSofaReferenceSources, 0.0); + distance_states_.assign(kSofaReferenceSources, ReferencePosition{}); + has_state_.assign(kSofaReferenceSources, 0); + last_position_.assign(kSofaReferenceSources * 3, 0.0); + std::memset(last_key_, 0, sizeof(last_key_)); + for (double& value : last_gain_) { + value = std::numeric_limits::quiet_NaN(); + } + status = build_lfe_filter(); + if (!status.ok()) { + return status; + } + return reset(); +} + +Status SofaReferenceRuntime::build_lfe_filter() { + const std::vector lowpass = lowpass_150(48000.0); + lfe_filter_.assign(2 * fft_size_, 0.0); + const int start = latency_samples_ - 64; + if (start < 0 || static_cast(start) + lowpass.size() > fft_size_) { + return runtime_error("LFE latency exceeds the filter budget"); + } + const double scale = 1.0 / std::sqrt(2.0); + for (int ear = 0; ear < 2; ++ear) { + for (std::size_t tap = 0; tap < lowpass.size(); ++tap) { + lfe_filter_[static_cast(ear) * fft_size_ + + static_cast(start) + tap] = lowpass[tap] * scale; + } + } + return Status::success(); +} + +Status SofaReferenceRuntime::reset() { + for (hrtf::RosellaBank* bank : {analysis_bank_.get(), synthesis_bank_.get()}) { + if (bank != nullptr) { + bank->reset(); + } + } + if (room_) { + room_->reset(); + } + convolver_.reset(); + timeline_.reset(); + output_.clear(); + std::fill(sends_.begin(), sends_.end(), 0.0); + std::fill(send_targets_.begin(), send_targets_.end(), 0.0); + std::fill(has_state_.begin(), has_state_.end(), 0); + for (double& value : last_gain_) { + value = std::numeric_limits::quiet_NaN(); + } + spectrum_cache_.clear(); + pending_room_.clear(); + pending_direct_.clear(); + pending_slots_ = 0; + pending_direct_samples_ = 0; + input_samples_ = 0; + processed_input_samples_ = 0; + parameter_updates_ = 0; + latency_remaining_ = latency_samples_; + std::fill(room_delay_state_.begin(), room_delay_state_.end(), 0.0); + std::fill(direct_delay_state_.begin(), direct_delay_state_.end(), 0.0); + return Status::success(); +} + +Status SofaReferenceRuntime::spectrum_at(const ReferencePosition& state, + std::vector>* out) { + double key[5] = {state.direction[0], state.direction[1], state.direction[2], + state.ear_distance_m, state.centre_weight}; + for (const SpectrumEntry& entry : spectrum_cache_) { + bool match = true; + for (int axis = 0; axis < 5; ++axis) { + if (entry.key[axis] != key[axis]) { + match = false; + break; + } + } + if (match) { + *out = entry.spectrum; + return Status::success(); + } + } + std::vector> interpolated; + Status status = field_.spectrum(state.direction, &interpolated); + if (!status.ok()) { + return status; + } + std::vector> ratio; + status = field_.radial_ratio(state.direction, state.ear_distance_m, &ratio); + if (!status.ok()) { + return status; + } + out->assign(2 * bins_, std::complex(0.0, 0.0)); + for (int ear = 0; ear < 2; ++ear) { + for (std::size_t bin = 0; bin < bins_; ++bin) { + const std::size_t offset = static_cast(ear) * bins_ + bin; + const std::complex radial = + interpolated[offset] * ratio[offset] * state.centre_weight; + (*out)[offset] = + radial + (1.0 - state.centre_weight) * field_.centre_spectrum()[bin]; + } + } + SpectrumEntry entry; + for (int axis = 0; axis < 5; ++axis) { + entry.key[axis] = key[axis]; + } + entry.spectrum = *out; + if (spectrum_cache_.size() >= 128) { + spectrum_cache_.erase(spectrum_cache_.begin()); + } + spectrum_cache_.push_back(std::move(entry)); + return Status::success(); +} + +Status SofaReferenceRuntime::set_source(int source, const double position[3], double gain, + bool special_lfe, bool fade) { + if (source < 0 || source >= kSofaReferenceSources) { + return runtime_error("source index is out of range"); + } + if (!std::isfinite(gain)) { + return runtime_error("source gain must be finite"); + } + const std::size_t slot = static_cast(source); + double key[5] = {position[0], position[1], position[2], gain, + special_lfe ? 1.0 : 0.0}; + bool unchanged = has_state_[slot] != 0; + for (int axis = 0; axis < 5 && unchanged; ++axis) { + unchanged = last_key_[slot][axis] == key[axis]; + } + if (unchanged) { + return Status::success(); + } + for (int axis = 0; axis < 5; ++axis) { + last_key_[slot][axis] = key[axis]; + } + const bool first = processed_input_samples_ == 0; + std::vector filters(2 * fft_size_, 0.0); + if (special_lfe) { + send_targets_[slot] = 0.0; + std::copy(lfe_filter_.begin(), lfe_filter_.end(), filters.begin()); + for (double& value : filters) { + value *= gain; + } + } else { + ReferencePosition state; + Status status = environment_.map_position(position, &state); + if (!status.ok()) { + return status; + } + distance_states_[slot] = state; + send_targets_[slot] = state.room_send * gain; + std::vector> spectrum; + status = spectrum_at(state, &spectrum); + if (!status.ok()) { + return status; + } + const double scale = state.direct_gain * level_calibration_ * gain; + for (std::complex& value : spectrum) { + value *= scale; + } + // One ear at a time: the field returns both ears side by side. + std::vector> ear_spectrum(bins_); + for (int ear = 0; ear < 2; ++ear) { + std::copy(spectrum.begin() + static_cast(ear * bins_), + spectrum.begin() + static_cast((ear + 1) * bins_), + ear_spectrum.begin()); + std::vector impulse; + irfft_to_real(ear_spectrum, fft_size_, &impulse); + roll_forward(impulse, fft_size_ / 2, &impulse); + std::copy(impulse.begin(), impulse.end(), + filters.begin() + static_cast(ear * fft_size_)); + } + } + const std::size_t fade_blocks = (first || !fade) ? 0u : 1u; + const Status status = convolver_.set_filter(static_cast(source), + filters.data(), fft_size_, fade_blocks); + if (!status.ok()) { + return status; + } + has_state_[slot] = 1; + if (first) { + sends_[slot] = send_targets_[slot]; + } + ++parameter_updates_; + return Status::success(); +} + +Status SofaReferenceRuntime::process_block() { + // One 128-sample block is two 64-sample hops of the 16 source channels. + std::vector hops(2 * kSofaReferenceSources * 64, 0.0); + for (std::size_t slot = 0; slot < 2; ++slot) { + for (std::size_t channel = 0; channel < kSofaReferenceSources; ++channel) { + for (std::size_t tap = 0; tap < 64; ++tap) { + hops[(slot * kSofaReferenceSources + channel) * 64 + tap] = + hop_block_[(slot * 64 + tap) * kSofaReferenceSources + channel]; + } + } + } + analysis_bank_->analyze_qmf(hops, 2, &qmf_); + analysis_bank_->analyze_hybrid(qmf_, 2, &hybrid_); + + const double amount[2] = {0.5, 1.0}; + std::fill(room_in_.begin(), room_in_.end(), std::complex(0.0, 0.0)); + for (std::size_t slot = 0; slot < 2; ++slot) { + for (std::size_t source = 0; source < kSofaReferenceSources; ++source) { + const double ramp = sends_[source] + + (send_targets_[source] - sends_[source]) * amount[slot]; + const std::complex* hybrid = + hybrid_.data() + (slot * kSofaReferenceSources + source) * 77; + for (std::size_t band = 0; band < 77; ++band) { + room_in_[slot * 77 + band] += hybrid[band] * ramp; + } + } + } + sends_ = send_targets_; + + const Status status = convolver_.process_block(hop_block_.data(), direct_.data()); + if (!status.ok()) { + return status; + } + pending_room_.insert(pending_room_.end(), room_in_.begin(), room_in_.end()); + pending_direct_.insert(pending_direct_.end(), direct_.begin(), direct_.end()); + pending_slots_ += 2; + pending_direct_samples_ += kSofaReferenceBlock; + processed_input_samples_ += kSofaReferenceBlock; + if (pending_slots_ * 64 < static_cast(kSofaReferenceBatch)) { + return Status::success(); + } + return flush_batch(nullptr); +} + +Status SofaReferenceRuntime::flush_batch(std::vector* /*out*/) { + const std::size_t slots = pending_slots_; + Status status = room_->process_chunk(pending_room_, slots, &room_out_); + if (!status.ok()) { + return status; + } + synthesis_bank_->synthesize_hybrid(room_out_, slots, &synth_); + synth_time_.assign(slots * 2 * 64, 0.0); + synthesis_bank_->synthesize_qmf(synth_, slots, &synth_time_); + const std::size_t samples = slots * 64; + wet_.assign(samples * 2, 0.0); + for (std::size_t sample = 0; sample < samples; ++sample) { + const std::size_t slot = sample / 64; + const std::size_t tap = sample % 64; + for (std::size_t ear = 0; ear < 2; ++ear) { + wet_[sample * 2 + ear] = synth_time_[(slot * 2 + ear) * 64 + tap]; + } + } + std::vector wet_delayed; + std::vector direct_delayed; + stereo_delay(wet_, static_cast(room_delay_samples_), &room_delay_state_, + &wet_delayed); + stereo_delay(pending_direct_, static_cast(direct_delay_samples_), + &direct_delay_state_, &direct_delayed); + std::vector combined(samples * 2, 0.0); + for (std::size_t index = 0; index < combined.size(); ++index) { + combined[index] = wet_delayed[index] + direct_delayed[index]; + } + const std::size_t skip = static_cast( + std::min(static_cast(samples), latency_remaining_)); + latency_remaining_ -= static_cast(skip); + for (std::size_t sample = skip; sample < samples; ++sample) { + for (std::size_t ear = 0; ear < 2; ++ear) { + output_.push_back(combined[sample * 2 + ear] * output_gain_); + } + } + pending_room_.clear(); + pending_direct_.clear(); + pending_slots_ = 0; + pending_direct_samples_ = 0; + return Status::success(); +} + +Status SofaReferenceRuntime::submit_frame(const float* objects16_planar, + const oamd::OamdUpdate* update, + std::int64_t /*frame_index*/, + std::int64_t outer_sample_offset, + std::int64_t object_delay_samples) { + if (objects16_planar == nullptr) { + return runtime_error("frame buffer is null"); + } + for (std::size_t channel = 0; channel < kSofaReferenceSources; ++channel) { + for (std::size_t sample = 0; sample < static_cast(kSofaReferenceBatch); + ++sample) { + staging_[sample * kSofaReferenceSources + channel] = + static_cast(objects16_planar[channel * kSofaReferenceBatch + sample]); + } + } + if (update != nullptr) { + const Status status = timeline_.submit_update(*update, input_samples_, + outer_sample_offset, + object_delay_samples, + processed_input_samples_); + if (!status.ok()) { + return status; + } + } + input_samples_ += kSofaReferenceBatch; + for (std::size_t block = 0; block < 12; ++block) { + double positions[timeline::kTimelineObjects][3] = {}; + Status status = timeline_.positions_at(processed_input_samples_, positions); + if (!status.ok()) { + return status; + } + status = set_source(0, kSofaReferenceLfePosition, 1.0, true, true); + if (!status.ok()) { + return status; + } + for (int object = 0; object < timeline::kTimelineObjects; ++object) { + status = set_source(object + 1, positions[object], 1.0, false, true); + if (!status.ok()) { + return status; + } + } + for (std::size_t sample = 0; sample < static_cast(kSofaReferenceBlock); + ++sample) { + for (std::size_t channel = 0; channel < kSofaReferenceSources; ++channel) { + hop_block_[sample * kSofaReferenceSources + channel] = + staging_[static_cast(block * kSofaReferenceBlock + sample) * + kSofaReferenceSources + channel]; + } + } + status = process_block(); + if (!status.ok()) { + return status; + } + } + return Status::success(); +} + +std::uint32_t SofaReferenceRuntime::finish_capacity(double tail_seconds) const { + const double tail = tail_seconds; + const double samples = tail * 48000.0 + static_cast(latency_samples_) + + static_cast(fft_size_); + const double blocks = std::ceil(samples / 128.0); + return static_cast(std::max(0.0, blocks * 128.0)); +} + +Status SofaReferenceRuntime::finish(std::uint32_t flush_samples, std::vector* out) { + if (out == nullptr) { + return runtime_error("finish output is null"); + } + std::vector tail; + const std::size_t before = output_.size(); + const std::size_t output_samples_before = before; + const std::size_t pending = pending_slots_ * 64; + const std::size_t count = flush_samples; + const double padded_blocks = + std::ceil((static_cast(count + pending)) / + static_cast(kSofaReferenceBatch)); + const std::size_t padded = static_cast( + std::max(0.0, padded_blocks * kSofaReferenceBatch - static_cast(pending))); + const std::int64_t expected = + std::max(0, processed_input_samples_ + static_cast(count) - + latency_samples_ - + static_cast(output_samples_before)); + std::fill(hop_block_.begin(), hop_block_.end(), 0.0); + for (std::size_t block = 0; block < padded / kSofaReferenceBlock; ++block) { + double positions[timeline::kTimelineObjects][3] = {}; + Status status = timeline_.positions_at(processed_input_samples_, positions); + if (!status.ok()) { + return status; + } + status = set_source(0, kSofaReferenceLfePosition, 1.0, true, true); + if (!status.ok()) { + return status; + } + for (int object = 0; object < timeline::kTimelineObjects; ++object) { + status = set_source(object + 1, positions[object], 1.0, false, true); + if (!status.ok()) { + return status; + } + } + status = process_block(); + if (!status.ok()) { + return status; + } + } + tail.assign(output_.begin() + static_cast(before), output_.end()); + if (static_cast(tail.size()) > expected) { + tail.resize(static_cast(expected)); + } + output_.resize(before + tail.size() / 2 * 2); + out->swap(tail); + return Status::success(); +} + +} // namespace joc::binaural diff --git a/src/binaural/sofa_reference_runtime.h b/src/binaural/sofa_reference_runtime.h new file mode 100644 index 0000000..626b5f0 --- /dev/null +++ b/src/binaural/sofa_reference_runtime.h @@ -0,0 +1,155 @@ + +#pragma once + +#include +#include +#include +#include +#include + +#include "binaural/binaural_reference.h" +#include "foundation/status.h" +#include "hrtf/full_hrir_field.h" +#include "hrtf/partitioned_convolution.h" +#include "hrtf/rosella_bank.h" +#include "hrtf/rosella_reference.h" +#include "oamd/oamd_parser.h" +#include "timeline/position_timeline.h" + +// SOFA binaural renderer with a configured reference environment, ported from +// the reference sofa_reference_backend.py: the direct sound is the interpolated +// full HRIR (delay-aligned, partitioned convolution), the room send goes through +// the reference model's room, and the two are summed after their own latency +// compensation. +namespace joc::binaural { + +inline constexpr int kSofaReferenceSources = 16; +inline constexpr int kSofaReferenceBlock = 128; +inline constexpr int kSofaReferenceBatch = 1536; +inline constexpr double kSofaReferenceLfePosition[3] = {0.0, 1.0, 0.0}; + +struct SofaReferenceOptions { + int mode_index = kProfileMid; + bool mode_specified = false; + std::string reference_config; // empty selects the default configuration + hrtf::HrirInterpolation interpolation = hrtf::HrirInterpolation::kLogMagnitudePhase; + bool has_shell_radius = false; + double shell_radius_m = 0.0; + double output_gain = 1.0; + std::int64_t object_delay_samples = 1473; + double tail_seconds = 5.0; +}; + +class SofaReferenceRuntime { +public: + SofaReferenceRuntime(); + ~SofaReferenceRuntime(); + SofaReferenceRuntime(const SofaReferenceRuntime&) = delete; + SofaReferenceRuntime& operator=(const SofaReferenceRuntime&) = delete; + + Status open(const std::string& sofa_path, const SofaReferenceOptions& options); + + // objects16_planar is channel-major: channel * 1536 + sample. + Status submit_frame(const float* objects16_planar, const oamd::OamdUpdate* update, + std::int64_t frame_index, std::int64_t outer_sample_offset, + std::int64_t object_delay_samples); + + Status finish(std::uint32_t flush_samples, std::vector* out); + std::uint32_t finish_capacity(double tail_seconds) const; + Status reset(); + + const std::vector& output() const { return output_; } + void take_output(std::vector* out) { + out->swap(output_); + output_.clear(); + } + + std::uint64_t input_samples() const { return input_samples_; } + std::uint64_t processed_input_samples() const { return processed_input_samples_; } + std::uint64_t parameter_updates() const { return parameter_updates_; } + const timeline::OamdPositionTimeline& timeline() const { return timeline_; } + + void set_fixed_position(int object_index, const double position[3]) { + timeline_.set_fixed_position(object_index, position); + } + + // Reporting surface (mirrors the reference backend's info()). + int latency_samples() const { return latency_samples_; } + int recording_origin_samples() const { return origin_samples_; } + double level_calibration() const { return level_calibration_; } + const ReferenceEnvironment& environment() const { return environment_; } + const hrtf::FullHrirField& field() const { return field_; } + bool fixed_reference() const { return true; } + +private: + Status process_block(); + Status flush_batch(std::vector* out); + Status set_source(int source, const double position[3], double gain, bool special_lfe, + bool fade); + Status spectrum_at(const ReferencePosition& state, + std::vector>* out); + Status build_lfe_filter(); + void rebuild_latency(); + + ReferenceEnvironment environment_; + hrtf::FullHrirField field_; + std::unique_ptr analysis_bank_; + std::unique_ptr synthesis_bank_; + std::unique_ptr room_; + hrtf::MultiSourcePartitionedConvolver convolver_; + + std::vector lfe_filter_; // [2][fft_size] + std::vector sends_; // [sources] + std::vector send_targets_; + std::vector distance_states_; + std::vector has_state_; + double last_gain_[kSofaReferenceSources] = {}; + std::vector last_position_; // [sources][3] + double last_key_[kSofaReferenceSources][5] = {}; + + std::vector> pending_room_; // [slots][77] + std::vector pending_direct_; // [samples][2] + std::size_t pending_slots_ = 0; + std::size_t pending_direct_samples_ = 0; + + std::vector hop_block_; // [block][sources] + std::vector staging_; // [frame][sources] + std::vector output_; + std::vector> qmf_; // [slots][sources][64] + std::vector> hybrid_; // [slots][sources][77] + std::vector> room_in_; // [bins][77] + std::vector> room_out_; // [bins][2][77] + std::vector> synth_; // [bins][2][64] + std::vector synth_time_; // [bins][2][64] + std::vector wet_; // [samples][2] + std::vector direct_; // [samples][2] + + // Reference-position spectrum cache (the reference keeps 128 entries). + struct SpectrumEntry { + double key[5] = {}; + std::vector> spectrum; + }; + std::vector spectrum_cache_; + + timeline::OamdPositionTimeline timeline_; + std::int64_t input_samples_ = 0; + std::int64_t processed_input_samples_ = 0; + std::uint64_t parameter_updates_ = 0; + int origin_samples_ = 0; + int latency_samples_ = 0; + int implementation_latency_ = 0; + int bank_latency_ = 0; + int room_delay_samples_ = 0; + int direct_delay_samples_ = 0; + std::int64_t latency_remaining_ = 0; + double level_calibration_ = 1.0; + double output_gain_ = 1.0; + std::size_t fft_size_ = 0; + std::size_t bins_ = 0; + std::vector frequencies_; + std::vector room_delay_state_; + std::vector direct_delay_state_; + bool empty_block_ = true; +}; + +} // namespace joc::binaural diff --git a/src/cli/joc_cli.cpp b/src/cli/joc_cli.cpp index 98441f1..f3a8035 100644 --- a/src/cli/joc_cli.cpp +++ b/src/cli/joc_cli.cpp @@ -48,7 +48,9 @@ struct Options { bool rosella_filterbank_set = false; std::string hrtf_cache_policy; std::string hrtf_cache_dir; - double hrtf_radius_m = 1.0; + double hrtf_radius_m = 0.0; // 0 selects the single measurement shell + std::string reference_config; + std::string sofa_interpolation; double binaural_tail_seconds = 5.0; double binaural_tail_threshold = 1.0e-8; int binaural_chunk_frames = 64; @@ -250,6 +252,8 @@ void parse_args(const std::vector& arguments, Options* options) { else if (arg == "--hrtf-cache-policy") { options->hrtf_cache_policy = require_value(arguments, &index); } else if (arg == "--hrtf-cache-dir") { options->hrtf_cache_dir = require_value(arguments, &index); } else if (arg == "--hrtf-radius-m") { options->hrtf_radius_m = to_double(require_value(arguments, &index), "--hrtf-radius-m"); } + else if (arg == "--reference-config") { options->reference_config = require_value(arguments, &index); } + else if (arg == "--sofa-interpolation") { options->sofa_interpolation = require_value(arguments, &index); } else if (arg == "--binaural-tail-seconds") { options->binaural_tail_seconds = to_double(require_value(arguments, &index), "--binaural-tail-seconds"); } else if (arg == "--binaural-tail-threshold") { options->binaural_tail_threshold = to_double(require_value(arguments, &index), "--binaural-tail-threshold"); } else if (arg == "--binaural-chunk-frames") { options->binaural_chunk_frames = static_cast(to_int(require_value(arguments, &index), "--binaural-chunk-frames")); } @@ -354,7 +358,10 @@ struct HrtfInput { std::string compiled_path; // or read this .jochrtf directly std::string cache_dir; // disk policy directory std::string personalized_path; // Rosella .personalized_headphone + std::string reference_config; // SOFA reference environment + std::string interpolation; // SOFA local interpolation bool disk = false; + bool compile = false; }; std::string resolve_compiled_hrtf(const Options& options, const std::string& project_directory) { @@ -396,12 +403,12 @@ HrtfInput resolve_hrtf_input(const Options& options, const std::string& project_ if (!options.compiled_hrtf_cache.empty() && !options.hrtf_cache_policy.empty()) { fail("显式 .jochrtf 输入不能再指定 --hrtf-cache-policy"); } - if (!options.compiled_hrtf_cache.empty() && options.hrtf_radius_m != 1.0) { + if (!options.compiled_hrtf_cache.empty() && options.hrtf_radius_m > 0.0) { fail("显式 .jochrtf 输入不能再选择 SOFA radius shell"); } if (options.personalized_headphone_used && (!options.hrtf_cache_policy.empty() || !options.hrtf_cache_dir.empty() || - options.hrtf_radius_m != 1.0)) { + options.hrtf_radius_m > 0.0)) { fail("Rosella 模型输入不能使用 --hrtf-cache-policy/--hrtf-cache-dir/--hrtf-radius-m"); } @@ -434,7 +441,7 @@ HrtfInput resolve_hrtf_input(const Options& options, const std::string& project_ } if (sofa.empty() && compiled.empty()) { if (!options.hrtf_cache_policy.empty() || !options.hrtf_cache_dir.empty() || - options.hrtf_radius_m != 1.0) { + options.hrtf_radius_m > 0.0) { fail("HRTF cache/radius 选项需要 --sofa-hrtf"); } fail("--binaural 未找到 HRTF 输入:默认 " + default_sofa + "、" + default_private + @@ -443,7 +450,7 @@ HrtfInput resolve_hrtf_input(const Options& options, const std::string& project_ "或 --compiled-hrtf-cache PATH 指定"); } if (sofa.empty() && (!options.hrtf_cache_policy.empty() || !options.hrtf_cache_dir.empty() || - options.hrtf_radius_m != 1.0)) { + options.hrtf_radius_m > 0.0)) { fail("HRTF cache/radius 选项需要 --sofa-hrtf"); } if (sofa.empty()) { @@ -452,11 +459,18 @@ HrtfInput resolve_hrtf_input(const Options& options, const std::string& project_ } const std::string effective_policy = options.hrtf_cache_policy.empty() ? "memory" : options.hrtf_cache_policy; + input.compile = !options.hrtf_cache_policy.empty() || !options.hrtf_cache_dir.empty(); if (!options.hrtf_cache_dir.empty() && effective_policy != "disk") { fail("--hrtf-cache-dir 需要 SOFA 与 disk cache policy 一起使用"); } input.sofa_path = sofa; input.disk = effective_policy == "disk"; + input.reference_config = + options.reference_config.empty() + ? fs_utf8::from_path(fs_utf8::to_path(project_directory) / "local" / + "binaural_reference.json") + : options.reference_config; + input.interpolation = options.sofa_interpolation; input.cache_dir = options.hrtf_cache_dir.empty() ? default_cache_dir : options.hrtf_cache_dir; if (!fs_utf8::exists(input.sofa_path)) { fail("SOFA HRTF 不存在: " + input.sofa_path); @@ -564,6 +578,10 @@ int main(int argc, char** argv) { {"ask", "continue", "float32", "abort"}); check_choice(options.binaural_mode, "--binaural-mode", {"off", "near", "mid", "far"}); check_choice(options.trajectory_mode, "--trajectory-mode", {"compact", "dense64"}); + if (!options.sofa_interpolation.empty()) { + check_choice(options.sofa_interpolation, "--sofa-interpolation", + {"lf_logmag_phase", "lf_mca_zero"}); + } check_choice(options.backend, "--backend", {"auto", "native", "python"}); check_choice(options.print_metadata, "--print-metadata", {"none", "summary", "frames"}); check_choice(options.metadata_backend, "--metadata-backend", {"auto", "emdf", "sidecar"}); @@ -605,7 +623,7 @@ int main(int argc, char** argv) { const bool hrtf_options_used = !options.sofa_hrtf.empty() || !options.compiled_hrtf_cache.empty() || options.personalized_headphone_used || !options.hrtf_cache_policy.empty() || - !options.hrtf_cache_dir.empty() || options.hrtf_radius_m != 1.0; + !options.hrtf_cache_dir.empty() || options.hrtf_radius_m > 0.0; if (hrtf_options_used && !binaural_mode) { fail("SOFA/HRTF 选项仅与 --binaural 一起使用"); } @@ -623,7 +641,7 @@ int main(int argc, char** argv) { if (options.binaural_chunk_frames <= 0) { fail("binaural-chunk-frames 必须大于 0"); } - if (!std::isfinite(options.hrtf_radius_m) || options.hrtf_radius_m <= 0.0) { + if (!std::isfinite(options.hrtf_radius_m) || options.hrtf_radius_m < 0.0) { fail("hrtf-radius-m 必须是正有限值"); } if (options.duration_set && options.duration <= 0.0) { @@ -718,12 +736,18 @@ int main(int argc, char** argv) { std::string hrtf_cache_dir; std::string personalized_path; std::string kernels_path; + std::string hrtf_reference_config; + std::string hrtf_interpolation; + bool hrtf_compile = false; if (binaural_mode) { try { const HrtfInput input = resolve_hrtf_input(options, project_directory); hrtf_path = input.compiled_path; hrtf_sofa_path = input.sofa_path; hrtf_cache_dir = input.cache_dir; + hrtf_reference_config = input.reference_config; + hrtf_interpolation = input.interpolation; + hrtf_compile = input.compile; personalized_path = input.personalized_path; kernels_path = find_kernels(options, arguments.empty() ? std::string() : arguments.front()); @@ -745,6 +769,12 @@ int main(int argc, char** argv) { : options.hrtf_cache_policy == "none" ? JOC_HRTF_CACHE_NONE : JOC_HRTF_CACHE_MEMORY; config.hrtf_radius_m = options.hrtf_radius_m; + config.reference_config_path = + hrtf_reference_config.empty() ? nullptr : hrtf_reference_config.c_str(); + config.sofa_interpolation = hrtf_interpolation == "lf_mca_zero" + ? JOC_SOFA_INTERPOLATION_MCA_ZERO + : JOC_SOFA_INTERPOLATION_LOGMAG_PHASE; + config.hrtf_compile = hrtf_compile ? 1u : 0u; config.kernels_path = kernels_path.empty() ? nullptr : kernels_path.c_str(); joc_validation_issue issues[32]; diff --git a/src/hrtf/full_hrir_field.cpp b/src/hrtf/full_hrir_field.cpp new file mode 100644 index 0000000..fd3421c --- /dev/null +++ b/src/hrtf/full_hrir_field.cpp @@ -0,0 +1,774 @@ + +#include "hrtf/full_hrir_field.h" + +#include +#include +#include +#include +#include + +#include "foundation/fft.h" +#include "hrtf/sofa_field.h" + +namespace joc::hrtf { +namespace { + +constexpr double kPi = 3.141592653589793238462643383279502884; +constexpr double kSoundSpeed = 343.3; +constexpr double kScanTolerance = 1.0e-10; +constexpr int kMcaBands = 41; + +Status field_error(const char* message) { + return Status::fail(JOC_ERR_HRTF_FORMAT, joc::stage::kRender, message); +} + +Status argument_error(const char* message) { + return Status::fail(JOC_ERR_INVALID_ARGUMENT, joc::stage::kRender, message); +} + +// Unnormalised forward transform of a real block, first n/2+1 bins (numpy rfft). +void real_fft(const double* time, std::size_t count, std::vector>* out) { + std::vector> buffer(count); + for (std::size_t index = 0; index < count; ++index) { + buffer[index] = std::complex(time[index], 0.0); + } + dsp::fft_radix2(&buffer, false); + out->assign(buffer.begin(), buffer.begin() + static_cast(count / 2 + 1)); +} + +// In-place phase unwrapping (numpy.unwrap with the default 2*pi period). +void unwrap_phase(const std::vector& wrapped, std::vector* out) { + out->assign(wrapped.size(), 0.0); + if (wrapped.empty()) { + return; + } + (*out)[0] = wrapped[0]; + for (std::size_t index = 1; index < wrapped.size(); ++index) { + double delta = wrapped[index] - wrapped[index - 1]; + delta -= 2.0 * kPi * std::round(delta / (2.0 * kPi)); + (*out)[index] = (*out)[index - 1] + delta; + } +} + +double median(std::vector values) { + if (values.empty()) { + return 0.0; + } + const std::size_t middle = values.size() / 2; + std::nth_element(values.begin(), values.begin() + static_cast(middle), + values.end()); + if (values.size() % 2 != 0) { + return values[middle]; + } + const double upper = values[middle]; + std::nth_element(values.begin(), values.begin() + static_cast(middle - 1), + values.end()); + return 0.5 * (values[middle - 1] + upper); +} + +// scipy.signal._cubic edge/interior slopes of PchipInterpolator. +double pchip_edge(double h0, double h1, double m0, double m1) { + double derivative = ((2.0 * h0 + h1) * m0 - h0 * m1) / (h0 + h1); + const bool opposite = std::signbit(derivative) != std::signbit(m0); + const bool clamp = (std::signbit(m0) != std::signbit(m1)) && + (std::fabs(derivative) > 3.0 * std::fabs(m0)); + if (opposite) { + derivative = 0.0; + } else if (clamp) { + derivative = 3.0 * m0; + } + return derivative; +} + +class PchipEvaluator { +public: + void build(const std::vector& x, const std::vector& y) { + x_ = x; + y_ = y; + const std::size_t count = x.size(); + d_.assign(count, 0.0); + std::vector h(count - 1); + std::vector delta(count - 1); + for (std::size_t index = 0; index + 1 < count; ++index) { + h[index] = x[index + 1] - x[index]; + delta[index] = h[index] != 0.0 ? (y[index + 1] - y[index]) / h[index] : 0.0; + } + if (count == 2) { + d_[0] = delta[0]; + d_[1] = delta[0]; + return; + } + d_[0] = pchip_edge(h[0], h[1], delta[0], delta[1]); + d_[count - 1] = pchip_edge(h[count - 2], h[count - 3], delta[count - 2], delta[count - 3]); + for (std::size_t index = 1; index + 1 < count; ++index) { + if (delta[index - 1] * delta[index] <= 0.0) { + d_[index] = 0.0; + continue; + } + const double w1 = 2.0 * h[index] + h[index - 1]; + const double w2 = h[index] + 2.0 * h[index - 1]; + d_[index] = (w1 + w2) / (w1 / delta[index - 1] + w2 / delta[index]); + } + } + + double evaluate(double value) const { + const std::size_t count = x_.size(); + if (count < 2) { + return count == 1 ? y_[0] : 0.0; + } + std::size_t index = 0; + if (value >= x_.back()) { + index = count - 2; + } else if (value > x_.front()) { + index = static_cast( + std::upper_bound(x_.begin(), x_.end(), value) - x_.begin()) - 1; + } + const double h = x_[index + 1] - x_[index]; + if (h == 0.0) { + return y_[index]; + } + const double t = (value - x_[index]) / h; + const double t2 = t * t; + const double t3 = t2 * t; + return (2.0 * t3 - 3.0 * t2 + 1.0) * y_[index] + + (t3 - 2.0 * t2 + t) * h * d_[index] + + (-2.0 * t3 + 3.0 * t2) * y_[index + 1] + + (t3 - t2) * h * d_[index + 1]; + } + +private: + std::vector x_; + std::vector y_; + std::vector d_; +}; + +} // namespace + +Status sphere_transfer(const std::vector& frequencies, double radius, double head_radius, + const std::vector& cosines, + std::vector>* out) { + if (out == nullptr || !(radius > head_radius) || head_radius <= 0.0) { + return argument_error("sphere transfer needs a source outside the head"); + } + const std::size_t bands = frequencies.size(); + out->assign(cosines.size() * bands, std::complex(0.0, 0.0)); + std::vector> x(bands); + std::vector> z(bands); + std::vector> ratio_a(bands); + std::vector> ratio_r(bands); + std::vector> product(bands, std::complex(1.0, 0.0)); + double maximum_x = 0.0; + for (std::size_t band = 0; band < bands; ++band) { + const double wave = 2.0 * kPi * frequencies[band] * head_radius / kSoundSpeed; + maximum_x = std::max(maximum_x, wave); + x[band] = std::complex(wave, 0.0); + z[band] = std::complex(wave * radius / head_radius, 0.0); + ratio_a[band] = x[band] / (1.0 + std::complex(0.0, 1.0) * x[band]); + ratio_r[band] = z[band] / (1.0 + std::complex(0.0, 1.0) * z[band]); + } + const double geometric = std::ceil(std::log(kScanTolerance) / std::log(head_radius / radius)) + 16.0; + const int order = static_cast(std::max(std::ceil(maximum_x) + 32.0, geometric)); + if (order > 4096) { + return argument_error("source too close to the sphere surface"); + } + std::vector legendre_previous(cosines.size(), 1.0); + std::vector legendre(cosines.size(), 0.0); + std::vector mu(cosines.size()); + for (std::size_t row = 0; row < cosines.size(); ++row) { + mu[row] = std::clamp(cosines[row], -1.0, 1.0); + legendre[row] = mu[row]; + for (std::size_t band = 0; band < bands; ++band) { + const std::complex value = 1.0 / (1.0 + std::complex(0.0, 1.0) * x[band]); + (*out)[row * bands + band] = value; + } + } + std::vector> term(bands); + for (int degree = 1; degree <= order; ++degree) { + for (std::size_t band = 0; band < bands; ++band) { + const std::complex quotient = + std::abs(z[band]) != 0.0 ? ratio_a[band] / ratio_r[band] + : std::complex(head_radius / radius, 0.0); + product[band] *= quotient; + term[band] = product[band] / + ((static_cast(degree) + 1.0) - + x[band] * ratio_a[band]); + } + for (std::size_t row = 0; row < cosines.size(); ++row) { + const double factor = (2.0 * static_cast(degree) + 1.0) * legendre[row]; + for (std::size_t band = 0; band < bands; ++band) { + (*out)[row * bands + band] += factor * term[band]; + } + } + // Legendre recurrence P_{n+1} = ((2n+1) mu P_n - n P_{n-1}) / (n+1). + for (std::size_t row = 0; row < cosines.size(); ++row) { + const double next = + ((2.0 * static_cast(degree) + 1.0) * mu[row] * legendre[row] - + static_cast(degree) * legendre_previous[row]) / + (static_cast(degree) + 1.0); + legendre_previous[row] = legendre[row]; + legendre[row] = next; + } + for (std::size_t band = 0; band < bands; ++band) { + const double denominator_a = + (2.0 * static_cast(degree) + 1.0); + ratio_a[band] = x[band] / (denominator_a - x[band] * ratio_a[band]); + ratio_r[band] = z[band] / (denominator_a - z[band] * ratio_r[band]); + } + } + for (std::size_t band = 0; band < bands; ++band) { + const std::complex phase = std::exp(std::complex(0.0, 1.0) * x[band]); + for (std::size_t row = 0; row < cosines.size(); ++row) { + (*out)[row * bands + band] *= phase; + } + } + for (const std::complex& value : *out) { + if (!std::isfinite(value.real()) || !std::isfinite(value.imag())) { + return field_error("sphere series failed to converge"); + } + } + return Status::success(); +} + +Status FullHrirField::build(const CanonicalHrtf& canonical, const FullHrirOptions& options) { + method_ = options.method; + if (std::fabs(canonical.sample_rate_hz - 48000.0) > 1e-9) { + return field_error("full HRIR renderer requires 48000 Hz"); + } + if (canonical.measurements == 0 || canonical.taps == 0) { + return field_error("canonical HRIR set is empty"); + } + if (canonical.hrir.size() < static_cast(canonical.measurements) * 2u * + canonical.taps || + canonical.delay_samples.size() < static_cast(canonical.measurements) * 2u) { + return field_error("canonical HRIR set is truncated"); + } + + // Shell selection: an explicit radius wins, otherwise a single shell is required. + std::vector shells(canonical.measurement_radius_m.begin(), + canonical.measurement_radius_m.end()); + std::sort(shells.begin(), shells.end()); + shells.erase(std::unique(shells.begin(), shells.end(), + [](double a, double b) { return std::fabs(a - b) < 1e-9; }), + shells.end()); + if (!options.has_shell_radius && shells.size() != 1) { + return field_error("multi-radius SOFA requires an explicit shell radius"); + } + if (options.has_shell_radius && (!std::isfinite(options.shell_radius_m) || + options.shell_radius_m <= 0.0)) { + return field_error("shell radius must be finite and positive"); + } + const double requested = options.has_shell_radius ? options.shell_radius_m : shells[0]; + double actual_radius = 0.0; + const std::vector selected = + canonical_shell_indices(canonical, requested, &actual_radius); + + // Every selected ear must carry energy; delays size the FFT budget. + double maximum_delay = 0.0; + for (const std::size_t row : selected) { + for (int ear = 0; ear < 2; ++ear) { + double energy = 0.0; + const double* taps = canonical.hrir.data() + + (row * 2u + static_cast(ear)) * canonical.taps; + for (std::uint32_t tap = 0; tap < canonical.taps; ++tap) { + energy += taps[tap] * taps[tap]; + } + if (!(energy > 0.0)) { + return field_error("each selected HRIR ear must contain signal energy"); + } + maximum_delay = std::max(maximum_delay, + canonical.delay_samples[row * 2u + + static_cast(ear)]); + } + } + taps_ = canonical.taps; + std::vector radii; + radii.reserve(selected.size()); + for (const std::size_t row : selected) { + radii.push_back(canonical.measurement_radius_m[row]); + } + measurement_radius_m_ = median(radii); + const double support = static_cast(canonical.taps) + + std::ceil(maximum_delay) + 1.0; + std::size_t fft_size = options.fft_size; + if (fft_size == 0) { + fft_size = 2048; + while (fft_size < static_cast(2.0 * support - 1.0)) { + fft_size <<= 1u; + } + if (fft_size > 16384) { + return field_error("HRIR and Data.Delay exceed the automatic FFT budget"); + } + } + if (fft_size < 64 || fft_size % 2 != 0 || + static_cast(canonical.taps) + maximum_delay >= + static_cast(fft_size / 2)) { + return field_error("HRIR and Data.Delay exceed the selected FFT time budget"); + } + fft_size_ = fft_size; + bins_ = fft_size / 2 + 1; + + // Receiver geometry: a fixed pair, centred on the listener, one radius. + if (canonical.receiver_position_cartesian_m.size() < 6u) { + return field_error("ReceiverPosition is missing"); + } + const double* left = canonical.receiver_position_cartesian_m.data(); + const double* right = left + 3; + const double mean[3] = {(left[0] + right[0]) * 0.5, (left[1] + right[1]) * 0.5, + (left[2] + right[2]) * 0.5}; + if (std::sqrt(mean[0] * mean[0] + mean[1] * mean[1] + mean[2] * mean[2]) > 1e-6) { + return field_error("radial model requires ears centred on the listener"); + } + head_radius_m_ = std::sqrt(left[0] * left[0] + left[1] * left[1] + left[2] * left[2]); + const double right_radius = std::sqrt(right[0] * right[0] + right[1] * right[1] + + right[2] * right[2]); + if (!(head_radius_m_ > 0.04 && head_radius_m_ < 0.15) || + std::fabs(right_radius - head_radius_m_) > 1e-6) { + return field_error("unsupported receiver geometry for the radial model"); + } + ear_directions_.assign(6, 0.0); + for (int axis = 0; axis < 3; ++axis) { + ear_directions_[static_cast(axis)] = left[axis] / head_radius_m_; + ear_directions_[3u + static_cast(axis)] = right[axis] / head_radius_m_; + } + + // Frequencies and phase-fit grid. + frequencies_.resize(bins_); + omega_.resize(bins_); + for (std::size_t bin = 0; bin < bins_; ++bin) { + frequencies_[bin] = static_cast(bin) * 48000.0 / static_cast(fft_size_); + omega_[bin] = 2.0 * kPi * frequencies_[bin] / 48000.0; + } + std::size_t fit_begin = 0; + std::size_t fit_end = 0; + double omega_mean = 0.0; + for (std::size_t bin = 0; bin < bins_; ++bin) { + if (frequencies_[bin] >= 200.0 && frequencies_[bin] <= 1500.0) { + if (fit_end == fit_begin) { + fit_begin = bin; + } + fit_end = bin + 1; + } + } + if (fit_end <= fit_begin) { + return field_error("the phase-fit band is empty"); + } + for (std::size_t bin = fit_begin; bin < fit_end; ++bin) { + omega_mean += omega_[bin]; + } + omega_mean /= static_cast(fit_end - fit_begin); + double x_squares = 0.0; + std::vector x_fit(fit_end - fit_begin); + for (std::size_t bin = fit_begin; bin < fit_end; ++bin) { + x_fit[bin - fit_begin] = omega_[bin] - omega_mean; + x_squares += x_fit[bin - fit_begin] * x_fit[bin - fit_begin]; + } + + // Raw spectra, then delay estimation and alignment. + const std::size_t rows = selected.size(); + std::vector> raw(rows * 2 * bins_); + for (std::size_t row = 0; row < rows; ++row) { + for (int ear = 0; ear < 2; ++ear) { + const double* taps = canonical.hrir.data() + + (selected[row] * 2u + static_cast(ear)) * + canonical.taps; + std::vector padded(fft_size_, 0.0); + std::memcpy(padded.data(), taps, sizeof(double) * canonical.taps); + std::vector> spectrum; + real_fft(padded.data(), fft_size_, &spectrum); + std::copy(spectrum.begin(), spectrum.end(), + raw.begin() + static_cast((row * 2u + + static_cast(ear)) * + bins_)); + } + } + delays_.assign(rows * 2, 0.0); + std::vector residuals; + std::vector wrapped(bins_); + std::vector unwrapped; + for (std::size_t row = 0; row < rows; ++row) { + for (int ear = 0; ear < 2; ++ear) { + const std::complex* spectrum = + raw.data() + (row * 2u + static_cast(ear)) * bins_; + for (std::size_t bin = 0; bin < bins_; ++bin) { + wrapped[bin] = std::arg(spectrum[bin]); + } + unwrap_phase(wrapped, &unwrapped); + double numerator = 0.0; + for (std::size_t bin = fit_begin; bin < fit_end; ++bin) { + numerator += x_fit[bin - fit_begin] * unwrapped[bin]; + } + const double data_delay = + canonical.delay_samples[selected[row] * 2u + static_cast(ear)]; + delays_[row * 2u + static_cast(ear)] = + -numerator / x_squares + data_delay; + } + } + // Residual phase spread of the estimate (reported, not used downstream). + for (std::size_t row = 0; row < rows; ++row) { + for (int ear = 0; ear < 2; ++ear) { + const std::complex* spectrum = + raw.data() + (row * 2u + static_cast(ear)) * bins_; + for (std::size_t bin = 0; bin < bins_; ++bin) { + wrapped[bin] = std::arg(spectrum[bin]); + } + unwrap_phase(wrapped, &unwrapped); + const double data_delay = + canonical.delay_samples[selected[row] * 2u + static_cast(ear)]; + const double delay = delays_[row * 2u + static_cast(ear)]; + double mean = 0.0; + std::vector residual(fit_end - fit_begin); + for (std::size_t bin = fit_begin; bin < fit_end; ++bin) { + residual[bin - fit_begin] = unwrapped[bin] + (delay - data_delay) * omega_[bin]; + mean += residual[bin - fit_begin]; + } + mean /= static_cast(residual.size()); + double squares = 0.0; + for (double& value : residual) { + value -= mean; + squares += value * value; + } + residuals.push_back(std::sqrt(squares / static_cast(residual.size()))); + } + } + std::sort(residuals.begin(), residuals.end()); + phase_fit_rms_rad_p95_ = residuals.empty() + ? 0.0 + : residuals[static_cast( + std::floor(0.95 * static_cast(residuals.size())))]; + + // Recording origin: the median mean delay over the measurements. + std::vector row_means(rows, 0.0); + for (std::size_t row = 0; row < rows; ++row) { + row_means[row] = 0.5 * (delays_[row * 2u] + delays_[row * 2u + 1u]); + } + recording_origin_samples_ = static_cast(std::llround(median(row_means))); + + // Physical spectra (Data.Delay removed) and the alignment delay. + std::vector> physical(raw.size()); + double maximum = 0.0; + for (std::size_t row = 0; row < rows; ++row) { + for (int ear = 0; ear < 2; ++ear) { + const std::size_t base = (row * 2u + static_cast(ear)) * bins_; + const double data_delay = + canonical.delay_samples[selected[row] * 2u + static_cast(ear)]; + for (std::size_t bin = 0; bin < bins_; ++bin) { + const std::complex rotation = + std::exp(std::complex(0.0, -1.0) * omega_[bin] * data_delay); + physical[base + bin] = raw[base + bin] * rotation; + maximum = std::max(maximum, std::abs(physical[base + bin])); + } + } + } + floor_ = std::max(maximum * 1e-6, 1e-15); + + // Directions: deduplicate coincident measurements, keeping the first, then the + // mesh over that set. + std::vector directions; + directions.reserve(rows * 3); + std::vector kept_rows; + for (std::size_t row = 0; row < rows; ++row) { + const double* unit = canonical.unit_directions.data() + selected[row] * 3u; + bool merged = false; + for (std::size_t existing = 0; existing < kept_rows.size(); ++existing) { + const double* other = canonical.unit_directions.data() + kept_rows[existing] * 3u; + if (std::round(unit[0] * 1e10) == std::round(other[0] * 1e10) && + std::round(unit[1] * 1e10) == std::round(other[1] * 1e10) && + std::round(unit[2] * 1e10) == std::round(other[2] * 1e10)) { + // Coincident measurements must agree, as the reference requires. + for (std::size_t tap = 0; tap < canonical.taps * 2u; ++tap) { + const double a = canonical.hrir[selected[row] * 2u * canonical.taps + tap]; + const double b = canonical.hrir[kept_rows[existing] * 2u * canonical.taps + tap]; + if (std::fabs(a - b) > 1e-10 + 1e-8 * std::fabs(b)) { + return field_error("coincident SOFA measurements disagree"); + } + } + merged = true; + break; + } + } + if (merged) { + continue; + } + kept_rows.push_back(selected[row]); + directions.push_back(unit[0]); + directions.push_back(unit[1]); + directions.push_back(unit[2]); + } + const Status mesh_status = mesh_.build(directions, kept_rows.size()); + if (!mesh_status.ok()) { + return mesh_status; + } + // The measurements must enclose the listener: every hull face must sit in + // front of the unit sphere's centre. + for (std::size_t face = 0; face < mesh_.face_count(); ++face) { + const int* ids = mesh_.faces().data() + face * 3; + const double* a = mesh_.directions().data() + ids[0] * 3; + const double* b = mesh_.directions().data() + ids[1] * 3; + const double* c = mesh_.directions().data() + ids[2] * 3; + double one[3]; + double two[3]; + for (int axis = 0; axis < 3; ++axis) { + one[axis] = b[axis] - a[axis]; + two[axis] = c[axis] - a[axis]; + } + double normal[3] = {one[1] * two[2] - one[2] * two[1], + one[2] * two[0] - one[0] * two[2], + one[0] * two[1] - one[1] * two[0]}; + const double length = std::sqrt(normal[0] * normal[0] + normal[1] * normal[1] + + normal[2] * normal[2]); + if (!(length > 0.0)) { + return field_error("ill-conditioned spherical interpolation mesh"); + } + for (double& value : normal) { + value /= length; + } + const double distance = normal[0] * a[0] + normal[1] * a[1] + normal[2] * a[2]; + if (std::fabs(distance) < 1e-9) { + return field_error("SOFA must enclose the listener"); + } + if (distance < 0.0) { + return field_error("ill-conditioned spherical interpolation mesh"); + } + } + + // Centre spectrum from the spherical Voronoi weights. + std::vector areas; + mesh_.voronoi_areas(&areas); + std::vector energy(bins_, 0.0); + for (std::size_t row = 0; row < rows; ++row) { + const double weight = row < areas.size() ? areas[row] : 0.0; + for (int ear = 0; ear < 2; ++ear) { + const std::size_t base = (row * 2u + static_cast(ear)) * bins_; + for (std::size_t bin = 0; bin < bins_; ++bin) { + energy[bin] += weight * std::norm(physical[base + bin]); + } + } + } + centre_spectrum_.assign(bins_, std::complex(0.0, 0.0)); + for (std::size_t bin = 0; bin < bins_; ++bin) { + const double magnitude = std::sqrt(energy[bin] * 0.5); + centre_spectrum_[bin] = magnitude * std::exp(std::complex( + 0.0, -omega_[bin] * + static_cast( + recording_origin_samples_))); + } + + // Aligned spectra and the magnitude representation used by the interpolation. + aligned_.assign(rows * 2 * bins_, std::complex(0.0, 0.0)); + log_magnitude_.assign(rows * 2 * bins_, 0.0); + for (std::size_t row = 0; row < rows; ++row) { + for (int ear = 0; ear < 2; ++ear) { + const std::size_t index = row * 2u + static_cast(ear); + const double delay = delays_[index]; + for (std::size_t bin = 0; bin < bins_; ++bin) { + aligned_[index * bins_ + bin] = + physical[index * bins_ + bin] * + std::exp(std::complex(0.0, omega_[bin] * delay)); + log_magnitude_[index * bins_ + bin] = + std::log(std::max(std::abs(physical[index * bins_ + bin]), floor_)); + } + } + } + + if (method_ == HrirInterpolation::kMcaZero) { + // 41 auditory bands (reference: erb-spaced centres, rounded exponential bank). + centres_hz_.resize(kMcaBands); + const auto erb = [](double frequency) { + return 21.4 * std::log10(1.0 + 0.00437 * frequency); + }; + const double low = erb(50.0); + const double high = erb(20000.0); + for (int band = 0; band < kMcaBands; ++band) { + const double position = low + (high - low) * band / (kMcaBands - 1); + centres_hz_[static_cast(band)] = + (std::pow(10.0, position / 21.4) - 1.0) / 0.00437; + } + bank_.assign(kMcaBands * bins_, 0.0); + for (int band = 0; band < kMcaBands; ++band) { + const double centre = centres_hz_[static_cast(band)]; + const double width = 1.019 * 24.7 * (1.0 + 0.00437 * centre); + double total = 0.0; + for (std::size_t bin = 0; bin < bins_; ++bin) { + const double ratio = (frequencies_[bin] - centre) / width; + double value = std::pow(1.0 + ratio * ratio, -4.0); + if (bin != 0 && bin + 1 != bins_) { + value *= 2.0; + } + bank_[static_cast(band) * bins_ + bin] = value; + total += value; + } + for (std::size_t bin = 0; bin < bins_; ++bin) { + bank_[static_cast(band) * bins_ + bin] /= total; + } + } + auditory_.assign(rows * 2 * kMcaBands, 0.0); + std::vector power(bins_, 0.0); + for (std::size_t row = 0; row < rows; ++row) { + for (int ear = 0; ear < 2; ++ear) { + const std::size_t index = row * 2u + static_cast(ear); + for (std::size_t bin = 0; bin < bins_; ++bin) { + power[bin] = std::norm(physical[index * bins_ + bin]); + } + for (int band = 0; band < kMcaBands; ++band) { + double sum = 0.0; + for (std::size_t bin = 0; bin < bins_; ++bin) { + sum += power[bin] * + bank_[static_cast(band) * bins_ + bin]; + } + auditory_[index * kMcaBands + static_cast(band)] = + 10.0 * std::log10(std::max(sum, 1e-24)); + } + } + } + } + return Status::success(); +} + +Status FullHrirField::radial_ratio(const double direction[3], double radius, + std::vector>* out) const { + if (out == nullptr) { + return argument_error("radial ratio output is null"); + } + if (std::fabs(radius - measurement_radius_m_) < 1e-10) { + out->assign(2 * bins_, std::complex(1.0, 0.0)); + return Status::success(); + } + if (!(radius > 0.0) || !std::isfinite(radius)) { + return argument_error("radial ratio needs a positive finite radius"); + } + std::vector cosines(2, 0.0); + for (int ear = 0; ear < 2; ++ear) { + cosines[static_cast(ear)] = + ear_directions_[static_cast(ear) * 3u] * direction[0] + + ear_directions_[static_cast(ear) * 3u + 1u] * direction[1] + + ear_directions_[static_cast(ear) * 3u + 2u] * direction[2]; + } + std::vector> source; + std::vector> target; + Status status = sphere_transfer(frequencies_, measurement_radius_m_, head_radius_m_, cosines, + &source); + if (!status.ok()) { + return status; + } + status = sphere_transfer(frequencies_, radius, head_radius_m_, cosines, &target); + if (!status.ok()) { + return status; + } + for (const std::complex& value : source) { + if (std::abs(value) < 1e-10) { + return field_error("singular radial reference response"); + } + } + out->resize(source.size()); + for (std::size_t index = 0; index < source.size(); ++index) { + (*out)[index] = target[index] / source[index]; + } + return Status::success(); +} + +Status FullHrirField::spectrum(const double direction[3], + std::vector>* out) { + if (out == nullptr) { + return argument_error("spectrum output is null"); + } + std::vector ids; + std::vector weights; + const Status status = mesh_.weights(direction, &ids, &weights); + if (!status.ok()) { + return status; + } + if (ids.empty()) { + return field_error("no interpolation weights for the direction"); + } + double delay[2] = {0.0, 0.0}; + for (std::size_t index = 0; index < ids.size(); ++index) { + const std::size_t id = static_cast(ids[index]); + for (int ear = 0; ear < 2; ++ear) { + delay[ear] += weights[index] * delays_[id * 2u + static_cast(ear)]; + } + } + out->assign(2 * bins_, std::complex(0.0, 0.0)); + for (std::size_t index = 0; index < ids.size(); ++index) { + const std::size_t id = static_cast(ids[index]); + for (int ear = 0; ear < 2; ++ear) { + const std::complex* aligned = + aligned_.data() + (id * 2u + static_cast(ear)) * bins_; + for (std::size_t bin = 0; bin < bins_; ++bin) { + (*out)[static_cast(ear) * bins_ + bin] += + weights[index] * aligned[bin]; + } + } + } + for (int ear = 0; ear < 2; ++ear) { + for (std::size_t bin = 0; bin < bins_; ++bin) { + (*out)[static_cast(ear) * bins_ + bin] *= + std::exp(std::complex(0.0, -omega_[bin] * + delay[static_cast(ear)])); + } + } + if (ids.size() > 1) { + if (method_ == HrirInterpolation::kLogMagnitudePhase) { + for (int ear = 0; ear < 2; ++ear) { + std::vector magnitude(bins_, 0.0); + for (std::size_t index = 0; index < ids.size(); ++index) { + const std::size_t id = static_cast(ids[index]); + const double* values = + log_magnitude_.data() + + (id * 2u + static_cast(ear)) * bins_; + for (std::size_t bin = 0; bin < bins_; ++bin) { + magnitude[bin] += weights[index] * values[bin]; + } + } + for (std::size_t bin = 0; bin < bins_; ++bin) { + std::complex& value = + (*out)[static_cast(ear) * bins_ + bin]; + const double target = std::exp(magnitude[bin]); + value = target * value / + std::max(std::abs(value), floor_); + } + } + } else { + for (int ear = 0; ear < 2; ++ear) { + std::vector target(kMcaBands, 0.0); + for (std::size_t index = 0; index < ids.size(); ++index) { + const std::size_t id = static_cast(ids[index]); + const double* values = + auditory_.data() + + (id * 2u + static_cast(ear)) * kMcaBands; + for (int band = 0; band < kMcaBands; ++band) { + target[static_cast(band)] += weights[index] * values[band]; + } + } + // Auditory band levels of the interpolated spectrum. + std::vector power(bins_, 0.0); + for (std::size_t bin = 0; bin < bins_; ++bin) { + power[bin] = std::norm((*out)[static_cast(ear) * bins_ + bin]); + } + std::vector difference(kMcaBands, 0.0); + for (int band = 0; band < kMcaBands; ++band) { + double sum = 0.0; + for (std::size_t bin = 0; bin < bins_; ++bin) { + sum += power[bin] * + bank_[static_cast(band) * bins_ + bin]; + } + difference[static_cast(band)] = + target[static_cast(band)] - + 10.0 * std::log10(std::max(sum, 1e-24)); + } + PchipEvaluator interpolator; + interpolator.build(centres_hz_, difference); + for (std::size_t bin = 0; bin < bins_; ++bin) { + const double frequency = + std::clamp(frequencies_[bin], centres_hz_.front(), centres_hz_.back()); + const double correction = std::clamp(interpolator.evaluate(frequency), + -12.0, 12.0); + (*out)[static_cast(ear) * bins_ + bin] *= + std::pow(10.0, correction / 20.0); + } + } + } + } + return Status::success(); +} + +} // namespace joc::hrtf diff --git a/src/hrtf/full_hrir_field.h b/src/hrtf/full_hrir_field.h new file mode 100644 index 0000000..9c1e0af --- /dev/null +++ b/src/hrtf/full_hrir_field.h @@ -0,0 +1,96 @@ + +#pragma once + +#include +#include +#include +#include + +#include "foundation/status.h" +#include "hrtf/spherical_mesh.h" + +// Full-HRIR directional field, ported from the reference sofa_hrir.py: the +// selected measurement shell keeps its complete FIR support, the interpolation +// works on delay-aligned complex spectra inside the containing spherical +// triangle, and the radial model is the analytic rigid-sphere series. This +// replaces the compiled 77-band/SH field for SOFA inputs. +namespace joc::hrtf { + +struct CanonicalHrtf; + +// Point-source pressure at `radius` over the free-field pressure at the sphere +// centre (rigid sphere, FFT sign convention) for the cosines in `cosines`. +// Output is [cosines][frequencies] complex, row-major. +Status sphere_transfer(const std::vector& frequencies, double radius, double head_radius, + const std::vector& cosines, + std::vector>* out); + +enum class HrirInterpolation : std::uint32_t { + kLogMagnitudePhase = 0, // lf_logmag_phase: geometric mean magnitude, interpolated phase + kMcaZero = 1, // lf_mca_zero: auditory-band magnitude correction +}; + +struct FullHrirOptions { + HrirInterpolation method = HrirInterpolation::kLogMagnitudePhase; + bool has_shell_radius = false; + double shell_radius_m = 0.0; + std::size_t fft_size = 0; // 0 selects the automatic size +}; + +class FullHrirField { +public: + // Builds the field from a canonical SimpleFreeFieldHRIR set (48 kHz only). + Status build(const CanonicalHrtf& canonical, const FullHrirOptions& options); + + std::size_t fft_size() const { return fft_size_; } + std::size_t frequency_bins() const { return bins_; } + const std::vector& frequencies() const { return frequencies_; } + const std::vector& omega() const { return omega_; } + double measurement_radius_m() const { return measurement_radius_m_; } + double head_radius_m() const { return head_radius_m_; } + const std::vector& ear_directions() const { return ear_directions_; } // [2][3] + std::size_t direction_count() const { return mesh_.vertex_count(); } + std::size_t triangle_count() const { return mesh_.face_count(); } + int recording_origin_samples() const { return recording_origin_samples_; } + int common_latency_samples() const { return static_cast(fft_size_ / 2); } + double max_triangle_edge_degrees() const { return mesh_.max_triangle_edge_degrees(); } + double phase_fit_rms_rad_p95() const { return phase_fit_rms_rad_p95_; } + double floor() const { return floor_; } + HrirInterpolation method() const { return method_; } + + // Interpolated two-ear spectrum at a direction (`out` is [2][bins] complex). + Status spectrum(const double direction[3], std::vector>* out); + + // Radial (distance) correction for a source at `radius` metres. + Status radial_ratio(const double direction[3], double radius, + std::vector>* out) const; + + // Centre (omnidirectional) spectrum used when a position is inside the head. + const std::vector>& centre_spectrum() const { return centre_spectrum_; } + +private: + Status build_interpolation(const CanonicalHrtf& canonical, const FullHrirOptions& options); + + std::size_t fft_size_ = 0; + std::size_t bins_ = 0; + std::size_t taps_ = 0; + HrirInterpolation method_ = HrirInterpolation::kLogMagnitudePhase; + SphericalMesh mesh_; + std::vector frequencies_; // [bins] + std::vector omega_; // [bins] + std::vector delays_; // [M][2] samples (estimated + Data.Delay) + std::vector> aligned_; // [M][2][bins] + std::vector log_magnitude_; // [M][2][bins] + std::vector auditory_; // [M][2][41] (MCA method only) + std::vector centres_hz_; // [41] (MCA method only) + std::vector bank_; // [41][bins] (MCA method only) + std::vector> centre_spectrum_; // [bins] + double measurement_radius_m_ = 0.0; + double head_radius_m_ = 0.0; + std::vector ear_directions_; // [2][3] + double floor_ = 1.0e-15; + int recording_origin_samples_ = 0; + double phase_fit_rms_rad_p95_ = 0.0; +}; + +} // namespace joc::hrtf diff --git a/src/hrtf/partitioned_convolution.cpp b/src/hrtf/partitioned_convolution.cpp new file mode 100644 index 0000000..637db5f --- /dev/null +++ b/src/hrtf/partitioned_convolution.cpp @@ -0,0 +1,336 @@ + +#include "hrtf/partitioned_convolution.h" + +#include +#include +#include + +namespace joc::hrtf { +namespace { + +using joc::dsp::Complex; + +bool is_power_of_two(std::size_t value) { return value != 0 && (value & (value - 1)) == 0; } + +Status argument_error(const char* message) { + return Status::fail(JOC_ERR_INVALID_ARGUMENT, joc::stage::kDsp, message); +} + +} // namespace + +UniformPartitionedConvolver::UniformPartitionedConvolver(std::size_t outputs, + std::size_t max_filter_taps, + std::size_t block_size) { + configure(outputs, max_filter_taps, block_size); +} + +void UniformPartitionedConvolver::configure(std::size_t outputs, std::size_t max_filter_taps, + std::size_t block_size) { + outputs_ = outputs; + max_filter_taps_ = max_filter_taps; + block_size_ = block_size; + fft_size_ = 2 * block_size; + bins_ = block_size + 1; + partitions_ = block_size == 0 ? 0 : (max_filter_taps + block_size - 1) / block_size; + history_.assign(partitions_ * bins_, Complex{}); + spectra_.assign(outputs_ * partitions_ * bins_, Complex{}); + target_spectra_.assign(outputs_ * partitions_ * bins_, Complex{}); + overlap_.assign(outputs_ * block_size, 0.0); + target_overlap_.assign(outputs_ * block_size, 0.0); + work_.assign(bins_, Complex{}); + fft_buffer_.assign(fft_size_, Complex{}); + time_work_.assign(fft_size_, 0.0); + ordered_.assign(partitions_ * bins_, Complex{}); + if (is_power_of_two(fft_size_)) { + forward_plan_ = std::make_unique(fft_size_, false); + inverse_plan_ = std::make_unique(fft_size_, true); + } else { + forward_plan_.reset(); + inverse_plan_.reset(); + } + write_index_ = -1; + processed_blocks_ = 0; + target_active_ = false; + fade_samples_ = 0; + fade_position_ = 0; +} + +void UniformPartitionedConvolver::reset() { + std::fill(history_.begin(), history_.end(), Complex{}); + std::fill(overlap_.begin(), overlap_.end(), 0.0); + std::fill(target_overlap_.begin(), target_overlap_.end(), 0.0); + std::fill(target_spectra_.begin(), target_spectra_.end(), Complex{}); + write_index_ = -1; + processed_blocks_ = 0; + target_active_ = false; + fade_samples_ = 0; + fade_position_ = 0; +} + +void UniformPartitionedConvolver::real_spectrum(const double* time, + std::vector* out) const { + // numpy.fft.rfft of a real block: the unnormalised forward transform, first + // block_size+1 bins of the size-2*block_size complex transform. + std::fill(fft_buffer_.begin(), fft_buffer_.end(), Complex{}); + for (std::size_t i = 0; i < fft_size_; ++i) { + fft_buffer_[i] = Complex(time[i], 0.0); + } + if (forward_plan_) { + dsp::fft_radix2(&fft_buffer_, *forward_plan_); + } else { + std::vector transformed; + dsp::fft_any(fft_buffer_, false, &transformed); + fft_buffer_ = std::move(transformed); + } + out->assign(fft_buffer_.begin(), fft_buffer_.begin() + static_cast(bins_)); +} + +void UniformPartitionedConvolver::real_signal(const std::vector& spectrum, + double* out) const { + // numpy.fft.irfft: rebuild the Hermitian half, inverse transform (1/N), real part. + std::fill(fft_buffer_.begin(), fft_buffer_.end(), Complex{}); + for (std::size_t bin = 0; bin < bins_; ++bin) { + fft_buffer_[bin] = spectrum[bin]; + } + for (std::size_t bin = 1; bin + 1 < bins_; ++bin) { + fft_buffer_[fft_size_ - bin] = std::conj(spectrum[bin]); + } + if (inverse_plan_) { + dsp::fft_radix2(&fft_buffer_, *inverse_plan_); + } else { + std::vector transformed; + dsp::fft_any(fft_buffer_, true, &transformed); + fft_buffer_ = std::move(transformed); + } + for (std::size_t i = 0; i < fft_size_; ++i) { + out[i] = fft_buffer_[i].real(); + } +} + +void UniformPartitionedConvolver::partition(const double* filters, std::size_t taps, + std::vector* out) { + std::vector padded(fft_size_); + out->assign(outputs_ * partitions_ * bins_, Complex{}); + for (std::size_t partition_index = 0; partition_index < partitions_; ++partition_index) { + const std::size_t start = partition_index * block_size_; + if (start >= taps) { + break; + } + const std::size_t stop = std::min(start + block_size_, taps); + for (std::size_t output = 0; output < outputs_; ++output) { + std::fill(padded.begin(), padded.end(), 0.0); + const double* row = filters + output * taps; + std::memcpy(padded.data(), row + start, sizeof(double) * (stop - start)); + real_spectrum(padded.data(), &work_); + std::copy(work_.begin(), work_.end(), + out->begin() + static_cast( + (output * partitions_ + partition_index) * bins_)); + } + } +} + +void UniformPartitionedConvolver::order_history() { + // index 0 is the block just written; index p is p blocks ago. + for (std::size_t partition_index = 0; partition_index < partitions_; ++partition_index) { + std::ptrdiff_t index = 0; + if (write_index_ >= 0) { + const std::ptrdiff_t count = static_cast(partitions_); + index = (write_index_ - static_cast(partition_index) + count) % count; + } + std::copy(history_.begin() + index * static_cast(bins_), + history_.begin() + (index + 1) * static_cast(bins_), + ordered_.begin() + static_cast(partition_index * bins_)); + } +} + +void UniformPartitionedConvolver::convolution_block(const std::vector& spectra, + const std::vector& overlap, + double* out, double* next_overlap) { + order_history(); + for (std::size_t output = 0; output < outputs_; ++output) { + std::fill(work_.begin(), work_.end(), Complex{}); + for (std::size_t partition_index = 0; partition_index < partitions_; ++partition_index) { + const Complex* input = ordered_.data() + partition_index * bins_; + const Complex* filter = + spectra.data() + (output * partitions_ + partition_index) * bins_; + for (std::size_t bin = 0; bin < bins_; ++bin) { + work_[bin] += input[bin] * filter[bin]; + } + } + real_signal(work_, time_work_.data()); + for (std::size_t sample = 0; sample < block_size_; ++sample) { + out[output * block_size_ + sample] = + time_work_[sample] + overlap[output * block_size_ + sample]; + next_overlap[output * block_size_ + sample] = time_work_[block_size_ + sample]; + } + } +} + +void UniformPartitionedConvolver::overlap_for_previous_block( + const std::vector& spectra, double* overlap) { + if (processed_blocks_ == 0) { + std::fill(overlap, overlap + outputs_ * block_size_, 0.0); + return; + } + order_history(); + for (std::size_t output = 0; output < outputs_; ++output) { + std::fill(work_.begin(), work_.end(), Complex{}); + for (std::size_t partition_index = 0; partition_index < partitions_; ++partition_index) { + const Complex* input = ordered_.data() + partition_index * bins_; + const Complex* filter = + spectra.data() + (output * partitions_ + partition_index) * bins_; + for (std::size_t bin = 0; bin < bins_; ++bin) { + work_[bin] += input[bin] * filter[bin]; + } + } + real_signal(work_, time_work_.data()); + std::memcpy(overlap + output * block_size_, time_work_.data() + block_size_, + sizeof(double) * block_size_); + } +} + +Status UniformPartitionedConvolver::set_filter(const double* filters, std::size_t taps, + std::size_t fade_blocks) { + if ((filters == nullptr && taps != 0) || block_size_ == 0 || outputs_ == 0 || + taps > max_filter_taps_) { + return argument_error("filter update exceeds the configured tap budget"); + } + for (std::size_t i = 0; i < outputs_ * taps; ++i) { + if (!std::isfinite(filters[i])) { + return argument_error("filter update contains a non-finite value"); + } + } + partition(filters, taps, &target_spectra_); + if (fade_blocks == 0) { + spectra_ = target_spectra_; + overlap_for_previous_block(spectra_, overlap_.data()); + target_active_ = false; + fade_samples_ = 0; + fade_position_ = 0; + return Status::success(); + } + overlap_for_previous_block(target_spectra_, target_overlap_.data()); + target_active_ = true; + fade_samples_ = static_cast(fade_blocks * block_size_); + fade_position_ = 0; + return Status::success(); +} + +Status UniformPartitionedConvolver::process_block(const double* block, double* out) { + if (block == nullptr || out == nullptr) { + return argument_error("block pointer is null"); + } + for (std::size_t i = 0; i < block_size_; ++i) { + if (!std::isfinite(block[i])) { + return argument_error("input block contains a non-finite value"); + } + } + std::fill(time_work_.begin(), time_work_.end(), 0.0); + std::memcpy(time_work_.data(), block, sizeof(double) * block_size_); + real_spectrum(time_work_.data(), &work_); + write_index_ = (write_index_ + 1) % static_cast(partitions_); + std::copy(work_.begin(), work_.end(), + history_.begin() + write_index_ * static_cast(bins_)); + + std::vector current(outputs_ * block_size_); + std::vector next_current_overlap(outputs_ * block_size_); + convolution_block(spectra_, overlap_, current.data(), next_current_overlap.data()); + if (!target_active_) { + overlap_ = next_current_overlap; + ++processed_blocks_; + for (std::size_t sample = 0; sample < block_size_; ++sample) { + for (std::size_t output = 0; output < outputs_; ++output) { + out[sample * outputs_ + output] = current[output * block_size_ + sample]; + } + } + return Status::success(); + } + + std::vector target(outputs_ * block_size_); + std::vector next_target_overlap(outputs_ * block_size_); + convolution_block(target_spectra_, target_overlap_, target.data(), + next_target_overlap.data()); + const double fade = static_cast(fade_samples_); + for (std::size_t sample = 0; sample < block_size_; ++sample) { + const double position = + static_cast(fade_position_ + static_cast(sample) + 1); + const double amount = + std::clamp(fade > 0.0 ? position / fade : 1.0, 0.0, 1.0); + for (std::size_t output = 0; output < outputs_; ++output) { + const double dry = current[output * block_size_ + sample]; + const double wet = target[output * block_size_ + sample]; + out[sample * outputs_ + output] = dry * (1.0 - amount) + wet * amount; + } + } + fade_position_ += static_cast(block_size_); + overlap_ = next_current_overlap; + target_overlap_ = next_target_overlap; + if (fade_position_ >= fade_samples_) { + spectra_ = target_spectra_; + overlap_ = target_overlap_; + target_active_ = false; + fade_samples_ = 0; + fade_position_ = 0; + } + ++processed_blocks_; + return Status::success(); +} + +MultiSourcePartitionedConvolver::MultiSourcePartitionedConvolver( + std::size_t source_count, std::size_t block_size, std::size_t max_filter_taps, + std::size_t output_channels) { + configure(source_count, block_size, max_filter_taps, output_channels); +} + +void MultiSourcePartitionedConvolver::configure(std::size_t source_count, + std::size_t block_size, + std::size_t max_filter_taps, + std::size_t output_channels) { + block_size_ = block_size; + max_filter_taps_ = max_filter_taps; + output_channels_ = output_channels; + convolvers_.clear(); + convolvers_.resize(source_count); + for (UniformPartitionedConvolver& convolver : convolvers_) { + convolver.configure(output_channels, max_filter_taps, block_size); + } + source_block_.assign(block_size, 0.0); + partial_.assign(block_size * output_channels, 0.0); +} + +void MultiSourcePartitionedConvolver::reset() { + for (UniformPartitionedConvolver& convolver : convolvers_) { + convolver.reset(); + } +} + +Status MultiSourcePartitionedConvolver::set_filter(std::size_t source, const double* filters, + std::size_t taps, std::size_t fade_blocks) { + if (source >= convolvers_.size()) { + return argument_error("source index is out of range"); + } + return convolvers_[source].set_filter(filters, taps, fade_blocks); +} + +Status MultiSourcePartitionedConvolver::process_block(const double* sources, double* out) { + if (sources == nullptr || out == nullptr) { + return argument_error("block pointer is null"); + } + std::fill(out, out + block_size_ * output_channels_, 0.0); + for (std::size_t source = 0; source < convolvers_.size(); ++source) { + for (std::size_t sample = 0; sample < block_size_; ++sample) { + source_block_[sample] = sources[sample * convolvers_.size() + source]; + } + const Status status = + convolvers_[source].process_block(source_block_.data(), partial_.data()); + if (!status.ok()) { + return status; + } + for (std::size_t i = 0; i < partial_.size(); ++i) { + out[i] += partial_[i]; + } + } + return Status::success(); +} + +} // namespace joc::hrtf diff --git a/src/hrtf/partitioned_convolution.h b/src/hrtf/partitioned_convolution.h new file mode 100644 index 0000000..4cc809e --- /dev/null +++ b/src/hrtf/partitioned_convolution.h @@ -0,0 +1,118 @@ + +#pragma once + +#include +#include +#include +#include +#include + +#include "foundation/fft.h" +#include "foundation/status.h" + +// Uniform partitioned convolution, ported from the reference +// partitioned_convolution.py. One input history is shared by the outputs of a +// source; a filter replacement can crossfade the output over a number of blocks +// while both the old and the new filter keep their own overlap tail. +namespace joc::hrtf { + +class UniformPartitionedConvolver { +public: + UniformPartitionedConvolver() = default; + UniformPartitionedConvolver(std::size_t outputs, std::size_t max_filter_taps, + std::size_t block_size); + + void configure(std::size_t outputs, std::size_t max_filter_taps, std::size_t block_size); + + std::size_t outputs() const { return outputs_; } + std::size_t block_size() const { return block_size_; } + std::size_t fft_size() const { return fft_size_; } + std::size_t max_filter_taps() const { return max_filter_taps_; } + bool transition_active() const { return target_active_; } + + // `filters` is [outputs][taps] row-major and `taps` <= max_filter_taps. A zero + // fade applies the filter immediately and rebuilds the overlap tail from the + // blocks already processed, so an impulse in flight is not dropped. + Status set_filter(const double* filters, std::size_t taps, std::size_t fade_blocks); + + // `block` is block_size doubles; `out` is block_size*outputs, sample-major. + Status process_block(const double* block, double* out); + + void reset(); + +private: + void partition(const double* filters, std::size_t taps, + std::vector>* out); + void convolution_block(const std::vector>& spectra, + const std::vector& overlap, double* out, + double* next_overlap); + void overlap_for_previous_block(const std::vector>& spectra, + double* overlap); + void order_history(); + void real_spectrum(const double* time, std::vector>* out) const; + void real_signal(const std::vector>& spectrum, double* out) const; + + std::size_t outputs_ = 0; + std::size_t max_filter_taps_ = 0; + std::size_t block_size_ = 0; + std::size_t fft_size_ = 0; + std::size_t bins_ = 0; + std::size_t partitions_ = 0; + + std::unique_ptr forward_plan_; + std::unique_ptr inverse_plan_; + mutable std::vector> fft_buffer_; + + std::vector> history_; // [partitions][bins] + std::vector> spectra_; // [outputs][partitions][bins] + std::vector> target_spectra_; + std::vector overlap_; // [outputs][block] + std::vector target_overlap_; // [outputs][block] + std::vector> work_; // [bins] scratch + std::vector time_work_; // [fft_size] scratch + std::vector> ordered_; // [partitions][bins] scratch + + std::ptrdiff_t write_index_ = -1; + std::uint64_t processed_blocks_ = 0; + bool target_active_ = false; + std::int64_t fade_samples_ = 0; + std::int64_t fade_position_ = 0; +}; + +class MultiSourcePartitionedConvolver { +public: + MultiSourcePartitionedConvolver() = default; + MultiSourcePartitionedConvolver(std::size_t source_count, std::size_t block_size, + std::size_t max_filter_taps, + std::size_t output_channels = 2); + + void configure(std::size_t source_count, std::size_t block_size, + std::size_t max_filter_taps, std::size_t output_channels = 2); + + std::size_t source_count() const { return convolvers_.size(); } + std::size_t block_size() const { return block_size_; } + std::size_t output_channels() const { return output_channels_; } + std::size_t max_filter_taps() const { return max_filter_taps_; } + const UniformPartitionedConvolver& convolver(std::size_t source) const { + return convolvers_[source]; + } + + Status set_filter(std::size_t source, const double* filters, std::size_t taps, + std::size_t fade_blocks); + + // `sources` is block_size*source_count (sample-major); `out` is + // block_size*output_channels and is overwritten. + Status process_block(const double* sources, double* out); + + void reset(); + +private: + std::vector convolvers_; + std::size_t block_size_ = 0; + std::size_t max_filter_taps_ = 0; + std::size_t output_channels_ = 2; + std::vector source_block_; + std::vector partial_; +}; + +} // namespace joc::hrtf diff --git a/src/hrtf/rosella_reference.cpp b/src/hrtf/rosella_reference.cpp new file mode 100644 index 0000000..cd72bc8 --- /dev/null +++ b/src/hrtf/rosella_reference.cpp @@ -0,0 +1,854 @@ +#include "hrtf/rosella_reference.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "foundation/fft.h" +#include "hrtf/public_filterbank.h" +#include "hrtf/rosella_renderer.h" +#include "simd/simd.h" + +// The reference arithmetic of the Rosella ".personalized_headphone" renderer +// (rosella_model.py, rosella_direct.py and rosella_room.py). rosella_renderer.cpp +// drives these through RosellaRuntime, and a renderer built on another field format +// calls them directly; the bodies are lifted unchanged from the renderer, so their +// operation order and their literals are part of the interface. + +namespace joc::hrtf { + +namespace { + +constexpr int kDirectionTerms = 36; +constexpr int kRoomBands = 64; +constexpr double kRoomInputGain = 0.70710677; +constexpr double kCentreEqual = 0.9998489618301392; +constexpr double kCentreAlternate = 0.7070000171661377; + +// The fixed sixteen-band low-pass a special (LFE) source is rendered through; the +// constants are the float32 bit patterns of rosella_direct._SPECIAL_LFE_LOW_16. +constexpr std::uint32_t kSpecialLfeLowBits[16] = { + 0x402695EAu, 0x3FE75979u, 0x3F28CAAAu, 0xBCE1FB2Eu, 0xBDD8AF65u, 0xBD8F426Eu, + 0x3D996821u, 0xBC16B3A0u, 0x3B64BAF1u, 0xBC81ECFDu, 0xBA3D892Fu, 0x3AF6A9F0u, + 0xB9DD1C5Fu, 0x380A193Fu, 0x38052059u, 0x351BCB34u}; + +Status render_fail(joc_error code, const std::string& message) { + return Status::fail(code, stage::kRender, message); +} + +std::vector special_lfe_low() { + std::vector values(kHybridBands, 0.0); + for (int index = 0; index < 16; ++index) { + float narrowed = 0.0f; + const std::uint32_t bits = kSpecialLfeLowBits[index]; + std::memcpy(&narrowed, &bits, sizeof(float)); + values[static_cast(index)] = static_cast(narrowed); + } + return values; +} + +// rosella_direct._round_away_from_zero. +double round_away_from_zero(double value) { + return value >= 0.0 ? std::floor(value + 0.5) : std::ceil(value - 0.5); +} + +// rosella_direct._q15_position: ADM coordinates on the Rosella metadata grid. +void q15_position(const double position[3], int encoded[3]) { + const double raw[3] = { + std::min(std::max((position[0] + 1.0) * 0.5, 0.0), 1.0), + std::min(std::max((1.0 - position[1]) * 0.5, 0.0), 1.0), + std::min(std::max(position[2], -1.0), 1.0), + }; + for (int axis = 0; axis < 3; ++axis) { + encoded[axis] = static_cast( + std::min(round_away_from_zero(raw[axis] * 32768.0), 32767.0)); + } +} + +struct EarGeometry { + double basis_minus[kDirectionTerms] = {}; + double basis_plus[kDirectionTerms] = {}; + double path_minus = 0.0; + double path_plus = 0.0; +}; + +// rosella_direct._ear_geometry. +EarGeometry ear_geometry(const RosellaModel& model, const RosellaDistanceProfile& profile, + const double direction[3], double clamped, double offset, + double correction) { + const double x = direction[0]; + const double y = direction[1]; + const double z = direction[2]; + const double inverse_distance = static_cast(profile.inverse_distance_per_m); + const double ear = offset * inverse_distance / clamped; + const double y_minus = y - ear; + const double y_plus = y + ear; + const double common = x * x + z * z; + const double length_minus = std::sqrt(y_minus * y_minus + common); + const double length_plus = std::sqrt(y_plus * y_plus + common); + EarGeometry result; + direction_basis(x / length_minus, y_minus / length_minus, z / length_minus, + result.basis_minus); + direction_basis(x / length_plus, y_plus / length_plus, z / length_plus, result.basis_plus); + result.path_minus = length_minus * clamped; + result.path_plus = length_plus * clamped; + if (correction != 0.0) { + const double multiplier = 2.0 * correction * inverse_distance; + double left = 0.0; + double right = 0.0; + for (int term = 0; term < kDirectionTerms; ++term) { + left += static_cast(model.vector_left[static_cast(term)]) * + result.basis_minus[term]; + right += static_cast(model.vector_right[static_cast(term)]) * + result.basis_plus[term]; + } + result.path_minus += std::max(left, 0.0) * multiplier; + result.path_plus += std::max(right, 0.0) * multiplier; + } + return result; +} + +// rosella_direct._phase_groups. +void phase_groups(const RosellaModel& model, double delay_samples, Complex* out) { + // The reference starts from np.ones(77) and only rewrites the flagged bands. + std::fill(out, out + kHybridBands, Complex(1.0, 0.0)); + Complex current(1.0, 0.0); + Complex step(1.0, 0.0); + std::size_t value_index = 0u; + for (std::size_t band = 0u; band < model.hybrid_flags.size() && band < kHybridBands; ++band) { + const int flag = model.hybrid_flags[band]; + if (flag != 2) { + if (flag == 1) { + const double angle = + static_cast(model.hybrid_values[value_index]) * delay_samples; + ++value_index; + step = Complex(std::cos(angle), std::sin(angle)); + } + current *= step; + } + out[band] = current; + } +} + +} // namespace + +// rosella_model.direction_basis: the observed 36-term basis. The operation order is +// part of the model, so the intermediates are named after the reference's. +void direction_basis(double x, double y, double z, double* out) { + const double yz = y * z; + const double x2 = x * x; + const double y2 = y * y; + const double x2m02 = x2 - 0.2; + const double xy = x * y; + out[0] = 1.0; + out[1] = x; + out[2] = y; + out[3] = z; + out[4] = x2 - (1.0 / 3.0); + out[5] = xy; + out[6] = x * z; + out[7] = y2 - (1.0 / 3.0); + out[8] = yz; + out[9] = (x2 - 0.6) * x; + out[10] = x2m02 * y; + out[11] = x2m02 * z; + out[12] = (y2 - 0.2) * x; + out[13] = yz * x; + out[14] = (y2 - 0.6) * y; + out[15] = (y2 - 0.2) * z; + out[16] = x2 * x2 - 0.2; + out[17] = xy * x2; + out[18] = (x * z) * x2; + out[19] = y2 * x2 - (1.0 / 15.0); + out[20] = yz * x2; + out[21] = x * y2 * y; + out[22] = x * y2 * z; + out[23] = y2 * y2 - 0.2; + const double x4 = x2 * x2; + const double x2y2 = y2 * x2; + const double y4 = y2 * y2; + out[24] = yz * y2; + out[25] = (x4 - (3.0 / 7.0)) * x; + out[26] = (x4 - (3.0 / 35.0)) * y; + out[27] = (x4 - (3.0 / 35.0)) * z; + out[28] = (x2y2 - (3.0 / 35.0)) * x; + out[29] = (x2 * z) * xy; + out[30] = (x2y2 - (3.0 / 35.0)) * y; + out[31] = (x2y2 - (1.0 / 35.0)) * z; + out[32] = (y4 - (3.0 / 35.0)) * x; + out[33] = (y2 * z) * xy; + out[34] = (y4 - (3.0 / 7.0)) * y; + out[35] = (y4 - (3.0 / 35.0)) * z; +} + +// rosella_direct._logical_field: the padded lane grid as [77][36][2] float64. +std::vector rosella_logical_field(const std::vector& padded) { + std::vector out; + out.assign(static_cast(kHybridBands) * kDirectionTerms * 2u, 0.0); + for (int band = 0; band < kHybridBands; ++band) { + const int block = band / 4; + const int lane = band % 4; + for (int term = 0; term < kDirectionTerms; ++term) { + for (int component = 0; component < 2; ++component) { + const std::size_t source = + static_cast(lane + 4 * (term * 2 + component + 72 * block)); + out[(static_cast(band) * kDirectionTerms + + static_cast(term)) * 2u + + static_cast(component)] = + source < padded.size() ? static_cast(padded[source]) : 0.0; + } + } + } + return out; +} + +// rosella_direct._profile_geometry. +Status profile_geometry(const RosellaModel& model, const double position[3], int profile_index, + ProfileGeometry* out) { + const RosellaDistanceProfile& profile = model.profiles[static_cast(profile_index)]; + int encoded[3] = {0, 0, 0}; + q15_position(position, encoded); + const double q_front = 1.0 - 2.0 * static_cast(encoded[1]) / 32768.0; + const double q_x = 2.0 * static_cast(encoded[0]) / 32768.0 - 1.0; + const double q_vertical = static_cast(encoded[2]) / 32768.0; + double mapped_front = q_front; + double mapped_lateral = -q_x; + double mapped_vertical = q_vertical; + if (model.header_integer_fields[0] != 0) { + if (q_x == 0.0 && q_front == 0.0) { + mapped_front = 0.0; + mapped_lateral = 0.0; + mapped_vertical = q_vertical; + } else { + const double horizontal_max = std::max(std::abs(q_x), std::abs(q_front)); + const double horizontal_norm = (q_x / horizontal_max) * (q_x / horizontal_max) + + (q_front / horizontal_max) * (q_front / horizontal_max); + double vertical_norm = 1.0; + if (q_vertical != 0.0) { + const double smaller = std::min(std::abs(q_vertical), horizontal_max); + const double larger = std::max(std::abs(q_vertical), horizontal_max); + vertical_norm = 1.0 + (smaller / larger) * (smaller / larger); + } + const double horizontal_factor = 1.0 / std::sqrt(horizontal_norm * vertical_norm); + const double vertical_factor = 1.0 / std::sqrt(vertical_norm); + mapped_front = q_front * horizontal_factor; + mapped_lateral = -q_x * horizontal_factor; + mapped_vertical = q_vertical * vertical_factor; + } + } + double scaled[3] = {mapped_front * static_cast(profile.axis_scales_internal[2]), + mapped_lateral * static_cast(profile.axis_scales_internal[0]), + mapped_vertical * static_cast(profile.axis_scales_internal[1])}; + double ray = 1.0; + for (int axis = 0; axis < 3; ++axis) { + const double value = scaled[axis]; + const double lower = static_cast(profile.bounds[static_cast(axis) * 2u]); + const double upper = + static_cast(profile.bounds[static_cast(axis) * 2u + 1u]); + if (value < lower) { + ray = std::min(ray, lower / value); + } else if (value > upper) { + ray = std::min(ray, upper / value); + } + } + if (ray < 1.0) { + scaled[0] *= ray; + scaled[1] *= ray; + scaled[2] *= ray; + } + ProfileGeometry result; + result.radius = std::sqrt(scaled[0] * scaled[0] + scaled[1] * scaled[1] + + scaled[2] * scaled[2]); + result.clamped = std::max(result.radius, static_cast(profile.minimum_normalized_radius)); + result.alpha = result.radius / result.clamped; + if (result.radius > 1.0e-30) { + for (int axis = 0; axis < 3; ++axis) { + result.direction[axis] = scaled[axis] / result.radius; + } + } + *out = result; + return Status::success(); +} + +// rosella_direct.special_lfe_direct. +const DirectResult& special_lfe_direct() { + static const DirectResult result = [] { + DirectResult direct; + const std::vector low = special_lfe_low(); + for (int ear = 0; ear < 2; ++ear) { + for (int band = 0; band < kHybridBands; ++band) { + direct.gains[ear][band] = Complex(low[static_cast(band)], 0.0); + } + } + return direct; + }(); + return result; +} + +// rosella_direct.direct_and_room_send. +Status direct_and_room_send(const RosellaModel& model, const std::vector& field_left, + const std::vector& field_right, const double position[3], + int profile_index, DirectResult* out) { + const RosellaDistanceProfile& profile = + model.profiles[static_cast(profile_index)]; + ProfileGeometry geometry; + const Status geometry_status = profile_geometry(model, position, profile_index, &geometry); + if (!geometry_status.ok()) { + return geometry_status; + } + + const EarGeometry delay_ears = + ear_geometry(model, profile, geometry.direction, geometry.clamped, + static_cast(model.model_scalars[1]), + static_cast(model.model_scalars[2])); + const double delay = std::abs(delay_ears.path_plus - delay_ears.path_minus) * + static_cast(profile.distance_scale_m) * + (static_cast(model.sample_rate) / 343.3) * geometry.alpha; + int delayed_ear = -1; + if (delay_ears.path_minus > delay_ears.path_plus) { + delayed_ear = 0; + } else if (delay_ears.path_plus > delay_ears.path_minus) { + delayed_ear = 1; + } + + const EarGeometry weight_ears = + ear_geometry(model, profile, geometry.direction, geometry.clamped, + static_cast(model.model_scalars[3]), + static_cast(model.model_scalars[4])); + const double weight_norm = + std::sqrt(weight_ears.path_minus * weight_ears.path_minus + + weight_ears.path_plus * weight_ears.path_plus); + const double weight_left = weight_ears.path_plus / weight_norm; + const double weight_right = weight_ears.path_minus / weight_norm; + + const double final_offset = static_cast(model.model_scalars[0]); + double basis_minus[kDirectionTerms] = {}; + double basis_plus[kDirectionTerms] = {}; + if (final_offset == 0.0) { + direction_basis(geometry.direction[0], geometry.direction[1], geometry.direction[2], + basis_minus); + std::copy(basis_minus, basis_minus + kDirectionTerms, basis_plus); + } else { + const double x = geometry.direction[0]; + const double y = geometry.direction[1]; + const double z = geometry.direction[2]; + const double ear = + final_offset * static_cast(profile.inverse_distance_per_m) / geometry.clamped; + const double y_minus = y - ear; + const double y_plus = y + ear; + const double common_length = x * x + z * z; + const double length_minus = std::sqrt(y_minus * y_minus + common_length); + const double length_plus = std::sqrt(y_plus * y_plus + common_length); + direction_basis(x / length_minus, y_minus / length_minus, z / length_minus, basis_minus); + direction_basis(x / length_plus, y_plus / length_plus, z / length_plus, basis_plus); + } + + DirectResult result; + Complex left[kHybridBands] = {}; + Complex right[kHybridBands] = {}; + for (int band = 0; band < kHybridBands; ++band) { + double left_real = 0.0; + double left_imag = 0.0; + double right_real = 0.0; + double right_imag = 0.0; + for (int term = 0; term < kDirectionTerms; ++term) { + const std::size_t index = + (static_cast(band) * kDirectionTerms + static_cast(term)) * + 2u; + left_real += field_left[index] * basis_minus[term]; + left_imag += field_left[index + 1u] * basis_minus[term]; + right_real += field_right[index] * basis_plus[term]; + right_imag += field_right[index + 1u] * basis_plus[term]; + } + left[band] = Complex(left_real, left_imag); + right[band] = Complex(right_real, right_imag); + } + if (delayed_ear >= 0) { + Complex phase[kHybridBands]; + phase_groups(model, delay, phase); + Complex* target = delayed_ear == 0 ? left : right; + for (int band = 0; band < kHybridBands; ++band) { + target[band] *= phase[band]; + } + } + + const double effective_radius = + geometry.radius * static_cast(model.header_float_scalars[0]) * + static_cast(profile.distance_scale_m); + double common = 1.0; + double room_send = 0.0; + if (profile_index == static_cast(RosellaProfile::Far) || + profile_index == static_cast(RosellaProfile::Mid)) { + common = 1.0 / std::sqrt(1.0 + static_cast(model.header_float_scalars[1]) * + effective_radius * effective_radius); + room_send = effective_radius * common; + } + + Complex left_term0[kHybridBands]; + Complex right_term0[kHybridBands]; + for (int band = 0; band < kHybridBands; ++band) { + const std::size_t index = static_cast(band) * kDirectionTerms * 2u; + left_term0[band] = Complex(field_left[index], field_left[index + 1u]); + right_term0[band] = Complex(field_right[index], field_right[index + 1u]); + } + const bool weights_are_default_equal = + static_cast(model.model_scalars[3]) == 0.0 && + static_cast(model.model_scalars[4]) == 0.0; + double centre_left = 0.0; + double centre_right = 0.0; + double right_direction_weight = weight_right; + if (weights_are_default_equal) { + centre_left = weight_left * (1.0 - geometry.alpha) * kCentreEqual; + centre_right = centre_left; + right_direction_weight = weight_left; + } else { + centre_left = (1.0 - geometry.alpha) * kCentreAlternate; + centre_right = centre_left; + } + for (int band = 0; band < kHybridBands; ++band) { + result.gains[0][band] = + common * (left[band] * (weight_left * geometry.alpha) + left_term0[band] * centre_left); + result.gains[1][band] = common * (right[band] * (right_direction_weight * geometry.alpha) + + right_term0[band] * centre_right); + } + result.room_send = room_send; + result.physical_radius_m = static_cast(profile.distance_scale_m) * geometry.radius; + result.normalized_radius = geometry.radius; + result.clamped_radius = geometry.clamped; + result.delay_samples = delay; + result.delayed_ear = delayed_ear; + *out = result; + return Status::success(); +} + +namespace { + +// rosella_room._RosellaRoomState: the recursive table-A room model. +class RoomState { +public: + explicit RoomState(const RosellaModel& model) : model_(model) { + bands_ = std::min(64, model.table_a_dimension); + delays_ = model.table_a_four_integers; + capacity_ = 0; + for (const int delay : delays_) { + capacity_ = std::max(capacity_, delay); + } + // The vectors are stored column-major (numpy's order="F"). + matrix_.assign(16u, 0.0); + for (int row = 0; row < 4; ++row) { + for (int column = 0; column < 4; ++column) { + matrix_[static_cast(row) + 4u * static_cast(column)] = + static_cast(model.table_a_vector16[static_cast(row) + + 4u * static_cast(column)]); + } + } + const std::vector& f8 = model.table_a_filter_8x64_padded; + const std::vector& f4 = model.table_a_filter_4x64_padded; + const std::vector& f16 = model.table_a_filter_16x64_padded; + feedback_real_.assign(static_cast(bands_) * 4u, 0.0); + feedback_imag_.assign(static_cast(bands_) * 4u, 0.0); + output_tap_.assign(static_cast(bands_) * 4u, 0.0); + left_real_.assign(static_cast(bands_) * 4u, 0.0); + left_imag_.assign(static_cast(bands_) * 4u, 0.0); + right_real_.assign(static_cast(bands_) * 4u, 0.0); + right_imag_.assign(static_cast(bands_) * 4u, 0.0); + for (int band = 0; band < bands_; ++band) { + const int group = band / 4; + const int lane = band % 4; + for (int tap = 0; tap < 4; ++tap) { + const std::size_t target = static_cast(band) * 4u + + static_cast(tap); + const std::size_t g8 = ((static_cast(group) * 4u + + static_cast(tap)) * 2u) * + 4u + + static_cast(lane); + feedback_real_[target] = static_cast(f8[g8]); + feedback_imag_[target] = static_cast(f8[g8 + 4u]); + output_tap_[target] = static_cast( + f4[(static_cast(group) * 4u + static_cast(tap)) * 4u + + static_cast(lane)]); + const std::size_t g16 = ((static_cast(group) * 4u + + static_cast(tap)) * 4u) * + 4u; + left_real_[target] = static_cast(f16[g16 + static_cast(lane)]); + left_imag_[target] = + static_cast(f16[g16 + 4u + static_cast(lane)]); + right_real_[target] = + static_cast(f16[g16 + 8u + static_cast(lane)]); + right_imag_[target] = + static_cast(f16[g16 + 12u + static_cast(lane)]); + } + } + allpass_gain_.resize(model.table_a_option_values.size()); + for (std::size_t index = 0u; index < model.table_a_option_values.size(); ++index) { + allpass_gain_[index] = static_cast(model.table_a_option_values[index]); + } + allpass_delay_ = model.table_a_option_ids; + for (const int delay : allpass_delay_) { + allpass_real_.emplace_back(static_cast(delay) * + static_cast(bands_), + 0.0); + allpass_imag_.emplace_back(static_cast(delay) * + static_cast(bands_), + 0.0); + } + allpass_position_.assign(allpass_real_.size(), 0); + memory_real_.assign(static_cast(capacity_) * static_cast(bands_) * 4u, + 0.0); + memory_imag_.assign(memory_real_.size(), 0.0); + extra_matrices_.resize(model.table_a_extra_indices.size()); + for (std::size_t index = 0u; index < extra_matrices_.size(); ++index) { + extra_matrices_[index].assign(16u, 0.0); + for (int row = 0; row < 4; ++row) { + for (int column = 0; column < 4; ++column) { + extra_matrices_[index][static_cast(row) + + 4u * static_cast(column)] = + static_cast( + model.table_a_extra_vectors[index * 16u + static_cast(row) + + 4u * static_cast(column)]); + } + } + } + } + + void reset() { + for (std::vector& value : allpass_real_) { + std::fill(value.begin(), value.end(), 0.0); + } + for (std::vector& value : allpass_imag_) { + std::fill(value.begin(), value.end(), 0.0); + } + std::fill(allpass_position_.begin(), allpass_position_.end(), 0); + std::fill(memory_real_.begin(), memory_real_.end(), 0.0); + std::fill(memory_imag_.begin(), memory_imag_.end(), 0.0); + position_ = 0; + } + + Status process_slot(const Complex* room_send, Complex* output) { + if (static_cast(model_.table_a_scalar) >= 0.5) { + return render_fail(JOC_ERR_NOT_SUPPORTED, "alternate Rosella table-A room mode"); + } + const std::size_t bands = static_cast(bands_); + std::vector input_real(bands, 0.0); + std::vector input_imag(bands, 0.0); + for (std::size_t band = 0u; band < bands; ++band) { + input_real[band] = room_send[band].real() * kRoomInputGain; + input_imag[band] = room_send[band].imag() * kRoomInputGain; + } + for (std::size_t index = 0u; index < allpass_gain_.size(); ++index) { + const std::size_t delay = static_cast(allpass_delay_[index]); + const std::size_t position = static_cast(allpass_position_[index]); + const double gain = allpass_gain_[index]; + for (std::size_t band = 0u; band < bands; ++band) { + const std::size_t cell = position * bands + band; + const double previous_real = allpass_real_[index][cell]; + const double previous_imag = allpass_imag_[index][cell]; + const double residual_real = input_real[band] - previous_real * gain; + const double residual_imag = input_imag[band] - previous_imag * gain; + input_real[band] = residual_real * gain + previous_real; + input_imag[band] = residual_imag * gain + previous_imag; + allpass_real_[index][cell] = residual_real; + allpass_imag_[index][cell] = residual_imag; + } + allpass_position_[index] = static_cast((position + 1u) % delay); + } + + std::vector branch_real(bands * 4u, 0.0); + std::vector branch_imag(bands * 4u, 0.0); + std::vector delayed_real(bands * 4u, 0.0); + std::vector delayed_imag(bands * 4u, 0.0); + for (int branch = 0; branch < 4; ++branch) { + const std::size_t slot = + static_cast((position_ - delays_[static_cast(branch)] + + capacity_) % + capacity_); + for (std::size_t band = 0u; band < bands; ++band) { + const std::size_t source = (slot * bands + band) * 4u + static_cast(branch); + delayed_real[band * 4u + static_cast(branch)] = memory_real_[source]; + delayed_imag[band * 4u + static_cast(branch)] = memory_imag_[source]; + } + } + for (std::size_t band = 0u; band < bands; ++band) { + for (int row = 0; row < 4; ++row) { + double real = input_real[band]; + double imag = input_imag[band]; + for (int column = 0; column < 4; ++column) { + const double coefficient = + matrix_[static_cast(row) + 4u * static_cast(column)]; + real += delayed_real[band * 4u + static_cast(column)] * coefficient; + imag += delayed_imag[band * 4u + static_cast(column)] * coefficient; + } + branch_real[band * 4u + static_cast(row)] = real; + branch_imag[band * 4u + static_cast(row)] = imag; + } + } + + const std::size_t tap_slot = static_cast( + (position_ - model_.table_a_integer + capacity_) % capacity_); + std::vector tap_real(bands * 4u, 0.0); + std::vector tap_imag(bands * 4u, 0.0); + for (std::size_t band = 0u; band < bands; ++band) { + for (int lane = 0; lane < 4; ++lane) { + const std::size_t source = (tap_slot * bands + band) * 4u + static_cast(lane); + tap_real[band * 4u + static_cast(lane)] = memory_real_[source]; + tap_imag[band * 4u + static_cast(lane)] = memory_imag_[source]; + } + } + for (std::size_t cell = 0u; cell < branch_real.size(); ++cell) { + const double real = branch_real[cell]; + const double imag = branch_imag[cell]; + memory_real_[(static_cast(position_) * bands) * 4u + cell] = + real * feedback_real_[cell] - imag * feedback_imag_[cell]; + memory_imag_[(static_cast(position_) * bands) * 4u + cell] = + imag * feedback_real_[cell] + real * feedback_imag_[cell]; + } + position_ = (position_ + 1) % capacity_; + + std::vector extra_real(bands * 4u, 0.0); + std::vector extra_imag(bands * 4u, 0.0); + for (std::size_t index = 0u; index < model_.table_a_extra_indices.size(); ++index) { + const std::size_t delay = + static_cast(model_.table_a_extra_indices[index]); + const std::size_t slot = static_cast( + (position_ - static_cast(delay) - 1 + capacity_ * 2) % capacity_); + for (std::size_t band = 0u; band < bands; ++band) { + for (int row = 0; row < 4; ++row) { + double mixed_real = 0.0; + double mixed_imag = 0.0; + for (int column = 0; column < 4; ++column) { + const std::size_t source = + (slot * bands + band) * 4u + static_cast(column); + const double coefficient = + extra_matrices_[index][static_cast(row) + + 4u * static_cast(column)]; + mixed_real += memory_real_[source] * coefficient; + mixed_imag += memory_imag_[source] * coefficient; + } + const int group = static_cast(band) / 4; + const int lane = static_cast(band) % 4; + const std::size_t field = + (index * 20u + static_cast(group)) * 2u * 4u; + const double coefficient_real = static_cast( + model_.table_a_extra_fields_padded[field + static_cast(lane)]); + const double coefficient_imag = static_cast( + model_.table_a_extra_fields_padded[field + 4u + static_cast(lane)]); + const std::size_t target = band * 4u + static_cast(row); + extra_real[target] += + mixed_real * coefficient_real - mixed_imag * coefficient_imag; + extra_imag[target] += + mixed_imag * coefficient_real + mixed_real * coefficient_imag; + } + } + } + + for (std::size_t band = 0u; band < bands; ++band) { + double left_real = 0.0; + double left_imag = 0.0; + double right_real = 0.0; + double right_imag = 0.0; + for (int lane = 0; lane < 4; ++lane) { + const std::size_t cell = band * 4u + static_cast(lane); + const double real = tap_real[cell] * output_tap_[cell] + extra_real[cell]; + const double imag = tap_imag[cell] * output_tap_[cell] + extra_imag[cell]; + left_real += left_real_[cell] * real - left_imag_[cell] * imag; + left_imag += left_imag_[cell] * real + left_real_[cell] * imag; + right_real += right_real_[cell] * real - right_imag_[cell] * imag; + right_imag += right_imag_[cell] * real + right_real_[cell] * imag; + } + output[band] = Complex(left_real, left_imag); + output[kHybridBands + band] = Complex(right_real, right_imag); + } + return Status::success(); + } + +private: + const RosellaModel& model_; + int bands_ = 0; + std::array delays_{}; + int capacity_ = 0; + std::vector matrix_; + std::vector feedback_real_; + std::vector feedback_imag_; + std::vector output_tap_; + std::vector left_real_; + std::vector left_imag_; + std::vector right_real_; + std::vector right_imag_; + std::vector allpass_gain_; + std::vector allpass_delay_; + std::vector> allpass_real_; + std::vector> allpass_imag_; + std::vector allpass_position_; + std::vector memory_real_; + std::vector memory_imag_; + int position_ = 0; + std::vector> extra_matrices_; +}; + +} // namespace + +RosellaRoomFir::RosellaRoomFir() = default; +RosellaRoomFir::~RosellaRoomFir() = default; + +Status RosellaRoomFir::configure(const RosellaModel& model, int impulse_slots) { + length_ = impulse_slots; + RoomState reference(model); + kernel_.assign(static_cast(length_) * 2u * kRoomBands, Complex(0.0, 0.0)); + Complex slot_output[2 * kHybridBands]; + for (int slot = 0; slot < length_; ++slot) { + Complex impulse[kHybridBands]; + for (int band = 0; band < kHybridBands; ++band) { + impulse[band] = Complex(slot == 0 && band < kRoomBands ? 1.0 : 0.0, 0.0); + } + const Status status = reference.process_slot(impulse, slot_output); + if (!status.ok()) { + return status; + } + for (int ear = 0; ear < 2; ++ear) { + for (int band = 0; band < kRoomBands; ++band) { + kernel_[(static_cast(slot) * 2u + static_cast(ear)) * + kRoomBands + + static_cast(band)] = + slot_output[ear * kHybridBands + band]; + } + } + } + tail_.assign(static_cast(length_ - 1) * 2u * kRoomBands, Complex(0.0, 0.0)); + valid_ = true; + return Status::success(); +} + +bool RosellaRoomFir::valid() const { return valid_; } + +void RosellaRoomFir::reset() { + std::fill(tail_.begin(), tail_.end(), Complex(0.0, 0.0)); + fft_cache_size_ = 0u; + forward_plan_.reset(); + inverse_plan_.reset(); +} + +// rosella_room.RosellaRoomFir.process_chunk. +Status RosellaRoomFir::process_chunk(const std::vector>& values, + std::size_t slots, + std::vector>* output) { + output->assign(slots * 2u * kHybridBands, Complex(0.0, 0.0)); + if (slots == 0u) { + return Status::success(); + } + const std::size_t needed = slots + static_cast(length_) - 1u; + std::size_t fft_size = 1u; + while (fft_size < needed) { + fft_size <<= 1u; + } + if (fft_size != fft_cache_size_) { + kernel_fft_.assign(fft_size * 2u * kRoomBands, Complex(0.0, 0.0)); + // The room transforms a fixed length for the whole chunk, so the radix-2 + // twiddle recurrences are materialised once here rather than re-derived + // inside every butterfly of every one of the 3 * kRoomBands transforms. + forward_plan_ = std::make_unique(fft_size, false); + inverse_plan_ = std::make_unique(fft_size, true); + line_.assign(fft_size, Complex(0.0, 0.0)); + for (int ear = 0; ear < 2; ++ear) { + for (int band = 0; band < kRoomBands; ++band) { + for (int slot = 0; slot < length_; ++slot) { + line_[static_cast(slot)] = + kernel_[(static_cast(slot) * 2u + + static_cast(ear)) * + kRoomBands + + static_cast(band)]; + } + std::fill(line_.begin() + length_, line_.end(), Complex(0.0, 0.0)); + dsp::fft_radix2(&line_, *forward_plan_); + for (std::size_t index = 0u; index < fft_size; ++index) { + // Band-major, so that the spectrum multiply below reads both + // of its operands as runs instead of striding through them + // one complex per cache line. Same values, same places. + kernel_fft_[(static_cast(ear) * kRoomBands + + static_cast(band)) * + fft_size + + index] = line_[index]; + } + } + } + fft_cache_size_ = fft_size; + } + // input_fft_ and block_ are indexed linearly and every entry of each is written + // before it is read (the per-band forward transform fills input_fft_ column by + // column, and the inverse transform fills block_ entry by entry), so their zero + // fills were dead. The line buffer, by contrast, is handed to fft_radix2, + // which transforms exactly data->size() entries -- it therefore has to keep the + // exact length and the exact initialisation it had as a local. + if (line_.size() != fft_size) { + line_.assign(fft_size, Complex(0.0, 0.0)); + } + const std::size_t input_fft_size = fft_size * kRoomBands; + if (input_fft_.size() < input_fft_size) { + input_fft_.resize(input_fft_size); + } + for (int band = 0; band < kRoomBands; ++band) { + for (std::size_t slot = 0u; slot < slots; ++slot) { + line_[slot] = values[slot * kHybridBands + static_cast(band)]; + } + std::fill(line_.begin() + static_cast(slots), line_.end(), + Complex(0.0, 0.0)); + dsp::fft_radix2(&line_, *forward_plan_); + for (std::size_t index = 0u; index < fft_size; ++index) { + // Band-major, matching kernel_fft_, so that the spectrum multiply + // reads both operands as runs. + input_fft_[static_cast(band) * fft_size + index] = line_[index]; + } + } + const std::size_t block_size = needed * 2u * kRoomBands; + if (block_.size() < block_size) { + block_.resize(block_size); + } + for (int ear = 0; ear < 2; ++ear) { + for (int band = 0; band < kRoomBands; ++band) { + // Both planes are band-major now, so this is one contiguous complex + // product per (ear, band) and the dispatched kernel puts neighbouring + // complexes in its lanes; each product keeps the caller's four + // multiplies and two roundings. + simd::complex_multiply( + reinterpret_cast(line_.data()), + reinterpret_cast( + input_fft_.data() + static_cast(band) * fft_size), + reinterpret_cast( + kernel_fft_.data() + + (static_cast(ear) * kRoomBands + + static_cast(band)) * + fft_size), + fft_size); + dsp::fft_radix2(&line_, *inverse_plan_); + for (std::size_t slot = 0u; slot < needed; ++slot) { + block_[(slot * 2u + static_cast(ear)) * kRoomBands + + static_cast(band)] = line_[slot]; + } + } + } + const std::size_t tail_length = static_cast(length_ - 1); + for (std::size_t index = 0u; index < tail_length * 2u * kRoomBands; ++index) { + block_[index] += tail_[index]; + } + for (std::size_t slot = 0u; slot < slots; ++slot) { + for (int ear = 0; ear < 2; ++ear) { + for (int band = 0; band < kRoomBands; ++band) { + (*output)[slot * 2u * kHybridBands + static_cast(ear) * kHybridBands + + static_cast(band)] = + block_[(slot * 2u + static_cast(ear)) * kRoomBands + + static_cast(band)]; + } + } + } + for (std::size_t index = 0u; index < tail_length * 2u * kRoomBands; ++index) { + tail_[index] = block_[slots * 2u * kRoomBands + index]; + } + return Status::success(); +} + +} // namespace joc::hrtf diff --git a/src/hrtf/rosella_reference.h b/src/hrtf/rosella_reference.h new file mode 100644 index 0000000..d1e2c02 --- /dev/null +++ b/src/hrtf/rosella_reference.h @@ -0,0 +1,107 @@ +#pragma once + +#include +#include +#include +#include + +#include "foundation/status.h" +#include "hrtf/rosella_model.h" + +// One of the room's transforms is a radix-2 FFT plan, so the type is named here and +// kept out of the interface. +namespace joc::dsp { +class FftPlan; +} // namespace joc::dsp + +// The reference arithmetic of the Rosella ".personalized_headphone" renderer +// (rosella_model.py, rosella_direct.py and rosella_room.py): the direction basis and +// the padded lane grid, the per-source direct gains and room send, and the recursive +// table-A room model. rosella_renderer.cpp drives these through RosellaRuntime; a +// renderer built on another field format can call them directly and get the same +// samples, so the operation order and the literals in the definitions are part of +// the interface. +namespace joc::hrtf { + +// rosella_direct._profile_geometry: a virtual position resolved on one distance +// profile's grid, as the direction of the direct path plus the radius factors the +// gain weights are scaled by. +struct ProfileGeometry { + double direction[3] = {1.0, 0.0, 0.0}; + double radius = 0.0; + double clamped = 0.0; + double alpha = 0.0; +}; + +// Quantises `position` onto the profile's metadata grid, maps it through the +// profile's axis scales and clamps it to the profile bounds; `out` takes the result. +Status profile_geometry(const RosellaModel& model, const double position[3], int profile_index, + ProfileGeometry* out); + +// rosella_model.direction_basis: the observed 36-term basis of one unit vector. +void direction_basis(double x, double y, double z, double out[36]); + +// rosella_direct._logical_field: the padded lane grid as [77][36][2] float64. +std::vector rosella_logical_field(const std::vector& padded); + +// One source's direct-path gains and room send, plus the radii and the interaural +// delay the caller reports. `delayed_ear` is -1 when neither ear is delayed, +// otherwise it is the ear (0 = left, 1 = right) whose phase carries +// `delay_samples`. +struct DirectResult { + std::complex gains[2][77] = {}; + double room_send = 0.0; + double physical_radius_m = 0.0; + double normalized_radius = 0.0; + double clamped_radius = 0.0; + double delay_samples = 0.0; + int delayed_ear = -1; +}; + +// rosella_direct.special_lfe_direct: the response a special (LFE) source is +// rendered with, built once. +const DirectResult& special_lfe_direct(); + +// rosella_direct.direct_and_room_send: the direct gains and the room send of the +// position `position` on profile `profile_index`, read through the logical field +// grids of `field_left`/`field_right`. +Status direct_and_room_send(const RosellaModel& model, const std::vector& field_left, + const std::vector& field_right, const double position[3], + int profile_index, DirectResult* out); + +// rosella_room.RosellaRoomFir: the recursive table-A room model, its room FIR and the +// overlap-add convolution that applies the FIR in the hybrid band domain. State is +// per instance and not thread-safe. +class RosellaRoomFir { +public: + RosellaRoomFir(); + ~RosellaRoomFir(); + RosellaRoomFir(const RosellaRoomFir&) = delete; + RosellaRoomFir& operator=(const RosellaRoomFir&) = delete; + + // Builds the room impulse response from `model` over `impulse_slots` slots. The + // room can only be processed once this succeeds; the alternate table-A mode is + // rejected here, as the reference rejects it. + Status configure(const RosellaModel& model, int impulse_slots); + // `values` is [slots][2][77], `output` is [slots][2][77] and stays zero outside + // the room bands. + Status process_chunk(const std::vector>& values, std::size_t slots, + std::vector>* output); + void reset(); + bool valid() const; + +private: + int length_ = 0; + bool valid_ = false; + std::vector> kernel_; + std::vector> tail_; + std::vector> kernel_fft_; + std::size_t fft_cache_size_ = 0u; + std::vector> line_; // scratch, exactly fft_size entries + std::vector> input_fft_; // scratch, [fft_size][64], fully written + std::vector> block_; // scratch, [needed][2][64], fully written + std::unique_ptr forward_plan_; + std::unique_ptr inverse_plan_; +}; + +} // namespace joc::hrtf diff --git a/src/hrtf/rosella_renderer.cpp b/src/hrtf/rosella_renderer.cpp index 488df9e..a78f3ca 100644 --- a/src/hrtf/rosella_renderer.cpp +++ b/src/hrtf/rosella_renderer.cpp @@ -1,18 +1,14 @@ #include "hrtf/rosella_renderer.h" -#include "simd/simd.h" - #include -#include #include #include -#include #include #include #include -#include "foundation/fft.h" #include "hrtf/rosella_bank.h" +#include "hrtf/rosella_reference.h" namespace joc::hrtf { @@ -20,850 +16,14 @@ namespace { constexpr int kSourceChannels = 16; constexpr int kOutputChannels = 2; -constexpr int kDirectionTerms = 36; constexpr int kBlockSamples = 512; constexpr int kFrameSamples = 1536; -constexpr int kRoomBands = 64; constexpr double kPi = 3.14159265358979323846; -constexpr double kRoomInputGain = 0.70710677; -constexpr double kCentreEqual = 0.9998489618301392; -constexpr double kCentreAlternate = 0.7070000171661377; - -// The fixed sixteen-band low-pass a special (LFE) source is rendered through; the -// constants are the float32 bit patterns of rosella_direct._SPECIAL_LFE_LOW_16. -constexpr std::uint32_t kSpecialLfeLowBits[16] = { - 0x402695EAu, 0x3FE75979u, 0x3F28CAAAu, 0xBCE1FB2Eu, 0xBDD8AF65u, 0xBD8F426Eu, - 0x3D996821u, 0xBC16B3A0u, 0x3B64BAF1u, 0xBC81ECFDu, 0xBA3D892Fu, 0x3AF6A9F0u, - 0xB9DD1C5Fu, 0x380A193Fu, 0x38052059u, 0x351BCB34u}; Status render_fail(joc_error code, const std::string& message) { return Status::fail(code, stage::kRender, message); } -std::vector special_lfe_low() { - std::vector values(kHybridBands, 0.0); - for (int index = 0; index < 16; ++index) { - float narrowed = 0.0f; - const std::uint32_t bits = kSpecialLfeLowBits[index]; - std::memcpy(&narrowed, &bits, sizeof(float)); - values[static_cast(index)] = static_cast(narrowed); - } - return values; -} - -// rosella_model.direction_basis: the observed 36-term basis. The operation order is -// part of the model, so the intermediates are named after the reference's. -void direction_basis(double x, double y, double z, double* out) { - const double yz = y * z; - const double x2 = x * x; - const double y2 = y * y; - const double x2m02 = x2 - 0.2; - const double xy = x * y; - out[0] = 1.0; - out[1] = x; - out[2] = y; - out[3] = z; - out[4] = x2 - (1.0 / 3.0); - out[5] = xy; - out[6] = x * z; - out[7] = y2 - (1.0 / 3.0); - out[8] = yz; - out[9] = (x2 - 0.6) * x; - out[10] = x2m02 * y; - out[11] = x2m02 * z; - out[12] = (y2 - 0.2) * x; - out[13] = yz * x; - out[14] = (y2 - 0.6) * y; - out[15] = (y2 - 0.2) * z; - out[16] = x2 * x2 - 0.2; - out[17] = xy * x2; - out[18] = (x * z) * x2; - out[19] = y2 * x2 - (1.0 / 15.0); - out[20] = yz * x2; - out[21] = x * y2 * y; - out[22] = x * y2 * z; - out[23] = y2 * y2 - 0.2; - const double x4 = x2 * x2; - const double x2y2 = y2 * x2; - const double y4 = y2 * y2; - out[24] = yz * y2; - out[25] = (x4 - (3.0 / 7.0)) * x; - out[26] = (x4 - (3.0 / 35.0)) * y; - out[27] = (x4 - (3.0 / 35.0)) * z; - out[28] = (x2y2 - (3.0 / 35.0)) * x; - out[29] = (x2 * z) * xy; - out[30] = (x2y2 - (3.0 / 35.0)) * y; - out[31] = (x2y2 - (1.0 / 35.0)) * z; - out[32] = (y4 - (3.0 / 35.0)) * x; - out[33] = (y2 * z) * xy; - out[34] = (y4 - (3.0 / 7.0)) * y; - out[35] = (y4 - (3.0 / 35.0)) * z; -} - -// rosella_direct._logical_field: the padded lane grid as [77][36][2] float64. -void logical_field(const std::vector& padded, std::vector* out) { - out->assign(static_cast(kHybridBands) * kDirectionTerms * 2u, 0.0); - for (int band = 0; band < kHybridBands; ++band) { - const int block = band / 4; - const int lane = band % 4; - for (int term = 0; term < kDirectionTerms; ++term) { - for (int component = 0; component < 2; ++component) { - const std::size_t source = - static_cast(lane + 4 * (term * 2 + component + 72 * block)); - (*out)[(static_cast(band) * kDirectionTerms + - static_cast(term)) * 2u + - static_cast(component)] = - source < padded.size() ? static_cast(padded[source]) : 0.0; - } - } - } -} - -// rosella_direct._round_away_from_zero. -double round_away_from_zero(double value) { - return value >= 0.0 ? std::floor(value + 0.5) : std::ceil(value - 0.5); -} - -// rosella_direct._q15_position: ADM coordinates on the Rosella metadata grid. -void q15_position(const double position[3], int encoded[3]) { - const double raw[3] = { - std::min(std::max((position[0] + 1.0) * 0.5, 0.0), 1.0), - std::min(std::max((1.0 - position[1]) * 0.5, 0.0), 1.0), - std::min(std::max(position[2], -1.0), 1.0), - }; - for (int axis = 0; axis < 3; ++axis) { - encoded[axis] = static_cast( - std::min(round_away_from_zero(raw[axis] * 32768.0), 32767.0)); - } -} - -struct ProfileGeometry { - double direction[3] = {1.0, 0.0, 0.0}; - double radius = 0.0; - double clamped = 0.0; - double alpha = 0.0; -}; - -// rosella_direct._profile_geometry. -ProfileGeometry profile_geometry(const RosellaModel& model, const double position[3], - int profile_index) { - const RosellaDistanceProfile& profile = model.profiles[static_cast(profile_index)]; - int encoded[3] = {0, 0, 0}; - q15_position(position, encoded); - const double q_front = 1.0 - 2.0 * static_cast(encoded[1]) / 32768.0; - const double q_x = 2.0 * static_cast(encoded[0]) / 32768.0 - 1.0; - const double q_vertical = static_cast(encoded[2]) / 32768.0; - double mapped_front = q_front; - double mapped_lateral = -q_x; - double mapped_vertical = q_vertical; - if (model.header_integer_fields[0] != 0) { - if (q_x == 0.0 && q_front == 0.0) { - mapped_front = 0.0; - mapped_lateral = 0.0; - mapped_vertical = q_vertical; - } else { - const double horizontal_max = std::max(std::abs(q_x), std::abs(q_front)); - const double horizontal_norm = (q_x / horizontal_max) * (q_x / horizontal_max) + - (q_front / horizontal_max) * (q_front / horizontal_max); - double vertical_norm = 1.0; - if (q_vertical != 0.0) { - const double smaller = std::min(std::abs(q_vertical), horizontal_max); - const double larger = std::max(std::abs(q_vertical), horizontal_max); - vertical_norm = 1.0 + (smaller / larger) * (smaller / larger); - } - const double horizontal_factor = 1.0 / std::sqrt(horizontal_norm * vertical_norm); - const double vertical_factor = 1.0 / std::sqrt(vertical_norm); - mapped_front = q_front * horizontal_factor; - mapped_lateral = -q_x * horizontal_factor; - mapped_vertical = q_vertical * vertical_factor; - } - } - double scaled[3] = {mapped_front * static_cast(profile.axis_scales_internal[2]), - mapped_lateral * static_cast(profile.axis_scales_internal[0]), - mapped_vertical * static_cast(profile.axis_scales_internal[1])}; - double ray = 1.0; - for (int axis = 0; axis < 3; ++axis) { - const double value = scaled[axis]; - const double lower = static_cast(profile.bounds[static_cast(axis) * 2u]); - const double upper = - static_cast(profile.bounds[static_cast(axis) * 2u + 1u]); - if (value < lower) { - ray = std::min(ray, lower / value); - } else if (value > upper) { - ray = std::min(ray, upper / value); - } - } - if (ray < 1.0) { - scaled[0] *= ray; - scaled[1] *= ray; - scaled[2] *= ray; - } - ProfileGeometry result; - result.radius = std::sqrt(scaled[0] * scaled[0] + scaled[1] * scaled[1] + - scaled[2] * scaled[2]); - result.clamped = std::max(result.radius, static_cast(profile.minimum_normalized_radius)); - result.alpha = result.radius / result.clamped; - if (result.radius > 1.0e-30) { - for (int axis = 0; axis < 3; ++axis) { - result.direction[axis] = scaled[axis] / result.radius; - } - } - return result; -} - -struct EarGeometry { - double basis_minus[kDirectionTerms] = {}; - double basis_plus[kDirectionTerms] = {}; - double path_minus = 0.0; - double path_plus = 0.0; -}; - -// rosella_direct._ear_geometry. -EarGeometry ear_geometry(const RosellaModel& model, const RosellaDistanceProfile& profile, - const double direction[3], double clamped, double offset, - double correction) { - const double x = direction[0]; - const double y = direction[1]; - const double z = direction[2]; - const double inverse_distance = static_cast(profile.inverse_distance_per_m); - const double ear = offset * inverse_distance / clamped; - const double y_minus = y - ear; - const double y_plus = y + ear; - const double common = x * x + z * z; - const double length_minus = std::sqrt(y_minus * y_minus + common); - const double length_plus = std::sqrt(y_plus * y_plus + common); - EarGeometry result; - direction_basis(x / length_minus, y_minus / length_minus, z / length_minus, - result.basis_minus); - direction_basis(x / length_plus, y_plus / length_plus, z / length_plus, result.basis_plus); - result.path_minus = length_minus * clamped; - result.path_plus = length_plus * clamped; - if (correction != 0.0) { - const double multiplier = 2.0 * correction * inverse_distance; - double left = 0.0; - double right = 0.0; - for (int term = 0; term < kDirectionTerms; ++term) { - left += static_cast(model.vector_left[static_cast(term)]) * - result.basis_minus[term]; - right += static_cast(model.vector_right[static_cast(term)]) * - result.basis_plus[term]; - } - result.path_minus += std::max(left, 0.0) * multiplier; - result.path_plus += std::max(right, 0.0) * multiplier; - } - return result; -} - -// rosella_direct._phase_groups. -void phase_groups(const RosellaModel& model, double delay_samples, Complex* out) { - // The reference starts from np.ones(77) and only rewrites the flagged bands. - std::fill(out, out + kHybridBands, Complex(1.0, 0.0)); - Complex current(1.0, 0.0); - Complex step(1.0, 0.0); - std::size_t value_index = 0u; - for (std::size_t band = 0u; band < model.hybrid_flags.size() && band < kHybridBands; ++band) { - const int flag = model.hybrid_flags[band]; - if (flag != 2) { - if (flag == 1) { - const double angle = - static_cast(model.hybrid_values[value_index]) * delay_samples; - ++value_index; - step = Complex(std::cos(angle), std::sin(angle)); - } - current *= step; - } - out[band] = current; - } -} - -struct DirectResult { - Complex gains[2][kHybridBands] = {}; - double room_send = 0.0; - double physical_radius_m = 0.0; - double normalized_radius = 0.0; - double clamped_radius = 0.0; - double delay_samples = 0.0; - int delayed_ear = -1; -}; - -// rosella_direct.special_lfe_direct. -const DirectResult& special_lfe_direct() { - static const DirectResult result = [] { - DirectResult direct; - const std::vector low = special_lfe_low(); - for (int ear = 0; ear < 2; ++ear) { - for (int band = 0; band < kHybridBands; ++band) { - direct.gains[ear][band] = Complex(low[static_cast(band)], 0.0); - } - } - return direct; - }(); - return result; -} - -// rosella_direct.direct_and_room_send. -DirectResult direct_and_room_send(const RosellaModel& model, - const std::vector& field_left, - const std::vector& field_right, const double position[3], - int profile_index) { - const RosellaDistanceProfile& profile = - model.profiles[static_cast(profile_index)]; - const ProfileGeometry geometry = profile_geometry(model, position, profile_index); - - const EarGeometry delay_ears = - ear_geometry(model, profile, geometry.direction, geometry.clamped, - static_cast(model.model_scalars[1]), - static_cast(model.model_scalars[2])); - const double delay = std::abs(delay_ears.path_plus - delay_ears.path_minus) * - static_cast(profile.distance_scale_m) * - (static_cast(model.sample_rate) / 343.3) * geometry.alpha; - int delayed_ear = -1; - if (delay_ears.path_minus > delay_ears.path_plus) { - delayed_ear = 0; - } else if (delay_ears.path_plus > delay_ears.path_minus) { - delayed_ear = 1; - } - - const EarGeometry weight_ears = - ear_geometry(model, profile, geometry.direction, geometry.clamped, - static_cast(model.model_scalars[3]), - static_cast(model.model_scalars[4])); - const double weight_norm = - std::sqrt(weight_ears.path_minus * weight_ears.path_minus + - weight_ears.path_plus * weight_ears.path_plus); - const double weight_left = weight_ears.path_plus / weight_norm; - const double weight_right = weight_ears.path_minus / weight_norm; - - const double final_offset = static_cast(model.model_scalars[0]); - double basis_minus[kDirectionTerms] = {}; - double basis_plus[kDirectionTerms] = {}; - if (final_offset == 0.0) { - direction_basis(geometry.direction[0], geometry.direction[1], geometry.direction[2], - basis_minus); - std::copy(basis_minus, basis_minus + kDirectionTerms, basis_plus); - } else { - const double x = geometry.direction[0]; - const double y = geometry.direction[1]; - const double z = geometry.direction[2]; - const double ear = - final_offset * static_cast(profile.inverse_distance_per_m) / geometry.clamped; - const double y_minus = y - ear; - const double y_plus = y + ear; - const double common_length = x * x + z * z; - const double length_minus = std::sqrt(y_minus * y_minus + common_length); - const double length_plus = std::sqrt(y_plus * y_plus + common_length); - direction_basis(x / length_minus, y_minus / length_minus, z / length_minus, basis_minus); - direction_basis(x / length_plus, y_plus / length_plus, z / length_plus, basis_plus); - } - - DirectResult result; - Complex left[kHybridBands] = {}; - Complex right[kHybridBands] = {}; - for (int band = 0; band < kHybridBands; ++band) { - double left_real = 0.0; - double left_imag = 0.0; - double right_real = 0.0; - double right_imag = 0.0; - for (int term = 0; term < kDirectionTerms; ++term) { - const std::size_t index = - (static_cast(band) * kDirectionTerms + static_cast(term)) * - 2u; - left_real += field_left[index] * basis_minus[term]; - left_imag += field_left[index + 1u] * basis_minus[term]; - right_real += field_right[index] * basis_plus[term]; - right_imag += field_right[index + 1u] * basis_plus[term]; - } - left[band] = Complex(left_real, left_imag); - right[band] = Complex(right_real, right_imag); - } - if (delayed_ear >= 0) { - Complex phase[kHybridBands]; - phase_groups(model, delay, phase); - Complex* target = delayed_ear == 0 ? left : right; - for (int band = 0; band < kHybridBands; ++band) { - target[band] *= phase[band]; - } - } - - const double effective_radius = - geometry.radius * static_cast(model.header_float_scalars[0]) * - static_cast(profile.distance_scale_m); - double common = 1.0; - double room_send = 0.0; - if (profile_index == static_cast(RosellaProfile::Far) || - profile_index == static_cast(RosellaProfile::Mid)) { - common = 1.0 / std::sqrt(1.0 + static_cast(model.header_float_scalars[1]) * - effective_radius * effective_radius); - room_send = effective_radius * common; - } - - Complex left_term0[kHybridBands]; - Complex right_term0[kHybridBands]; - for (int band = 0; band < kHybridBands; ++band) { - const std::size_t index = static_cast(band) * kDirectionTerms * 2u; - left_term0[band] = Complex(field_left[index], field_left[index + 1u]); - right_term0[band] = Complex(field_right[index], field_right[index + 1u]); - } - const bool weights_are_default_equal = - static_cast(model.model_scalars[3]) == 0.0 && - static_cast(model.model_scalars[4]) == 0.0; - double centre_left = 0.0; - double centre_right = 0.0; - double right_direction_weight = weight_right; - if (weights_are_default_equal) { - centre_left = weight_left * (1.0 - geometry.alpha) * kCentreEqual; - centre_right = centre_left; - right_direction_weight = weight_left; - } else { - centre_left = (1.0 - geometry.alpha) * kCentreAlternate; - centre_right = centre_left; - } - for (int band = 0; band < kHybridBands; ++band) { - result.gains[0][band] = - common * (left[band] * (weight_left * geometry.alpha) + left_term0[band] * centre_left); - result.gains[1][band] = common * (right[band] * (right_direction_weight * geometry.alpha) + - right_term0[band] * centre_right); - } - result.room_send = room_send; - result.physical_radius_m = static_cast(profile.distance_scale_m) * geometry.radius; - result.normalized_radius = geometry.radius; - result.clamped_radius = geometry.clamped; - result.delay_samples = delay; - result.delayed_ear = delayed_ear; - return result; -} - -// rosella_room._RosellaRoomState: the recursive table-A room model. -class RoomState { -public: - explicit RoomState(const RosellaModel& model) : model_(model) { - bands_ = std::min(64, model.table_a_dimension); - delays_ = model.table_a_four_integers; - capacity_ = 0; - for (const int delay : delays_) { - capacity_ = std::max(capacity_, delay); - } - // The vectors are stored column-major (numpy's order="F"). - matrix_.assign(16u, 0.0); - for (int row = 0; row < 4; ++row) { - for (int column = 0; column < 4; ++column) { - matrix_[static_cast(row) + 4u * static_cast(column)] = - static_cast(model.table_a_vector16[static_cast(row) + - 4u * static_cast(column)]); - } - } - const std::vector& f8 = model.table_a_filter_8x64_padded; - const std::vector& f4 = model.table_a_filter_4x64_padded; - const std::vector& f16 = model.table_a_filter_16x64_padded; - feedback_real_.assign(static_cast(bands_) * 4u, 0.0); - feedback_imag_.assign(static_cast(bands_) * 4u, 0.0); - output_tap_.assign(static_cast(bands_) * 4u, 0.0); - left_real_.assign(static_cast(bands_) * 4u, 0.0); - left_imag_.assign(static_cast(bands_) * 4u, 0.0); - right_real_.assign(static_cast(bands_) * 4u, 0.0); - right_imag_.assign(static_cast(bands_) * 4u, 0.0); - for (int band = 0; band < bands_; ++band) { - const int group = band / 4; - const int lane = band % 4; - for (int tap = 0; tap < 4; ++tap) { - const std::size_t target = static_cast(band) * 4u + - static_cast(tap); - const std::size_t g8 = ((static_cast(group) * 4u + - static_cast(tap)) * 2u) * - 4u + - static_cast(lane); - feedback_real_[target] = static_cast(f8[g8]); - feedback_imag_[target] = static_cast(f8[g8 + 4u]); - output_tap_[target] = static_cast( - f4[(static_cast(group) * 4u + static_cast(tap)) * 4u + - static_cast(lane)]); - const std::size_t g16 = ((static_cast(group) * 4u + - static_cast(tap)) * 4u) * - 4u; - left_real_[target] = static_cast(f16[g16 + static_cast(lane)]); - left_imag_[target] = - static_cast(f16[g16 + 4u + static_cast(lane)]); - right_real_[target] = - static_cast(f16[g16 + 8u + static_cast(lane)]); - right_imag_[target] = - static_cast(f16[g16 + 12u + static_cast(lane)]); - } - } - allpass_gain_.resize(model.table_a_option_values.size()); - for (std::size_t index = 0u; index < model.table_a_option_values.size(); ++index) { - allpass_gain_[index] = static_cast(model.table_a_option_values[index]); - } - allpass_delay_ = model.table_a_option_ids; - for (const int delay : allpass_delay_) { - allpass_real_.emplace_back(static_cast(delay) * - static_cast(bands_), - 0.0); - allpass_imag_.emplace_back(static_cast(delay) * - static_cast(bands_), - 0.0); - } - allpass_position_.assign(allpass_real_.size(), 0); - memory_real_.assign(static_cast(capacity_) * static_cast(bands_) * 4u, - 0.0); - memory_imag_.assign(memory_real_.size(), 0.0); - extra_matrices_.resize(model.table_a_extra_indices.size()); - for (std::size_t index = 0u; index < extra_matrices_.size(); ++index) { - extra_matrices_[index].assign(16u, 0.0); - for (int row = 0; row < 4; ++row) { - for (int column = 0; column < 4; ++column) { - extra_matrices_[index][static_cast(row) + - 4u * static_cast(column)] = - static_cast( - model.table_a_extra_vectors[index * 16u + static_cast(row) + - 4u * static_cast(column)]); - } - } - } - } - - void reset() { - for (std::vector& value : allpass_real_) { - std::fill(value.begin(), value.end(), 0.0); - } - for (std::vector& value : allpass_imag_) { - std::fill(value.begin(), value.end(), 0.0); - } - std::fill(allpass_position_.begin(), allpass_position_.end(), 0); - std::fill(memory_real_.begin(), memory_real_.end(), 0.0); - std::fill(memory_imag_.begin(), memory_imag_.end(), 0.0); - position_ = 0; - } - - Status process_slot(const Complex* room_send, Complex* output) { - if (static_cast(model_.table_a_scalar) >= 0.5) { - return render_fail(JOC_ERR_NOT_SUPPORTED, "alternate Rosella table-A room mode"); - } - const std::size_t bands = static_cast(bands_); - std::vector input_real(bands, 0.0); - std::vector input_imag(bands, 0.0); - for (std::size_t band = 0u; band < bands; ++band) { - input_real[band] = room_send[band].real() * kRoomInputGain; - input_imag[band] = room_send[band].imag() * kRoomInputGain; - } - for (std::size_t index = 0u; index < allpass_gain_.size(); ++index) { - const std::size_t delay = static_cast(allpass_delay_[index]); - const std::size_t position = static_cast(allpass_position_[index]); - const double gain = allpass_gain_[index]; - for (std::size_t band = 0u; band < bands; ++band) { - const std::size_t cell = position * bands + band; - const double previous_real = allpass_real_[index][cell]; - const double previous_imag = allpass_imag_[index][cell]; - const double residual_real = input_real[band] - previous_real * gain; - const double residual_imag = input_imag[band] - previous_imag * gain; - input_real[band] = residual_real * gain + previous_real; - input_imag[band] = residual_imag * gain + previous_imag; - allpass_real_[index][cell] = residual_real; - allpass_imag_[index][cell] = residual_imag; - } - allpass_position_[index] = static_cast((position + 1u) % delay); - } - - std::vector branch_real(bands * 4u, 0.0); - std::vector branch_imag(bands * 4u, 0.0); - std::vector delayed_real(bands * 4u, 0.0); - std::vector delayed_imag(bands * 4u, 0.0); - for (int branch = 0; branch < 4; ++branch) { - const std::size_t slot = - static_cast((position_ - delays_[static_cast(branch)] + - capacity_) % - capacity_); - for (std::size_t band = 0u; band < bands; ++band) { - const std::size_t source = (slot * bands + band) * 4u + static_cast(branch); - delayed_real[band * 4u + static_cast(branch)] = memory_real_[source]; - delayed_imag[band * 4u + static_cast(branch)] = memory_imag_[source]; - } - } - for (std::size_t band = 0u; band < bands; ++band) { - for (int row = 0; row < 4; ++row) { - double real = input_real[band]; - double imag = input_imag[band]; - for (int column = 0; column < 4; ++column) { - const double coefficient = - matrix_[static_cast(row) + 4u * static_cast(column)]; - real += delayed_real[band * 4u + static_cast(column)] * coefficient; - imag += delayed_imag[band * 4u + static_cast(column)] * coefficient; - } - branch_real[band * 4u + static_cast(row)] = real; - branch_imag[band * 4u + static_cast(row)] = imag; - } - } - - const std::size_t tap_slot = static_cast( - (position_ - model_.table_a_integer + capacity_) % capacity_); - std::vector tap_real(bands * 4u, 0.0); - std::vector tap_imag(bands * 4u, 0.0); - for (std::size_t band = 0u; band < bands; ++band) { - for (int lane = 0; lane < 4; ++lane) { - const std::size_t source = (tap_slot * bands + band) * 4u + static_cast(lane); - tap_real[band * 4u + static_cast(lane)] = memory_real_[source]; - tap_imag[band * 4u + static_cast(lane)] = memory_imag_[source]; - } - } - for (std::size_t cell = 0u; cell < branch_real.size(); ++cell) { - const double real = branch_real[cell]; - const double imag = branch_imag[cell]; - memory_real_[(static_cast(position_) * bands) * 4u + cell] = - real * feedback_real_[cell] - imag * feedback_imag_[cell]; - memory_imag_[(static_cast(position_) * bands) * 4u + cell] = - imag * feedback_real_[cell] + real * feedback_imag_[cell]; - } - position_ = (position_ + 1) % capacity_; - - std::vector extra_real(bands * 4u, 0.0); - std::vector extra_imag(bands * 4u, 0.0); - for (std::size_t index = 0u; index < model_.table_a_extra_indices.size(); ++index) { - const std::size_t delay = - static_cast(model_.table_a_extra_indices[index]); - const std::size_t slot = static_cast( - (position_ - static_cast(delay) - 1 + capacity_ * 2) % capacity_); - for (std::size_t band = 0u; band < bands; ++band) { - for (int row = 0; row < 4; ++row) { - double mixed_real = 0.0; - double mixed_imag = 0.0; - for (int column = 0; column < 4; ++column) { - const std::size_t source = - (slot * bands + band) * 4u + static_cast(column); - const double coefficient = - extra_matrices_[index][static_cast(row) + - 4u * static_cast(column)]; - mixed_real += memory_real_[source] * coefficient; - mixed_imag += memory_imag_[source] * coefficient; - } - const int group = static_cast(band) / 4; - const int lane = static_cast(band) % 4; - const std::size_t field = - (index * 20u + static_cast(group)) * 2u * 4u; - const double coefficient_real = static_cast( - model_.table_a_extra_fields_padded[field + static_cast(lane)]); - const double coefficient_imag = static_cast( - model_.table_a_extra_fields_padded[field + 4u + static_cast(lane)]); - const std::size_t target = band * 4u + static_cast(row); - extra_real[target] += - mixed_real * coefficient_real - mixed_imag * coefficient_imag; - extra_imag[target] += - mixed_imag * coefficient_real + mixed_real * coefficient_imag; - } - } - } - - for (std::size_t band = 0u; band < bands; ++band) { - double left_real = 0.0; - double left_imag = 0.0; - double right_real = 0.0; - double right_imag = 0.0; - for (int lane = 0; lane < 4; ++lane) { - const std::size_t cell = band * 4u + static_cast(lane); - const double real = tap_real[cell] * output_tap_[cell] + extra_real[cell]; - const double imag = tap_imag[cell] * output_tap_[cell] + extra_imag[cell]; - left_real += left_real_[cell] * real - left_imag_[cell] * imag; - left_imag += left_imag_[cell] * real + left_real_[cell] * imag; - right_real += right_real_[cell] * real - right_imag_[cell] * imag; - right_imag += right_imag_[cell] * real + right_real_[cell] * imag; - } - output[band] = Complex(left_real, left_imag); - output[kHybridBands + band] = Complex(right_real, right_imag); - } - return Status::success(); - } - -private: - const RosellaModel& model_; - int bands_ = 0; - std::array delays_{}; - int capacity_ = 0; - std::vector matrix_; - std::vector feedback_real_; - std::vector feedback_imag_; - std::vector output_tap_; - std::vector left_real_; - std::vector left_imag_; - std::vector right_real_; - std::vector right_imag_; - std::vector allpass_gain_; - std::vector allpass_delay_; - std::vector> allpass_real_; - std::vector> allpass_imag_; - std::vector allpass_position_; - std::vector memory_real_; - std::vector memory_imag_; - int position_ = 0; - std::vector> extra_matrices_; -}; - -// rosella_room.RosellaRoomFir: the room FIR plus its overlap-add convolution. -class RoomFir { -public: - RoomFir(const RosellaModel& model, int impulse_slots) : length_(impulse_slots) { - RoomState reference(model); - kernel_.assign(static_cast(length_) * 2u * kRoomBands, Complex(0.0, 0.0)); - Complex slot_output[2 * kHybridBands]; - for (int slot = 0; slot < length_; ++slot) { - Complex impulse[kHybridBands]; - for (int band = 0; band < kHybridBands; ++band) { - impulse[band] = Complex(slot == 0 && band < kRoomBands ? 1.0 : 0.0, 0.0); - } - const Status status = reference.process_slot(impulse, slot_output); - if (!status.ok()) { - return; - } - for (int ear = 0; ear < 2; ++ear) { - for (int band = 0; band < kRoomBands; ++band) { - kernel_[(static_cast(slot) * 2u + static_cast(ear)) * - kRoomBands + - static_cast(band)] = - slot_output[ear * kHybridBands + band]; - } - } - } - tail_.assign(static_cast(length_ - 1) * 2u * kRoomBands, Complex(0.0, 0.0)); - valid_ = true; - } - - bool valid() const { return valid_; } - - void reset() { - std::fill(tail_.begin(), tail_.end(), Complex(0.0, 0.0)); - fft_cache_size_ = 0u; - forward_plan_.reset(); - inverse_plan_.reset(); - } - - // rosella_room.RosellaRoomFir.process_chunk. - Status process_chunk(const std::vector& values, std::size_t slots, - std::vector* output) { - output->assign(slots * 2u * kHybridBands, Complex(0.0, 0.0)); - if (slots == 0u) { - return Status::success(); - } - const std::size_t needed = slots + static_cast(length_) - 1u; - std::size_t fft_size = 1u; - while (fft_size < needed) { - fft_size <<= 1u; - } - if (fft_size != fft_cache_size_) { - kernel_fft_.assign(fft_size * 2u * kRoomBands, Complex(0.0, 0.0)); - // The room transforms a fixed length for the whole chunk, so the radix-2 - // twiddle recurrences are materialised once here rather than re-derived - // inside every butterfly of every one of the 3 * kRoomBands transforms. - forward_plan_ = std::make_unique(fft_size, false); - inverse_plan_ = std::make_unique(fft_size, true); - line_.assign(fft_size, Complex(0.0, 0.0)); - for (int ear = 0; ear < 2; ++ear) { - for (int band = 0; band < kRoomBands; ++band) { - for (int slot = 0; slot < length_; ++slot) { - line_[static_cast(slot)] = - kernel_[(static_cast(slot) * 2u + - static_cast(ear)) * - kRoomBands + - static_cast(band)]; - } - std::fill(line_.begin() + length_, line_.end(), Complex(0.0, 0.0)); - dsp::fft_radix2(&line_, *forward_plan_); - for (std::size_t index = 0u; index < fft_size; ++index) { - // Band-major, so that the spectrum multiply below reads both - // of its operands as runs instead of striding through them - // one complex per cache line. Same values, same places. - kernel_fft_[(static_cast(ear) * kRoomBands + - static_cast(band)) * - fft_size + - index] = line_[index]; - } - } - } - fft_cache_size_ = fft_size; - } - // input_fft_ and block_ are indexed linearly and every entry of each is written - // before it is read (the per-band forward transform fills input_fft_ column by - // column, and the inverse transform fills block_ entry by entry), so their zero - // fills were dead. The line buffer, by contrast, is handed to fft_radix2, - // which transforms exactly data->size() entries -- it therefore has to keep the - // exact length and the exact initialisation it had as a local. - if (line_.size() != fft_size) { - line_.assign(fft_size, Complex(0.0, 0.0)); - } - const std::size_t input_fft_size = fft_size * kRoomBands; - if (input_fft_.size() < input_fft_size) { - input_fft_.resize(input_fft_size); - } - for (int band = 0; band < kRoomBands; ++band) { - for (std::size_t slot = 0u; slot < slots; ++slot) { - line_[slot] = values[slot * kHybridBands + static_cast(band)]; - } - std::fill(line_.begin() + static_cast(slots), line_.end(), - Complex(0.0, 0.0)); - dsp::fft_radix2(&line_, *forward_plan_); - for (std::size_t index = 0u; index < fft_size; ++index) { - // Band-major, matching kernel_fft_, so that the spectrum multiply - // reads both operands as runs. - input_fft_[static_cast(band) * fft_size + index] = line_[index]; - } - } - const std::size_t block_size = needed * 2u * kRoomBands; - if (block_.size() < block_size) { - block_.resize(block_size); - } - for (int ear = 0; ear < 2; ++ear) { - for (int band = 0; band < kRoomBands; ++band) { - // Both planes are band-major now, so this is one contiguous complex - // product per (ear, band) and the dispatched kernel puts neighbouring - // complexes in its lanes; each product keeps the caller's four - // multiplies and two roundings. - simd::complex_multiply( - reinterpret_cast(line_.data()), - reinterpret_cast( - input_fft_.data() + static_cast(band) * fft_size), - reinterpret_cast( - kernel_fft_.data() + - (static_cast(ear) * kRoomBands + - static_cast(band)) * - fft_size), - fft_size); - dsp::fft_radix2(&line_, *inverse_plan_); - for (std::size_t slot = 0u; slot < needed; ++slot) { - block_[(slot * 2u + static_cast(ear)) * kRoomBands + - static_cast(band)] = line_[slot]; - } - } - } - const std::size_t tail_length = static_cast(length_ - 1); - for (std::size_t index = 0u; index < tail_length * 2u * kRoomBands; ++index) { - block_[index] += tail_[index]; - } - for (std::size_t slot = 0u; slot < slots; ++slot) { - for (int ear = 0; ear < 2; ++ear) { - for (int band = 0; band < kRoomBands; ++band) { - (*output)[slot * 2u * kHybridBands + static_cast(ear) * kHybridBands + - static_cast(band)] = - block_[(slot * 2u + static_cast(ear)) * kRoomBands + - static_cast(band)]; - } - } - } - for (std::size_t index = 0u; index < tail_length * 2u * kRoomBands; ++index) { - tail_[index] = block_[slots * 2u * kRoomBands + index]; - } - return Status::success(); - } - -private: - int length_ = 0; - bool valid_ = false; - std::vector kernel_; - std::vector tail_; - std::vector kernel_fft_; - std::size_t fft_cache_size_ = 0u; - std::vector line_; // scratch, exactly fft_size entries - std::vector input_fft_; // scratch, [fft_size][kRoomBands], fully written - std::vector block_; // scratch, [needed][2][kRoomBands], fully written - std::unique_ptr forward_plan_; - std::unique_ptr inverse_plan_; -}; - // rosella_core.RosellaRenderer: per-source direct gains plus the room send. class Core { public: @@ -872,9 +32,10 @@ public: model_ = &model; field_left_ = &field_left; field_right_ = &field_right; - room_ = std::make_unique(model, room_impulse_slots); - if (!room_->valid()) { - return render_fail(JOC_ERR_NOT_SUPPORTED, "alternate Rosella table-A room mode"); + room_ = std::make_unique(); + const Status room_status = room_->configure(model, room_impulse_slots); + if (!room_status.ok()) { + return room_status; } gains_.assign(static_cast(kSourceChannels) * 2u * kHybridBands, Complex(0.0, 0.0)); @@ -908,8 +69,11 @@ public: if (special_lfe) { result = special_lfe_direct(); } else { - result = direct_and_room_send(*model_, *field_left_, *field_right_, position, - effective_profile); + const Status status = direct_and_room_send(*model_, *field_left_, *field_right_, + position, effective_profile, &result); + if (!status.ok()) { + return status; + } } for (int ear = 0; ear < 2; ++ear) { for (int band = 0; band < kHybridBands; ++band) { @@ -989,7 +153,7 @@ private: const RosellaModel* model_ = nullptr; const std::vector* field_left_ = nullptr; const std::vector* field_right_ = nullptr; - std::unique_ptr room_; + std::unique_ptr room_; std::vector gains_; std::vector room_sends_; std::vector keys_; @@ -1080,8 +244,8 @@ struct RosellaRuntime::Impl { static_cast(first_slot * kHybridBands)); } // The room runs once for the whole chunk, as the reference does. Its output - // keeps a zero fill of its own: only the first kRoomBands of each ear are - // written, and the caller adds the whole 77-band slot back into `rendered`. + // keeps a zero fill of its own: only the first 64 of the 77 bands of each ear + // are written, and the caller adds the whole 77-band slot back into `rendered`. Status status = core.room_process(room_sends_buffer_, slots, &room_out_); if (!status.ok()) { return status; @@ -1196,8 +360,8 @@ Status RosellaRuntime::open(const RosellaModel& model, const RosellaRenderOption } impl_->model = model; impl_->options = options; - logical_field(impl_->model.field_left_padded, &impl_->field_left); - logical_field(impl_->model.field_right_padded, &impl_->field_right); + impl_->field_left = rosella_logical_field(impl_->model.field_left_padded); + impl_->field_right = rosella_logical_field(impl_->model.field_right_padded); // The core and the room keep references, so they must point at this runtime's // own copy of the model rather than at the caller's. Status status = impl_->core.configure(impl_->model, impl_->field_left, impl_->field_right, diff --git a/src/hrtf/sofa_field.cpp b/src/hrtf/sofa_field.cpp index 70909cd..5bd7931 100644 --- a/src/hrtf/sofa_field.cpp +++ b/src/hrtf/sofa_field.cpp @@ -1447,6 +1447,14 @@ Status canonicalize_sofa(const SofaHrir& sofa, CanonicalHrtf* out) { canonical.taps = static_cast(taps); canonical.left_receiver_index = left_index; canonical.right_receiver_index = right_index; + canonical.receiver_position_cartesian_m.assign(6u, 0.0); + for (int ear = 0; ear < 2; ++ear) { + const int receiver = ear == 0 ? left_index : right_index; + for (int axis = 0; axis < 3; ++axis) { + canonical.receiver_position_cartesian_m[static_cast(ear * 3 + axis)] = + receiver_cartesian[static_cast(receiver * 3 + axis)]; + } + } canonical.source_position_cartesian_m = source_local; canonical.unit_directions.resize(measurements * 3u, 0.0); for (std::size_t row = 0u; row < measurements; ++row) { diff --git a/src/hrtf/sofa_field.h b/src/hrtf/sofa_field.h index bb80fba..61313ba 100644 --- a/src/hrtf/sofa_field.h +++ b/src/hrtf/sofa_field.h @@ -55,6 +55,9 @@ struct CanonicalHrtf { std::vector measurement_radius_m; // [M] std::vector hrir; // [M,2,N] canonical L/R std::vector delay_samples; // [M,2], not applied + // ReceiverPosition in listener-local metres, left then right (the SOFA + // variable is one constant pair for the whole file). + std::vector receiver_position_cartesian_m; // [2,3] }; // Port of load_simple_free_field_hrir(): strict SimpleFreeFieldHRIR import. diff --git a/src/hrtf/spherical_mesh.cpp b/src/hrtf/spherical_mesh.cpp new file mode 100644 index 0000000..f093dfd --- /dev/null +++ b/src/hrtf/spherical_mesh.cpp @@ -0,0 +1,531 @@ + +#include "hrtf/spherical_mesh.h" + +#include +#include +#include +#include + +namespace joc::hrtf { +namespace { + +constexpr double kPi = 3.141592653589793238462643383279502884; + +Status mesh_error(const char* message) { + return Status::fail(JOC_ERR_INVALID_ARGUMENT, joc::stage::kRender, message); +} + +void normalize(double vector[3]) { + const double length = std::sqrt(vector[0] * vector[0] + vector[1] * vector[1] + + vector[2] * vector[2]); + if (length > 0.0) { + vector[0] /= length; + vector[1] /= length; + vector[2] /= length; + } +} + +void cross(const double a[3], const double b[3], double out[3]) { + out[0] = a[1] * b[2] - a[2] * b[1]; + out[1] = a[2] * b[0] - a[0] * b[2]; + out[2] = a[0] * b[1] - a[1] * b[0]; +} + +double dot(const double a[3], const double b[3]) { + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; +} + +// Solid angle of the spherical triangle (a, b, c), Van Oosterom & Strackee. +double triangle_area(const double a[3], const double b[3], const double c[3]) { + const double determinant = a[0] * (b[1] * c[2] - b[2] * c[1]) - + a[1] * (b[0] * c[2] - b[2] * c[0]) + + a[2] * (b[0] * c[1] - b[1] * c[0]); + const double denominator = 1.0 + dot(a, b) + dot(b, c) + dot(c, a); + return 2.0 * std::atan2(std::fabs(determinant), denominator); +} + +// In-place inverse of a row-major 3x3 matrix. Returns false when singular. +bool invert3(const double matrix[9], double out[9]) { + const double c00 = matrix[4] * matrix[8] - matrix[5] * matrix[7]; + const double c01 = matrix[5] * matrix[6] - matrix[3] * matrix[8]; + const double c02 = matrix[3] * matrix[7] - matrix[4] * matrix[6]; + const double determinant = matrix[0] * c00 + matrix[1] * c01 + matrix[2] * c02; + if (!std::isfinite(determinant) || std::fabs(determinant) < 1e-12) { + return false; + } + const double inverse = 1.0 / determinant; + out[0] = c00 * inverse; + out[1] = (matrix[2] * matrix[7] - matrix[1] * matrix[8]) * inverse; + out[2] = (matrix[1] * matrix[5] - matrix[2] * matrix[4]) * inverse; + out[3] = c01 * inverse; + out[4] = (matrix[0] * matrix[8] - matrix[2] * matrix[6]) * inverse; + out[5] = (matrix[2] * matrix[3] - matrix[0] * matrix[5]) * inverse; + out[6] = c02 * inverse; + out[7] = (matrix[1] * matrix[6] - matrix[0] * matrix[7]) * inverse; + out[8] = (matrix[0] * matrix[4] - matrix[1] * matrix[3]) * inverse; + return true; +} + +struct HullFace { + int vertex[3] = {0, 0, 0}; + double normal[3] = {0.0, 0.0, 0.0}; + bool alive = true; +}; + +} // namespace + +Status SphericalMesh::build(const std::vector& directions, std::size_t count) { + if (count < 4 || directions.size() < count * 3) { + return mesh_error("spherical mesh needs at least four directions"); + } + directions_.clear(); + faces_.clear(); + inverse_.clear(); + max_triangle_edge_degrees_ = 0.0; + + // Merge coincident directions, keeping the first occurrence (reference order). + std::vector rounded(count * 3); + for (std::size_t index = 0; index < count * 3; ++index) { + rounded[index] = std::round(directions[index] * 1e10) / 1e10; + } + std::vector kept; + std::size_t slot_count = 0; + for (std::size_t index = 0; index < count; ++index) { + std::size_t found = slot_count; + for (std::size_t slot = 0; slot < kept.size(); ++slot) { + const std::size_t first = kept[slot]; + if (rounded[first * 3] == rounded[index * 3] && + rounded[first * 3 + 1] == rounded[index * 3 + 1] && + rounded[first * 3 + 2] == rounded[index * 3 + 2]) { + found = slot; + break; + } + } + if (found == slot_count) { + kept.push_back(index); + ++slot_count; + } + } + if (kept.size() < 4) { + return mesh_error("spherical mesh needs four distinct directions"); + } + directions_.resize(kept.size() * 3); + for (std::size_t slot = 0; slot < kept.size(); ++slot) { + double value[3] = {directions[kept[slot] * 3], directions[kept[slot] * 3 + 1], + directions[kept[slot] * 3 + 2]}; + if (!std::isfinite(value[0]) || !std::isfinite(value[1]) || !std::isfinite(value[2])) { + return mesh_error("measurement direction is not finite"); + } + normalize(value); + std::copy(value, value + 3, directions_.begin() + static_cast(slot * 3)); + } + build_faces(); + compute_face_inverses(); + return Status::success(); +} + +void SphericalMesh::build_faces() { + const std::size_t count = vertex_count(); + const auto point = [this](std::size_t index) { + return directions_.data() + index * 3; + }; + + // Initial tetrahedron: the farthest pair, then the farthest point from that + // line, then the farthest point from that plane. + std::size_t i0 = 0; + std::size_t i1 = 0; + double best = -1.0; + for (std::size_t index = 0; index < count; ++index) { + const double dx = point(index)[0] - point(i0)[0]; + const double dy = point(index)[1] - point(i0)[1]; + const double dz = point(index)[2] - point(i0)[2]; + const double distance = dx * dx + dy * dy + dz * dz; + if (distance > best) { + best = distance; + i1 = index; + } + } + if (best <= 0.0) { + return; + } + std::size_t i2 = 0; + best = -1.0; + double edge[3]; + for (std::size_t index = 0; index < count; ++index) { + double a[3]; + double b[3]; + for (int axis = 0; axis < 3; ++axis) { + a[axis] = point(i1)[axis] - point(i0)[axis]; + b[axis] = point(index)[axis] - point(i0)[axis]; + } + cross(a, b, edge); + const double area = dot(edge, edge); + if (area > best) { + best = area; + i2 = index; + } + } + if (best <= 1e-24) { + return; + } + std::size_t i3 = 0; + best = -1.0; + double plane[3]; + { + double a[3]; + double b[3]; + for (int axis = 0; axis < 3; ++axis) { + a[axis] = point(i1)[axis] - point(i0)[axis]; + b[axis] = point(i2)[axis] - point(i0)[axis]; + } + cross(a, b, plane); + } + for (std::size_t index = 0; index < count; ++index) { + double delta[3]; + for (int axis = 0; axis < 3; ++axis) { + delta[axis] = point(index)[axis] - point(i0)[axis]; + } + const double volume = std::fabs(dot(plane, delta)); + if (volume > best) { + best = volume; + i3 = index; + } + } + if (best <= 1e-24) { + return; + } + + // Centroid of the tetrahedron is strictly inside the hull, so a face normal is + // outward when it points away from it. + double centroid[3] = {0.0, 0.0, 0.0}; + const std::size_t seed[4] = {i0, i1, i2, i3}; + for (std::size_t index = 0; index < 4; ++index) { + for (int axis = 0; axis < 3; ++axis) { + centroid[axis] += point(seed[index])[axis] * 0.25; + } + } + std::vector faces; + faces.reserve(count * 2); + const int seed_index[4] = {static_cast(i0), static_cast(i1), + static_cast(i2), static_cast(i3)}; + const int initial[4][3] = {{0, 1, 2}, {0, 1, 3}, {0, 2, 3}, {1, 2, 3}}; + for (const int* triple : initial) { + HullFace face; + for (int corner = 0; corner < 3; ++corner) { + face.vertex[corner] = seed_index[triple[corner]]; + } + double a[3]; + double b[3]; + for (int axis = 0; axis < 3; ++axis) { + a[axis] = point(face.vertex[1])[axis] - point(face.vertex[0])[axis]; + b[axis] = point(face.vertex[2])[axis] - point(face.vertex[0])[axis]; + } + cross(a, b, face.normal); + double to_center[3]; + for (int axis = 0; axis < 3; ++axis) { + to_center[axis] = centroid[axis] - point(face.vertex[0])[axis]; + } + if (dot(face.normal, to_center) > 0.0) { + std::swap(face.vertex[1], face.vertex[2]); + for (int axis = 0; axis < 3; ++axis) { + face.normal[axis] = -face.normal[axis]; + } + } + faces.push_back(face); + } + + struct Edge { + int a; + int b; + int count; + }; + std::vector boundary; + std::vector visible; + for (std::size_t index = 0; index < count; ++index) { + const double* p = point(index); + bool seed_point = false; + for (int corner = 0; corner < 4; ++corner) { + if (seed_index[corner] == static_cast(index)) { + seed_point = true; + break; + } + } + if (seed_point) { + continue; + } + visible.clear(); + for (std::size_t face_index = 0; face_index < faces.size(); ++face_index) { + HullFace& face = faces[face_index]; + if (!face.alive) { + continue; + } + double delta[3]; + for (int axis = 0; axis < 3; ++axis) { + delta[axis] = p[axis] - point(face.vertex[0])[axis]; + } + if (dot(face.normal, delta) > 1e-12) { + visible.push_back(face_index); + } + } + if (visible.empty()) { + continue; + } + boundary.clear(); + const auto add_edge = [&boundary](int a, int b) { + for (Edge& edge : boundary) { + if ((edge.a == a && edge.b == b) || (edge.a == b && edge.b == a)) { + ++edge.count; + return; + } + } + boundary.push_back(Edge{a, b, 1}); + }; + for (const std::size_t face_index : visible) { + const HullFace& face = faces[face_index]; + add_edge(face.vertex[0], face.vertex[1]); + add_edge(face.vertex[1], face.vertex[2]); + add_edge(face.vertex[2], face.vertex[0]); + } + for (const std::size_t face_index : visible) { + faces[face_index].alive = false; + } + for (const Edge& edge : boundary) { + if (edge.count != 1) { + continue; + } + HullFace face; + face.vertex[0] = edge.a; + face.vertex[1] = edge.b; + face.vertex[2] = static_cast(index); + double a[3]; + double b[3]; + for (int axis = 0; axis < 3; ++axis) { + a[axis] = point(face.vertex[1])[axis] - point(face.vertex[0])[axis]; + b[axis] = point(face.vertex[2])[axis] - point(face.vertex[0])[axis]; + } + cross(a, b, face.normal); + double inside[3]; + for (int axis = 0; axis < 3; ++axis) { + inside[axis] = centroid[axis] - point(face.vertex[0])[axis]; + } + if (dot(face.normal, inside) > 0.0) { + std::swap(face.vertex[1], face.vertex[2]); + for (int axis = 0; axis < 3; ++axis) { + face.normal[axis] = -face.normal[axis]; + } + } + faces.push_back(face); + } + } + + faces_.clear(); + max_triangle_edge_degrees_ = 0.0; + for (const HullFace& face : faces) { + if (!face.alive) { + continue; + } + faces_.push_back(face.vertex[0]); + faces_.push_back(face.vertex[1]); + faces_.push_back(face.vertex[2]); + const double* a = point(face.vertex[0]); + const double* b = point(face.vertex[1]); + const double* c = point(face.vertex[2]); + for (int first = 0; first < 3; ++first) { + for (int second = first + 1; second < 3; ++second) { + const double* u = point(face.vertex[first]); + const double* v = point(face.vertex[second]); + const double edge_dot = std::clamp(dot(u, v), -1.0, 1.0); + const double degrees = std::acos(edge_dot) * 180.0 / kPi; + max_triangle_edge_degrees_ = std::max(max_triangle_edge_degrees_, degrees); + } + } + } +} + +void SphericalMesh::compute_face_inverses() { + inverse_.assign(face_count() * 9, 0.0); + std::vector kept; + for (std::size_t face_index = 0; face_index < face_count(); ++face_index) { + const int* ids = faces_.data() + face_index * 3; + double matrix[9]; + // Columns are the vertices: the barycentric weights of q are the solution + // of matrix * w = q, so the inverse is applied to the query afterwards. + for (int corner = 0; corner < 3; ++corner) { + const double* vertex = directions_.data() + ids[corner] * 3; + for (int axis = 0; axis < 3; ++axis) { + matrix[axis * 3 + corner] = vertex[axis]; + } + } + if (!invert3(matrix, inverse_.data() + face_index * 9)) { + continue; + } + kept.push_back(static_cast(face_index)); + } + if (kept.size() != face_count()) { + // Drop singular faces: they cannot contain a query direction strictly. + std::vector compact; + std::vector compact_inverse; + for (const int face_index : kept) { + for (int corner = 0; corner < 3; ++corner) { + compact.push_back(faces_[face_index * 3 + corner]); + } + for (int value = 0; value < 9; ++value) { + compact_inverse.push_back(inverse_[face_index * 9 + value]); + } + } + faces_ = compact; + inverse_ = compact_inverse; + } +} + +std::size_t SphericalMesh::nearest(const double direction[3], double* distance) const { + const std::size_t count = vertex_count(); + std::size_t best = 0; + double best_distance = std::numeric_limits::infinity(); + for (std::size_t index = 0; index < count; ++index) { + const double* vertex = directions_.data() + index * 3; + const double dx = vertex[0] - direction[0]; + const double dy = vertex[1] - direction[1]; + const double dz = vertex[2] - direction[2]; + const double squared = dx * dx + dy * dy + dz * dz; + if (squared < best_distance) { + best_distance = squared; + best = index; + } + } + if (distance != nullptr) { + *distance = std::sqrt(best_distance); + } + return best; +} + +Status SphericalMesh::weights(const double direction[3], std::vector* ids, + std::vector* weights) const { + if (ids == nullptr || weights == nullptr) { + return mesh_error("weight output is null"); + } + double query[3] = {direction[0], direction[1], direction[2]}; + const double length = std::sqrt(dot(query, query)); + if (!std::isfinite(length) || length <= 1e-12) { + return mesh_error("query direction is degenerate"); + } + normalize(query); + double distance = 0.0; + const std::size_t nearest_index = nearest(query, &distance); + ids->clear(); + weights->clear(); + if (distance < 1e-10) { + ids->push_back(static_cast(nearest_index)); + weights->push_back(1.0); + return Status::success(); + } + for (std::size_t face_index = 0; face_index < face_count(); ++face_index) { + const double* inverse = inverse_.data() + face_index * 9; + const double barycentric[3] = { + inverse[0] * query[0] + inverse[1] * query[1] + inverse[2] * query[2], + inverse[3] * query[0] + inverse[4] * query[1] + inverse[5] * query[2], + inverse[6] * query[0] + inverse[7] * query[1] + inverse[8] * query[2]}; + if (std::min({barycentric[0], barycentric[1], barycentric[2]}) < -1e-10) { + continue; + } + double total = 0.0; + double clamped[3]; + for (int corner = 0; corner < 3; ++corner) { + clamped[corner] = std::max(barycentric[corner], 0.0); + total += clamped[corner]; + } + if (total <= 0.0) { + continue; + } + for (int corner = 0; corner < 3; ++corner) { + ids->push_back(faces_[face_index * 3 + corner]); + weights->push_back(clamped[corner] / total); + } + return Status::success(); + } + // The query fell outside every triangle (only possible on numerical margins). + ids->push_back(static_cast(nearest_index)); + weights->push_back(1.0); + return Status::success(); +} + +void SphericalMesh::voronoi_areas(std::vector* areas) const { + const std::size_t vertices = vertex_count(); + const std::size_t face_total = face_count(); + areas->assign(vertices, 0.0); + if (vertices == 0 || face_total == 0) { + return; + } + // Circumcenter of a triangle of unit vectors is its normalized face normal. + std::vector circumcenters(face_total * 3, 0.0); + for (std::size_t face_index = 0; face_index < face_total; ++face_index) { + const int* ids = faces_.data() + face_index * 3; + const double* a = directions_.data() + ids[0] * 3; + const double* b = directions_.data() + ids[1] * 3; + const double* c = directions_.data() + ids[2] * 3; + double normal[3]; + double edge_one[3]; + double edge_two[3]; + for (int axis = 0; axis < 3; ++axis) { + edge_one[axis] = b[axis] - a[axis]; + edge_two[axis] = c[axis] - a[axis]; + } + cross(edge_one, edge_two, normal); + normalize(normal); + std::copy(normal, normal + 3, + circumcenters.begin() + static_cast(face_index * 3)); + } + std::vector> incident(vertices); + for (std::size_t face_index = 0; face_index < face_total; ++face_index) { + for (int corner = 0; corner < 3; ++corner) { + incident[static_cast(faces_[face_index * 3 + corner])] + .push_back(face_index); + } + } + std::vector> ordered; + double total = 0.0; + for (std::size_t vertex = 0; vertex < vertices; ++vertex) { + const double* centre = directions_.data() + vertex * 3; + if (incident[vertex].size() < 3) { + continue; + } + // Tangent basis at the vertex. + double tangent[3] = {0.0, 0.0, 0.0}; + if (std::fabs(centre[0]) < 0.9) { + tangent[0] = 1.0; + } else { + tangent[1] = 1.0; + } + const double projection = dot(tangent, centre); + double axis_one[3]; + double axis_two[3]; + for (int axis = 0; axis < 3; ++axis) { + axis_one[axis] = tangent[axis] - projection * centre[axis]; + } + normalize(axis_one); + cross(centre, axis_one, axis_two); + ordered.clear(); + for (const std::size_t face_index : incident[vertex]) { + const double* point = circumcenters.data() + face_index * 3; + const double angle = std::atan2(dot(point, axis_two), dot(point, axis_one)); + ordered.emplace_back(angle, face_index); + } + std::sort(ordered.begin(), ordered.end()); + double area = 0.0; + for (std::size_t index = 0; index < ordered.size(); ++index) { + const double* first = circumcenters.data() + ordered[index].second * 3; + const double* second = + circumcenters.data() + ordered[(index + 1) % ordered.size()].second * 3; + area += triangle_area(centre, first, second); + } + (*areas)[vertex] = area; + total += area; + } + if (total > 0.0) { + for (double& value : *areas) { + value /= total; + } + } +} + +} // namespace joc::hrtf diff --git a/src/hrtf/spherical_mesh.h b/src/hrtf/spherical_mesh.h new file mode 100644 index 0000000..c292555 --- /dev/null +++ b/src/hrtf/spherical_mesh.h @@ -0,0 +1,50 @@ + +#pragma once + +#include +#include + +#include "foundation/status.h" + +// Spherical interpolation mesh over the measurement directions, ported from the +// reference sofa_hrir.FullHrirField: the convex hull of the unit directions gives +// the containing spherical triangle, its barycentric weights give the local +// interpolation, and the spherical Voronoi cells give the centre weighting. +namespace joc::hrtf { + +class SphericalMesh { +public: + // `directions` is [count][3]; entries need not be unit length. Coincident + // directions are merged (first occurrence wins) and their duplicates must be + // identical, as the reference requires. + Status build(const std::vector& directions, std::size_t count); + + std::size_t vertex_count() const { return directions_.size() / 3; } + std::size_t face_count() const { return faces_.size() / 3; } + const std::vector& directions() const { return directions_; } + const std::vector& faces() const { return faces_; } + double max_triangle_edge_degrees() const { return max_triangle_edge_degrees_; } + + // Barycentric interpolation weights of a query direction: an exact measurement + // returns one id with weight 1, otherwise the containing triangle's vertices + // with non-negative weights summing to one. + Status weights(const double direction[3], std::vector* ids, + std::vector* weights) const; + + // Spherical Voronoi area of every vertex, normalized to sum to one. + void voronoi_areas(std::vector* areas) const; + + // Nearest measurement direction (exact search; the vertex count is small). + std::size_t nearest(const double direction[3], double* distance) const; + +private: + void build_faces(); + void compute_face_inverses(); + + std::vector directions_; // [V][3] unit + std::vector faces_; // [F][3] + std::vector inverse_; // [F][9] row-major inverse of the vertex matrix + double max_triangle_edge_degrees_ = 0.0; +}; + +} // namespace joc::hrtf diff --git a/src/joc_bitstream/bed_layout_map.cpp b/src/joc_bitstream/bed_layout_map.cpp index d5aae8d..d3a0e1d 100644 --- a/src/joc_bitstream/bed_layout_map.cpp +++ b/src/joc_bitstream/bed_layout_map.cpp @@ -3,8 +3,6 @@ #include -#include "speaker/speaker_layouts.h" - namespace joc::joc { namespace { @@ -31,16 +29,16 @@ constexpr StandardBit kStandardBits[10] = { {BedChannel::kLfe2, BedChannel::kCount}, }; -// Table 13 的 17 bit 非标准掩码(LSB 在前)。 +// Table 13 的 17 bit 非标准掩码,按 channel order 排列:RC_L 是最先的声道 +// (array position 16),落在 LSB;RC_LFE2 最后(array position 0)。 constexpr BedChannel kNonStandardBits[17] = { - BedChannel::kLfe2, BedChannel::kRightWide, BedChannel::kLeftWide, - BedChannel::kTopBackRight, BedChannel::kTopBackLeft, - BedChannel::kTopSideRight, BedChannel::kTopSideLeft, - BedChannel::kTopFrontRight, BedChannel::kTopFrontLeft, - BedChannel::kRightRearSurround, BedChannel::kLeftRearSurround, - BedChannel::kRightSurround, BedChannel::kLeftSurround, - BedChannel::kLfe, BedChannel::kCenter, BedChannel::kRight, - BedChannel::kLeft, + BedChannel::kLeft, BedChannel::kRight, BedChannel::kCenter, + BedChannel::kLfe, BedChannel::kLeftSurround, BedChannel::kRightSurround, + BedChannel::kLeftRearSurround, BedChannel::kRightRearSurround, + BedChannel::kTopFrontLeft, BedChannel::kTopFrontRight, + BedChannel::kTopSideLeft, BedChannel::kTopSideRight, + BedChannel::kTopBackLeft, BedChannel::kTopBackRight, + BedChannel::kLeftWide, BedChannel::kRightWide, BedChannel::kLfe2, }; struct ChannelSpec { @@ -50,24 +48,27 @@ struct ChannelSpec { double xyz[3]; }; +// 固定方位:ETSI TS 103 420 V1.2.1 clause B.2.1.4 Table B.10 的 ADM 笛卡尔坐标 +// (x 右、y 前、z 上)。bed 声道的位置只由声道标签决定,与目标输出布局无关。 +// Table B.10 把 Lw/Rw 的 Y 记作 0,677419,即 ADM 62 级网格上的 42/62 = 21/31。 constexpr ChannelSpec kChannels[] = { {BedChannel::kLeft, "L", "RC_L", {-1.0, 1.0, 0.0}}, {BedChannel::kRight, "R", "RC_R", {1.0, 1.0, 0.0}}, {BedChannel::kCenter, "C", "RC_C", {0.0, 1.0, 0.0}}, - {BedChannel::kLfe, "LFE", "RC_LFE", {0.0, 0.0, 0.0}}, + {BedChannel::kLfe, "LFE", "RC_LFE", {-1.0, 1.0, -1.0}}, {BedChannel::kLeftSurround, "Ls", "RC_LS", {-1.0, 0.0, 0.0}}, {BedChannel::kRightSurround, "Rs", "RC_RS", {1.0, 0.0, 0.0}}, {BedChannel::kLeftRearSurround, "Lrs", "RC_LB", {-1.0, -1.0, 0.0}}, {BedChannel::kRightRearSurround, "Rrs", "RC_RB", {1.0, -1.0, 0.0}}, - {BedChannel::kTopFrontLeft, "Ltf", "RC_TFL", {-0.5, 0.5, 1.0}}, - {BedChannel::kTopFrontRight, "Rtf", "RC_TFR", {0.5, 0.5, 1.0}}, + {BedChannel::kTopFrontLeft, "Ltf", "RC_TFL", {-1.0, 1.0, 1.0}}, + {BedChannel::kTopFrontRight, "Rtf", "RC_TFR", {1.0, 1.0, 1.0}}, {BedChannel::kTopSideLeft, "Ltm", "RC_TSL", {-1.0, 0.0, 1.0}}, {BedChannel::kTopSideRight, "Rtm", "RC_TSR", {1.0, 0.0, 1.0}}, - {BedChannel::kTopBackLeft, "Ltr", "RC_TBL", {-0.5, -0.5, 1.0}}, - {BedChannel::kTopBackRight, "Rtr", "RC_TBR", {0.5, -0.5, 1.0}}, - {BedChannel::kLeftWide, "Lw", "RC_LW", {-1.0, 0.5, 0.0}}, - {BedChannel::kRightWide, "Rw", "RC_RW", {1.0, 0.5, 0.0}}, - {BedChannel::kLfe2, "LFE2", "RC_LFE2", {0.0, 0.0, 0.0}}, + {BedChannel::kTopBackLeft, "Ltr", "RC_TBL", {-1.0, -1.0, 1.0}}, + {BedChannel::kTopBackRight, "Rtr", "RC_TBR", {1.0, -1.0, 1.0}}, + {BedChannel::kLeftWide, "Lw", "RC_LW", {-1.0, 21.0 / 31.0, 0.0}}, + {BedChannel::kRightWide, "Rw", "RC_RW", {1.0, 21.0 / 31.0, 0.0}}, + {BedChannel::kLfe2, "LFE2", "RC_LFE2", {1.0, 1.0, -1.0}}, }; const ChannelSpec* spec_of(BedChannel channel) { @@ -168,38 +169,6 @@ const char* layout_name_for(std::uint32_t mask) { return ""; } -// 每个扬声器布局的标准声道名(按该布局的标准声道顺序排列)。 -struct LayoutChannels { - std::uint32_t out_ch_config; - std::uint32_t count; - const char* names[16]; -}; - -constexpr LayoutChannels kLayoutChannels[] = { - {0, 2, {"L", "R"}}, - {3, 4, {"L", "R", "C", "LFE"}}, - {255, 4, {"L", "R", "Lrs", "Rrs"}}, - {7, 6, {"L", "R", "C", "LFE", "Ls", "Rs"}}, - {11, 8, {"L", "R", "C", "LFE", "Ls", "Rs", "Lrs", "Rrs"}}, - {13, 8, {"L", "R", "C", "LFE", "Ls", "Rs", "Ltm", "Rtm"}}, - {14, 10, {"L", "R", "C", "LFE", "Ls", "Rs", "Ltf", "Rtf", "Ltr", "Rtr"}}, - {15, 10, {"L", "R", "C", "LFE", "Ls", "Rs", "Lrs", "Rrs", "Ltm", "Rtm"}}, - {16, 12, {"L", "R", "C", "LFE", "Ls", "Rs", "Lrs", "Rrs", "Ltf", "Rtf", "Ltr", "Rtr"}}, - {19, 14, {"L", "R", "C", "LFE", "Ls", "Rs", "Lrs", "Rrs", "Lw", "Rw", - "Ltf", "Rtf", "Ltr", "Rtr"}}, - {20, 16, {"L", "R", "C", "LFE", "Ls", "Rs", "Lrs", "Rrs", "Lw", "Rw", - "Ltf", "Rtf", "Ltm", "Rtm", "Ltr", "Rtr"}}, -}; - -const ejoc::speaker_tables::LayoutGeometry* geometry_for(std::uint32_t out_ch_config) { - for (const ejoc::speaker_tables::LayoutGeometry& layout : ejoc::speaker_tables::kLayouts) { - if (layout.out_ch_config == out_ch_config) { - return &layout; - } - } - return nullptr; -} - } // namespace BedLayout resolve_bed_layout(const std::vector& assignments, @@ -284,48 +253,4 @@ bool bed_channel_q15(BedChannel channel, std::uint16_t q15[3]) { return true; } -bool layout_bed_anchor(std::uint32_t out_ch_config, BedChannel channel, - std::uint16_t q15[3]) { - if (q15 == nullptr) { - return false; - } - const char* name = bed_channel_short_name(channel); - if (name == nullptr || name[0] == '\0' || std::strcmp(name, "LFE") == 0) { - return false; - } - const LayoutChannels* table = nullptr; - for (const LayoutChannels& entry : kLayoutChannels) { - if (entry.out_ch_config == out_ch_config) { - table = &entry; - break; - } - } - const ejoc::speaker_tables::LayoutGeometry* geometry = geometry_for(out_ch_config); - if (table == nullptr || geometry == nullptr) { - return false; - } - int standard_index = -1; - for (std::uint32_t index = 0; index < table->count; ++index) { - if (std::strcmp(table->names[index], name) == 0) { - standard_index = static_cast(index); - break; - } - } - if (standard_index < 0) { - return false; - } - const std::uint8_t internal = geometry->standard_from_internal[standard_index]; - const ejoc::speaker_tables::RegionGeometry& region = geometry->regions[0]; - for (std::uint32_t point = 0; point < region.point_count; ++point) { - if (region.points[point].speaker_id != internal) { - continue; - } - for (int axis = 0; axis < 3; ++axis) { - q15[axis] = region.points[point].coordinate_q15[axis]; - } - return true; - } - return false; -} - } // namespace joc::joc diff --git a/src/joc_bitstream/bed_layout_map.h b/src/joc_bitstream/bed_layout_map.h index 306c2d3..0936fa6 100644 --- a/src/joc_bitstream/bed_layout_map.h +++ b/src/joc_bitstream/bed_layout_map.h @@ -57,12 +57,8 @@ const char* bed_channel_short_name(BedChannel channel); const char* bed_channel_label(BedChannel channel); // 固定方位:ADM 笛卡尔坐标(x 右、y 前、z 上)与渲染器使用的 Q15 方位。 +// 位置只由声道标签决定(ETSI TS 103 420 Table B.10),与目标输出布局无关。 bool bed_channel_adm_position(BedChannel channel, double xyz[3]); bool bed_channel_q15(BedChannel channel, std::uint16_t q15[3]); -// 目标布局里同名扬声器的 Q15 方位:bed 声道直连到该扬声器,而不是参与声像。 -// 目标布局没有这个声道时返回 false。 -bool layout_bed_anchor(std::uint32_t out_ch_config, BedChannel channel, - std::uint16_t q15[3]); - } // namespace joc::joc diff --git a/src/stream/stream.cpp b/src/stream/stream.cpp index de00edd..3dd1adc 100644 --- a/src/stream/stream.cpp +++ b/src/stream/stream.cpp @@ -221,8 +221,8 @@ void Stream::apply_bed_positions(const FrameMetadata& entry) { continue; } std::uint16_t q15[3] = {0, 0, 0}; - if (!::joc::joc::layout_bed_anchor(speaker_.layout.out_ch_config, channel, q15) && - !::joc::joc::bed_channel_q15(channel, q15)) { + // bed 声道是固定位置的源:方位由声道标签决定,与目标布局无关。 + if (!::joc::joc::bed_channel_q15(channel, q15)) { continue; } speaker_.bed_override[slot] = true; diff --git a/src/task/task.cpp b/src/task/task.cpp index 21cdaec..fd23b91 100644 --- a/src/task/task.cpp +++ b/src/task/task.cpp @@ -29,6 +29,7 @@ #include "adm/adm_metadata.h" #include "adm/adm_tracks.h" #include "binaural/binaural_runtime.h" +#include "binaural/sofa_reference_runtime.h" #include "eac3_transport/eac3_reader.h" #include "emdf/emdf_parser.h" #include "foundation/mini_json.h" @@ -95,6 +96,9 @@ struct Settings { std::string hrtf_path; std::string kernels_path; std::string hrtf_sofa_path; + std::string reference_config_path; + std::uint32_t sofa_interpolation = JOC_SOFA_INTERPOLATION_LOGMAG_PHASE; + bool hrtf_compile = false; std::string personalized_headphone_path; std::string hrtf_cache_dir; std::uint32_t hrtf_cache_policy = JOC_HRTF_CACHE_MEMORY; @@ -145,6 +149,11 @@ Settings normalise(const joc_task_config& raw) { if (raw.speaker_layout_name != nullptr) { cfg.layout = raw.speaker_layout_name; } if (raw.hrtf_path != nullptr) { cfg.hrtf_path = raw.hrtf_path; } if (raw.hrtf_sofa_path != nullptr) { cfg.hrtf_sofa_path = raw.hrtf_sofa_path; } + if (raw.reference_config_path != nullptr) { + cfg.reference_config_path = raw.reference_config_path; + } + cfg.sofa_interpolation = raw.sofa_interpolation; + cfg.hrtf_compile = raw.hrtf_compile != 0u; if (raw.personalized_headphone_path != nullptr) { cfg.personalized_headphone_path = raw.personalized_headphone_path; } @@ -1099,9 +1108,9 @@ Status validate(const joc_task_config& raw, std::vector* i fail(JOC_ERR_INVALID_CONFIG, "hrtf_cache_dir", "the disk cache policy needs a cache directory"); } - if (!std::isfinite(cfg.hrtf_radius_m) || cfg.hrtf_radius_m <= 0.0) { + if (!std::isfinite(cfg.hrtf_radius_m) || cfg.hrtf_radius_m < 0.0) { fail(JOC_ERR_INVALID_CONFIG, "hrtf_radius_m", - "hrtf_radius_m must be positive and finite"); + "hrtf_radius_m must be finite and not negative"); } } // The filterbank tables are compiled in; a path is only a verification override. @@ -1355,6 +1364,8 @@ Status run(const joc_task_config& raw, const joc_event_sink* sink, joc_task_resu } ::joc::binaural::SofaBinauralRuntime binaural; + ::joc::binaural::SofaReferenceRuntime binaural_reference; + bool use_reference = false; ::joc::hrtf::RosellaRuntime rosella; const bool binaural_enabled = cfg.operation == JOC_OP_BINAURAL; const bool rosella_enabled = !cfg.personalized_headphone_path.empty(); @@ -1398,50 +1409,85 @@ Status run(const joc_task_config& raw, const joc_event_sink* sink, joc_task_resu ::joc::hrtf::Kernels kernels; ::joc::binaural::Profile profile = ::joc::binaural::Profile::Mid; Status status = Status::success(); - if (!cfg.hrtf_sofa_path.empty()) { - // The .jochrtf is an internal cache: the SOFA is the user-facing input. - ::joc::hrtf::SofaFieldRequest request; - request.sofa_path = cfg.hrtf_sofa_path; - request.options.shell_radius_m = cfg.hrtf_radius_m; - request.cache_dir = cfg.hrtf_cache_dir; - switch (cfg.hrtf_cache_policy) { - case JOC_HRTF_CACHE_NONE: request.policy = ::joc::hrtf::CachePolicy::None; break; - case JOC_HRTF_CACHE_DISK: request.policy = ::joc::hrtf::CachePolicy::Disk; break; - default: request.policy = ::joc::hrtf::CachePolicy::Memory; break; - } - std::string cache_path; - status = ::joc::hrtf::load_or_compile_sofa_field(request, &field, &cache_path); + if (!cfg.hrtf_sofa_path.empty() && !cfg.hrtf_compile) { + // The SOFA keeps its full HRIRs: the direct path renders through the + // reference environment, and a .jochrtf stays an explicit input below. + ::joc::binaural::SofaReferenceOptions options; + options.mode_specified = cfg.binaural_mode != JOC_BINAURAL_MID; + options.mode_index = cfg.binaural_mode == JOC_BINAURAL_NEAR + ? ::joc::binaural::kProfileNear + : (cfg.binaural_mode == JOC_BINAURAL_FAR + ? ::joc::binaural::kProfileFar + : ::joc::binaural::kProfileMid); + options.reference_config = cfg.reference_config_path; + options.interpolation = + cfg.sofa_interpolation == JOC_SOFA_INTERPOLATION_MCA_ZERO + ? ::joc::hrtf::HrirInterpolation::kMcaZero + : ::joc::hrtf::HrirInterpolation::kLogMagnitudePhase; + options.has_shell_radius = cfg.hrtf_radius_m > 0.0; + options.shell_radius_m = cfg.hrtf_radius_m; + options.object_delay_samples = cfg.object_delay_samples; + options.tail_seconds = cfg.tail_seconds; + status = binaural_reference.open(cfg.hrtf_sofa_path, options); + use_reference = status.ok(); if (status.ok()) { bus.emit(JOC_EV_HRTF_LOADED, JOC_STAGE_RENDER, JOC_LOG_INFO, - "SOFA HRTF compiled: " + field.source_sha256.substr(0, 16) + - (cache_path.empty() ? std::string() : " -> " + cache_path)); + fmt("SOFA full-HRIR field: %zu bins, %zu directions, %zu triangles, " + "latency %d samples", + binaural_reference.field().fft_size(), + binaural_reference.field().direction_count(), + binaural_reference.field().triangle_count(), + binaural_reference.latency_samples())); } } else { + if (!cfg.hrtf_sofa_path.empty()) { + // Explicit legacy compile: the SOFA becomes a .jochrtf cache. + ::joc::hrtf::SofaFieldRequest request; + request.sofa_path = cfg.hrtf_sofa_path; + request.options.shell_radius_m = + cfg.hrtf_radius_m > 0.0 ? cfg.hrtf_radius_m : 1.0; + request.cache_dir = cfg.hrtf_cache_dir; + switch (cfg.hrtf_cache_policy) { + case JOC_HRTF_CACHE_NONE: request.policy = ::joc::hrtf::CachePolicy::None; break; + case JOC_HRTF_CACHE_DISK: request.policy = ::joc::hrtf::CachePolicy::Disk; break; + default: request.policy = ::joc::hrtf::CachePolicy::Memory; break; + } + std::string cache_path; + status = ::joc::hrtf::load_or_compile_sofa_field(request, &field, &cache_path); + if (status.ok()) { + bus.emit(JOC_EV_HRTF_LOADED, JOC_STAGE_RENDER, JOC_LOG_INFO, + "SOFA HRTF compiled: " + field.source_sha256.substr(0, 16) + + (cache_path.empty() ? std::string() : " -> " + cache_path)); + } + } else { status = ::joc::hrtf::load_jochrtf(cfg.hrtf_path, &field); if (status.ok()) { bus.emit(JOC_EV_HRTF_LOADED, JOC_STAGE_RENDER, JOC_LOG_INFO, "compiled HRTF: " + field.source_sha256.substr(0, 16)); } - } - if (status.ok()) { - status = cfg.kernels_path.empty() - ? (kernels = ::joc::hrtf::builtin_kernels(), Status::success()) - : ::joc::hrtf::load_kernels(cfg.kernels_path, &kernels); - } - if (status.ok() && cfg.binaural_mode == JOC_BINAURAL_NEAR) { - profile = ::joc::binaural::Profile::Near; - } else if (status.ok() && cfg.binaural_mode == JOC_BINAURAL_FAR) { - profile = ::joc::binaural::Profile::Far; - } - if (status.ok()) { - status = binaural.open(field, kernels, profile); + } + if (status.ok()) { + status = cfg.kernels_path.empty() + ? (kernels = ::joc::hrtf::builtin_kernels(), Status::success()) + : ::joc::hrtf::load_kernels(cfg.kernels_path, &kernels); + } + if (status.ok() && cfg.binaural_mode == JOC_BINAURAL_NEAR) { + profile = ::joc::binaural::Profile::Near; + } else if (status.ok() && cfg.binaural_mode == JOC_BINAURAL_FAR) { + profile = ::joc::binaural::Profile::Far; + } + if (status.ok()) { + status = binaural.open(field, kernels, profile); + } } if (!status.ok()) { return finalize_with_error(status.code(), "render", "binaural setup failed: " + status.message()); } bus.emit(JOC_EV_RENDERER_INITIALIZED, JOC_STAGE_RENDER, JOC_LOG_INFO, - "binaural runtime ready (the first 961 samples are the kernel latency)"); + use_reference + ? "binaural runtime ready (full-HRIR SOFA reference path)" + : "binaural runtime ready (the first 961 samples are the kernel latency)"); } } @@ -1798,8 +1844,13 @@ Status run(const joc_task_config& raw, const joc_event_sink* sink, joc_task_resu static_cast(slot) + 1, xyz[0], xyz[1], xyz[2]); if (binaural_enabled) { - binaural.set_fixed_position( - static_cast(slot) + 1, xyz); + if (use_reference) { + binaural_reference.set_fixed_position( + static_cast(slot) + 1, xyz); + } else { + binaural.set_fixed_position( + static_cast(slot) + 1, xyz); + } } if (rosella_enabled) { rosella.set_fixed_position( @@ -1809,11 +1860,10 @@ Status run(const joc_task_config& raw, const joc_event_sink* sink, joc_task_resu if (!speaker_enabled) { continue; } - // 目标布局有同名扬声器时直连该声道,否则按标准方位声像。 + // bed 声道是固定位置的源:方位由声道标签决定, + // 与目标布局无关,按对象路径声像。 std::uint16_t q15[3] = {0, 0, 0}; - if (!::joc::joc::layout_bed_anchor( - speaker.layout.out_ch_config, channel, q15) && - !::joc::joc::bed_channel_q15(channel, q15)) { + if (!::joc::joc::bed_channel_q15(channel, q15)) { continue; } speaker.bed_override[slot] = true; @@ -1977,19 +2027,26 @@ Status run(const joc_task_config& raw, const joc_event_sink* sink, joc_task_resu failed = true; } } else { - const Status submitted = - rosella_enabled - ? rosella.submit_frame(frame.objects16.data(), - frame.has_update ? &frame.update : nullptr, - static_cast(frame.index), - frame.outer_offset, - static_cast(cfg.object_delay_samples)) - : binaural.submit_frame(frame.objects16.data(), - frame.has_update ? &frame.update : nullptr, - static_cast(frame.index), - frame.outer_offset, - static_cast( - cfg.object_delay_samples)); + Status submitted = Status::success(); + if (rosella_enabled) { + submitted = rosella.submit_frame( + frame.objects16.data(), + frame.has_update ? &frame.update : nullptr, + static_cast(frame.index), frame.outer_offset, + static_cast(cfg.object_delay_samples)); + } else if (use_reference) { + submitted = binaural_reference.submit_frame( + frame.objects16.data(), + frame.has_update ? &frame.update : nullptr, + static_cast(frame.index), frame.outer_offset, + static_cast(cfg.object_delay_samples)); + } else { + submitted = binaural.submit_frame( + frame.objects16.data(), + frame.has_update ? &frame.update : nullptr, + static_cast(frame.index), frame.outer_offset, + static_cast(cfg.object_delay_samples)); + } if (!submitted.ok()) { failures.record(frame.index, StageRank::kDsp, submitted.code(), "render", "frame " + std::to_string(frame.index) + ": " + @@ -2031,6 +2088,8 @@ Status run(const joc_task_config& raw, const joc_event_sink* sink, joc_task_resu } else { if (rosella_enabled) { rosella.take_output(&pending.dsp); + } else if (use_reference) { + binaural_reference.take_output(&pending.dsp); } else { binaural.take_output(&pending.dsp); } @@ -2069,13 +2128,18 @@ Status run(const joc_task_config& raw, const joc_event_sink* sink, joc_task_resu static_cast(written_samples) / 48000.0); bus.emit(JOC_EV_OAMD_STATS, JOC_STAGE_METADATA, JOC_LOG_DEBUG, fmt("oamd payloads=%llu transitions=%llu", - static_cast(binaural_enabled - ? binaural.timeline().payload_count() - : 0ull), - static_cast(binaural_enabled - ? binaural.timeline() - .transition_count() - : 0ull))); + static_cast( + !binaural_enabled + ? 0ull + : (use_reference + ? binaural_reference.timeline().payload_count() + : binaural.timeline().payload_count())), + static_cast( + !binaural_enabled + ? 0ull + : (use_reference + ? binaural_reference.timeline().transition_count() + : binaural.timeline().transition_count())))); } } @@ -2181,10 +2245,17 @@ Status run(const joc_task_config& raw, const joc_event_sink* sink, joc_task_resu const std::uint64_t program_samples = written_samples; if (binaural_enabled) { std::vector tail; - const Status drained_tail = - rosella_enabled - ? rosella.finish(rosella.finish_capacity(cfg.tail_seconds), &tail) - : binaural.finish(binaural.finish_capacity(cfg.tail_seconds), &tail); + Status drained_tail = Status::success(); + if (rosella_enabled) { + drained_tail = + rosella.finish(rosella.finish_capacity(cfg.tail_seconds), &tail); + } else if (use_reference) { + drained_tail = binaural_reference.finish( + binaural_reference.finish_capacity(cfg.tail_seconds), &tail); + } else { + drained_tail = + binaural.finish(binaural.finish_capacity(cfg.tail_seconds), &tail); + } if (!drained_tail.ok()) { return finalize_with_error(drained_tail.code(), "output", drained_tail.message()); @@ -2291,9 +2362,16 @@ Status run(const joc_task_config& raw, const joc_event_sink* sink, joc_task_resu out->output_over_unity_values = over_unity; out->output_file_bytes = file_bytes; std::snprintf(out->output_sha256, sizeof(out->output_sha256), "%s", digest.c_str()); - out->oamd_payloads = binaural_enabled ? binaural.timeline().payload_count() : out->oamd_payloads; + out->oamd_payloads = + !binaural_enabled + ? out->oamd_payloads + : (use_reference ? binaural_reference.timeline().payload_count() + : binaural.timeline().payload_count()); out->oamd_transitions = - binaural_enabled ? binaural.timeline().transition_count() : out->oamd_transitions; + !binaural_enabled + ? out->oamd_transitions + : (use_reference ? binaural_reference.timeline().transition_count() + : binaural.timeline().transition_count()); out->t_decode_bed = t_decode; out->t_render = t_render; out->t_write = write_seconds; diff --git a/tests/test_core.cpp b/tests/test_core.cpp index fee2bd1..3cba9f4 100644 --- a/tests/test_core.cpp +++ b/tests/test_core.cpp @@ -17,11 +17,14 @@ #include "foundation/fs_utf8.h" #include "foundation/geometry.h" #include "hrtf/jochrtf.h" +#include "hrtf/full_hrir_field.h" +#include "hrtf/partitioned_convolution.h" #include "hrtf/sofa.h" #include "hrtf/sofa_cache.h" #include "hrtf/rosella_model.h" #include "hrtf/rosella_renderer.h" #include "hrtf/sofa_field.h" +#include "hrtf/spherical_mesh.h" #include "foundation/mini_json.h" #include "foundation/sha256.h" #include "io/inflate.h" @@ -31,6 +34,7 @@ #include "io/process.h" #include "io/wav_writer.h" #include "io/zip_reader.h" +#include "joc_bitstream/bed_layout_map.h" #include "oamd/oamd_parser.h" #include "stream/stream.h" #include "task/task.h" @@ -294,6 +298,94 @@ void test_oamd_and_adm_helpers() { // DBMD segment 10: the nine configuration trims must default to automatic (bit 0 // of each trim's first byte), and every other field of the block must keep the // layout the writer has always produced. +// Spherical interpolation mesh: a 200-point Fibonacci sphere exercises the hull, +// the barycentric weights and the Voronoi areas against values captured from the +// reference implementation. +void test_spherical_mesh() { + constexpr int kPoints = 200; + const double pi = 3.14159265358979323846; + std::vector directions; + directions.reserve(static_cast(kPoints) * 3u); + for (int index = 0; index < kPoints; ++index) { + const double z = 1.0 - 2.0 * (static_cast(index) + 0.5) / kPoints; + const double phi = static_cast(index) * pi * (3.0 - std::sqrt(5.0)); + const double radius = std::sqrt(std::max(0.0, 1.0 - z * z)); + directions.push_back(radius * std::cos(phi)); + directions.push_back(radius * std::sin(phi)); + directions.push_back(z); + } + joc::hrtf::SphericalMesh mesh; + joc::Status status = mesh.build(directions, kPoints); + CHECK(status.ok()); + CHECK(mesh.vertex_count() == 200u); + CHECK(mesh.face_count() == 396u); + + const double query[3] = {0.30304576336566319, 0.5050762722761053, 0.80812203564176865}; + const int expected_ids[3] = {16, 11, 24}; + const double expected_weights[3] = {0.18552244832037465, 0.23319383531074916, + 0.5812837163688762}; + std::vector ids; + std::vector weights; + status = mesh.weights(query, &ids, &weights); + CHECK(status.ok()); + CHECK(ids.size() == 3u); + double difference = 0.0; + for (std::size_t index = 0; index < ids.size(); ++index) { + double expected = 0.0; + for (int corner = 0; corner < 3; ++corner) { + if (expected_ids[corner] == ids[index]) { + expected = expected_weights[corner]; + } + } + difference += std::fabs(weights[index] - expected); + } + CHECK(difference < 1e-9); + CHECK(std::fabs(weights[0] + weights[1] + weights[2] - 1.0) < 1e-12); + + std::vector areas; + mesh.voronoi_areas(&areas); + CHECK(areas.size() == 200u); + double smallest = areas[0]; + double largest = areas[0]; + double total = 0.0; + for (const double area : areas) { + smallest = std::min(smallest, area); + largest = std::max(largest, area); + total += area; + } + CHECK(std::fabs(total - 1.0) < 1e-12); + CHECK(std::fabs(largest / smallest - 1.1197648114672005) < 1e-9); +} + +// Rigid-sphere radial model: values captured from the reference implementation. +void test_sphere_transfer() { + const std::vector frequencies = {100.0, 1000.0, 10000.0}; + const std::vector cosines = {1.0, -1.0, 0.0}; + std::vector> transfer; + const joc::Status status = + joc::hrtf::sphere_transfer(frequencies, 1.0, 0.0875, cosines, &transfer); + CHECK(status.ok()); + CHECK(transfer.size() == 9u); + const double expected[9][2] = { + {1.1168983535509718, 0.2665696486002471}, + {-0.510805763797141, 1.6128537943222743}, + {-2.028923793661157, -0.7781755079388825}, + {0.8548955324773564, -0.21699922414442047}, + {-0.8363832921619323, -0.5012251963562975}, + {0.03287720702899299, -0.7655728018349265}, + {0.9881471826293796, -0.01028038096534477}, + {1.0740047354266093, 0.11688911754407418}, + {0.9394042077254096, -0.7927884777210633}, + }; + for (std::size_t index = 0; index < 9u; ++index) { + const double real = expected[index][0]; + const double imaginary = expected[index][1]; + CHECK(std::fabs(transfer[index].real() - real) < 1e-9 * (1.0 + std::fabs(real))); + CHECK(std::fabs(transfer[index].imag() - imaginary) < + 1e-9 * (1.0 + std::fabs(imaginary))); + } +} + void test_dbmd_auto_trim() { std::string dbmd; CHECK(joc::adm::build_dbmd(25, joc::adm::BinauralMode::Mid, &dbmd).ok()); @@ -1133,6 +1225,186 @@ void test_clipgain_reset_clears_the_previous_gain() { sizeof(float) * after_reset.size()) == 0); } +// CBA bed:每个声道的方位来自它的标签(ETSI TS 103 420 V1.2.1 table B.10), +// 声道顺序来自 table 12/13,因此 bed 声道不会随输出布局移动。 +void test_bed_layout_map() { + using joc::joc::BedChannel; + using joc::joc::BedLayout; + + double xyz[3] = {0.0, 0.0, 0.0}; + CHECK(joc::joc::bed_channel_adm_position(BedChannel::kLeft, xyz)); + CHECK(xyz[0] == -1.0 && xyz[1] == 1.0 && xyz[2] == 0.0); + CHECK(joc::joc::bed_channel_adm_position(BedChannel::kCenter, xyz)); + CHECK(xyz[0] == 0.0 && xyz[1] == 1.0 && xyz[2] == 0.0); + CHECK(joc::joc::bed_channel_adm_position(BedChannel::kTopFrontLeft, xyz)); + CHECK(xyz[0] == -1.0 && xyz[1] == 1.0 && xyz[2] == 1.0); + CHECK(joc::joc::bed_channel_adm_position(BedChannel::kTopFrontRight, xyz)); + CHECK(xyz[0] == 1.0 && xyz[1] == 1.0 && xyz[2] == 1.0); + CHECK(joc::joc::bed_channel_adm_position(BedChannel::kTopSideLeft, xyz)); + CHECK(xyz[0] == -1.0 && xyz[1] == 0.0 && xyz[2] == 1.0); + CHECK(joc::joc::bed_channel_adm_position(BedChannel::kTopBackLeft, xyz)); + CHECK(xyz[0] == -1.0 && xyz[1] == -1.0 && xyz[2] == 1.0); + CHECK(joc::joc::bed_channel_adm_position(BedChannel::kTopBackRight, xyz)); + CHECK(xyz[0] == 1.0 && xyz[1] == -1.0 && xyz[2] == 1.0); + CHECK(joc::joc::bed_channel_adm_position(BedChannel::kLeftWide, xyz)); + CHECK(xyz[0] == -1.0 && xyz[1] == 21.0 / 31.0 && xyz[2] == 0.0); + CHECK(joc::joc::bed_channel_adm_position(BedChannel::kRightWide, xyz)); + CHECK(xyz[0] == 1.0 && xyz[1] == 21.0 / 31.0 && xyz[2] == 0.0); + CHECK(joc::joc::bed_channel_adm_position(BedChannel::kLfe, xyz)); + CHECK(xyz[0] == -1.0 && xyz[1] == 1.0 && xyz[2] == -1.0); + CHECK(joc::joc::bed_channel_adm_position(BedChannel::kLfe2, xyz)); + CHECK(xyz[0] == 1.0 && xyz[1] == 1.0 && xyz[2] == -1.0); + + std::uint16_t q15[3] = {0, 0, 0}; + CHECK(joc::joc::bed_channel_q15(BedChannel::kLeft, q15)); + CHECK(q15[0] == 0 && q15[1] == 0 && q15[2] == 0); + CHECK(joc::joc::bed_channel_q15(BedChannel::kTopFrontLeft, q15)); + CHECK(q15[0] == 0 && q15[1] == 0 && q15[2] == 32767); + CHECK(joc::joc::bed_channel_q15(BedChannel::kLeftWide, q15)); + CHECK(q15[0] == 0 && q15[1] == 5285 && q15[2] == 0); + CHECK(joc::joc::bed_channel_q15(BedChannel::kTopBackLeft, q15)); + CHECK(q15[0] == 0 && q15[1] == 32767 && q15[2] == 32767); + + // table 12:0x0F 是 5.1,0x1FF 是 9.1.6(不含 LFE2)。 + std::vector standard(1); + standard[0].standard = true; + standard[0].mask = 0x0Fu; + const BedLayout five_one = joc::joc::resolve_bed_layout(standard, true); + CHECK(five_one.present && five_one.bed_only && five_one.complete); + CHECK(std::string(five_one.name) == "5.1"); + CHECK(five_one.channels.size() == 5); + CHECK(five_one.channels[0] == BedChannel::kLeft); + CHECK(five_one.channels[1] == BedChannel::kRight); + CHECK(five_one.channels[2] == BedChannel::kCenter); + CHECK(five_one.channels[3] == BedChannel::kLeftSurround); + CHECK(five_one.channels[4] == BedChannel::kRightSurround); + + standard[0].mask = 0x1FFu; + const BedLayout nine_six = joc::joc::resolve_bed_layout(standard, true); + CHECK(std::string(nine_six.name) == "9.1.6"); + CHECK(nine_six.channels.size() == 15); + CHECK(nine_six.channels[0] == BedChannel::kLeft); + CHECK(nine_six.channels[2] == BedChannel::kCenter); + CHECK(nine_six.channels[3] == BedChannel::kLeftSurround); + CHECK(nine_six.channels[7] == BedChannel::kTopFrontLeft); + CHECK(nine_six.channels[9] == BedChannel::kTopSideLeft); + CHECK(nine_six.channels[11] == BedChannel::kTopBackLeft); + CHECK(nine_six.channels[13] == BedChannel::kLeftWide); + CHECK(nine_six.channels[14] == BedChannel::kRightWide); + // LFE 留在声明掩码里,但不占 JOC 对象槽。 + CHECK(joc::joc::bed_layout_has(nine_six, BedChannel::kLfe)); + bool lfe_slot = false; + for (const BedChannel channel : nine_six.channels) { + lfe_slot = lfe_slot || channel == BedChannel::kLfe || + channel == BedChannel::kLfe2; + } + CHECK(!lfe_slot); + CHECK(joc::joc::bed_layout_has(nine_six, BedChannel::kRightWide)); + + // table 13:17 bit 掩码的 channel order 与 table 12 相同。 + std::vector nonstandard(1); + nonstandard[0].standard = false; + nonstandard[0].mask = 0x3Fu; + const BedLayout ns_five = joc::joc::resolve_bed_layout(nonstandard, true); + CHECK(ns_five.channels.size() == 5); + CHECK(ns_five.channels[0] == BedChannel::kLeft); + CHECK(ns_five.channels[4] == BedChannel::kRightSurround); + nonstandard[0].mask = 0xFFFFu; + const BedLayout ns_nine = joc::joc::resolve_bed_layout(nonstandard, true); + CHECK(ns_nine.channels.size() == 15); + CHECK(ns_nine.channels[0] == BedChannel::kLeft); + CHECK(ns_nine.channels[13] == BedChannel::kLeftWide); + CHECK(ns_nine.channels[14] == BedChannel::kRightWide); +} + +// Partitioned convolution: overlap-add with a shared input history and a filter +// crossfade. The expected values come from the reference +// partitioned_convolution.py running the same filters and blocks. +void test_partitioned_convolution() { + constexpr std::size_t kBlock = 128; + constexpr std::size_t kTaps = 300; + constexpr std::size_t kMaxTaps = 512; + constexpr std::size_t kOutputs = 2; + + auto make_filter = [](double step, double decay, bool cosine, double offset) { + std::vector values(kOutputs * kTaps); + for (std::size_t output = 0; output < kOutputs; ++output) { + for (std::size_t tap = 0; tap < kTaps; ++tap) { + const double angle = step * static_cast(tap + 1) * + (static_cast(output) + offset); + const double trig = cosine ? std::cos(angle) : std::sin(angle); + values[output * kTaps + tap] = trig * std::exp(-static_cast(tap) / decay); + } + } + return values; + }; + + joc::hrtf::UniformPartitionedConvolver convolver(kOutputs, kMaxTaps, kBlock); + const std::vector first = make_filter(0.017, 100.0, false, 1.0); + CHECK(convolver.set_filter(first.data(), kTaps, 0).ok()); + CHECK(!convolver.transition_active()); + + std::vector block(kBlock); + std::vector out(kBlock * kOutputs); + std::vector rendered; + for (std::size_t index = 0; index < 8; ++index) { + for (std::size_t sample = 0; sample < kBlock; ++sample) { + const double n = static_cast(index * kBlock + sample); + block[sample] = std::sin(0.01 * n) + 0.5 * std::cos(0.003 * n); + } + CHECK(convolver.process_block(block.data(), out.data()).ok()); + rendered.insert(rendered.end(), out.begin(), out.end()); + } + CHECK(rendered.size() == 8 * kBlock * kOutputs); + CHECK(std::fabs(rendered[0] - 0.0084995905892490953) < 1e-9); + CHECK(std::fabs(rendered[1] - 0.016996724855971479) < 1e-9); + CHECK(std::fabs(rendered[2] - 0.025497145937656285) < 1e-9); + CHECK(std::fabs(rendered[63] - 8.1367456949245849) < 1e-9); + CHECK(std::fabs(rendered[64] - 4.6183248807335495) < 1e-9); + CHECK(std::fabs(rendered[65] - 8.5945567424849969) < 1e-9); + CHECK(std::fabs(rendered[127] - 24.744288415490566) < 1e-8); + CHECK(std::fabs(rendered[128] - 16.21718594261781) < 1e-8); + CHECK(std::fabs(rendered[129] - 25.258064041591062) < 1e-8); + CHECK(std::fabs(rendered[255] - 42.867139711879446) < 1e-8); + CHECK(std::fabs(rendered[256] - 47.087759632531053) < 1e-8); + CHECK(std::fabs(rendered[511] - 30.307479702394744) < 1e-8); + CHECK(std::fabs(rendered[512] - 64.6869976138059) < 1e-8); + CHECK(std::fabs(rendered[700] - 26.847289638199864) < 1e-8); + CHECK(std::fabs(rendered[701] - 2.9756928363101984) < 1e-8); + CHECK(std::fabs(rendered[1023] - -25.703654152948225) < 1e-8); + CHECK(std::fabs(rendered[1024] - -43.300148318757053) < 1e-8); + + // A two-block crossfade keeps both filters' overlap tails alive. + const std::vector second = make_filter(0.011, 140.0, true, 2.0); + CHECK(convolver.set_filter(second.data(), kTaps, 2).ok()); + CHECK(convolver.transition_active()); + std::vector faded; + for (std::size_t index = 0; index < 4; ++index) { + for (std::size_t sample = 0; sample < kBlock; ++sample) { + const double n = 1000.0 + static_cast(index * kBlock + sample); + block[sample] = std::sin(0.02 * n) + 0.3; + } + CHECK(convolver.process_block(block.data(), out.data()).ok()); + faded.insert(faded.end(), out.begin(), out.end()); + } + CHECK(!convolver.transition_active()); + CHECK(std::fabs(faded[0] - -31.254215789646686) < 1e-8); + CHECK(std::fabs(faded[1] - -30.520784088408671) < 1e-8); + CHECK(std::fabs(faded[2] - -31.64727283056877) < 1e-8); + CHECK(std::fabs(faded[127] - 45.044850086774559) < 1e-8); + CHECK(std::fabs(faded[128] - 16.242938783563034) < 1e-8); + CHECK(std::fabs(faded[129] - 45.566945939182197) < 1e-8); + CHECK(std::fabs(faded[255] - -11.640284877460564) < 1e-8); + CHECK(std::fabs(faded[256] - 20.660738168974696) < 1e-8); + CHECK(std::fabs(faded[383] - -24.027948887675439) < 1e-8); + CHECK(std::fabs(faded[384] - -41.675076381317034) < 1e-8); + CHECK(std::fabs(faded[511] - 34.95547294171444) < 1e-8); + CHECK(std::fabs(faded[512] - -4.9486250951335764) < 1e-8); + + convolver.reset(); + CHECK(!convolver.transition_active()); +} + int main() { test_sha256(); test_crc32_and_inflate(); @@ -1144,6 +1416,10 @@ int main() { test_task_result_json(); test_npy(); test_oamd_and_adm_helpers(); + test_bed_layout_map(); + test_partitioned_convolution(); + test_spherical_mesh(); + test_sphere_transfer(); test_dbmd_auto_trim(); test_int24_packing(); test_utf8_paths();