mirror of
https://github.com/overte-org/overte.git
synced 2025-07-23 02:07:14 +02:00
1096 lines
43 KiB
C++
1096 lines
43 KiB
C++
//
|
|
// AudioFOA.cpp
|
|
// libraries/audio/src
|
|
//
|
|
// Created by Ken Cooke on 10/28/16.
|
|
// Copyright 2016 High Fidelity, Inc.
|
|
//
|
|
// Distributed under the Apache License, Version 2.0.
|
|
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
|
|
//
|
|
|
|
#include "AudioFOA.h"
|
|
|
|
#include <string.h>
|
|
#include <assert.h>
|
|
|
|
#include "AudioFOAData.h"
|
|
|
|
#if defined(_MSC_VER)
|
|
#define ALIGN32 __declspec(align(32))
|
|
#elif defined(__GNUC__)
|
|
#define ALIGN32 __attribute__((aligned(32)))
|
|
#else
|
|
#define ALIGN32
|
|
#endif
|
|
|
|
//
|
|
// Vectorized FFT based on radix 2/4/8 Stockham auto-sort
|
|
// scalar reference version
|
|
//
|
|
typedef struct {
|
|
float re;
|
|
float im;
|
|
} complex_t;
|
|
|
|
static const float SQRT1_2 = 0.707106781f; // 1/sqrt(2)
|
|
|
|
static const complex_t w_fft_256_0[] = {
|
|
{ 1.000000000e+00f, 0.000000000e+00f },
|
|
{ 9.238795042e-01f, 3.826834261e-01f },
|
|
{ 7.071067691e-01f, 7.071067691e-01f },
|
|
{ 3.826834261e-01f, 9.238795042e-01f },
|
|
{ 0.000000000e+00f, 1.000000000e+00f },
|
|
{-3.826834261e-01f, 9.238795042e-01f },
|
|
{-7.071067691e-01f, 7.071067691e-01f },
|
|
{-9.238795042e-01f, 3.826834261e-01f },
|
|
};
|
|
|
|
static const complex_t w_fft_256_1[] = {
|
|
{ 1.000000000e+00f, 0.000000000e+00f }, { 1.000000000e+00f, 0.000000000e+00f }, { 1.000000000e+00f, 0.000000000e+00f },
|
|
{ 9.951847196e-01f, 9.801714122e-02f }, { 9.807852507e-01f, 1.950903237e-01f }, { 9.569403529e-01f, 2.902846634e-01f },
|
|
{ 9.807852507e-01f, 1.950903237e-01f }, { 9.238795042e-01f, 3.826834261e-01f }, { 8.314695954e-01f, 5.555702448e-01f },
|
|
{ 9.569403529e-01f, 2.902846634e-01f }, { 8.314695954e-01f, 5.555702448e-01f }, { 6.343932748e-01f, 7.730104327e-01f },
|
|
{ 9.238795042e-01f, 3.826834261e-01f }, { 7.071067691e-01f, 7.071067691e-01f }, { 3.826834261e-01f, 9.238795042e-01f },
|
|
{ 8.819212914e-01f, 4.713967443e-01f }, { 5.555702448e-01f, 8.314695954e-01f }, { 9.801714122e-02f, 9.951847196e-01f },
|
|
{ 8.314695954e-01f, 5.555702448e-01f }, { 3.826834261e-01f, 9.238795042e-01f }, {-1.950903237e-01f, 9.807852507e-01f },
|
|
{ 7.730104327e-01f, 6.343932748e-01f }, { 1.950903237e-01f, 9.807852507e-01f }, {-4.713967443e-01f, 8.819212914e-01f },
|
|
{ 7.071067691e-01f, 7.071067691e-01f }, { 0.000000000e+00f, 1.000000000e+00f }, {-7.071067691e-01f, 7.071067691e-01f },
|
|
{ 6.343932748e-01f, 7.730104327e-01f }, {-1.950903237e-01f, 9.807852507e-01f }, {-8.819212914e-01f, 4.713967443e-01f },
|
|
{ 5.555702448e-01f, 8.314695954e-01f }, {-3.826834261e-01f, 9.238795042e-01f }, {-9.807852507e-01f, 1.950903237e-01f },
|
|
{ 4.713967443e-01f, 8.819212914e-01f }, {-5.555702448e-01f, 8.314695954e-01f }, {-9.951847196e-01f,-9.801714122e-02f },
|
|
{ 3.826834261e-01f, 9.238795042e-01f }, {-7.071067691e-01f, 7.071067691e-01f }, {-9.238795042e-01f,-3.826834261e-01f },
|
|
{ 2.902846634e-01f, 9.569403529e-01f }, {-8.314695954e-01f, 5.555702448e-01f }, {-7.730104327e-01f,-6.343932748e-01f },
|
|
{ 1.950903237e-01f, 9.807852507e-01f }, {-9.238795042e-01f, 3.826834261e-01f }, {-5.555702448e-01f,-8.314695954e-01f },
|
|
{ 9.801714122e-02f, 9.951847196e-01f }, {-9.807852507e-01f, 1.950903237e-01f }, {-2.902846634e-01f,-9.569403529e-01f },
|
|
};
|
|
|
|
static const complex_t w_fft_256_2[] = {
|
|
{ 1.000000000e+00f, 0.000000000e+00f }, { 1.000000000e+00f, 0.000000000e+00f }, { 1.000000000e+00f, 0.000000000e+00f },
|
|
{ 9.996988177e-01f, 2.454122901e-02f }, { 9.987954497e-01f, 4.906767607e-02f }, { 9.972904325e-01f, 7.356456667e-02f },
|
|
{ 9.987954497e-01f, 4.906767607e-02f }, { 9.951847196e-01f, 9.801714122e-02f }, { 9.891765118e-01f, 1.467304677e-01f },
|
|
{ 9.972904325e-01f, 7.356456667e-02f }, { 9.891765118e-01f, 1.467304677e-01f }, { 9.757021070e-01f, 2.191012353e-01f },
|
|
{ 9.951847196e-01f, 9.801714122e-02f }, { 9.807852507e-01f, 1.950903237e-01f }, { 9.569403529e-01f, 2.902846634e-01f },
|
|
{ 9.924795628e-01f, 1.224106774e-01f }, { 9.700312614e-01f, 2.429801822e-01f }, { 9.329928160e-01f, 3.598950505e-01f },
|
|
{ 9.891765118e-01f, 1.467304677e-01f }, { 9.569403529e-01f, 2.902846634e-01f }, { 9.039893150e-01f, 4.275550842e-01f },
|
|
{ 9.852776527e-01f, 1.709618866e-01f }, { 9.415440559e-01f, 3.368898630e-01f }, { 8.700869679e-01f, 4.928981960e-01f },
|
|
{ 9.807852507e-01f, 1.950903237e-01f }, { 9.238795042e-01f, 3.826834261e-01f }, { 8.314695954e-01f, 5.555702448e-01f },
|
|
{ 9.757021070e-01f, 2.191012353e-01f }, { 9.039893150e-01f, 4.275550842e-01f }, { 7.883464098e-01f, 6.152315736e-01f },
|
|
{ 9.700312614e-01f, 2.429801822e-01f }, { 8.819212914e-01f, 4.713967443e-01f }, { 7.409511209e-01f, 6.715589762e-01f },
|
|
{ 9.637760520e-01f, 2.667127550e-01f }, { 8.577286005e-01f, 5.141027570e-01f }, { 6.895405650e-01f, 7.242470980e-01f },
|
|
{ 9.569403529e-01f, 2.902846634e-01f }, { 8.314695954e-01f, 5.555702448e-01f }, { 6.343932748e-01f, 7.730104327e-01f },
|
|
{ 9.495281577e-01f, 3.136817515e-01f }, { 8.032075167e-01f, 5.956993103e-01f }, { 5.758081675e-01f, 8.175848126e-01f },
|
|
{ 9.415440559e-01f, 3.368898630e-01f }, { 7.730104327e-01f, 6.343932748e-01f }, { 5.141027570e-01f, 8.577286005e-01f },
|
|
{ 9.329928160e-01f, 3.598950505e-01f }, { 7.409511209e-01f, 6.715589762e-01f }, { 4.496113360e-01f, 8.932242990e-01f },
|
|
{ 9.238795042e-01f, 3.826834261e-01f }, { 7.071067691e-01f, 7.071067691e-01f }, { 3.826834261e-01f, 9.238795042e-01f },
|
|
{ 9.142097831e-01f, 4.052413106e-01f }, { 6.715589762e-01f, 7.409511209e-01f }, { 3.136817515e-01f, 9.495281577e-01f },
|
|
{ 9.039893150e-01f, 4.275550842e-01f }, { 6.343932748e-01f, 7.730104327e-01f }, { 2.429801822e-01f, 9.700312614e-01f },
|
|
{ 8.932242990e-01f, 4.496113360e-01f }, { 5.956993103e-01f, 8.032075167e-01f }, { 1.709618866e-01f, 9.852776527e-01f },
|
|
{ 8.819212914e-01f, 4.713967443e-01f }, { 5.555702448e-01f, 8.314695954e-01f }, { 9.801714122e-02f, 9.951847196e-01f },
|
|
{ 8.700869679e-01f, 4.928981960e-01f }, { 5.141027570e-01f, 8.577286005e-01f }, { 2.454122901e-02f, 9.996988177e-01f },
|
|
{ 8.577286005e-01f, 5.141027570e-01f }, { 4.713967443e-01f, 8.819212914e-01f }, {-4.906767607e-02f, 9.987954497e-01f },
|
|
{ 8.448535800e-01f, 5.349976420e-01f }, { 4.275550842e-01f, 9.039893150e-01f }, {-1.224106774e-01f, 9.924795628e-01f },
|
|
{ 8.314695954e-01f, 5.555702448e-01f }, { 3.826834261e-01f, 9.238795042e-01f }, {-1.950903237e-01f, 9.807852507e-01f },
|
|
{ 8.175848126e-01f, 5.758081675e-01f }, { 3.368898630e-01f, 9.415440559e-01f }, {-2.667127550e-01f, 9.637760520e-01f },
|
|
{ 8.032075167e-01f, 5.956993103e-01f }, { 2.902846634e-01f, 9.569403529e-01f }, {-3.368898630e-01f, 9.415440559e-01f },
|
|
{ 7.883464098e-01f, 6.152315736e-01f }, { 2.429801822e-01f, 9.700312614e-01f }, {-4.052413106e-01f, 9.142097831e-01f },
|
|
{ 7.730104327e-01f, 6.343932748e-01f }, { 1.950903237e-01f, 9.807852507e-01f }, {-4.713967443e-01f, 8.819212914e-01f },
|
|
{ 7.572088242e-01f, 6.531728506e-01f }, { 1.467304677e-01f, 9.891765118e-01f }, {-5.349976420e-01f, 8.448535800e-01f },
|
|
{ 7.409511209e-01f, 6.715589762e-01f }, { 9.801714122e-02f, 9.951847196e-01f }, {-5.956993103e-01f, 8.032075167e-01f },
|
|
{ 7.242470980e-01f, 6.895405650e-01f }, { 4.906767607e-02f, 9.987954497e-01f }, {-6.531728506e-01f, 7.572088242e-01f },
|
|
{ 7.071067691e-01f, 7.071067691e-01f }, { 0.000000000e+00f, 1.000000000e+00f }, {-7.071067691e-01f, 7.071067691e-01f },
|
|
{ 6.895405650e-01f, 7.242470980e-01f }, {-4.906767607e-02f, 9.987954497e-01f }, {-7.572088242e-01f, 6.531728506e-01f },
|
|
{ 6.715589762e-01f, 7.409511209e-01f }, {-9.801714122e-02f, 9.951847196e-01f }, {-8.032075167e-01f, 5.956993103e-01f },
|
|
{ 6.531728506e-01f, 7.572088242e-01f }, {-1.467304677e-01f, 9.891765118e-01f }, {-8.448535800e-01f, 5.349976420e-01f },
|
|
{ 6.343932748e-01f, 7.730104327e-01f }, {-1.950903237e-01f, 9.807852507e-01f }, {-8.819212914e-01f, 4.713967443e-01f },
|
|
{ 6.152315736e-01f, 7.883464098e-01f }, {-2.429801822e-01f, 9.700312614e-01f }, {-9.142097831e-01f, 4.052413106e-01f },
|
|
{ 5.956993103e-01f, 8.032075167e-01f }, {-2.902846634e-01f, 9.569403529e-01f }, {-9.415440559e-01f, 3.368898630e-01f },
|
|
{ 5.758081675e-01f, 8.175848126e-01f }, {-3.368898630e-01f, 9.415440559e-01f }, {-9.637760520e-01f, 2.667127550e-01f },
|
|
{ 5.555702448e-01f, 8.314695954e-01f }, {-3.826834261e-01f, 9.238795042e-01f }, {-9.807852507e-01f, 1.950903237e-01f },
|
|
{ 5.349976420e-01f, 8.448535800e-01f }, {-4.275550842e-01f, 9.039893150e-01f }, {-9.924795628e-01f, 1.224106774e-01f },
|
|
{ 5.141027570e-01f, 8.577286005e-01f }, {-4.713967443e-01f, 8.819212914e-01f }, {-9.987954497e-01f, 4.906767607e-02f },
|
|
{ 4.928981960e-01f, 8.700869679e-01f }, {-5.141027570e-01f, 8.577286005e-01f }, {-9.996988177e-01f,-2.454122901e-02f },
|
|
{ 4.713967443e-01f, 8.819212914e-01f }, {-5.555702448e-01f, 8.314695954e-01f }, {-9.951847196e-01f,-9.801714122e-02f },
|
|
{ 4.496113360e-01f, 8.932242990e-01f }, {-5.956993103e-01f, 8.032075167e-01f }, {-9.852776527e-01f,-1.709618866e-01f },
|
|
{ 4.275550842e-01f, 9.039893150e-01f }, {-6.343932748e-01f, 7.730104327e-01f }, {-9.700312614e-01f,-2.429801822e-01f },
|
|
{ 4.052413106e-01f, 9.142097831e-01f }, {-6.715589762e-01f, 7.409511209e-01f }, {-9.495281577e-01f,-3.136817515e-01f },
|
|
{ 3.826834261e-01f, 9.238795042e-01f }, {-7.071067691e-01f, 7.071067691e-01f }, {-9.238795042e-01f,-3.826834261e-01f },
|
|
{ 3.598950505e-01f, 9.329928160e-01f }, {-7.409511209e-01f, 6.715589762e-01f }, {-8.932242990e-01f,-4.496113360e-01f },
|
|
{ 3.368898630e-01f, 9.415440559e-01f }, {-7.730104327e-01f, 6.343932748e-01f }, {-8.577286005e-01f,-5.141027570e-01f },
|
|
{ 3.136817515e-01f, 9.495281577e-01f }, {-8.032075167e-01f, 5.956993103e-01f }, {-8.175848126e-01f,-5.758081675e-01f },
|
|
{ 2.902846634e-01f, 9.569403529e-01f }, {-8.314695954e-01f, 5.555702448e-01f }, {-7.730104327e-01f,-6.343932748e-01f },
|
|
{ 2.667127550e-01f, 9.637760520e-01f }, {-8.577286005e-01f, 5.141027570e-01f }, {-7.242470980e-01f,-6.895405650e-01f },
|
|
{ 2.429801822e-01f, 9.700312614e-01f }, {-8.819212914e-01f, 4.713967443e-01f }, {-6.715589762e-01f,-7.409511209e-01f },
|
|
{ 2.191012353e-01f, 9.757021070e-01f }, {-9.039893150e-01f, 4.275550842e-01f }, {-6.152315736e-01f,-7.883464098e-01f },
|
|
{ 1.950903237e-01f, 9.807852507e-01f }, {-9.238795042e-01f, 3.826834261e-01f }, {-5.555702448e-01f,-8.314695954e-01f },
|
|
{ 1.709618866e-01f, 9.852776527e-01f }, {-9.415440559e-01f, 3.368898630e-01f }, {-4.928981960e-01f,-8.700869679e-01f },
|
|
{ 1.467304677e-01f, 9.891765118e-01f }, {-9.569403529e-01f, 2.902846634e-01f }, {-4.275550842e-01f,-9.039893150e-01f },
|
|
{ 1.224106774e-01f, 9.924795628e-01f }, {-9.700312614e-01f, 2.429801822e-01f }, {-3.598950505e-01f,-9.329928160e-01f },
|
|
{ 9.801714122e-02f, 9.951847196e-01f }, {-9.807852507e-01f, 1.950903237e-01f }, {-2.902846634e-01f,-9.569403529e-01f },
|
|
{ 7.356456667e-02f, 9.972904325e-01f }, {-9.891765118e-01f, 1.467304677e-01f }, {-2.191012353e-01f,-9.757021070e-01f },
|
|
{ 4.906767607e-02f, 9.987954497e-01f }, {-9.951847196e-01f, 9.801714122e-02f }, {-1.467304677e-01f,-9.891765118e-01f },
|
|
{ 2.454122901e-02f, 9.996988177e-01f }, {-9.987954497e-01f, 4.906767607e-02f }, {-7.356456667e-02f,-9.972904325e-01f },
|
|
};
|
|
|
|
static complex_t w_rfft_512[] = {
|
|
{-4.999623597e-01f, 4.938642383e-01f }, {-4.998494089e-01f, 4.877293706e-01f },
|
|
{-4.996611774e-01f, 4.815963805e-01f }, {-4.993977249e-01f, 4.754661620e-01f },
|
|
{-4.990590513e-01f, 4.693396389e-01f }, {-4.986452162e-01f, 4.632177055e-01f },
|
|
{-4.981563091e-01f, 4.571013451e-01f }, {-4.975923598e-01f, 4.509914219e-01f },
|
|
{-4.969534874e-01f, 4.448888898e-01f }, {-4.962397814e-01f, 4.387946725e-01f },
|
|
{-4.954513311e-01f, 4.327096343e-01f }, {-4.945882559e-01f, 4.266347587e-01f },
|
|
{-4.936507046e-01f, 4.205709100e-01f }, {-4.926388264e-01f, 4.145190716e-01f },
|
|
{-4.915527403e-01f, 4.084800482e-01f }, {-4.903926253e-01f, 4.024548531e-01f },
|
|
{-4.891586900e-01f, 3.964443207e-01f }, {-4.878510535e-01f, 3.904493749e-01f },
|
|
{-4.864699841e-01f, 3.844709396e-01f }, {-4.850156307e-01f, 3.785099089e-01f },
|
|
{-4.834882319e-01f, 3.725671768e-01f }, {-4.818880260e-01f, 3.666436076e-01f },
|
|
{-4.802152514e-01f, 3.607401550e-01f }, {-4.784701765e-01f, 3.548576832e-01f },
|
|
{-4.766530097e-01f, 3.489970267e-01f }, {-4.747640789e-01f, 3.431591392e-01f },
|
|
{-4.728036523e-01f, 3.373448551e-01f }, {-4.707720280e-01f, 3.315550685e-01f },
|
|
{-4.686695039e-01f, 3.257906437e-01f }, {-4.664964080e-01f, 3.200524747e-01f },
|
|
{-4.642530382e-01f, 3.143413961e-01f }, {-4.619397521e-01f, 3.086583018e-01f },
|
|
{-4.595569372e-01f, 3.030039668e-01f }, {-4.571048915e-01f, 2.973793447e-01f },
|
|
{-4.545840025e-01f, 2.917852402e-01f }, {-4.519946575e-01f, 2.862224579e-01f },
|
|
{-4.493372440e-01f, 2.806918621e-01f }, {-4.466121495e-01f, 2.751943469e-01f },
|
|
{-4.438198209e-01f, 2.697306275e-01f }, {-4.409606457e-01f, 2.643016279e-01f },
|
|
{-4.380350411e-01f, 2.589080930e-01f }, {-4.350434840e-01f, 2.535508871e-01f },
|
|
{-4.319864213e-01f, 2.482308149e-01f }, {-4.288643003e-01f, 2.429486215e-01f },
|
|
{-4.256775975e-01f, 2.377051711e-01f }, {-4.224267900e-01f, 2.325011790e-01f },
|
|
{-4.191123545e-01f, 2.273375094e-01f }, {-4.157347977e-01f, 2.222148776e-01f },
|
|
{-4.122946560e-01f, 2.171340883e-01f }, {-4.087924063e-01f, 2.120959163e-01f },
|
|
{-4.052285850e-01f, 2.071010768e-01f }, {-4.016037583e-01f, 2.021503448e-01f },
|
|
{-3.979184628e-01f, 1.972444654e-01f }, {-3.941732049e-01f, 1.923842132e-01f },
|
|
{-3.903686106e-01f, 1.875702441e-01f }, {-3.865052164e-01f, 1.828033626e-01f },
|
|
{-3.825836182e-01f, 1.780842245e-01f }, {-3.786044121e-01f, 1.734135747e-01f },
|
|
{-3.745681942e-01f, 1.687920988e-01f }, {-3.704755604e-01f, 1.642205119e-01f },
|
|
{-3.663271368e-01f, 1.596994996e-01f }, {-3.621235490e-01f, 1.552297175e-01f },
|
|
{-3.578654230e-01f, 1.508118808e-01f }, {-3.535533845e-01f, 1.464466155e-01f },
|
|
{-3.491881192e-01f, 1.421345770e-01f }, {-3.447702825e-01f, 1.378764510e-01f },
|
|
{-3.403005004e-01f, 1.336728632e-01f }, {-3.357794881e-01f, 1.295244396e-01f },
|
|
{-3.312079012e-01f, 1.254318058e-01f }, {-3.265864253e-01f, 1.213955879e-01f },
|
|
{-3.219157755e-01f, 1.174163818e-01f }, {-3.171966374e-01f, 1.134947836e-01f },
|
|
{-3.124297559e-01f, 1.096313894e-01f }, {-3.076157868e-01f, 1.058267951e-01f },
|
|
{-3.027555346e-01f, 1.020815372e-01f }, {-2.978496552e-01f, 9.839624166e-02f },
|
|
{-2.928989232e-01f, 9.477141500e-02f }, {-2.879040837e-01f, 9.120759368e-02f },
|
|
{-2.828659117e-01f, 8.770534396e-02f }, {-2.777851224e-01f, 8.426520228e-02f },
|
|
{-2.726624906e-01f, 8.088764548e-02f }, {-2.674988210e-01f, 7.757321000e-02f },
|
|
{-2.622948289e-01f, 7.432240248e-02f }, {-2.570513785e-01f, 7.113569975e-02f },
|
|
{-2.517691851e-01f, 6.801357865e-02f }, {-2.464490980e-01f, 6.495651603e-02f },
|
|
{-2.410918921e-01f, 6.196495891e-02f }, {-2.356983721e-01f, 5.903935432e-02f },
|
|
{-2.302693576e-01f, 5.618017912e-02f }, {-2.248056680e-01f, 5.338785052e-02f },
|
|
{-2.193081230e-01f, 5.066275597e-02f }, {-2.137775421e-01f, 4.800534248e-02f },
|
|
{-2.082147747e-01f, 4.541599751e-02f }, {-2.026206553e-01f, 4.289510846e-02f },
|
|
{-1.969960183e-01f, 4.044306278e-02f }, {-1.913417131e-01f, 3.806024790e-02f },
|
|
{-1.856586039e-01f, 3.574696183e-02f }, {-1.799475253e-01f, 3.350359201e-02f },
|
|
{-1.742093414e-01f, 3.133049607e-02f }, {-1.684449315e-01f, 2.922797203e-02f },
|
|
{-1.626551449e-01f, 2.719634771e-02f }, {-1.568408757e-01f, 2.523592114e-02f },
|
|
{-1.510029733e-01f, 2.334699035e-02f }, {-1.451423317e-01f, 2.152982354e-02f },
|
|
{-1.392598450e-01f, 1.978474855e-02f }, {-1.333563775e-01f, 1.811197400e-02f },
|
|
{-1.274328232e-01f, 1.651176810e-02f }, {-1.214900911e-01f, 1.498436928e-02f },
|
|
{-1.155290529e-01f, 1.353001595e-02f }, {-1.095506176e-01f, 1.214894652e-02f },
|
|
{-1.035556868e-01f, 1.084131002e-02f }, {-9.754516184e-02f, 9.607374668e-03f },
|
|
{-9.151994437e-02f, 8.447259665e-03f }, {-8.548094332e-02f, 7.361173630e-03f },
|
|
{-7.942907512e-02f, 6.349295378e-03f }, {-7.336523384e-02f, 5.411744118e-03f },
|
|
{-6.729035079e-02f, 4.548668861e-03f }, {-6.120533869e-02f, 3.760218620e-03f },
|
|
{-5.511110276e-02f, 3.046512604e-03f }, {-4.900857061e-02f, 2.407640219e-03f },
|
|
{-4.289865494e-02f, 1.843690872e-03f }, {-3.678228334e-02f, 1.354783773e-03f },
|
|
{-3.066036850e-02f, 9.409487247e-04f }, {-2.453383803e-02f, 6.022751331e-04f },
|
|
{-1.840361208e-02f, 3.388226032e-04f }, {-1.227061450e-02f, 1.505911350e-04f },
|
|
{-6.135769188e-03f, 3.764033318e-05f }, {-0.000000000e+00f, 0.000000000e+00f },
|
|
};
|
|
|
|
// x[n] in interleaved order
|
|
// y[n] in interleaved order
|
|
// n >= 2
|
|
static void fft_radix2_pass(complex_t* x, complex_t* y, const complex_t* w, int n, int p) {
|
|
|
|
size_t t = n/2;
|
|
assert(n >= 1);
|
|
assert(p >= 1);
|
|
|
|
complex_t* xp0 = &x[0*t];
|
|
complex_t* xp1 = &x[1*t];
|
|
|
|
complex_t* yp0 = &y[0*p];
|
|
complex_t* yp1 = &y[1*p];
|
|
|
|
for (size_t i = 0; i < t; i++) {
|
|
|
|
size_t k = i & (p-1); // twiddle index
|
|
size_t j = 2*(i-k) + k; // output index
|
|
|
|
float ar0 = xp0[i].re;
|
|
float ai0 = xp0[i].im;
|
|
float ar1 = xp1[i].re;
|
|
float ai1 = xp1[i].im;
|
|
|
|
float wr1 = w[k].re;
|
|
float wi1 = w[k].im;
|
|
|
|
float br1 = ar1 * wr1;
|
|
float bi1 = ai1 * wr1;
|
|
|
|
br1 += ai1 * wi1;
|
|
bi1 -= ar1 * wi1;
|
|
|
|
float cr0 = ar0 + br1;
|
|
float ci0 = ai0 + bi1;
|
|
float cr1 = ar0 - br1;
|
|
float ci1 = ai0 - bi1;
|
|
|
|
yp0[j].re = cr0;
|
|
yp0[j].im = ci0;
|
|
yp1[j].re = cr1;
|
|
yp1[j].im = ci1;
|
|
}
|
|
}
|
|
|
|
// x[n] in interleaved order
|
|
// y[n] in interleaved order
|
|
// n >= 4
|
|
static void fft_radix4_pass(complex_t* x, complex_t* y, const complex_t* w, int n, int p) {
|
|
|
|
size_t t = n/4;
|
|
assert(n >= 1);
|
|
assert(p >= 1);
|
|
|
|
complex_t* xp0 = &x[0*t];
|
|
complex_t* xp1 = &x[1*t];
|
|
complex_t* xp2 = &x[2*t];
|
|
complex_t* xp3 = &x[3*t];
|
|
|
|
complex_t* yp0 = &y[0*p];
|
|
complex_t* yp1 = &y[1*p];
|
|
complex_t* yp2 = &y[2*p];
|
|
complex_t* yp3 = &y[3*p];
|
|
|
|
for (size_t i = 0; i < t; i++) {
|
|
|
|
size_t k = i & (p-1); // twiddle index
|
|
size_t j = 4*(i-k) + k; // output index
|
|
|
|
float ar1 = xp1[i].re;
|
|
float ai1 = xp1[i].im;
|
|
float ar2 = xp2[i].re;
|
|
float ai2 = xp2[i].im;
|
|
float ar3 = xp3[i].re;
|
|
float ai3 = xp3[i].im;
|
|
|
|
float wr1 = w[3*k+0].re;
|
|
float wi1 = w[3*k+0].im;
|
|
float wr2 = w[3*k+1].re;
|
|
float wi2 = w[3*k+1].im;
|
|
float wr3 = w[3*k+2].re;
|
|
float wi3 = w[3*k+2].im;
|
|
|
|
float br1 = ar1 * wr1;
|
|
float bi1 = ai1 * wr1;
|
|
float br2 = ar2 * wr2;
|
|
float bi2 = ai2 * wr2;
|
|
float br3 = ar3 * wr3;
|
|
float bi3 = ai3 * wr3;
|
|
|
|
br1 += ai1 * wi1;
|
|
bi1 -= ar1 * wi1;
|
|
br2 += ai2 * wi2;
|
|
bi2 -= ar2 * wi2;
|
|
br3 += ai3 * wi3;
|
|
bi3 -= ar3 * wi3;
|
|
|
|
float ar0 = xp0[i].re;
|
|
float ai0 = xp0[i].im;
|
|
|
|
float cr0 = ar0 + br2;
|
|
float ci0 = ai0 + bi2;
|
|
float cr2 = ar0 - br2;
|
|
float ci2 = ai0 - bi2;
|
|
float cr1 = br1 + br3;
|
|
float ci1 = bi1 + bi3;
|
|
float cr3 = br1 - br3;
|
|
float ci3 = bi1 - bi3;
|
|
|
|
float dr0 = cr0 + cr1;
|
|
float di0 = ci0 + ci1;
|
|
float dr1 = cr2 + ci3;
|
|
float di1 = ci2 - cr3;
|
|
float dr2 = cr0 - cr1;
|
|
float di2 = ci0 - ci1;
|
|
float dr3 = cr2 - ci3;
|
|
float di3 = ci2 + cr3;
|
|
|
|
yp0[j].re = dr0;
|
|
yp0[j].im = di0;
|
|
yp1[j].re = dr1;
|
|
yp1[j].im = di1;
|
|
yp2[j].re = dr2;
|
|
yp2[j].im = di2;
|
|
yp3[j].re = dr3;
|
|
yp3[j].im = di3;
|
|
}
|
|
}
|
|
|
|
static auto& fft_radix4_last = fft_radix4_pass;
|
|
|
|
// x[n] in interleaved order
|
|
// y[n] in interleaved order
|
|
// n >= 4
|
|
static void ifft_radix4_last(complex_t* x, complex_t* y, const complex_t* w, int n, int p) {
|
|
|
|
size_t t = n/4;
|
|
assert(n >= 1);
|
|
assert(p >= 1);
|
|
|
|
complex_t* xp0 = &x[0*t];
|
|
complex_t* xp1 = &x[1*t];
|
|
complex_t* xp2 = &x[2*t];
|
|
complex_t* xp3 = &x[3*t];
|
|
|
|
complex_t* yp0 = &y[0*p];
|
|
complex_t* yp1 = &y[1*p];
|
|
complex_t* yp2 = &y[2*p];
|
|
complex_t* yp3 = &y[3*p];
|
|
|
|
for (size_t i = 0; i < t; i++) {
|
|
|
|
size_t k = i & (p-1); // twiddle index
|
|
size_t j = 4*(i-k) + k; // output index
|
|
|
|
float ar1 = xp1[i].re;
|
|
float ai1 = xp1[i].im;
|
|
float ar2 = xp2[i].re;
|
|
float ai2 = xp2[i].im;
|
|
float ar3 = xp3[i].re;
|
|
float ai3 = xp3[i].im;
|
|
|
|
float wr1 = w[3*k+0].re;
|
|
float wi1 = w[3*k+0].im;
|
|
float wr2 = w[3*k+1].re;
|
|
float wi2 = w[3*k+1].im;
|
|
float wr3 = w[3*k+2].re;
|
|
float wi3 = w[3*k+2].im;
|
|
|
|
float br1 = ar1 * wr1;
|
|
float bi1 = ai1 * wr1;
|
|
float br2 = ar2 * wr2;
|
|
float bi2 = ai2 * wr2;
|
|
float br3 = ar3 * wr3;
|
|
float bi3 = ai3 * wr3;
|
|
|
|
br1 += ai1 * wi1;
|
|
bi1 -= ar1 * wi1;
|
|
br2 += ai2 * wi2;
|
|
bi2 -= ar2 * wi2;
|
|
br3 += ai3 * wi3;
|
|
bi3 -= ar3 * wi3;
|
|
|
|
float ar0 = xp0[i].re;
|
|
float ai0 = xp0[i].im;
|
|
|
|
float cr0 = ar0 + br2;
|
|
float ci0 = ai0 + bi2;
|
|
float cr2 = ar0 - br2;
|
|
float ci2 = ai0 - bi2;
|
|
float cr1 = br1 + br3;
|
|
float ci1 = bi1 + bi3;
|
|
float cr3 = br1 - br3;
|
|
float ci3 = bi1 - bi3;
|
|
|
|
float dr0 = cr0 + cr1;
|
|
float di0 = ci0 + ci1;
|
|
float dr1 = cr2 + ci3;
|
|
float di1 = ci2 - cr3;
|
|
float dr2 = cr0 - cr1;
|
|
float di2 = ci0 - ci1;
|
|
float dr3 = cr2 - ci3;
|
|
float di3 = ci2 + cr3;
|
|
|
|
float scale = 1.0f/n; // scaled by 1/n for ifft
|
|
dr0 *= scale;
|
|
di0 *= scale;
|
|
dr1 *= scale;
|
|
di1 *= scale;
|
|
dr2 *= scale;
|
|
di2 *= scale;
|
|
dr3 *= scale;
|
|
di3 *= scale;
|
|
|
|
yp0[j].re = di0; // swapped re,im for ifft
|
|
yp0[j].im = dr0;
|
|
yp1[j].re = di1;
|
|
yp1[j].im = dr1;
|
|
yp2[j].re = di2;
|
|
yp2[j].im = dr2;
|
|
yp3[j].re = di3;
|
|
yp3[j].im = dr3;
|
|
}
|
|
}
|
|
|
|
// x[n] in interleaved order
|
|
// y[n] in interleaved order
|
|
// n >= 8
|
|
static void fft_radix8_first(complex_t* x, complex_t* y, int n, int p) {
|
|
|
|
size_t t = n/8;
|
|
assert(t >= 1);
|
|
assert(p == 1);
|
|
|
|
complex_t* xp0 = &x[0*t];
|
|
complex_t* xp1 = &x[1*t];
|
|
complex_t* xp2 = &x[2*t];
|
|
complex_t* xp3 = &x[3*t];
|
|
complex_t* xp4 = &x[4*t];
|
|
complex_t* xp5 = &x[5*t];
|
|
complex_t* xp6 = &x[6*t];
|
|
complex_t* xp7 = &x[7*t];
|
|
|
|
for (size_t i = 0; i < t; i++) {
|
|
|
|
//
|
|
// even
|
|
//
|
|
float ar0 = xp0[i].re;
|
|
float ai0 = xp0[i].im;
|
|
float ar2 = xp2[i].re;
|
|
float ai2 = xp2[i].im;
|
|
float ar4 = xp4[i].re;
|
|
float ai4 = xp4[i].im;
|
|
float ar6 = xp6[i].re;
|
|
float ai6 = xp6[i].im;
|
|
|
|
float cr0 = ar0 + ar4;
|
|
float ci0 = ai0 + ai4;
|
|
float cr2 = ar2 + ar6;
|
|
float ci2 = ai2 + ai6;
|
|
float cr4 = ar0 - ar4;
|
|
float ci4 = ai0 - ai4;
|
|
float cr6 = ar2 - ar6;
|
|
float ci6 = ai2 - ai6;
|
|
|
|
float dr0 = cr0 + cr2;
|
|
float di0 = ci0 + ci2;
|
|
float dr2 = cr0 - cr2;
|
|
float di2 = ci0 - ci2;
|
|
float dr4 = cr4 + ci6;
|
|
float di4 = ci4 - cr6;
|
|
float dr6 = cr4 - ci6;
|
|
float di6 = ci4 + cr6;
|
|
|
|
//
|
|
// odd
|
|
//
|
|
float ar1 = xp1[i].re;
|
|
float ai1 = xp1[i].im;
|
|
float ar3 = xp3[i].re;
|
|
float ai3 = xp3[i].im;
|
|
float ar5 = xp5[i].re;
|
|
float ai5 = xp5[i].im;
|
|
float ar7 = xp7[i].re;
|
|
float ai7 = xp7[i].im;
|
|
|
|
float br1 = ar1 + ar5;
|
|
float bi1 = ai1 + ai5;
|
|
float br3 = ar3 + ar7;
|
|
float bi3 = ai3 + ai7;
|
|
float br5 = ar1 - ar5;
|
|
float bi5 = ai1 - ai5;
|
|
float br7 = ar3 - ar7;
|
|
float bi7 = ai3 - ai7;
|
|
|
|
float cr1 = br1;
|
|
float ci1 = bi1;
|
|
float cr3 = br3;
|
|
float ci3 = bi3;
|
|
float cr5 = bi5 + br5;
|
|
float ci5 = bi5 - br5;
|
|
float cr7 = bi7 - br7;
|
|
float ci7 = bi7 + br7;
|
|
|
|
cr5 *= SQRT1_2;
|
|
ci5 *= SQRT1_2;
|
|
cr7 *= SQRT1_2;
|
|
ci7 *= SQRT1_2;
|
|
|
|
float dr1 = cr1 + cr3;
|
|
float di1 = ci1 + ci3;
|
|
float dr3 = cr1 - cr3;
|
|
float di3 = ci1 - ci3;
|
|
float dr5 = cr5 + cr7;
|
|
float di5 = ci5 - ci7;
|
|
float dr7 = cr5 - cr7;
|
|
float di7 = ci5 + ci7;
|
|
|
|
//
|
|
// merge
|
|
//
|
|
float er0 = dr0 + dr1;
|
|
float ei0 = di0 + di1;
|
|
float er1 = dr4 + dr5;
|
|
float ei1 = di4 + di5;
|
|
float er2 = dr2 + di3;
|
|
float ei2 = di2 - dr3;
|
|
float er3 = dr6 + di7;
|
|
float ei3 = di6 - dr7;
|
|
float er4 = dr0 - dr1;
|
|
float ei4 = di0 - di1;
|
|
float er5 = dr4 - dr5;
|
|
float ei5 = di4 - di5;
|
|
float er6 = dr2 - di3;
|
|
float ei6 = di2 + dr3;
|
|
float er7 = dr6 - di7;
|
|
float ei7 = di6 + dr7;
|
|
|
|
y[8*i+0].re = er0;
|
|
y[8*i+0].im = ei0;
|
|
y[8*i+1].re = er1;
|
|
y[8*i+1].im = ei1;
|
|
y[8*i+2].re = er2;
|
|
y[8*i+2].im = ei2;
|
|
y[8*i+3].re = er3;
|
|
y[8*i+3].im = ei3;
|
|
y[8*i+4].re = er4;
|
|
y[8*i+4].im = ei4;
|
|
y[8*i+5].re = er5;
|
|
y[8*i+5].im = ei5;
|
|
y[8*i+6].re = er6;
|
|
y[8*i+6].im = ei6;
|
|
y[8*i+7].re = er7;
|
|
y[8*i+7].im = ei7;
|
|
}
|
|
}
|
|
|
|
// x[n] in interleaved order
|
|
// y[n] in interleaved order
|
|
// n >= 8
|
|
static void ifft_radix8_first(complex_t* x, complex_t* y, int n, int p) {
|
|
|
|
size_t t = n/8;
|
|
assert(t >= 1);
|
|
assert(p == 1);
|
|
|
|
complex_t* xp0 = &x[0*t];
|
|
complex_t* xp1 = &x[1*t];
|
|
complex_t* xp2 = &x[2*t];
|
|
complex_t* xp3 = &x[3*t];
|
|
complex_t* xp4 = &x[4*t];
|
|
complex_t* xp5 = &x[5*t];
|
|
complex_t* xp6 = &x[6*t];
|
|
complex_t* xp7 = &x[7*t];
|
|
|
|
for (size_t i = 0; i < t; i++) {
|
|
|
|
//
|
|
// even
|
|
//
|
|
float ar0 = xp0[i].im; // swapped re,im for ifft
|
|
float ai0 = xp0[i].re;
|
|
float ar2 = xp2[i].im;
|
|
float ai2 = xp2[i].re;
|
|
float ar4 = xp4[i].im;
|
|
float ai4 = xp4[i].re;
|
|
float ar6 = xp6[i].im;
|
|
float ai6 = xp6[i].re;
|
|
|
|
float cr0 = ar0 + ar4;
|
|
float ci0 = ai0 + ai4;
|
|
float cr2 = ar2 + ar6;
|
|
float ci2 = ai2 + ai6;
|
|
float cr4 = ar0 - ar4;
|
|
float ci4 = ai0 - ai4;
|
|
float cr6 = ar2 - ar6;
|
|
float ci6 = ai2 - ai6;
|
|
|
|
float dr0 = cr0 + cr2;
|
|
float di0 = ci0 + ci2;
|
|
float dr2 = cr0 - cr2;
|
|
float di2 = ci0 - ci2;
|
|
float dr4 = cr4 + ci6;
|
|
float di4 = ci4 - cr6;
|
|
float dr6 = cr4 - ci6;
|
|
float di6 = ci4 + cr6;
|
|
|
|
//
|
|
// odd
|
|
//
|
|
float ar1 = xp1[i].im; // swapped re,im for ifft
|
|
float ai1 = xp1[i].re;
|
|
float ar3 = xp3[i].im;
|
|
float ai3 = xp3[i].re;
|
|
float ar5 = xp5[i].im;
|
|
float ai5 = xp5[i].re;
|
|
float ar7 = xp7[i].im;
|
|
float ai7 = xp7[i].re;
|
|
|
|
float br1 = ar1 + ar5;
|
|
float bi1 = ai1 + ai5;
|
|
float br3 = ar3 + ar7;
|
|
float bi3 = ai3 + ai7;
|
|
float br5 = ar1 - ar5;
|
|
float bi5 = ai1 - ai5;
|
|
float br7 = ar3 - ar7;
|
|
float bi7 = ai3 - ai7;
|
|
|
|
float cr1 = br1;
|
|
float ci1 = bi1;
|
|
float cr3 = br3;
|
|
float ci3 = bi3;
|
|
float cr5 = bi5 + br5;
|
|
float ci5 = bi5 - br5;
|
|
float cr7 = bi7 - br7;
|
|
float ci7 = bi7 + br7;
|
|
|
|
cr5 *= SQRT1_2;
|
|
ci5 *= SQRT1_2;
|
|
cr7 *= SQRT1_2;
|
|
ci7 *= SQRT1_2;
|
|
|
|
float dr1 = cr1 + cr3;
|
|
float di1 = ci1 + ci3;
|
|
float dr3 = cr1 - cr3;
|
|
float di3 = ci1 - ci3;
|
|
float dr5 = cr5 + cr7;
|
|
float di5 = ci5 - ci7;
|
|
float dr7 = cr5 - cr7;
|
|
float di7 = ci5 + ci7;
|
|
|
|
//
|
|
// merge
|
|
//
|
|
float er0 = dr0 + dr1;
|
|
float ei0 = di0 + di1;
|
|
float er1 = dr4 + dr5;
|
|
float ei1 = di4 + di5;
|
|
float er2 = dr2 + di3;
|
|
float ei2 = di2 - dr3;
|
|
float er3 = dr6 + di7;
|
|
float ei3 = di6 - dr7;
|
|
float er4 = dr0 - dr1;
|
|
float ei4 = di0 - di1;
|
|
float er5 = dr4 - dr5;
|
|
float ei5 = di4 - di5;
|
|
float er6 = dr2 - di3;
|
|
float ei6 = di2 + dr3;
|
|
float er7 = dr6 - di7;
|
|
float ei7 = di6 + dr7;
|
|
|
|
y[8*i+0].re = er0;
|
|
y[8*i+0].im = ei0;
|
|
y[8*i+1].re = er1;
|
|
y[8*i+1].im = ei1;
|
|
y[8*i+2].re = er2;
|
|
y[8*i+2].im = ei2;
|
|
y[8*i+3].re = er3;
|
|
y[8*i+3].im = ei3;
|
|
y[8*i+4].re = er4;
|
|
y[8*i+4].im = ei4;
|
|
y[8*i+5].re = er5;
|
|
y[8*i+5].im = ei5;
|
|
y[8*i+6].re = er6;
|
|
y[8*i+6].im = ei6;
|
|
y[8*i+7].re = er7;
|
|
y[8*i+7].im = ei7;
|
|
}
|
|
}
|
|
|
|
// x[n] in interleaved order
|
|
// n >= 4
|
|
static void rfft_post(complex_t* x, const complex_t* w, int n) {
|
|
|
|
size_t t = n/4;
|
|
assert(t >= 1);
|
|
|
|
// NOTE: x[n/2].re is packed into x[0].im
|
|
float tr = x[0].re;
|
|
float ti = x[0].im;
|
|
x[0].re = tr + ti;
|
|
x[0].im = tr - ti;
|
|
|
|
complex_t* xp0 = &x[1];
|
|
complex_t* xp1 = &x[n/2 - 1];
|
|
|
|
for (size_t i = 0; i < t; i++) {
|
|
|
|
float ar = xp0[i].re;
|
|
float ai = xp0[i].im;
|
|
float br = xp1[0-i].re;
|
|
float bi = xp1[0-i].im;
|
|
|
|
float wr = w[i].re;
|
|
float wi = w[i].im;
|
|
|
|
float cr = ar - br;
|
|
float ci = ai + bi;
|
|
|
|
float dr = cr * wr;
|
|
float di = ci * wr;
|
|
|
|
dr += ci * wi;
|
|
di -= cr * wi;
|
|
|
|
ar = ar + di;
|
|
ai = dr - ai;
|
|
|
|
br = br - di;
|
|
bi = dr - bi;
|
|
|
|
xp0[i].re = br;
|
|
xp0[i].im = bi;
|
|
xp1[0-i].re = ar;
|
|
xp1[0-i].im = ai;
|
|
}
|
|
}
|
|
|
|
// x[n] in interleaved order
|
|
// n >= 4
|
|
static void rifft_pre(complex_t* x, const complex_t* w, int n) {
|
|
|
|
size_t t = n/4;
|
|
assert(t >= 1);
|
|
|
|
// NOTE: x[n/2].re is packed into x[0].im
|
|
float tr = x[0].re;
|
|
float ti = x[0].im;
|
|
x[0].re = 0.5f * (tr + ti); // halved for ifft
|
|
x[0].im = 0.5f * (tr - ti); // halved for ifft
|
|
|
|
complex_t* xp0 = &x[1];
|
|
complex_t* xp1 = &x[n/2 - 1];
|
|
|
|
for (size_t i = 0; i < t; i++) {
|
|
|
|
float ar = xp0[i].re;
|
|
float ai = xp0[i].im;
|
|
float br = xp1[0-i].re;
|
|
float bi = xp1[0-i].im;
|
|
|
|
float wr = w[i].re; // negated for ifft
|
|
float wi = w[i].im;
|
|
|
|
float cr = br - ar;
|
|
float ci = ai + bi;
|
|
|
|
float dr = cr * wr;
|
|
float di = cr * wi;
|
|
|
|
dr += ci * wi;
|
|
di -= ci * wr;
|
|
|
|
ar = ar + di;
|
|
ai = dr - ai;
|
|
|
|
br = br - di;
|
|
bi = dr - bi;
|
|
|
|
xp0[i].re = br;
|
|
xp0[i].im = bi;
|
|
xp1[0-i].re = ar;
|
|
xp1[0-i].im = ai;
|
|
}
|
|
}
|
|
|
|
// in-place complex FFT
|
|
// buf[n] in interleaved order
|
|
static void fft256_ref(float buf[512]) {
|
|
|
|
complex_t* x = (complex_t*)buf;
|
|
complex_t y[256];
|
|
|
|
fft_radix8_first(x, y, 256, 1);
|
|
fft_radix2_pass(y, x, w_fft_256_0, 256, 8);
|
|
fft_radix4_pass(x, y, w_fft_256_1, 256, 16);
|
|
fft_radix4_last(y, x, w_fft_256_2, 256, 64);
|
|
}
|
|
|
|
// in-place complex IFFT
|
|
// buf[n] in interleaved order
|
|
static void ifft256_ref(float buf[512]) {
|
|
|
|
complex_t* x = (complex_t*)buf;
|
|
complex_t y[256];
|
|
|
|
ifft_radix8_first(x, y, 256, 1);
|
|
fft_radix2_pass(y, x, w_fft_256_0, 256, 8);
|
|
fft_radix4_pass(x, y, w_fft_256_1, 256, 16);
|
|
ifft_radix4_last(y, x, w_fft_256_2, 256, 64);
|
|
}
|
|
|
|
// in-place real FFT
|
|
// buf[n] in interleaved order
|
|
static void rfft512_ref(float buf[512]) {
|
|
|
|
fft256_ref(buf); // half size complex
|
|
rfft_post((complex_t*)buf, w_rfft_512, 512);
|
|
}
|
|
|
|
// in-place real IFFT
|
|
// buf[n] in interleaved order
|
|
static void rifft512_ref(float buf[512]) {
|
|
|
|
rifft_pre((complex_t*)buf, w_rfft_512, 512);
|
|
ifft256_ref(buf); // half size complex
|
|
}
|
|
|
|
// fft-domain complex multiply-add, for packed complex-conjugate symmetric
|
|
// 1 channel input, 2 channel output
|
|
static void rfft512_cmadd_1X2_ref(const float src[512], const float coef0[512], const float coef1[512], float dst0[512], float dst1[512]) {
|
|
|
|
// NOTE: x[n/2].re is packed into x[0].im
|
|
dst0[0] += src[0] * coef0[0]; // first bin is real
|
|
dst0[1] += src[1] * coef0[1]; // last bin is real
|
|
|
|
dst1[0] += src[0] * coef1[0]; // first bin is real
|
|
dst1[1] += src[1] * coef1[1]; // last bin is real
|
|
|
|
for (int i = 1; i < 512/2; i++) {
|
|
dst0[2*i+0] += src[2*i+0] * coef0[2*i+0] - src[2*i+1] * coef0[2*i+1]; // re
|
|
dst0[2*i+1] += src[2*i+0] * coef0[2*i+1] + src[2*i+1] * coef0[2*i+0]; // im
|
|
|
|
dst1[2*i+0] += src[2*i+0] * coef1[2*i+0] - src[2*i+1] * coef1[2*i+1]; // re
|
|
dst1[2*i+1] += src[2*i+0] * coef1[2*i+1] + src[2*i+1] * coef1[2*i+0]; // im
|
|
}
|
|
}
|
|
|
|
#ifdef FOA_INPUT_FUMA // input is FuMa (B-format) channel order and normalization
|
|
|
|
// convert to deinterleaved float (B-format)
|
|
static void convertInput_ref(int16_t* src, float *dst[4], float gain, int numFrames) {
|
|
|
|
const float scale = gain * (1/32768.0f);
|
|
|
|
for (int i = 0; i < numFrames; i++) {
|
|
dst[0][i] = (float)src[4*i+0] * scale; // W
|
|
dst[1][i] = (float)src[4*i+1] * scale; // X
|
|
dst[2][i] = (float)src[4*i+2] * scale; // Y
|
|
dst[3][i] = (float)src[4*i+3] * scale; // Z
|
|
}
|
|
}
|
|
|
|
#else // input is ambiX (ACN/SN3D) channel order and normalization
|
|
|
|
// convert to deinterleaved float (B-format)
|
|
static void convertInput_ref(int16_t* src, float *dst[4], float gain, int numFrames) {
|
|
|
|
const float scaleW = gain * (1/32768.0f) * SQRT1_2; // -3dB
|
|
const float scale = gain * (1/32768.0f);
|
|
|
|
for (int i = 0; i < numFrames; i++) {
|
|
dst[0][i] = (float)src[4*i+0] * scaleW; // W
|
|
dst[2][i] = (float)src[4*i+1] * scale; // Y
|
|
dst[3][i] = (float)src[4*i+2] * scale; // Z
|
|
dst[1][i] = (float)src[4*i+3] * scale; // X
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
// in-place rotation of the soundfield
|
|
// crossfade between old and new rotation, to prevent artifacts
|
|
static void rotate_3x3_ref(float* buf[4], const float m0[3][3], const float m1[3][3], const float* win, int numFrames) {
|
|
|
|
const float md[3][3] = {
|
|
{ m0[0][0] - m1[0][0], m0[0][1] - m1[0][1], m0[0][2] - m1[0][2] },
|
|
{ m0[1][0] - m1[1][0], m0[1][1] - m1[1][1], m0[1][2] - m1[1][2] },
|
|
{ m0[2][0] - m1[2][0], m0[2][1] - m1[2][1], m0[2][2] - m1[2][2] },
|
|
};
|
|
|
|
for (int i = 0; i < numFrames; i++) {
|
|
|
|
float frac = win[i];
|
|
|
|
// interpolate the matrix
|
|
float m00 = m1[0][0] + frac * md[0][0];
|
|
float m10 = m1[1][0] + frac * md[1][0];
|
|
float m20 = m1[2][0] + frac * md[2][0];
|
|
|
|
float m01 = m1[0][1] + frac * md[0][1];
|
|
float m11 = m1[1][1] + frac * md[1][1];
|
|
float m21 = m1[2][1] + frac * md[2][1];
|
|
|
|
float m02 = m1[0][2] + frac * md[0][2];
|
|
float m12 = m1[1][2] + frac * md[1][2];
|
|
float m22 = m1[2][2] + frac * md[2][2];
|
|
|
|
// matrix multiply
|
|
float x = m00 * buf[1][i] + m01 * buf[2][i] + m02 * buf[3][i];
|
|
float y = m10 * buf[1][i] + m11 * buf[2][i] + m12 * buf[3][i];
|
|
float z = m20 * buf[1][i] + m21 * buf[2][i] + m22 * buf[3][i];
|
|
|
|
buf[1][i] = x;
|
|
buf[2][i] = y;
|
|
buf[3][i] = z;
|
|
}
|
|
}
|
|
|
|
#if defined(_M_IX86) || defined(_M_X64) || defined(__i386__) || defined(__x86_64__)
|
|
|
|
//
|
|
// Runtime CPU dispatch
|
|
//
|
|
|
|
#include "CPUDetect.h"
|
|
|
|
void rfft512_AVX2(float buf[512]);
|
|
void rifft512_AVX2(float buf[512]);
|
|
void rfft512_cmadd_1X2_AVX2(const float src[512], const float coef0[512], const float coef1[512], float dst0[512], float dst1[512]);
|
|
void convertInput_AVX2(int16_t* src, float *dst[4], float gain, int numFrames);
|
|
void rotate_3x3_AVX2(float* buf[4], const float m0[3][3], const float m1[3][3], const float* win, int numFrames);
|
|
|
|
static void rfft512(float buf[512]) {
|
|
static auto f = cpuSupportsAVX2() ? rfft512_AVX2 : rfft512_ref;
|
|
(*f)(buf); // dispatch
|
|
}
|
|
|
|
static void rifft512(float buf[512]) {
|
|
static auto f = cpuSupportsAVX2() ? rifft512_AVX2 : rifft512_ref;
|
|
(*f)(buf); // dispatch
|
|
}
|
|
|
|
static void rfft512_cmadd_1X2(const float src[512], const float coef0[512], const float coef1[512], float dst0[512], float dst1[512]) {
|
|
static auto f = cpuSupportsAVX2() ? rfft512_cmadd_1X2_AVX2 : rfft512_cmadd_1X2_ref;
|
|
(*f)(src, coef0, coef1, dst0, dst1); // dispatch
|
|
}
|
|
|
|
static void convertInput(int16_t* src, float *dst[4], float gain, int numFrames) {
|
|
static auto f = cpuSupportsAVX2() ? convertInput_AVX2 : convertInput_ref;
|
|
(*f)(src, dst, gain, numFrames); // dispatch
|
|
}
|
|
|
|
static void rotate_3x3(float* buf[4], const float m0[3][3], const float m1[3][3], const float* win, int numFrames) {
|
|
static auto f = cpuSupportsAVX2() ? rotate_3x3_AVX2 : rotate_3x3_ref;
|
|
(*f)(buf, m0, m1, win, numFrames); // dispatch
|
|
}
|
|
|
|
#else // portable reference code
|
|
|
|
static auto& rfft512 = rfft512_ref;
|
|
static auto& rifft512 = rifft512_ref;
|
|
static auto& rfft512_cmadd_1X2 = rfft512_cmadd_1X2_ref;
|
|
static auto& convertInput = convertInput_ref;
|
|
static auto& rotate_3x3 = rotate_3x3_ref;
|
|
|
|
#endif
|
|
|
|
//
|
|
// Equal-gain crossfade
|
|
//
|
|
// Cos(x)^2 window minimizes the modulation sidebands when a pure tone is panned.
|
|
//
|
|
ALIGN32 static const float crossfadeTable[FOA_BLOCK] = {
|
|
0.9999571638f, 0.9998286625f, 0.9996145181f, 0.9993147674f, 0.9989294616f, 0.9984586669f, 0.9979024638f, 0.9972609477f,
|
|
0.9965342285f, 0.9957224307f, 0.9948256934f, 0.9938441703f, 0.9927780295f, 0.9916274538f, 0.9903926402f, 0.9890738004f,
|
|
0.9876711603f, 0.9861849602f, 0.9846154549f, 0.9829629131f, 0.9812276182f, 0.9794098674f, 0.9775099722f, 0.9755282581f,
|
|
0.9734650647f, 0.9713207455f, 0.9690956680f, 0.9667902132f, 0.9644047764f, 0.9619397663f, 0.9593956051f, 0.9567727288f,
|
|
0.9540715869f, 0.9512926422f, 0.9484363708f, 0.9455032621f, 0.9424938187f, 0.9394085563f, 0.9362480035f, 0.9330127019f,
|
|
0.9297032058f, 0.9263200822f, 0.9228639109f, 0.9193352840f, 0.9157348062f, 0.9120630943f, 0.9083207776f, 0.9045084972f,
|
|
0.9006269063f, 0.8966766701f, 0.8926584654f, 0.8885729807f, 0.8844209160f, 0.8802029828f, 0.8759199037f, 0.8715724127f,
|
|
0.8671612547f, 0.8626871855f, 0.8581509717f, 0.8535533906f, 0.8488952299f, 0.8441772878f, 0.8394003728f, 0.8345653032f,
|
|
0.8296729076f, 0.8247240242f, 0.8197195010f, 0.8146601955f, 0.8095469747f, 0.8043807145f, 0.7991623003f, 0.7938926261f,
|
|
0.7885725950f, 0.7832031185f, 0.7777851165f, 0.7723195175f, 0.7668072580f, 0.7612492824f, 0.7556465430f, 0.7500000000f,
|
|
0.7443106207f, 0.7385793801f, 0.7328072602f, 0.7269952499f, 0.7211443451f, 0.7152555484f, 0.7093298688f, 0.7033683215f,
|
|
0.6973719282f, 0.6913417162f, 0.6852787188f, 0.6791839748f, 0.6730585285f, 0.6669034296f, 0.6607197327f, 0.6545084972f,
|
|
0.6482707875f, 0.6420076724f, 0.6357202249f, 0.6294095226f, 0.6230766465f, 0.6167226819f, 0.6103487175f, 0.6039558454f,
|
|
0.5975451610f, 0.5911177627f, 0.5846747519f, 0.5782172325f, 0.5717463110f, 0.5652630961f, 0.5587686987f, 0.5522642316f,
|
|
0.5457508093f, 0.5392295479f, 0.5327015646f, 0.5261679781f, 0.5196299079f, 0.5130884742f, 0.5065447978f, 0.5000000000f,
|
|
0.4934552022f, 0.4869115258f, 0.4803700921f, 0.4738320219f, 0.4672984354f, 0.4607704521f, 0.4542491907f, 0.4477357684f,
|
|
0.4412313013f, 0.4347369039f, 0.4282536890f, 0.4217827675f, 0.4153252481f, 0.4088822373f, 0.4024548390f, 0.3960441546f,
|
|
0.3896512825f, 0.3832773181f, 0.3769233535f, 0.3705904774f, 0.3642797751f, 0.3579923276f, 0.3517292125f, 0.3454915028f,
|
|
0.3392802673f, 0.3330965704f, 0.3269414715f, 0.3208160252f, 0.3147212812f, 0.3086582838f, 0.3026280718f, 0.2966316785f,
|
|
0.2906701312f, 0.2847444516f, 0.2788556549f, 0.2730047501f, 0.2671927398f, 0.2614206199f, 0.2556893793f, 0.2500000000f,
|
|
0.2443534570f, 0.2387507176f, 0.2331927420f, 0.2276804825f, 0.2222148835f, 0.2167968815f, 0.2114274050f, 0.2061073739f,
|
|
0.2008376997f, 0.1956192855f, 0.1904530253f, 0.1853398045f, 0.1802804990f, 0.1752759758f, 0.1703270924f, 0.1654346968f,
|
|
0.1605996272f, 0.1558227122f, 0.1511047701f, 0.1464466094f, 0.1418490283f, 0.1373128145f, 0.1328387453f, 0.1284275873f,
|
|
0.1240800963f, 0.1197970172f, 0.1155790840f, 0.1114270193f, 0.1073415346f, 0.1033233299f, 0.0993730937f, 0.0954915028f,
|
|
0.0916792224f, 0.0879369057f, 0.0842651938f, 0.0806647160f, 0.0771360891f, 0.0736799178f, 0.0702967942f, 0.0669872981f,
|
|
0.0637519965f, 0.0605914437f, 0.0575061813f, 0.0544967379f, 0.0515636292f, 0.0487073578f, 0.0459284131f, 0.0432272712f,
|
|
0.0406043949f, 0.0380602337f, 0.0355952236f, 0.0332097868f, 0.0309043320f, 0.0286792545f, 0.0265349353f, 0.0244717419f,
|
|
0.0224900278f, 0.0205901326f, 0.0187723818f, 0.0170370869f, 0.0153845451f, 0.0138150398f, 0.0123288397f, 0.0109261996f,
|
|
0.0096073598f, 0.0083725462f, 0.0072219705f, 0.0061558297f, 0.0051743066f, 0.0042775693f, 0.0034657715f, 0.0027390523f,
|
|
0.0020975362f, 0.0015413331f, 0.0010705384f, 0.0006852326f, 0.0003854819f, 0.0001713375f, 0.0000428362f, 0.0000000000f,
|
|
};
|
|
|
|
// convert quaternion to a column-major 3x3 rotation matrix
|
|
static void quatToMatrix_3x3(float w, float x, float y, float z, float m[3][3]) {
|
|
|
|
float xx = x * (x + x);
|
|
float xy = x * (y + y);
|
|
float xz = x * (z + z);
|
|
|
|
float yy = y * (y + y);
|
|
float yz = y * (z + z);
|
|
float zz = z * (z + z);
|
|
|
|
float wx = w * (x + x);
|
|
float wy = w * (y + y);
|
|
float wz = w * (z + z);
|
|
|
|
m[0][0] = 1.0f - (yy + zz);
|
|
m[0][1] = xy - wz;
|
|
m[0][2] = xz + wy;
|
|
|
|
m[1][0] = xy + wz;
|
|
m[1][1] = 1.0f - (xx + zz);
|
|
m[1][2] = yz - wx;
|
|
|
|
m[2][0] = xz - wy;
|
|
m[2][1] = yz + wx;
|
|
m[2][2] = 1.0f - (xx + yy);
|
|
}
|
|
|
|
// Ambisonic to binaural render
|
|
void AudioFOA::render(int16_t* input, float* output, int index, float qw, float qx, float qy, float qz, float gain, int numFrames) {
|
|
|
|
assert(index >= 0);
|
|
assert(index < FOA_TABLES);
|
|
assert(numFrames == FOA_BLOCK);
|
|
|
|
ALIGN32 float fftBuffer[FOA_NFFT]; // in-place FFT buffer
|
|
ALIGN32 float accBuffer[2][FOA_NFFT] = {}; // binaural accumulation buffers
|
|
ALIGN32 float inBuffer[4][FOA_BLOCK]; // deinterleaved input buffers
|
|
|
|
float* in[4] = { inBuffer[0], inBuffer[1], inBuffer[2], inBuffer[3] };
|
|
float rotation[3][3];
|
|
|
|
// convert input to deinterleaved float
|
|
convertInput(input, in, FOA_GAIN * gain, FOA_BLOCK);
|
|
|
|
// convert quaternion to 3x3 rotation
|
|
quatToMatrix_3x3(qw, qx, qy, qz, rotation);
|
|
|
|
// rotate the soundfield
|
|
rotate_3x3(in, _rotationState, rotation, crossfadeTable, FOA_BLOCK);
|
|
|
|
// rotation history update
|
|
memcpy(_rotationState, rotation, sizeof(_rotationState));
|
|
|
|
//
|
|
// Accumulate the contribution of each spherical harmonic into binaural output buffers.
|
|
// Performed in the frequency domain, using overlap-save FFT.
|
|
//
|
|
for (int n = 0; n < 4; n++) {
|
|
|
|
// fill with overlapped input
|
|
memcpy(fftBuffer, _fftState[n], FOA_OVERLAP * sizeof(float)); // old input
|
|
memcpy(&fftBuffer[FOA_OVERLAP], in[n], FOA_BLOCK * sizeof(float)); // new input
|
|
|
|
// input history update
|
|
memcpy(_fftState[n], &fftBuffer[FOA_BLOCK], FOA_OVERLAP * sizeof(float));
|
|
|
|
// forward transform
|
|
rfft512(fftBuffer);
|
|
|
|
// multiply-accumulate with filter kernels
|
|
rfft512_cmadd_1X2(fftBuffer, foa_table_table[index][n][0], foa_table_table[index][n][1], accBuffer[0], accBuffer[1]);
|
|
}
|
|
|
|
// inverse transform
|
|
rifft512(accBuffer[0]);
|
|
rifft512(accBuffer[1]);
|
|
|
|
//
|
|
// Mix into the interleaved output buffer.
|
|
// The first FOA_OVERLAP samples are discarded, due to overlap-save FFT.
|
|
//
|
|
for (int i = 0; i < FOA_BLOCK; i++) {
|
|
output[2*i+0] += accBuffer[0][i + FOA_OVERLAP];
|
|
output[2*i+1] += accBuffer[1][i + FOA_OVERLAP];
|
|
}
|
|
}
|