From 3ee1bb572ca0927e073e88e51744c2333dd66948 Mon Sep 17 00:00:00 2001 From: ceski <56656010+ceski-1@users.noreply.github.com> Date: Sat, 7 Sep 2024 07:13:47 -0700 Subject: [PATCH] Add gamepad rumble support (#1876) --- README.md | 8 +- src/CMakeLists.txt | 2 + src/g_game.c | 6 +- src/i_gyro.c | 3 + src/i_input.c | 5 + src/i_oalsound.c | 2 + src/i_rumble.c | 799 +++++++++++++++ src/i_rumble.h | 78 ++ src/i_sound.c | 3 + src/m_cheat.c | 2 +- src/m_config.c | 2 + src/mn_setup.c | 33 +- src/p_enemy.c | 8 +- src/p_inter.c | 6 +- src/p_map.c | 6 +- src/p_mobj.c | 14 +- src/p_pspr.c | 23 +- src/p_spec.c | 2 +- src/p_telept.c | 10 +- src/s_sound.c | 135 ++- src/s_sound.h | 35 +- src/sounds.h | 9 + third-party/CMakeLists.txt | 5 + third-party/pffft/LICENSE | 45 + third-party/pffft/pffft.c | 1877 ++++++++++++++++++++++++++++++++++++ third-party/pffft/pffft.h | 177 ++++ 26 files changed, 3250 insertions(+), 45 deletions(-) create mode 100644 src/i_rumble.c create mode 100644 src/i_rumble.h create mode 100644 third-party/pffft/LICENSE create mode 100644 third-party/pffft/pffft.c create mode 100644 third-party/pffft/pffft.h diff --git a/README.md b/README.md index a378037f..e307e4de 100644 --- a/README.md +++ b/README.md @@ -41,7 +41,7 @@ If you turn the [Doom logo upside down](https://www.reddit.com/r/Doom/comments/8 * Support for voxels in KVX format. * 3D audio, supporting stereo and up to 7.1 surround sound with an optional HRTF mode, as well as PC speaker emulation. * Several music backends: native MIDI, FluidSynth with a bundled soundfont, built-in OPL3 emulator. Digital music and sound formats supported by libsndfile, module music supported by libxmp. - * Modern gamepad support, including gyro controls and the flick stick control scheme. + * Modern gamepad support, including rumble, gyro, and flick stick. * Mouselook. * Autoload directories. * Savegame backward compatibility up to `MBF.EXE`. @@ -237,6 +237,12 @@ Copyright: © 2013-2014 RAD Game Tools and Valve Software. License: [MIT](https://opensource.org/licenses/MIT) +Files: `third-party/pffft/*` +Copyright: + © 2004 The University Corporation for Atmospheric Research ("UCAR"); + © 2013 Julien Pommier. +License: [FFTPACK License](https://bitbucket.org/jpommier/pffft/src/master/pffft.h) + Files: `third-party/spng/*` Copyright: © 2018-2023 Randy. License: [BSD-2-Clause](https://opensource.org/license/bsd-2-clause) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 0f9a2de6..c92ddd87 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -48,6 +48,7 @@ set(WOOF_SOURCES i_oalstream.h i_oplmusic.c i_printf.c i_printf.h + i_rumble.c i_rumble.h i_sndfile.c i_sndfile.h i_sound.c i_sound.h i_system.c i_system.h @@ -186,6 +187,7 @@ target_link_libraries(woof PRIVATE OpenAL::OpenAL SndFile::sndfile opl + pffft textscreen miniz spng diff --git a/src/g_game.c b/src/g_game.c index 0bbe1e9a..e13807c6 100644 --- a/src/g_game.c +++ b/src/g_game.c @@ -46,6 +46,7 @@ #include "i_gyro.h" #include "i_input.h" #include "i_printf.h" +#include "i_rumble.h" #include "i_system.h" #include "i_timer.h" #include "i_video.h" @@ -850,6 +851,7 @@ void G_ClearInput(void) G_ClearCarry(); memset(&basecmd, 0, sizeof(basecmd)); I_ResetRelativeMouseState(); + I_ResetAllRumbleChannels(); } // @@ -3020,8 +3022,8 @@ static boolean G_CheckSpot(int playernum, mapthing_t *mthing) ss->sector->floorheight, MT_TFOG); } - if (players[consoleplayer].viewz != 1) - S_StartSound(mo, sfx_telept); // don't start sound on first frame + if (players[consoleplayer].viewz != 1) // don't start sound on first frame + S_StartSoundSource(players[consoleplayer].mo, mo, sfx_telept); return true; } diff --git a/src/i_gyro.c b/src/i_gyro.c index c26c7664..de35b2dc 100644 --- a/src/i_gyro.c +++ b/src/i_gyro.c @@ -32,6 +32,7 @@ #include "i_printf.h" #include "g_game.h" #include "i_gamepad.h" +#include "i_rumble.h" #include "i_timer.h" #include "i_video.h" #include "m_config.h" @@ -224,6 +225,7 @@ void I_UpdateGyroCalibrationState(void) switch (cal.state) { case GYRO_CALIBRATION_INACTIVE: + I_DisableRumble(); cal.state = GYRO_CALIBRATION_STARTING; cal.start_time = I_GetTimeMS(); break; @@ -252,6 +254,7 @@ void I_UpdateGyroCalibrationState(void) if (I_GetTimeMS() - cal.finish_time > 1500) { cal.state = GYRO_CALIBRATION_INACTIVE; + I_UpdateRumbleEnabled(); } break; } diff --git a/src/i_input.c b/src/i_input.c index 1490f4c9..8d471fc4 100644 --- a/src/i_input.c +++ b/src/i_input.c @@ -24,6 +24,7 @@ #include "i_gamepad.h" #include "i_gyro.h" #include "i_printf.h" +#include "i_rumble.h" #include "i_system.h" #include "i_timer.h" #include "m_config.h" @@ -406,6 +407,7 @@ static void DisableGamepadEvents(void) static void I_ShutdownGamepad(void) { + I_ShutdownRumble(); SDL_QuitSubSystem(SDL_INIT_GAMECONTROLLER); } @@ -431,6 +433,7 @@ void I_InitGamepad(void) } DisableGamepadEvents(); + I_InitRumble(); I_Printf(VB_INFO, "I_InitGamepad: Initialize gamepad."); @@ -453,6 +456,7 @@ void I_OpenGamepad(int which) "I_OpenGamepad: Found a valid gamepad, named: %s", SDL_GameControllerName(gamepad)); + I_SetRumbleSupported(gamepad); I_ResetGamepad(); I_LoadGyroCalibration(); UpdatePlatform(); @@ -483,6 +487,7 @@ void I_CloseGamepad(SDL_JoystickID instance_id) { SDL_GameControllerClose(gamepad); gamepad = NULL; + I_SetRumbleSupported(NULL); DisableGamepadEvents(); UpdatePlatform(); I_ResetGamepad(); diff --git a/src/i_oalsound.c b/src/i_oalsound.c index 56131a56..39a62bea 100644 --- a/src/i_oalsound.c +++ b/src/i_oalsound.c @@ -29,6 +29,7 @@ #include "i_oalequalizer.h" #include "i_oalsound.h" #include "i_printf.h" +#include "i_rumble.h" #include "i_sndfile.h" #include "i_sound.h" #include "m_array.h" @@ -713,6 +714,7 @@ boolean I_OAL_CacheSound(sfxinfo_t *sfx) sfx->buffer = buffer; sfx->cached = true; + I_CacheRumble(sfx, format, sampledata, size, freq); } // don't need original lump data any more diff --git a/src/i_rumble.c b/src/i_rumble.c new file mode 100644 index 00000000..6faffa38 --- /dev/null +++ b/src/i_rumble.c @@ -0,0 +1,799 @@ +// +// Copyright(C) 2024 ceski +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation; either version 2 +// of the License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// DESCRIPTION: +// Gamepad rumble. +// + +#include "alext.h" +#include "pffft.h" + +#include +#include + +#include "doomstat.h" +#include "i_gamepad.h" +#include "i_rumble.h" +#include "i_sound.h" +#include "m_config.h" +#include "p_mobj.h" +#include "sounds.h" +#include "w_wad.h" + +#define MAX_RUMBLE_COUNT 4 +#define MAX_RUMBLE_SDL 0xFFFF +#define RUMBLE_DURATION 1000 +#define R_CLOSE 64.0f +#define R_CLIP 320.0f +#define RUMBLE_TICS 7 +#define MAX_TICLENGTH 70 // TICRATE * 2 + +static int joy_rumble; +static int last_rumble; + +typedef struct rumble_channel_s +{ + struct rumble_channel_s *prev, *next; + int handle; // Sound channel using this rumble channel. + rumble_type_t type; // Rumble type. + float scale; // Rumble scale for this channel. + int pos; // Position in rumble sequence. + const float *low; // Pointer to cached low frequency rumble values. + const float *high; // Pointer to cached high frequency rumble values. + int ticlength; // Array size equal to sound duration in tics. +} rumble_channel_t; + +typedef struct +{ + SDL_GameController *gamepad; + boolean enabled; // Rumble enabled? + boolean supported; // Rumble supported? + float scale; // Overall rumble scale, based on joy_rumble. + rumble_channel_t *channels; // Rumble channel for each sound channel. + rumble_channel_t base; // Linked list. + int count; // Number of active rumble channels. +} rumble_t; + +static rumble_t rumble; + +typedef struct +{ + PFFFT_Setup *setup; + float *in; + float *out; + float *window; + int size; + float amp_scale; + float freq_scale; + int start; + int end; +} fft_t; + +static fft_t fft; + +// Rumble pattern presets. + +static const float rumble_itemup[] = {0.12f, 0.0f}; +static const float rumble_wpnup[] = {0.12f, 0.07f, 0.04f, 0.16f, 0.0f}; +static const float rumble_getpow[] = {0.07f, 0.14f, 0.16f, 0.14f, 0.0f}; +static const float rumble_oof[] = {0.14f, 0.12f, 0.0f}; +static const float rumble_pain[] = {0.12f, 0.31f, 0.26f, 0.22f, + 0.32f, 0.34f, 0.0f}; +static const float rumble_hitfloor[] = {0.48f, 0.45f, 0.40f, 0.35f, 0.30f, + 0.25f, 0.20f, 0.15f, 0.0f}; + +typedef struct +{ + const float *data; + int ticlength; +} rumble_preset_t; + +static const rumble_preset_t presets[] = { + [RUMBLE_NONE] = {NULL, 0 }, + [RUMBLE_ITEMUP] = {rumble_itemup, arrlen(rumble_itemup) }, + [RUMBLE_WPNUP] = {rumble_wpnup, arrlen(rumble_wpnup) }, + [RUMBLE_GETPOW] = {rumble_getpow, arrlen(rumble_getpow) }, + [RUMBLE_OOF] = {rumble_oof, arrlen(rumble_oof) }, + [RUMBLE_PAIN] = {rumble_pain, arrlen(rumble_pain) }, + [RUMBLE_HITFLOOR] = {rumble_hitfloor, arrlen(rumble_hitfloor)}, +}; + +static const float default_scale[] = { + [RUMBLE_NONE] = 0.0f, + [RUMBLE_ITEMUP] = 1.0f, + [RUMBLE_WPNUP] = 1.0f, + [RUMBLE_GETPOW] = 1.0f, + [RUMBLE_OOF] = 1.0f, + [RUMBLE_PAIN] = 1.0f, + [RUMBLE_HITFLOOR] = 0.0f, + [RUMBLE_PLAYER] = 1.0f, + [RUMBLE_ORIGIN] = 0.8f, + [RUMBLE_PISTOL] = 0.55f, + [RUMBLE_SHOTGUN] = 1.0f, + [RUMBLE_SSG] = 1.25f, + [RUMBLE_CGUN] = 0.55f, + [RUMBLE_ROCKET] = 1.15f, + [RUMBLE_PLASMA] = 0.75f, + [RUMBLE_BFG] = 1.0f, +}; + +static void ResetChannel(int handle) +{ + rumble_channel_t *node = &rumble.channels[handle]; + memset(node, 0, sizeof(*node)); + node->prev = node->next = node; + node->handle = handle; +} + +static void ResetAllChannels(void) +{ + for (int i = 0; i < MAX_CHANNELS; i++) + { + ResetChannel(i); + } + + rumble.base.prev = rumble.base.next = &rumble.base; + rumble.count = 0; +} + +static void UnlinkNode(rumble_channel_t *node) +{ + node->prev->next = node->next; + node->next->prev = node->prev; + rumble.count--; +} + +static boolean RemoveOldNodes(void) +{ + do + { + rumble_channel_t *base = &rumble.base; + rumble_channel_t *node = base->next; + + if (node == base) + { + break; + } + + while (node->next != base) + { + node = node->next; + } + + UnlinkNode(node); + ResetChannel(node->handle); + } while (rumble.count >= MAX_RUMBLE_COUNT); + + return (rumble.count < MAX_RUMBLE_COUNT); +} + +static void RemoveNode(int handle) +{ + rumble_channel_t *base = &rumble.base; + rumble_channel_t *node; + + for (node = base->next; node != base; node = node->next) + { + if (node->handle == handle) + { + UnlinkNode(node); + break; + } + } + + ResetChannel(handle); +} + +static void AddNode(int handle) +{ + rumble_channel_t *base = &rumble.base; + rumble_channel_t *node = &rumble.channels[handle]; + node->prev = base; + node->next = base->next; + base->next->prev = node; + base->next = node; + rumble.count++; +} + +static void FreeFFT(void) +{ + if (fft.window) + { + pffft_aligned_free(fft.window); + fft.window = NULL; + } + + if (fft.out) + { + pffft_aligned_free(fft.out); + fft.out = NULL; + } + + if (fft.in) + { + pffft_aligned_free(fft.in); + fft.in = NULL; + } + + if (fft.setup) + { + pffft_destroy_setup(fft.setup); + fft.setup = NULL; + } +} + +void I_ShutdownRumble(void) +{ + if (!I_GamepadEnabled()) + { + return; + } + + if (rumble.enabled) + { + SDL_GameControllerRumble(rumble.gamepad, 0, 0, 0); + } + + for (int i = 1; i < num_sfx; i++) + { + sfxinfo_t *sfx = &S_sfx[i]; + + if (sfx->name && sfx->cached) + { + free(sfx->rumble.low); + free(sfx->rumble.high); + } + } + + free(rumble.channels); + FreeFFT(); +} + +void I_InitRumble(void) +{ + if (!I_GamepadEnabled()) + { + return; + } + + last_rumble = joy_rumble; + rumble.channels = malloc(sizeof(*rumble.channels) * MAX_CHANNELS); + ResetAllChannels(); +} + +static uint32_t RoundUpPowerOfTwo(int n) +{ + // https://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 + uint32_t v = n; + v--; + v |= v >> 1; + v |= v >> 2; + v |= v >> 4; + v |= v >> 8; + v |= v >> 16; + v++; + return v; +} + +static void InitFFT(int rate, int step) +{ + static int last_rate = -1; + + if (last_rate == rate) + { + return; + } + + last_rate = rate; + FreeFFT(); + + // FFT size must be a power of two (see pffft.h). + const uint32_t step2 = RoundUpPowerOfTwo(step); + fft.size = BETWEEN(128, 8192, step2); + + fft.setup = pffft_new_setup(fft.size, PFFFT_REAL); + + const size_t size = sizeof(float) * fft.size; + fft.in = pffft_aligned_malloc(size); + fft.out = pffft_aligned_malloc(size); + fft.window = pffft_aligned_malloc(size); + + const float mult = 2.0f * PI_F / (fft.size - 1.0f); + float sum = 0.0f; + + for (int i = 0; i < fft.size; i++) + { + // Hann window. + fft.window[i] = 0.5f * (1.0f - cosf(i * mult)); + sum += fft.window[i]; + } + + fft.amp_scale = 2.0f / sum; + fft.freq_scale = 0.5f * rate / fft.size; + fft.start = lroundf(ceilf(100.0f / fft.freq_scale * 0.5f)) * 2; + fft.end = lroundf(ceilf(0.9f * 0.5f * rate / fft.freq_scale * 0.5f)) * 2; +} + +static float Normalize_Mono8(const void *data, int pos) +{ + const float val = (((byte *)data)[pos] - 128) / 127.0f; + return MAX(-1.0f, val); +} + +static float Normalize_Mono16(const void *data, int pos) +{ + const float val = ((int16_t *)data)[pos] / 32767.0f; + return MAX(-1.0f, val); +} + +static float Normalize_Mono32(const void *data, int pos) +{ + const float val = ((float *)data)[pos]; + return BETWEEN(-1.0f, 1.0f, val); +} + +static float (*Normalize)(const void *data, int pos); + +static void CalcPeakFFT(const byte *data, int rate, int length, int pos, + float *amp, float *freq) +{ + const int remaining = length - pos; + const int size = MIN(fft.size, remaining); + + for (int i = 0; i < size; i++) + { + // Fill input buffer with data. + fft.in[i] = Normalize(data, pos + i); + + // Apply window. + fft.in[i] *= fft.window[i]; + } + + for (int i = size; i < fft.size; i++) + { + // Fill remaining space with zeros. + fft.in[i] = 0.0f; + } + + // Forward transform. + pffft_transform_ordered(fft.setup, fft.in, fft.out, NULL, PFFFT_FORWARD); + + // Find index of max value. + int max_index = fft.start; + float max_value = fft.out[max_index] * fft.out[max_index] + + fft.out[max_index + 1] * fft.out[max_index + 1]; + + for (int i = fft.start + 2; i < fft.end; i += 2) + { + // PFFFT returns interleaved values. + const float current_value = + fft.out[i] * fft.out[i] + fft.out[i + 1] * fft.out[i + 1]; + + if (max_value < current_value) + { + max_value = current_value; + max_index = i; + } + } + + *amp = sqrtf(max_value) * fft.amp_scale; + *freq = max_index * fft.freq_scale; +} + +static void SfxToRumble(const byte *data, int rate, int length, + float **low, float **high, int *ticlength) +{ + const int ticlen = *ticlength - 1; + *low = malloc(sizeof(float) * (ticlen + 1)); + *high = malloc(sizeof(float) * (ticlen + 1)); + (*low)[ticlen] = 0.0f; + (*high)[ticlen] = 0.0f; + + const int step = rate / TICRATE; + InitFFT(rate, step); + + for (int i = 0; i < ticlen; i++) + { + const int pos = i * step; + float amp_peak, freq_peak; + CalcPeakFFT(data, rate, length, pos, &_peak, &freq_peak); + + if (amp_peak > 0.0001f) + { + float weight = (freq_peak - 640.0f) * 0.00625f; // 1/160 + weight = BETWEEN(-1.0f, 1.0f, weight); + + const float dBFS = 20.0f * log10f(amp_peak) + 6.0f; + const float dBFS_low = dBFS - 6.0f * weight; + const float dBFS_high = dBFS + 6.0f * weight; + + (*low)[i] = powf(10.0f, MIN(dBFS_low, -12.0f) * 0.05f); + (*high)[i] = powf(10.0f, MIN(dBFS_high, -12.0f) * 0.05f); + } + else + { + (*low)[i] = 0.0f; + (*high)[i] = 0.0f; + } + } + + for (int i = 0; i < ticlen; i++) + { + if ((*low)[i] > 0.0f || (*high)[i] > 0.0f) + { + return; + } + } + + free(*low); + free(*high); + *low = NULL; + *high = NULL; + *ticlength = 0; +} + +static int GetMonoLength(const sfxinfo_t *sfx, int size) +{ + ALint bits = 0; + alGetError(); + alGetBufferi(sfx->buffer, AL_BITS, &bits); + return ((alGetError() == AL_NO_ERROR && bits > 0) ? (size * 8 / bits) : 0); +} + +void I_CacheRumble(sfxinfo_t *sfx, int format, const byte *data, int size, + int rate) +{ + if (!I_GamepadEnabled() || !sfx || !alIsBuffer(sfx->buffer) || !data + || size < 1 || rate < TICRATE) + { + return; + } + + switch (format) + { + case AL_FORMAT_MONO8: + Normalize = Normalize_Mono8; + break; + case AL_FORMAT_MONO16: + Normalize = Normalize_Mono16; + break; + case AL_FORMAT_MONO_FLOAT32: + Normalize = Normalize_Mono32; + break; + + default: // Unsupported format. + return; + } + + const int length = GetMonoLength(sfx, size); + int ticlength = length * TICRATE / rate; + + if (!length || !ticlength) + { + return; + } + + ticlength++; + ticlength = MIN(ticlength, MAX_TICLENGTH); + float *low, *high; + SfxToRumble(data, rate, length, &low, &high, &ticlength); + sfx->rumble.low = low; + sfx->rumble.high = high; + sfx->rumble.ticlength = ticlength; +} + +boolean I_RumbleEnabled(void) +{ + return rumble.enabled; +} + +boolean I_RumbleSupported(void) +{ + return rumble.supported; +} + +void I_RumbleMenuFeedback(void) +{ + if (!rumble.supported || last_rumble == joy_rumble) + { + return; + } + + last_rumble = joy_rumble; + const uint16_t test = (uint16_t)(rumble.scale * 0.25f); + SDL_GameControllerRumble(rumble.gamepad, test, test, 125); +} + +void I_UpdateRumbleEnabled(void) +{ + rumble.scale = 0.2f * joy_rumble * MAX_RUMBLE_SDL; + rumble.enabled = (joy_rumble && rumble.supported); +} + +void I_SetRumbleSupported(SDL_GameController *gamepad) +{ + rumble.gamepad = gamepad; + rumble.supported = + gamepad && (SDL_GameControllerHasRumble(gamepad) == SDL_TRUE); + I_UpdateRumbleEnabled(); +} + +void I_ResetRumbleChannel(int handle) +{ + if (!rumble.supported) + { + return; + } + +#ifdef RANGECHECK + if (handle < 0 || handle >= MAX_CHANNELS) + { + return; + } +#endif + + RemoveNode(handle); +} + +void I_ResetAllRumbleChannels(void) +{ + if (!rumble.supported) + { + return; + } + + ResetAllChannels(); + SDL_GameControllerRumble(rumble.gamepad, 0, 0, 0); +} + +static void GetNodeScale(const rumble_channel_t *node, float *scale_down, + float *scale) +{ + *scale = node->scale; + + if (node->type == RUMBLE_BFG) + { + if (node->pos > 40) + { + *scale *= 1.0f - (float)(node->pos - 40) / (node->ticlength - 40); + } + } + else if (node->pos > RUMBLE_TICS) + { + *scale *= 1.0f - (node->pos - RUMBLE_TICS) / 3.0f; + } + + *scale = MAX(*scale, 0.0f); + + if (node->type != RUMBLE_ORIGIN) + { + *scale *= *scale_down; + *scale_down *= 0.1f; + } +} + +void I_UpdateRumble(void) +{ + if (!rumble.enabled || menuactive || demoplayback) + { + return; + } + + float scale_low = 0.0f; + float scale_high = 0.0f; + float scale_down = 1.0f; + float scale; + rumble_channel_t *base = &rumble.base; + rumble_channel_t *node, *next; + + for (node = base->next; node != base; node = next) + { + next = node->next; + GetNodeScale(node, &scale_down, &scale); + scale_low += node->low[node->pos] * scale; + scale_high += node->high[node->pos] * scale; + node->pos++; + + if (node->pos >= node->ticlength) + { + UnlinkNode(node); + ResetChannel(node->handle); + } + } + + scale_low *= rumble.scale; + scale_high *= rumble.scale; + const uint16_t low = lroundf(MIN(scale_low, MAX_RUMBLE_SDL)); + const uint16_t high = lroundf(MIN(scale_high, MAX_RUMBLE_SDL)); + SDL_GameControllerRumble(rumble.gamepad, low, high, RUMBLE_DURATION); +} + +static boolean CalcChannelScale(const mobj_t *listener, const mobj_t *origin, + rumble_channel_t *node) +{ + if (node->type != RUMBLE_ORIGIN || !origin || !listener) + { + return true; + } + + const float dx = (listener->x >> FRACBITS) - (origin->x >> FRACBITS); + const float dy = (listener->y >> FRACBITS) - (origin->y >> FRACBITS); + const float dist = sqrtf(dx * dx + dy * dy); + + if (dist <= R_CLOSE) + { + node->scale = 1.0f; + } + else if (dist >= R_CLIP) + { + node->scale = 0.0f; + } + else + { + node->scale = (R_CLIP - dist) * R_CLOSE / ((R_CLIP - R_CLOSE) * dist); + } + + return (node->scale > 0.0f); +} + +void I_UpdateRumbleParams(const mobj_t *listener, const mobj_t *origin, + int handle) +{ + if (!rumble.enabled || menuactive || demoplayback) + { + return; + } + +#ifdef RANGECHECK + if (handle < 0 || handle >= MAX_CHANNELS) + { + return; + } +#endif + + rumble_channel_t *node = &rumble.channels[handle]; + + if (node->type == RUMBLE_NONE) + { + return; + } + + if (!CalcChannelScale(listener, origin, node)) + { + RemoveNode(handle); + } +} + +static float ScaleHitFloor(const mobj_t *listener) +{ + if (!listener || listener->momz >= -8 * GRAVITY) + { + return 0.25f; + } + else if (listener->momz <= -40 * GRAVITY) + { + return 1.0f; + } + else + { + float scale = (float)FIXED2DOUBLE(listener->momz) + 8.0f; + //scale = (1 - 0.25) / pow(40 - 8, 2) * pow(scale, 2) + 0.25; + scale = 0.75f / 1024.0f * scale * scale + 0.25f; + return BETWEEN(0.25f, 1.0f, scale); + } +} + +static boolean GetSFXParams(const mobj_t *listener, const sfxinfo_t *sfx, + rumble_channel_t *node) +{ + switch (node->type) + { + case RUMBLE_ITEMUP: + case RUMBLE_WPNUP: + case RUMBLE_GETPOW: + case RUMBLE_OOF: + case RUMBLE_PAIN: + node->low = node->high = presets[node->type].data; + node->ticlength = presets[node->type].ticlength; + break; + + case RUMBLE_HITFLOOR: + node->low = node->high = presets[node->type].data; + node->scale = ScaleHitFloor(listener); + node->ticlength = + lroundf(node->scale * presets[node->type].ticlength); + break; + + case RUMBLE_PLAYER: + case RUMBLE_ORIGIN: + case RUMBLE_PISTOL: + case RUMBLE_SHOTGUN: + case RUMBLE_SSG: + case RUMBLE_CGUN: + case RUMBLE_ROCKET: + case RUMBLE_PLASMA: + case RUMBLE_BFG: + node->low = sfx->rumble.low; + node->high = sfx->rumble.high; + node->ticlength = sfx->rumble.ticlength; + break; + + default: + node->low = node->high = NULL; + node->ticlength = 0; + break; + } + + return (node->low && node->high && node->ticlength > 0); +} + +void I_StartRumble(const mobj_t *listener, const mobj_t *origin, + const sfxinfo_t *sfx, int handle, rumble_type_t rumble_type) +{ + if (!rumble.enabled || menuactive || demoplayback || !sfx + || !sfx->rumble.low || !sfx->rumble.high) + { + return; + } + +#ifdef RANGECHECK + if (handle < 0 || handle >= MAX_CHANNELS) + { + return; + } +#endif + + rumble_channel_t *node = &rumble.channels[handle]; + + if (node->type != RUMBLE_NONE) + { + RemoveNode(handle); + } + + if (rumble.count >= MAX_RUMBLE_COUNT && !RemoveOldNodes()) + { + return; + } + + node->type = rumble_type; + node->scale = default_scale[node->type]; + + if (CalcChannelScale(listener, origin, node) + && GetSFXParams(listener, sfx, node)) + { + AddNode(handle); + } + else + { + ResetChannel(handle); + } +} + +// For gyro calibration only. +void I_DisableRumble(void) +{ + if (!rumble.enabled) + { + return; + } + + rumble.enabled = false; + ResetAllChannels(); + SDL_GameControllerRumble(rumble.gamepad, 0, 0, 0); +} + +void I_BindRumbleVariables(void) +{ + BIND_NUM_GENERAL(joy_rumble, 5, 0, 10, + "Rumble intensity (0 = Off; 10 = 100%)"); +} diff --git a/src/i_rumble.h b/src/i_rumble.h new file mode 100644 index 00000000..abc9c6ef --- /dev/null +++ b/src/i_rumble.h @@ -0,0 +1,78 @@ +// +// Copyright(C) 2024 ceski +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation; either version 2 +// of the License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// DESCRIPTION: +// Gamepad rumble. +// + +#ifndef __I_RUMBLE__ +#define __I_RUMBLE__ + +#include "SDL.h" + +#include "doomtype.h" + +struct mobj_s; +struct sfxinfo_s; + +typedef enum +{ + RUMBLE_NONE, + + RUMBLE_ITEMUP, + RUMBLE_WPNUP, + RUMBLE_GETPOW, + RUMBLE_OOF, + RUMBLE_PAIN, + RUMBLE_HITFLOOR, + + RUMBLE_PLAYER, + RUMBLE_ORIGIN, + + RUMBLE_PISTOL, + RUMBLE_SHOTGUN, + RUMBLE_SSG, + RUMBLE_CGUN, + RUMBLE_ROCKET, + RUMBLE_PLASMA, + RUMBLE_BFG, +} rumble_type_t; + +void I_ShutdownRumble(void); +void I_InitRumble(void); +void I_CacheRumble(struct sfxinfo_s *sfx, int format, const byte *data, + int size, int rate); + +boolean I_RumbleEnabled(void); +boolean I_RumbleSupported(void); +void I_RumbleMenuFeedback(void); +void I_UpdateRumbleEnabled(void); +void I_SetRumbleSupported(SDL_GameController *gamepad); + +void I_ResetRumbleChannel(int handle); +void I_ResetAllRumbleChannels(void); + +void I_UpdateRumble(void); +void I_UpdateRumbleParams(const struct mobj_s *listener, + const struct mobj_s *origin, int handle); + +void I_StartRumble(const struct mobj_s *listener, const struct mobj_s *origin, + const struct sfxinfo_s *sfx, int handle, + rumble_type_t rumble_type); + +// For gyro calibration only. +void I_DisableRumble(void); + +void I_BindRumbleVariables(void); + +#endif diff --git a/src/i_sound.c b/src/i_sound.c index ee4642f7..162f6d53 100644 --- a/src/i_sound.c +++ b/src/i_sound.c @@ -27,6 +27,7 @@ #include "doomtype.h" #include "i_oalstream.h" #include "i_printf.h" +#include "i_rumble.h" #include "i_system.h" #include "m_array.h" #include "mn_menu.h" @@ -111,6 +112,8 @@ static void StopChannel(int channel) channelinfo[channel].enabled = false; } + + I_ResetRumbleChannel(channel); } // diff --git a/src/m_cheat.c b/src/m_cheat.c index bb779e68..24c4183a 100644 --- a/src/m_cheat.c +++ b/src/m_cheat.c @@ -440,7 +440,7 @@ static void cheat_god() // [crispy] spawn a teleport fog an = plyr->mo->angle >> ANGLETOFINESHIFT; P_SpawnMobj(plyr->mo->x+20*finecosine[an], plyr->mo->y+20*finesine[an], plyr->mo->z, MT_TFOG); - S_StartSound(plyr->mo, sfx_slop); + S_StartSoundEx(plyr->mo, sfx_slop); P_MapEnd(); // Fix reviving as "zombie" if god mode was already enabled diff --git a/src/m_config.c b/src/m_config.c index 7afe7011..3d512576 100644 --- a/src/m_config.c +++ b/src/m_config.c @@ -39,6 +39,7 @@ #include "i_gamepad.h" #include "i_gyro.h" #include "i_printf.h" +#include "i_rumble.h" #include "i_sound.h" #include "i_system.h" #include "i_video.h" @@ -121,6 +122,7 @@ void M_InitConfig(void) G_BindMouseVariables(); I_BindGamepadVariables(); + I_BindRumbleVariables(); I_BindFlickStickVariables(); I_BindGyroVaribales(); diff --git a/src/mn_setup.c b/src/mn_setup.c index 341481ed..1ce4eb91 100644 --- a/src/mn_setup.c +++ b/src/mn_setup.c @@ -30,6 +30,7 @@ #include "i_input.h" #include "i_oalequalizer.h" #include "i_oalsound.h" +#include "i_rumble.h" #include "i_sound.h" #include "i_timer.h" #include "i_video.h" @@ -303,6 +304,7 @@ enum { str_empty, str_layout, + str_rumble, str_curve, str_center_weapon, str_screensize, @@ -2451,6 +2453,16 @@ static const char *layout_strings[] = { "Flick Stick Southpaw", }; +static void UpdateRumble(void) +{ + I_UpdateRumbleEnabled(); + I_RumbleMenuFeedback(); +} + +static const char *rumble_strings[] = { + "Off", "10%", "20%", "30%", "40%", "50%", "60%", "70%", "80%", "90%", "100%" +}; + static const char *curve_strings[] = { "", "", "", "", "", "", "", "", "", "", // Dummy values, start at 1.0. @@ -2469,6 +2481,9 @@ static setup_menu_t gen_settings4[] = { {"Invert Look", S_ONOFF, CNTR_X, M_SPC, {"joy_invert_look"}, .action = I_ResetGamepad}, + {"Rumble", S_THERMO, CNTR_X, M_THRM_SPC, {"joy_rumble"}, + .strings_id = str_rumble, .action = UpdateRumble}, + MI_GAP, {"Turn Speed", S_THERMO | S_THRM_SIZE11, CNTR_X, M_THRM_SPC, @@ -2494,14 +2509,9 @@ static setup_menu_t gen_settings4[] = { static void UpdateGamepadItems(void) { boolean condition = - (!I_UseGamepad() || !I_GamepadEnabled() || !I_UseStickLayout()); + (!I_UseGamepad() || !I_GamepadEnabled() || !I_RumbleSupported()); - DisableItem(condition, gen_settings4, "joy_invert_look"); - DisableItem(condition, gen_settings4, "joy_movement_inner_deadzone"); - DisableItem(condition, gen_settings4, "joy_camera_inner_deadzone"); - DisableItem(condition, gen_settings4, "joy_turn_speed"); - DisableItem(condition, gen_settings4, "joy_look_speed"); - DisableItem(condition, gen_settings4, "joy_camera_curve"); + DisableItem(condition, gen_settings4, "joy_rumble"); // Allow padlook toggle when the gamepad is using gyro, even if the // stick layout is set to off. @@ -2510,6 +2520,14 @@ static void UpdateGamepadItems(void) || (!I_UseStickLayout() && (!I_GyroEnabled() || !I_GyroSupported()))); DisableItem(condition, gen_settings4, "padlook"); + + condition = (!I_UseGamepad() || !I_GamepadEnabled() || !I_UseStickLayout()); + DisableItem(condition, gen_settings4, "joy_invert_look"); + DisableItem(condition, gen_settings4, "joy_movement_inner_deadzone"); + DisableItem(condition, gen_settings4, "joy_camera_inner_deadzone"); + DisableItem(condition, gen_settings4, "joy_turn_speed"); + DisableItem(condition, gen_settings4, "joy_look_speed"); + DisableItem(condition, gen_settings4, "joy_camera_curve"); } static void UpdateGyroItems(void); @@ -4193,6 +4211,7 @@ void MN_DrawTitle(int x, int y, const char *patch, const char *alttext) static const char **selectstrings[] = { NULL, // str_empty layout_strings, + rumble_strings, curve_strings, center_weapon_strings, screensize_strings, diff --git a/src/p_enemy.c b/src/p_enemy.c index ffde7229..c04e66c4 100644 --- a/src/p_enemy.c +++ b/src/p_enemy.c @@ -2197,13 +2197,13 @@ void A_Scream(mobj_t *actor) void A_XScream(mobj_t *actor) { - S_StartSound(actor, sfx_slop); + S_StartSoundEx(actor, sfx_slop); } void A_Pain(mobj_t *actor) { if (actor->info->painsound) - S_StartSound(actor, actor->info->painsound); + S_StartSoundPain(actor, actor->info->painsound); } void A_Fall(mobj_t *actor) @@ -2709,7 +2709,7 @@ void A_PlayerScream(mobj_t *mo) int sound = sfx_pldeth; // Default death sound. if (gamemode != shareware && mo->health < -50) // killough 12/98 sound = sfx_pdiehi; // IF THE PLAYER DIES LESS THAN -50% WITHOUT GIBBING - S_StartSound(mo, sound); + S_StartSoundEx(mo, sound); } // @@ -2790,7 +2790,7 @@ void A_PlaySound(mobj_t *mo) { if (demo_version < DV_MBF) return; - S_StartSound(mo->state->misc2 ? NULL : mo, mo->state->misc1); + S_StartSoundOrigin(mo, mo->state->misc2 ? NULL : mo, mo->state->misc1); } void A_RandomJump(mobj_t *mo) diff --git a/src/p_inter.c b/src/p_inter.c index 02ad67f7..e9cd6645 100644 --- a/src/p_inter.c +++ b/src/p_inter.c @@ -207,7 +207,7 @@ boolean P_GiveWeapon(player_t *player, weapontype_t weapon, boolean dropped) P_GiveAmmo(player, weaponinfo[weapon].ammo, deathmatch ? 5 : 2); player->pendingweapon = weapon; - S_StartSound(player->mo, sfx_wpnup); // killough 4/25/98, 12/98 + S_StartSoundPreset(player->mo, sfx_wpnup, PITCH_FULL); // killough 4/25/98, 12/98 return false; } @@ -652,8 +652,8 @@ void P_TouchSpecialThing(mobj_t *special, mobj_t *toucher) P_RemoveMobj (special); player->bonuscount += BONUSADD; - S_StartSoundPitch(player->mo, sound, // killough 4/25/98, 12/98 - sound == sfx_itemup ? PITCH_NONE : PITCH_FULL); + S_StartSoundPreset(player->mo, sound, // killough 4/25/98, 12/98 + sound == sfx_itemup ? PITCH_NONE : PITCH_FULL); } // diff --git a/src/p_map.c b/src/p_map.c index 4267f9a1..4c1dc022 100644 --- a/src/p_map.c +++ b/src/p_map.c @@ -1178,7 +1178,7 @@ static void P_HitSlideLine(line_t *ld) { if (icyfloor && abs(tmymove) > abs(tmxmove)) { - S_StartSound(slidemo,sfx_oof); // oooff! + S_StartSoundPreset(slidemo, sfx_oof, PITCH_FULL); // oooff! tmxmove /= 2; // absorb half the momentum tmymove = -tmymove/2; } @@ -1191,7 +1191,7 @@ static void P_HitSlideLine(line_t *ld) { if (icyfloor && abs(tmxmove) > abs(tmymove)) { - S_StartSound(slidemo,sfx_oof); // oooff! + S_StartSoundPreset(slidemo, sfx_oof, PITCH_FULL); // oooff! tmxmove = -tmxmove/2; // absorb half the momentum tmymove /= 2; } @@ -1223,7 +1223,7 @@ static void P_HitSlideLine(line_t *ld) if (icyfloor && deltaangle > ANG45 && deltaangle < ANG90+ANG45) { - S_StartSound(slidemo,sfx_oof); // oooff! + S_StartSoundPreset(slidemo, sfx_oof, PITCH_FULL); // oooff! moveangle = lineangle - deltaangle; movelen /= 2; // absorb moveangle >>= ANGLETOFINESHIFT; diff --git a/src/p_mobj.c b/src/p_mobj.c index 4c594f13..345295d9 100644 --- a/src/p_mobj.c +++ b/src/p_mobj.c @@ -145,8 +145,16 @@ void P_ExplodeMissile (mobj_t* mo) mo->flags &= ~MF_MISSILE; if (mo->info->deathsound) - S_StartSoundPitch(mo, mo->info->deathsound, - brainexplode ? PITCH_NONE : PITCH_FULL); + { + if (brainexplode) + { + S_StartSoundPitch(mo, mo->info->deathsound, PITCH_NONE); + } + else + { + S_StartSoundOrigin(mo->target, mo, mo->info->deathsound); + } + } } // @@ -1533,7 +1541,7 @@ mobj_t* P_SpawnPlayerMissile(mobj_t* source,mobjtype_t type) th = P_SpawnMobj (x,y,z, type); if (th->info->seesound) - S_StartSound (th, th->info->seesound); + S_StartSoundMissile(source, th, th->info->seesound); P_SetTarget(&th->target, source); // killough 11/98 th->angle = an; diff --git a/src/p_pspr.c b/src/p_pspr.c index d5238dcf..3a16e928 100644 --- a/src/p_pspr.c +++ b/src/p_pspr.c @@ -155,7 +155,7 @@ static void P_BringUpWeapon(player_t *player) player->pendingweapon = player->readyweapon; if (player->pendingweapon == wp_chainsaw) - S_StartSoundPitch(player->mo, sfx_sawup, PITCH_HALF); + S_StartSoundPitchEx(player->mo, sfx_sawup, PITCH_HALF); if (player->pendingweapon >= NUMWEAPONS) { @@ -695,7 +695,7 @@ void A_Punch(player_t *player, pspdef_t *psp) if (!linetarget) return; - S_StartSound(player->mo, sfx_punch); + S_StartSoundEx(player->mo, sfx_punch); // turn to face target @@ -735,11 +735,11 @@ void A_Saw(player_t *player, pspdef_t *psp) if (!linetarget) { - S_StartSoundPitch(player->mo, sfx_sawful, PITCH_HALF); + S_StartSoundPitchEx(player->mo, sfx_sawful, PITCH_HALF); return; } - S_StartSoundPitch(player->mo, sfx_sawhit, PITCH_HALF); + S_StartSoundPitchEx(player->mo, sfx_sawhit, PITCH_HALF); // turn to face target angle = R_PointToAngle2(player->mo->x, player->mo->y, @@ -929,7 +929,7 @@ void P_GunShot(mobj_t *mo, boolean accurate) void A_FirePistol(player_t *player, pspdef_t *psp) { - S_StartSound(player->mo, sfx_pistol); + S_StartSoundPistol(player->mo, sfx_pistol); P_SetMobjState(player->mo, S_PLAY_ATK2); P_SubtractAmmo(player, 1); @@ -948,7 +948,7 @@ void A_FireShotgun(player_t *player, pspdef_t *psp) { int i; - S_StartSound(player->mo, sfx_shotgn); + S_StartSoundShotgun(player->mo, sfx_shotgn); P_SetMobjState(player->mo, S_PLAY_ATK2); P_SubtractAmmo(player, 1); @@ -970,7 +970,7 @@ void A_FireShotgun2(player_t *player, pspdef_t *psp) { int i; - S_StartSound(player->mo, sfx_dshtgn); + S_StartSoundSSG(player->mo, sfx_dshtgn); P_SetMobjState(player->mo, S_PLAY_ATK2); P_SubtractAmmo(player, 2); @@ -998,7 +998,7 @@ void A_FireShotgun2(player_t *player, pspdef_t *psp) void A_FireCGun(player_t *player, pspdef_t *psp) { - S_StartSound(player->mo, sfx_pistol); + S_StartSoundCGun(player->mo, sfx_pistol); if (!player->ammo[weaponinfo[player->readyweapon].ammo]) return; @@ -1079,7 +1079,7 @@ void A_BFGSpray(mobj_t *mo) void A_BFGsound(player_t *player, pspdef_t *psp) { - S_StartSound(player->mo, sfx_bfg); + S_StartSoundBFG(player->mo, sfx_bfg); } // @@ -1317,7 +1317,7 @@ void A_WeaponMeleeAttack(player_t *player, pspdef_t *psp) return; // un-missed! - S_StartSound(player->mo, hitsound); + S_StartSoundEx(player->mo, hitsound); // turn to face target SavePlayerAngle(player); @@ -1336,7 +1336,8 @@ void A_WeaponSound(player_t *player, pspdef_t *psp) if (!mbf21 || !psp->state) return; - S_StartSound(psp->state->args[1] ? NULL : player->mo, psp->state->args[0]); + S_StartSoundOrigin(player->mo, (psp->state->args[1] ? NULL : player->mo), + psp->state->args[0]); } // diff --git a/src/p_spec.c b/src/p_spec.c index 8529c105..c6effc8b 100644 --- a/src/p_spec.c +++ b/src/p_spec.c @@ -235,7 +235,7 @@ void P_HitFloor (mobj_t *mo, int oof) {sfx_lavsml, sfx_lvsiz} // terrain_lava }; - S_StartSound(mo, hitsound[terrain][oof]); + S_StartSoundHitFloor(mo, hitsound[terrain][oof]); } /////////////////////////////////////////////////////////////// diff --git a/src/p_telept.c b/src/p_telept.c index 9818cd60..953e50f5 100644 --- a/src/p_telept.c +++ b/src/p_telept.c @@ -92,12 +92,10 @@ int EV_Teleport(line_t *line, int side, mobj_t *thing) S_StartSound(P_SpawnMobj(oldx, oldy, oldz, MT_TFOG), sfx_telept); // spawn teleport fog and emit sound at destination - S_StartSound(P_SpawnMobj(m->x + - 20*finecosine[m->angle>>ANGLETOFINESHIFT], - m->y + - 20*finesine[m->angle>>ANGLETOFINESHIFT], - thing->z, MT_TFOG), - sfx_telept); + fixed_t fog_x = m->x + 20 * finecosine[m->angle >> ANGLETOFINESHIFT]; + fixed_t fog_y = m->y + 20 * finesine[m->angle >> ANGLETOFINESHIFT]; + mobj_t *fog_mo = P_SpawnMobj(fog_x, fog_y, thing->z, MT_TFOG); + S_StartSoundSource(thing, fog_mo, sfx_telept); if (thing->player) // don't move for a bit // killough 10/98 // killough 10/98: beta teleporters were a bit faster diff --git a/src/s_sound.c b/src/s_sound.c index f528fc33..81cc4269 100644 --- a/src/s_sound.c +++ b/src/s_sound.c @@ -25,6 +25,7 @@ #include "doomdef.h" #include "doomstat.h" #include "i_printf.h" +#include "i_rumble.h" #include "i_sound.h" #include "i_system.h" #include "m_misc.h" @@ -197,7 +198,8 @@ static int S_getChannel(const mobj_t *origin, sfxinfo_t *sfxinfo, int priority, return cnum; } -void S_StartSoundPitch(const mobj_t *origin, int sfx_id, const pitchrange_t pitch_range) +static void StartSound(const mobj_t *origin, int sfx_id, + pitchrange_t pitch_range, rumble_type_t rumble_type) { int sep, pitch, o_priority, priority, singularity, cnum, handle; int volumeScale = 127; @@ -302,6 +304,12 @@ void S_StartSoundPitch(const mobj_t *origin, int sfx_id, const pitchrange_t pitc channels[cnum].priority = priority; // scaled priority channels[cnum].singularity = singularity; channels[cnum].idnum = I_SoundID(handle); // unique instance id + + if (rumble_type != RUMBLE_NONE) + { + I_StartRumble(players[displayplayer].mo, origin, sfx, handle, + rumble_type); + } } else // haleyjd: the sound didn't start, so clear the channel info { @@ -309,6 +317,128 @@ void S_StartSoundPitch(const mobj_t *origin, int sfx_id, const pitchrange_t pitc } } +void S_StartSoundPitch(const mobj_t *origin, int sfx_id, + pitchrange_t pitch_range) +{ + StartSound(origin, sfx_id, pitch_range, RUMBLE_NONE); +} + +static boolean IsRumblePlayer(const mobj_t *mo) +{ + return (I_RumbleEnabled() && mo && mo == players[displayplayer].mo); +} + +static rumble_type_t RumbleType(const mobj_t *mo, rumble_type_t rumble_type) +{ + return (IsRumblePlayer(mo) ? rumble_type : RUMBLE_NONE); +} + +void S_StartSoundPitchEx(const mobj_t *origin, int sfx_id, + pitchrange_t pitch_range) +{ + StartSound(origin, sfx_id, pitch_range, RumbleType(origin, RUMBLE_PLAYER)); +} + +void S_StartSoundPistol(const mobj_t *origin, int sfx_id) +{ + StartSound(origin, sfx_id, PITCH_FULL, RumbleType(origin, RUMBLE_PISTOL)); +} + +void S_StartSoundShotgun(const mobj_t *origin, int sfx_id) +{ + StartSound(origin, sfx_id, PITCH_FULL, RumbleType(origin, RUMBLE_SHOTGUN)); +} + +void S_StartSoundSSG(const mobj_t *origin, int sfx_id) +{ + StartSound(origin, sfx_id, PITCH_FULL, RumbleType(origin, RUMBLE_SSG)); +} + +void S_StartSoundCGun(const mobj_t *origin, int sfx_id) +{ + StartSound(origin, sfx_id, PITCH_FULL, RumbleType(origin, RUMBLE_CGUN)); +} + +void S_StartSoundBFG(const mobj_t *origin, int sfx_id) +{ + StartSound(origin, sfx_id, PITCH_FULL, RumbleType(origin, RUMBLE_BFG)); +} + +static rumble_type_t RumbleTypePreset(const mobj_t *origin, int sfx_id) +{ + if (IsRumblePlayer(origin)) + { + switch (sfx_id) + { + case sfx_itemup: + return RUMBLE_ITEMUP; + case sfx_wpnup: + return RUMBLE_WPNUP; + case sfx_getpow: + return RUMBLE_GETPOW; + case sfx_oof: + return RUMBLE_OOF; + } + } + return RUMBLE_NONE; +} + +void S_StartSoundPreset(const mobj_t *origin, int sfx_id, + pitchrange_t pitch_range) +{ + StartSound(origin, sfx_id, pitch_range, RumbleTypePreset(origin, sfx_id)); +} + +void S_StartSoundPain(const mobj_t *origin, int sfx_id) +{ + StartSound(origin, sfx_id, PITCH_FULL, RumbleType(origin, RUMBLE_PAIN)); +} + +void S_StartSoundHitFloor(const mobj_t *origin, int sfx_id) +{ + StartSound(origin, sfx_id, PITCH_FULL, RumbleType(origin, RUMBLE_HITFLOOR)); +} + +void S_StartSoundSource(const mobj_t *source, const mobj_t *origin, int sfx_id) +{ + StartSound(origin, sfx_id, PITCH_FULL, RumbleType(source, RUMBLE_PLAYER)); +} + +static rumble_type_t RumbleTypeMissile(const mobj_t *source, + const mobj_t *origin) +{ + if (IsRumblePlayer(source)) + { + if (origin) + { + switch (origin->type) + { + case MT_ROCKET: + return RUMBLE_ROCKET; + case MT_PLASMA: + case MT_PLASMA1: + case MT_PLASMA2: + return RUMBLE_PLASMA; + + default: + break; + } + } + return RUMBLE_PLAYER; + } + return RUMBLE_NONE; +} + +void S_StartSoundMissile(const mobj_t *source, const mobj_t *origin, int sfx_id) +{ + StartSound(origin, sfx_id, PITCH_FULL, RumbleTypeMissile(source, origin)); +} + +void S_StartSoundOrigin(const mobj_t *source, const mobj_t *origin, int sfx_id) +{ + StartSound(origin, sfx_id, PITCH_FULL, RumbleType(source, RUMBLE_ORIGIN)); +} + // // S_StopSound // @@ -451,6 +581,8 @@ void S_UpdateSounds(const mobj_t *listener) c->priority = pri; // haleyjd } } + + I_UpdateRumbleParams(listener, c->origin, c->handle); } else // if channel is allocated but sound has stopped, free it { @@ -461,6 +593,7 @@ void S_UpdateSounds(const mobj_t *listener) I_UpdateListenerParams(listener); I_ProcessSoundUpdates(); + I_UpdateRumble(); } void S_SetMusicVolume(int volume) diff --git a/src/s_sound.h b/src/s_sound.h index 68d0e1e0..167fa676 100644 --- a/src/s_sound.h +++ b/src/s_sound.h @@ -48,8 +48,39 @@ void S_Start(void); // Start sound for thing at // using from sounds.h // -#define S_StartSound(o,i) S_StartSoundPitch((o),(i),PITCH_FULL) -void S_StartSoundPitch(const struct mobj_s *origin, int sound_id, const pitchrange_t pitch_range); + +// Thing at emits sound. No rumble. +#define S_StartSound(o, i) S_StartSoundPitch((o), (i), PITCH_FULL) +void S_StartSoundPitch(const struct mobj_s *origin, int sfx_id, + pitchrange_t pitch_range); + +// Thing at emits sound. Rumbles if displayplayer is . +#define S_StartSoundEx(o, i) S_StartSoundPitchEx((o), (i), PITCH_FULL) +void S_StartSoundPitchEx(const struct mobj_s *origin, int sfx_id, + pitchrange_t pitch_range); +void S_StartSoundPistol(const struct mobj_s *origin, int sfx_id); +void S_StartSoundShotgun(const struct mobj_s *origin, int sfx_id); +void S_StartSoundSSG(const struct mobj_s *origin, int sfx_id); +void S_StartSoundCGun(const struct mobj_s *origin, int sfx_id); +void S_StartSoundBFG(const struct mobj_s *origin, int sfx_id); + +// Thing at emits sound. Rumbles preset if displayplayer is . +void S_StartSoundPreset(const struct mobj_s *origin, int sfx_id, + pitchrange_t pitch_range); +void S_StartSoundPain(const struct mobj_s *origin, int sfx_id); +void S_StartSoundHitFloor(const struct mobj_s *origin, int sfx_id); + +// Thing at causes sound. Thing at emits sound. +// Rumbles if displayplayer is . +void S_StartSoundSource(const struct mobj_s *source, + const struct mobj_s *origin, int sfx_id); +void S_StartSoundMissile(const struct mobj_s *source, + const struct mobj_s *origin, int sfx_id); + +// Thing at causes sound. Thing at emits sound. +// Rumbles if displayplayer is , based on distance to . +void S_StartSoundOrigin(const struct mobj_s *source, + const struct mobj_s *origin, int sfx_id); // Stop sound for thing at void S_StopSound(const struct mobj_s *origin); diff --git a/src/sounds.h b/src/sounds.h index 7aa99a67..2439646e 100644 --- a/src/sounds.h +++ b/src/sounds.h @@ -27,6 +27,13 @@ // SoundFX struct. // +typedef struct sfxrumble_s +{ + float *low; // Pointer to array of low frequency rumble values. + float *high; // Pointer to array of high frequency rumble values. + int ticlength; // Array size equal to sound duration in tics. +} sfxrumble_t; + typedef struct sfxinfo_s { // up to 6-character name @@ -58,6 +65,8 @@ typedef struct sfxinfo_s boolean cached; + sfxrumble_t rumble; + } sfxinfo_t; // diff --git a/third-party/CMakeLists.txt b/third-party/CMakeLists.txt index 9863ee55..3542c6a0 100644 --- a/third-party/CMakeLists.txt +++ b/third-party/CMakeLists.txt @@ -5,6 +5,11 @@ target_woof_settings(miniz) target_compile_definitions(miniz PRIVATE MINIZ_NO_TIME) target_include_directories(miniz INTERFACE miniz) +add_library(pffft STATIC pffft/pffft.c) +target_woof_settings(pffft) +target_include_directories(pffft INTERFACE pffft) +target_link_libraries(pffft) + add_library(spng STATIC spng/spng.c) target_woof_settings(spng) target_compile_definitions(spng PRIVATE SPNG_USE_MINIZ INTERFACE SPNG_STATIC) diff --git a/third-party/pffft/LICENSE b/third-party/pffft/LICENSE new file mode 100644 index 00000000..ea8623f3 --- /dev/null +++ b/third-party/pffft/LICENSE @@ -0,0 +1,45 @@ +Copyright (c) 2013 Julien Pommier ( pommier@modartt.com ) + +Based on original fortran 77 code from FFTPACKv4 from NETLIB, +authored by Dr Paul Swarztrauber of NCAR, in 1985. + +As confirmed by the NCAR fftpack software curators, the following +FFTPACKv5 license applies to FFTPACKv4 sources. My changes are +released under the same terms. + +FFTPACK license: + +http://www.cisl.ucar.edu/css/software/fftpack5/ftpk.html + +Copyright (c) 2004 the University Corporation for Atmospheric +Research ("UCAR"). All rights reserved. Developed by NCAR's +Computational and Information Systems Laboratory, UCAR, +www.cisl.ucar.edu. + +Redistribution and use of the Software in source and binary forms, +with or without modification, is permitted provided that the +following conditions are met: + +- Neither the names of NCAR's Computational and Information Systems +Laboratory, the University Corporation for Atmospheric Research, +nor the names of its sponsors or contributors may be used to +endorse or promote products derived from this Software without +specific prior written permission. + +- Redistributions of source code must retain the above copyright +notices, this list of conditions, and the disclaimer below. + +- Redistributions in binary form must reproduce the above copyright +notice, this list of conditions, and the disclaimer below in the +documentation and/or other materials provided with the +distribution. + +THIS SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING, BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +NONINFRINGEMENT. IN NO EVENT SHALL THE CONTRIBUTORS OR COPYRIGHT +HOLDERS BE LIABLE FOR ANY CLAIM, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES OR OTHER LIABILITY, WHETHER IN AN +ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN +CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS WITH THE +SOFTWARE. diff --git a/third-party/pffft/pffft.c b/third-party/pffft/pffft.c new file mode 100644 index 00000000..89f55c2f --- /dev/null +++ b/third-party/pffft/pffft.c @@ -0,0 +1,1877 @@ +/* Copyright (c) 2013 Julien Pommier ( pommier@modartt.com ) + + Based on original fortran 77 code from FFTPACKv4 from NETLIB + (http://www.netlib.org/fftpack), authored by Dr Paul Swarztrauber + of NCAR, in 1985. + + As confirmed by the NCAR fftpack software curators, the following + FFTPACKv5 license applies to FFTPACKv4 sources. My changes are + released under the same terms. + + FFTPACK license: + + http://www.cisl.ucar.edu/css/software/fftpack5/ftpk.html + + Copyright (c) 2004 the University Corporation for Atmospheric + Research ("UCAR"). All rights reserved. Developed by NCAR's + Computational and Information Systems Laboratory, UCAR, + www.cisl.ucar.edu. + + Redistribution and use of the Software in source and binary forms, + with or without modification, is permitted provided that the + following conditions are met: + + - Neither the names of NCAR's Computational and Information Systems + Laboratory, the University Corporation for Atmospheric Research, + nor the names of its sponsors or contributors may be used to + endorse or promote products derived from this Software without + specific prior written permission. + + - Redistributions of source code must retain the above copyright + notices, this list of conditions, and the disclaimer below. + + - Redistributions in binary form must reproduce the above copyright + notice, this list of conditions, and the disclaimer below in the + documentation and/or other materials provided with the + distribution. + + THIS SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + EXPRESS OR IMPLIED, INCLUDING, BUT NOT LIMITED TO THE WARRANTIES OF + MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + NONINFRINGEMENT. IN NO EVENT SHALL THE CONTRIBUTORS OR COPYRIGHT + HOLDERS BE LIABLE FOR ANY CLAIM, INDIRECT, INCIDENTAL, SPECIAL, + EXEMPLARY, OR CONSEQUENTIAL DAMAGES OR OTHER LIABILITY, WHETHER IN AN + ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS WITH THE + SOFTWARE. + + + PFFFT : a Pretty Fast FFT. + + This file is largerly based on the original FFTPACK implementation, modified in + order to take advantage of SIMD instructions of modern CPUs. +*/ + +/* + ChangeLog: + - 2011/10/02, version 1: This is the very first release of this file. +*/ + +#define _USE_MATH_DEFINES +#include "pffft.h" +#include +#include +#include +#include + +/* detect compiler flavour */ +#if defined(_MSC_VER) +# define COMPILER_MSVC +#elif defined(__GNUC__) +# define COMPILER_GCC +#endif + +#if defined(COMPILER_GCC) +# define ALWAYS_INLINE(return_type) inline return_type __attribute__ ((always_inline)) +# define NEVER_INLINE(return_type) return_type __attribute__ ((noinline)) +# define RESTRICT __restrict +# define VLA_ARRAY_ON_STACK(type__, varname__, size__) type__ varname__[size__]; +#elif defined(COMPILER_MSVC) +# define ALWAYS_INLINE(return_type) __forceinline return_type +# define NEVER_INLINE(return_type) __declspec(noinline) return_type +# define RESTRICT __restrict +# define VLA_ARRAY_ON_STACK(type__, varname__, size__) type__ *varname__ = (type__*)_alloca(size__ * sizeof(type__)) +#endif + + +/* + vector support macros: the rest of the code is independant of + SSE/Altivec/NEON -- adding support for other platforms with 4-element + vectors should be limited to these macros +*/ + + +// define PFFFT_SIMD_DISABLE if you want to use scalar code instead of simd code +//#define PFFFT_SIMD_DISABLE + +/* + Altivec support macros +*/ +#if !defined(PFFFT_SIMD_DISABLE) && (defined(__ppc__) || defined(__ppc64__) || defined(__powerpc__) || defined(__powerpc64__)) +#include +typedef vector float v4sf; +# define SIMD_SZ 4 +# define VZERO() ((vector float) vec_splat_u8(0)) +# define VMUL(a,b) vec_madd(a,b, VZERO()) +# define VADD(a,b) vec_add(a,b) +# define VMADD(a,b,c) vec_madd(a,b,c) +# define VSUB(a,b) vec_sub(a,b) +inline v4sf ld_ps1(const float *p) { v4sf v=vec_lde(0,p); return vec_splat(vec_perm(v, v, vec_lvsl(0, p)), 0); } +# define LD_PS1(p) ld_ps1(&p) +# define INTERLEAVE2(in1, in2, out1, out2) { v4sf tmp__ = vec_mergeh(in1, in2); out2 = vec_mergel(in1, in2); out1 = tmp__; } +# define UNINTERLEAVE2(in1, in2, out1, out2) { \ + vector unsigned char vperm1 = (vector unsigned char){0,1,2,3,8,9,10,11,16,17,18,19,24,25,26,27}; \ + vector unsigned char vperm2 = (vector unsigned char){4,5,6,7,12,13,14,15,20,21,22,23,28,29,30,31}; \ + v4sf tmp__ = vec_perm(in1, in2, vperm1); out2 = vec_perm(in1, in2, vperm2); out1 = tmp__; \ + } +# define VTRANSPOSE4(x0,x1,x2,x3) { \ + v4sf y0 = vec_mergeh(x0, x2); \ + v4sf y1 = vec_mergel(x0, x2); \ + v4sf y2 = vec_mergeh(x1, x3); \ + v4sf y3 = vec_mergel(x1, x3); \ + x0 = vec_mergeh(y0, y2); \ + x1 = vec_mergel(y0, y2); \ + x2 = vec_mergeh(y1, y3); \ + x3 = vec_mergel(y1, y3); \ + } +# define VSWAPHL(a,b) vec_perm(a,b, (vector unsigned char){16,17,18,19,20,21,22,23,8,9,10,11,12,13,14,15}) +# define VALIGNED(ptr) ((((long long)(ptr)) & 0xF) == 0) + +/* + SSE1 support macros +*/ +#elif !defined(PFFFT_SIMD_DISABLE) && (defined(__x86_64__) || defined(_M_X64) || defined(__i386__) || defined(i386) || defined(_M_IX86)) + +#include +typedef __m128 v4sf; +# define SIMD_SZ 4 // 4 floats by simd vector -- this is pretty much hardcoded in the preprocess/finalize functions anyway so you will have to work if you want to enable AVX with its 256-bit vectors. +# define VZERO() _mm_setzero_ps() +# define VMUL(a,b) _mm_mul_ps(a,b) +# define VADD(a,b) _mm_add_ps(a,b) +# define VMADD(a,b,c) _mm_add_ps(_mm_mul_ps(a,b), c) +# define VSUB(a,b) _mm_sub_ps(a,b) +# define LD_PS1(p) _mm_set1_ps(p) +# define INTERLEAVE2(in1, in2, out1, out2) { v4sf tmp__ = _mm_unpacklo_ps(in1, in2); out2 = _mm_unpackhi_ps(in1, in2); out1 = tmp__; } +# define UNINTERLEAVE2(in1, in2, out1, out2) { v4sf tmp__ = _mm_shuffle_ps(in1, in2, _MM_SHUFFLE(2,0,2,0)); out2 = _mm_shuffle_ps(in1, in2, _MM_SHUFFLE(3,1,3,1)); out1 = tmp__; } +# define VTRANSPOSE4(x0,x1,x2,x3) _MM_TRANSPOSE4_PS(x0,x1,x2,x3) +# define VSWAPHL(a,b) _mm_shuffle_ps(b, a, _MM_SHUFFLE(3,2,1,0)) +# define VALIGNED(ptr) ((((long long)(ptr)) & 0xF) == 0) + +/* + ARM NEON support macros +*/ +#elif !defined(PFFFT_SIMD_DISABLE) && (defined(__arm__) || defined(__aarch64__) || defined(__arm64__)) +# include +typedef float32x4_t v4sf; +# define SIMD_SZ 4 +# define VZERO() vdupq_n_f32(0) +# define VMUL(a,b) vmulq_f32(a,b) +# define VADD(a,b) vaddq_f32(a,b) +# define VMADD(a,b,c) vmlaq_f32(c,a,b) +# define VSUB(a,b) vsubq_f32(a,b) +# define LD_PS1(p) vld1q_dup_f32(&(p)) +# define INTERLEAVE2(in1, in2, out1, out2) { float32x4x2_t tmp__ = vzipq_f32(in1,in2); out1=tmp__.val[0]; out2=tmp__.val[1]; } +# define UNINTERLEAVE2(in1, in2, out1, out2) { float32x4x2_t tmp__ = vuzpq_f32(in1,in2); out1=tmp__.val[0]; out2=tmp__.val[1]; } +# define VTRANSPOSE4(x0,x1,x2,x3) { \ + float32x4x2_t t0_ = vzipq_f32(x0, x2); \ + float32x4x2_t t1_ = vzipq_f32(x1, x3); \ + float32x4x2_t u0_ = vzipq_f32(t0_.val[0], t1_.val[0]); \ + float32x4x2_t u1_ = vzipq_f32(t0_.val[1], t1_.val[1]); \ + x0 = u0_.val[0]; x1 = u0_.val[1]; x2 = u1_.val[0]; x3 = u1_.val[1]; \ + } +// marginally faster version +//# define VTRANSPOSE4(x0,x1,x2,x3) { asm("vtrn.32 %q0, %q1;\n vtrn.32 %q2,%q3\n vswp %f0,%e2\n vswp %f1,%e3" : "+w"(x0), "+w"(x1), "+w"(x2), "+w"(x3)::); } +# define VSWAPHL(a,b) vcombine_f32(vget_low_f32(b), vget_high_f32(a)) +# define VALIGNED(ptr) ((((long long)(ptr)) & 0x3) == 0) +#else +# if !defined(PFFFT_SIMD_DISABLE) +# warning "building with simd disabled !\n"; +# define PFFFT_SIMD_DISABLE // fallback to scalar code +# endif +#endif + +// fallback mode for situations where SSE/Altivec are not available, use scalar mode instead +#ifdef PFFFT_SIMD_DISABLE +typedef float v4sf; +# define SIMD_SZ 1 +# define VZERO() 0.f +# define VMUL(a,b) ((a)*(b)) +# define VADD(a,b) ((a)+(b)) +# define VMADD(a,b,c) ((a)*(b)+(c)) +# define VSUB(a,b) ((a)-(b)) +# define LD_PS1(p) (p) +# define VALIGNED(ptr) ((((long long)(ptr)) & 0x3) == 0) +#endif + +// shortcuts for complex multiplcations +#define VCPLXMUL(ar,ai,br,bi) { v4sf tmp; tmp=VMUL(ar,bi); ar=VMUL(ar,br); ar=VSUB(ar,VMUL(ai,bi)); ai=VMUL(ai,br); ai=VADD(ai,tmp); } +#define VCPLXMULCONJ(ar,ai,br,bi) { v4sf tmp; tmp=VMUL(ar,bi); ar=VMUL(ar,br); ar=VADD(ar,VMUL(ai,bi)); ai=VMUL(ai,br); ai=VSUB(ai,tmp); } +#ifndef SVMUL +// multiply a scalar with a vector +#define SVMUL(f,v) VMUL(LD_PS1(f),v) +#endif + +#if !defined(PFFFT_SIMD_DISABLE) +typedef union v4sf_union { + v4sf v; + float f[4]; +} v4sf_union; + +#include + +#define assertv4(v,f0,f1,f2,f3) assert(v.f[0] == (f0) && v.f[1] == (f1) && v.f[2] == (f2) && v.f[3] == (f3)) + +/* detect bugs with the vector support macros */ +void validate_pffft_simd(void) { + float f[16] = { 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 }; + v4sf_union a0, a1, a2, a3, t, u; + memcpy(a0.f, f, 4*sizeof(float)); + memcpy(a1.f, f+4, 4*sizeof(float)); + memcpy(a2.f, f+8, 4*sizeof(float)); + memcpy(a3.f, f+12, 4*sizeof(float)); + + t = a0; u = a1; t.v = VZERO(); + printf("VZERO=[%2g %2g %2g %2g]\n", t.f[0], t.f[1], t.f[2], t.f[3]); assertv4(t, 0, 0, 0, 0); + t.v = VADD(a1.v, a2.v); + printf("VADD(4:7,8:11)=[%2g %2g %2g %2g]\n", t.f[0], t.f[1], t.f[2], t.f[3]); assertv4(t, 12, 14, 16, 18); + t.v = VMUL(a1.v, a2.v); + printf("VMUL(4:7,8:11)=[%2g %2g %2g %2g]\n", t.f[0], t.f[1], t.f[2], t.f[3]); assertv4(t, 32, 45, 60, 77); + t.v = VMADD(a1.v, a2.v,a0.v); + printf("VMADD(4:7,8:11,0:3)=[%2g %2g %2g %2g]\n", t.f[0], t.f[1], t.f[2], t.f[3]); assertv4(t, 32, 46, 62, 80); + + INTERLEAVE2(a1.v,a2.v,t.v,u.v); + printf("INTERLEAVE2(4:7,8:11)=[%2g %2g %2g %2g] [%2g %2g %2g %2g]\n", t.f[0], t.f[1], t.f[2], t.f[3], u.f[0], u.f[1], u.f[2], u.f[3]); + assertv4(t, 4, 8, 5, 9); assertv4(u, 6, 10, 7, 11); + UNINTERLEAVE2(a1.v,a2.v,t.v,u.v); + printf("UNINTERLEAVE2(4:7,8:11)=[%2g %2g %2g %2g] [%2g %2g %2g %2g]\n", t.f[0], t.f[1], t.f[2], t.f[3], u.f[0], u.f[1], u.f[2], u.f[3]); + assertv4(t, 4, 6, 8, 10); assertv4(u, 5, 7, 9, 11); + + t.v=LD_PS1(f[15]); + printf("LD_PS1(15)=[%2g %2g %2g %2g]\n", t.f[0], t.f[1], t.f[2], t.f[3]); + assertv4(t, 15, 15, 15, 15); + t.v = VSWAPHL(a1.v, a2.v); + printf("VSWAPHL(4:7,8:11)=[%2g %2g %2g %2g]\n", t.f[0], t.f[1], t.f[2], t.f[3]); + assertv4(t, 8, 9, 6, 7); + VTRANSPOSE4(a0.v, a1.v, a2.v, a3.v); + printf("VTRANSPOSE4(0:3,4:7,8:11,12:15)=[%2g %2g %2g %2g] [%2g %2g %2g %2g] [%2g %2g %2g %2g] [%2g %2g %2g %2g]\n", + a0.f[0], a0.f[1], a0.f[2], a0.f[3], a1.f[0], a1.f[1], a1.f[2], a1.f[3], + a2.f[0], a2.f[1], a2.f[2], a2.f[3], a3.f[0], a3.f[1], a3.f[2], a3.f[3]); + assertv4(a0, 0, 4, 8, 12); assertv4(a1, 1, 5, 9, 13); assertv4(a2, 2, 6, 10, 14); assertv4(a3, 3, 7, 11, 15); +} +#else +void validate_pffft_simd() {} // allow test_pffft.c to call this function even when simd is not available.. +#endif //!PFFFT_SIMD_DISABLE + +/* SSE and co like 16-bytes aligned pointers */ +#define MALLOC_V4SF_ALIGNMENT 64 // with a 64-byte alignment, we are even aligned on L2 cache lines... +void *pffft_aligned_malloc(size_t nb_bytes) { + void *p, *p0 = malloc(nb_bytes + MALLOC_V4SF_ALIGNMENT); + if (!p0) return (void *) 0; + p = (void *) (((size_t) p0 + MALLOC_V4SF_ALIGNMENT) & (~((size_t) (MALLOC_V4SF_ALIGNMENT-1)))); + *((void **) p - 1) = p0; + return p; +} + +void pffft_aligned_free(void *p) { + if (p) free(*((void **) p - 1)); +} + +int pffft_simd_size(void) { return SIMD_SZ; } + +/* + passf2 and passb2 has been merged here, fsign = -1 for passf2, +1 for passb2 +*/ +static NEVER_INLINE(void) passf2_ps(int ido, int l1, const v4sf *cc, v4sf *ch, const float *wa1, float fsign) { + int k, i; + int l1ido = l1*ido; + if (ido <= 2) { + for (k=0; k < l1ido; k += ido, ch += ido, cc+= 2*ido) { + ch[0] = VADD(cc[0], cc[ido+0]); + ch[l1ido] = VSUB(cc[0], cc[ido+0]); + ch[1] = VADD(cc[1], cc[ido+1]); + ch[l1ido + 1] = VSUB(cc[1], cc[ido+1]); + } + } else { + for (k=0; k < l1ido; k += ido, ch += ido, cc += 2*ido) { + for (i=0; i 2); + for (k=0; k< l1ido; k += ido, cc+= 3*ido, ch +=ido) { + for (i=0; i 2); + for (k = 0; k < l1; ++k, cc += 5*ido, ch += ido) { + for (i = 0; i < ido-1; i += 2) { + ti5 = VSUB(cc_ref(i , 2), cc_ref(i , 5)); + ti2 = VADD(cc_ref(i , 2), cc_ref(i , 5)); + ti4 = VSUB(cc_ref(i , 3), cc_ref(i , 4)); + ti3 = VADD(cc_ref(i , 3), cc_ref(i , 4)); + tr5 = VSUB(cc_ref(i-1, 2), cc_ref(i-1, 5)); + tr2 = VADD(cc_ref(i-1, 2), cc_ref(i-1, 5)); + tr4 = VSUB(cc_ref(i-1, 3), cc_ref(i-1, 4)); + tr3 = VADD(cc_ref(i-1, 3), cc_ref(i-1, 4)); + ch_ref(i-1, 1) = VADD(cc_ref(i-1, 1), VADD(tr2, tr3)); + ch_ref(i , 1) = VADD(cc_ref(i , 1), VADD(ti2, ti3)); + cr2 = VADD(cc_ref(i-1, 1), VADD(SVMUL(tr11, tr2),SVMUL(tr12, tr3))); + ci2 = VADD(cc_ref(i , 1), VADD(SVMUL(tr11, ti2),SVMUL(tr12, ti3))); + cr3 = VADD(cc_ref(i-1, 1), VADD(SVMUL(tr12, tr2),SVMUL(tr11, tr3))); + ci3 = VADD(cc_ref(i , 1), VADD(SVMUL(tr12, ti2),SVMUL(tr11, ti3))); + cr5 = VADD(SVMUL(ti11, tr5), SVMUL(ti12, tr4)); + ci5 = VADD(SVMUL(ti11, ti5), SVMUL(ti12, ti4)); + cr4 = VSUB(SVMUL(ti12, tr5), SVMUL(ti11, tr4)); + ci4 = VSUB(SVMUL(ti12, ti5), SVMUL(ti11, ti4)); + dr3 = VSUB(cr3, ci4); + dr4 = VADD(cr3, ci4); + di3 = VADD(ci3, cr4); + di4 = VSUB(ci3, cr4); + dr5 = VADD(cr2, ci5); + dr2 = VSUB(cr2, ci5); + di5 = VSUB(ci2, cr5); + di2 = VADD(ci2, cr5); + wr1=wa1[i]; wi1=fsign*wa1[i+1]; wr2=wa2[i]; wi2=fsign*wa2[i+1]; + wr3=wa3[i]; wi3=fsign*wa3[i+1]; wr4=wa4[i]; wi4=fsign*wa4[i+1]; + VCPLXMUL(dr2, di2, LD_PS1(wr1), LD_PS1(wi1)); + ch_ref(i - 1, 2) = dr2; + ch_ref(i, 2) = di2; + VCPLXMUL(dr3, di3, LD_PS1(wr2), LD_PS1(wi2)); + ch_ref(i - 1, 3) = dr3; + ch_ref(i, 3) = di3; + VCPLXMUL(dr4, di4, LD_PS1(wr3), LD_PS1(wi3)); + ch_ref(i - 1, 4) = dr4; + ch_ref(i, 4) = di4; + VCPLXMUL(dr5, di5, LD_PS1(wr4), LD_PS1(wi4)); + ch_ref(i - 1, 5) = dr5; + ch_ref(i, 5) = di5; + } + } +#undef ch_ref +#undef cc_ref +} + +static NEVER_INLINE(void) radf2_ps(int ido, int l1, const v4sf * RESTRICT cc, v4sf * RESTRICT ch, const float *wa1) { + static const float minus_one = -1.f; + int i, k, l1ido = l1*ido; + for (k=0; k < l1ido; k += ido) { + v4sf a = cc[k], b = cc[k + l1ido]; + ch[2*k] = VADD(a, b); + ch[2*(k+ido)-1] = VSUB(a, b); + } + if (ido < 2) return; + if (ido != 2) { + for (k=0; k < l1ido; k += ido) { + for (i=2; i 5) { + wa[i1-1] = wa[i-1]; + wa[i1] = wa[i]; + } + } + l1 = l2; + } +} /* cffti1 */ + + +v4sf *cfftf1_ps(int n, const v4sf *input_readonly, v4sf *work1, v4sf *work2, const float *wa, const int *ifac, int isign) { + v4sf *in = (v4sf*)input_readonly; + v4sf *out = (in == work2 ? work1 : work2); + int nf = ifac[1], k1; + int l1 = 1; + int iw = 0; + assert(in != out && work1 != work2); + for (k1=2; k1<=nf+1; k1++) { + int ip = ifac[k1]; + int l2 = ip*l1; + int ido = n / l2; + int idot = ido + ido; + switch (ip) { + case 5: { + int ix2 = iw + idot; + int ix3 = ix2 + idot; + int ix4 = ix3 + idot; + passf5_ps(idot, l1, in, out, &wa[iw], &wa[ix2], &wa[ix3], &wa[ix4], isign); + } break; + case 4: { + int ix2 = iw + idot; + int ix3 = ix2 + idot; + passf4_ps(idot, l1, in, out, &wa[iw], &wa[ix2], &wa[ix3], isign); + } break; + case 2: { + passf2_ps(idot, l1, in, out, &wa[iw], isign); + } break; + case 3: { + int ix2 = iw + idot; + passf3_ps(idot, l1, in, out, &wa[iw], &wa[ix2], isign); + } break; + default: + assert(0); + } + l1 = l2; + iw += (ip - 1)*idot; + if (out == work2) { + out = work1; in = work2; + } else { + out = work2; in = work1; + } + } + + return in; /* this is in fact the output .. */ +} + + +struct PFFFT_Setup { + int N; + int Ncvec; // nb of complex simd vectors (N/4 if PFFFT_COMPLEX, N/8 if PFFFT_REAL) + int ifac[15]; + pffft_transform_t transform; + v4sf *data; // allocated room for twiddle coefs + float *e; // points into 'data' , N/4*3 elements + float *twiddle; // points into 'data', N/4 elements +}; + +PFFFT_Setup *pffft_new_setup(int N, pffft_transform_t transform) { + PFFFT_Setup *s = (PFFFT_Setup*)malloc(sizeof(PFFFT_Setup)); + int k, m; + /* unfortunately, the fft size must be a multiple of 16 for complex FFTs + and 32 for real FFTs -- a lot of stuff would need to be rewritten to + handle other cases (or maybe just switch to a scalar fft, I don't know..) */ + if (transform == PFFFT_REAL) { assert((N%(2*SIMD_SZ*SIMD_SZ))==0 && N>0); } + if (transform == PFFFT_COMPLEX) { assert((N%(SIMD_SZ*SIMD_SZ))==0 && N>0); } + //assert((N % 32) == 0); + s->N = N; + s->transform = transform; + /* nb of complex simd vectors */ + s->Ncvec = (transform == PFFFT_REAL ? N/2 : N)/SIMD_SZ; + s->data = (v4sf*)pffft_aligned_malloc(2*s->Ncvec * sizeof(v4sf)); + s->e = (float*)s->data; + s->twiddle = (float*)(s->data + (2*s->Ncvec*(SIMD_SZ-1))/SIMD_SZ); + + for (k=0; k < s->Ncvec; ++k) { + int i = k/SIMD_SZ; + int j = k%SIMD_SZ; + for (m=0; m < SIMD_SZ-1; ++m) { + float A = -2*M_PI*(m+1)*k / N; + s->e[(2*(i*3 + m) + 0) * SIMD_SZ + j] = cos(A); + s->e[(2*(i*3 + m) + 1) * SIMD_SZ + j] = sin(A); + } + } + + if (transform == PFFFT_REAL) { + rffti1_ps(N/SIMD_SZ, s->twiddle, s->ifac); + } else { + cffti1_ps(N/SIMD_SZ, s->twiddle, s->ifac); + } + + /* check that N is decomposable with allowed prime factors */ + for (k=0, m=1; k < s->ifac[1]; ++k) { m *= s->ifac[2+k]; } + if (m != N/SIMD_SZ) { + pffft_destroy_setup(s); s = 0; + } + + return s; +} + + +void pffft_destroy_setup(PFFFT_Setup *s) { + pffft_aligned_free(s->data); + free(s); +} + +#if !defined(PFFFT_SIMD_DISABLE) + +/* [0 0 1 2 3 4 5 6 7 8] -> [0 8 7 6 5 4 3 2 1] */ +static void reversed_copy(int N, const v4sf *in, int in_stride, v4sf *out) { + v4sf g0, g1; + int k; + INTERLEAVE2(in[0], in[1], g0, g1); in += in_stride; + + *--out = VSWAPHL(g0, g1); // [g0l, g0h], [g1l g1h] -> [g1l, g0h] + for (k=1; k < N; ++k) { + v4sf h0, h1; + INTERLEAVE2(in[0], in[1], h0, h1); in += in_stride; + *--out = VSWAPHL(g1, h0); + *--out = VSWAPHL(h0, h1); + g1 = h1; + } + *--out = VSWAPHL(g1, g0); +} + +static void unreversed_copy(int N, const v4sf *in, v4sf *out, int out_stride) { + v4sf g0, g1, h0, h1; + int k; + g0 = g1 = in[0]; ++in; + for (k=1; k < N; ++k) { + h0 = *in++; h1 = *in++; + g1 = VSWAPHL(g1, h0); + h0 = VSWAPHL(h0, h1); + UNINTERLEAVE2(h0, g1, out[0], out[1]); out += out_stride; + g1 = h1; + } + h0 = *in++; h1 = g0; + g1 = VSWAPHL(g1, h0); + h0 = VSWAPHL(h0, h1); + UNINTERLEAVE2(h0, g1, out[0], out[1]); +} + +void pffft_zreorder(PFFFT_Setup *setup, const float *in, float *out, pffft_direction_t direction) { + int k, N = setup->N, Ncvec = setup->Ncvec; + const v4sf *vin = (const v4sf*)in; + v4sf *vout = (v4sf*)out; + assert(in != out); + if (setup->transform == PFFFT_REAL) { + int dk = N/32; + if (direction == PFFFT_FORWARD) { + for (k=0; k < dk; ++k) { + INTERLEAVE2(vin[k*8 + 0], vin[k*8 + 1], vout[2*(0*dk + k) + 0], vout[2*(0*dk + k) + 1]); + INTERLEAVE2(vin[k*8 + 4], vin[k*8 + 5], vout[2*(2*dk + k) + 0], vout[2*(2*dk + k) + 1]); + } + reversed_copy(dk, vin+2, 8, (v4sf*)(out + N/2)); + reversed_copy(dk, vin+6, 8, (v4sf*)(out + N)); + } else { + for (k=0; k < dk; ++k) { + UNINTERLEAVE2(vin[2*(0*dk + k) + 0], vin[2*(0*dk + k) + 1], vout[k*8 + 0], vout[k*8 + 1]); + UNINTERLEAVE2(vin[2*(2*dk + k) + 0], vin[2*(2*dk + k) + 1], vout[k*8 + 4], vout[k*8 + 5]); + } + unreversed_copy(dk, (v4sf*)(in + N/4), (v4sf*)(out + N - 6*SIMD_SZ), -8); + unreversed_copy(dk, (v4sf*)(in + 3*N/4), (v4sf*)(out + N - 2*SIMD_SZ), -8); + } + } else { + if (direction == PFFFT_FORWARD) { + for (k=0; k < Ncvec; ++k) { + int kk = (k/4) + (k%4)*(Ncvec/4); + INTERLEAVE2(vin[k*2], vin[k*2+1], vout[kk*2], vout[kk*2+1]); + } + } else { + for (k=0; k < Ncvec; ++k) { + int kk = (k/4) + (k%4)*(Ncvec/4); + UNINTERLEAVE2(vin[kk*2], vin[kk*2+1], vout[k*2], vout[k*2+1]); + } + } + } +} + +void pffft_cplx_finalize(int Ncvec, const v4sf *in, v4sf *out, const v4sf *e) { + int k, dk = Ncvec/SIMD_SZ; // number of 4x4 matrix blocks + v4sf r0, i0, r1, i1, r2, i2, r3, i3; + v4sf sr0, dr0, sr1, dr1, si0, di0, si1, di1; + assert(in != out); + for (k=0; k < dk; ++k) { + r0 = in[8*k+0]; i0 = in[8*k+1]; + r1 = in[8*k+2]; i1 = in[8*k+3]; + r2 = in[8*k+4]; i2 = in[8*k+5]; + r3 = in[8*k+6]; i3 = in[8*k+7]; + VTRANSPOSE4(r0,r1,r2,r3); + VTRANSPOSE4(i0,i1,i2,i3); + VCPLXMUL(r1,i1,e[k*6+0],e[k*6+1]); + VCPLXMUL(r2,i2,e[k*6+2],e[k*6+3]); + VCPLXMUL(r3,i3,e[k*6+4],e[k*6+5]); + + sr0 = VADD(r0,r2); dr0 = VSUB(r0, r2); + sr1 = VADD(r1,r3); dr1 = VSUB(r1, r3); + si0 = VADD(i0,i2); di0 = VSUB(i0, i2); + si1 = VADD(i1,i3); di1 = VSUB(i1, i3); + + /* + transformation for each column is: + + [1 1 1 1 0 0 0 0] [r0] + [1 0 -1 0 0 -1 0 1] [r1] + [1 -1 1 -1 0 0 0 0] [r2] + [1 0 -1 0 0 1 0 -1] [r3] + [0 0 0 0 1 1 1 1] * [i0] + [0 1 0 -1 1 0 -1 0] [i1] + [0 0 0 0 1 -1 1 -1] [i2] + [0 -1 0 1 1 0 -1 0] [i3] + */ + + r0 = VADD(sr0, sr1); i0 = VADD(si0, si1); + r1 = VADD(dr0, di1); i1 = VSUB(di0, dr1); + r2 = VSUB(sr0, sr1); i2 = VSUB(si0, si1); + r3 = VSUB(dr0, di1); i3 = VADD(di0, dr1); + + *out++ = r0; *out++ = i0; *out++ = r1; *out++ = i1; + *out++ = r2; *out++ = i2; *out++ = r3; *out++ = i3; + } +} + +void pffft_cplx_preprocess(int Ncvec, const v4sf *in, v4sf *out, const v4sf *e) { + int k, dk = Ncvec/SIMD_SZ; // number of 4x4 matrix blocks + v4sf r0, i0, r1, i1, r2, i2, r3, i3; + v4sf sr0, dr0, sr1, dr1, si0, di0, si1, di1; + assert(in != out); + for (k=0; k < dk; ++k) { + r0 = in[8*k+0]; i0 = in[8*k+1]; + r1 = in[8*k+2]; i1 = in[8*k+3]; + r2 = in[8*k+4]; i2 = in[8*k+5]; + r3 = in[8*k+6]; i3 = in[8*k+7]; + + sr0 = VADD(r0,r2); dr0 = VSUB(r0, r2); + sr1 = VADD(r1,r3); dr1 = VSUB(r1, r3); + si0 = VADD(i0,i2); di0 = VSUB(i0, i2); + si1 = VADD(i1,i3); di1 = VSUB(i1, i3); + + r0 = VADD(sr0, sr1); i0 = VADD(si0, si1); + r1 = VSUB(dr0, di1); i1 = VADD(di0, dr1); + r2 = VSUB(sr0, sr1); i2 = VSUB(si0, si1); + r3 = VADD(dr0, di1); i3 = VSUB(di0, dr1); + + VCPLXMULCONJ(r1,i1,e[k*6+0],e[k*6+1]); + VCPLXMULCONJ(r2,i2,e[k*6+2],e[k*6+3]); + VCPLXMULCONJ(r3,i3,e[k*6+4],e[k*6+5]); + + VTRANSPOSE4(r0,r1,r2,r3); + VTRANSPOSE4(i0,i1,i2,i3); + + *out++ = r0; *out++ = i0; *out++ = r1; *out++ = i1; + *out++ = r2; *out++ = i2; *out++ = r3; *out++ = i3; + } +} + + +static ALWAYS_INLINE(void) pffft_real_finalize_4x4(const v4sf *in0, const v4sf *in1, const v4sf *in, + const v4sf *e, v4sf *out) { + v4sf r0, i0, r1, i1, r2, i2, r3, i3; + v4sf sr0, dr0, sr1, dr1, si0, di0, si1, di1; + r0 = *in0; i0 = *in1; + r1 = *in++; i1 = *in++; r2 = *in++; i2 = *in++; r3 = *in++; i3 = *in++; + VTRANSPOSE4(r0,r1,r2,r3); + VTRANSPOSE4(i0,i1,i2,i3); + + /* + transformation for each column is: + + [1 1 1 1 0 0 0 0] [r0] + [1 0 -1 0 0 -1 0 1] [r1] + [1 0 -1 0 0 1 0 -1] [r2] + [1 -1 1 -1 0 0 0 0] [r3] + [0 0 0 0 1 1 1 1] * [i0] + [0 -1 0 1 -1 0 1 0] [i1] + [0 -1 0 1 1 0 -1 0] [i2] + [0 0 0 0 -1 1 -1 1] [i3] + */ + + //cerr << "matrix initial, before e , REAL:\n 1: " << r0 << "\n 1: " << r1 << "\n 1: " << r2 << "\n 1: " << r3 << "\n"; + //cerr << "matrix initial, before e, IMAG :\n 1: " << i0 << "\n 1: " << i1 << "\n 1: " << i2 << "\n 1: " << i3 << "\n"; + + VCPLXMUL(r1,i1,e[0],e[1]); + VCPLXMUL(r2,i2,e[2],e[3]); + VCPLXMUL(r3,i3,e[4],e[5]); + + //cerr << "matrix initial, real part:\n 1: " << r0 << "\n 1: " << r1 << "\n 1: " << r2 << "\n 1: " << r3 << "\n"; + //cerr << "matrix initial, imag part:\n 1: " << i0 << "\n 1: " << i1 << "\n 1: " << i2 << "\n 1: " << i3 << "\n"; + + sr0 = VADD(r0,r2); dr0 = VSUB(r0,r2); + sr1 = VADD(r1,r3); dr1 = VSUB(r3,r1); + si0 = VADD(i0,i2); di0 = VSUB(i0,i2); + si1 = VADD(i1,i3); di1 = VSUB(i3,i1); + + r0 = VADD(sr0, sr1); + r3 = VSUB(sr0, sr1); + i0 = VADD(si0, si1); + i3 = VSUB(si1, si0); + r1 = VADD(dr0, di1); + r2 = VSUB(dr0, di1); + i1 = VSUB(dr1, di0); + i2 = VADD(dr1, di0); + + *out++ = r0; + *out++ = i0; + *out++ = r1; + *out++ = i1; + *out++ = r2; + *out++ = i2; + *out++ = r3; + *out++ = i3; + +} + +static NEVER_INLINE(void) pffft_real_finalize(int Ncvec, const v4sf *in, v4sf *out, const v4sf *e) { + int k, dk = Ncvec/SIMD_SZ; // number of 4x4 matrix blocks + /* fftpack order is f0r f1r f1i f2r f2i ... f(n-1)r f(n-1)i f(n)r */ + + v4sf_union cr, ci, *uout = (v4sf_union*)out; + v4sf save = in[7], zero=VZERO(); + float xr0, xi0, xr1, xi1, xr2, xi2, xr3, xi3; + static const float s = (float)(M_SQRT2/2); + + cr.v = in[0]; ci.v = in[Ncvec*2-1]; + assert(in != out); + pffft_real_finalize_4x4(&zero, &zero, in+1, e, out); + + /* + [cr0 cr1 cr2 cr3 ci0 ci1 ci2 ci3] + + [Xr(1)] ] [1 1 1 1 0 0 0 0] + [Xr(N/4) ] [0 0 0 0 1 s 0 -s] + [Xr(N/2) ] [1 0 -1 0 0 0 0 0] + [Xr(3N/4)] [0 0 0 0 1 -s 0 s] + [Xi(1) ] [1 -1 1 -1 0 0 0 0] + [Xi(N/4) ] [0 0 0 0 0 -s -1 -s] + [Xi(N/2) ] [0 -1 0 1 0 0 0 0] + [Xi(3N/4)] [0 0 0 0 0 -s 1 -s] + */ + + xr0=(cr.f[0]+cr.f[2]) + (cr.f[1]+cr.f[3]); uout[0].f[0] = xr0; + xi0=(cr.f[0]+cr.f[2]) - (cr.f[1]+cr.f[3]); uout[1].f[0] = xi0; + xr2=(cr.f[0]-cr.f[2]); uout[4].f[0] = xr2; + xi2=(cr.f[3]-cr.f[1]); uout[5].f[0] = xi2; + xr1= ci.f[0] + s*(ci.f[1]-ci.f[3]); uout[2].f[0] = xr1; + xi1=-ci.f[2] - s*(ci.f[1]+ci.f[3]); uout[3].f[0] = xi1; + xr3= ci.f[0] - s*(ci.f[1]-ci.f[3]); uout[6].f[0] = xr3; + xi3= ci.f[2] - s*(ci.f[1]+ci.f[3]); uout[7].f[0] = xi3; + + for (k=1; k < dk; ++k) { + v4sf save_next = in[8*k+7]; + pffft_real_finalize_4x4(&save, &in[8*k+0], in + 8*k+1, + e + k*6, out + k*8); + save = save_next; + } + +} + +static ALWAYS_INLINE(void) pffft_real_preprocess_4x4(const v4sf *in, + const v4sf *e, v4sf *out, int first) { + v4sf r0=in[0], i0=in[1], r1=in[2], i1=in[3], r2=in[4], i2=in[5], r3=in[6], i3=in[7]; + /* + transformation for each column is: + + [1 1 1 1 0 0 0 0] [r0] + [1 0 0 -1 0 -1 -1 0] [r1] + [1 -1 -1 1 0 0 0 0] [r2] + [1 0 0 -1 0 1 1 0] [r3] + [0 0 0 0 1 -1 1 -1] * [i0] + [0 -1 1 0 1 0 0 1] [i1] + [0 0 0 0 1 1 -1 -1] [i2] + [0 1 -1 0 1 0 0 1] [i3] + */ + + v4sf sr0 = VADD(r0,r3), dr0 = VSUB(r0,r3); + v4sf sr1 = VADD(r1,r2), dr1 = VSUB(r1,r2); + v4sf si0 = VADD(i0,i3), di0 = VSUB(i0,i3); + v4sf si1 = VADD(i1,i2), di1 = VSUB(i1,i2); + + r0 = VADD(sr0, sr1); + r2 = VSUB(sr0, sr1); + r1 = VSUB(dr0, si1); + r3 = VADD(dr0, si1); + i0 = VSUB(di0, di1); + i2 = VADD(di0, di1); + i1 = VSUB(si0, dr1); + i3 = VADD(si0, dr1); + + VCPLXMULCONJ(r1,i1,e[0],e[1]); + VCPLXMULCONJ(r2,i2,e[2],e[3]); + VCPLXMULCONJ(r3,i3,e[4],e[5]); + + VTRANSPOSE4(r0,r1,r2,r3); + VTRANSPOSE4(i0,i1,i2,i3); + + if (!first) { + *out++ = r0; + *out++ = i0; + } + *out++ = r1; + *out++ = i1; + *out++ = r2; + *out++ = i2; + *out++ = r3; + *out++ = i3; +} + +static NEVER_INLINE(void) pffft_real_preprocess(int Ncvec, const v4sf *in, v4sf *out, const v4sf *e) { + int k, dk = Ncvec/SIMD_SZ; // number of 4x4 matrix blocks + /* fftpack order is f0r f1r f1i f2r f2i ... f(n-1)r f(n-1)i f(n)r */ + + v4sf_union Xr, Xi, *uout = (v4sf_union*)out; + float cr0, ci0, cr1, ci1, cr2, ci2, cr3, ci3; + static const float s = (float)M_SQRT2; + assert(in != out); + for (k=0; k < 4; ++k) { + Xr.f[k] = ((float*)in)[8*k]; + Xi.f[k] = ((float*)in)[8*k+4]; + } + + pffft_real_preprocess_4x4(in, e, out+1, 1); // will write only 6 values + + /* + [Xr0 Xr1 Xr2 Xr3 Xi0 Xi1 Xi2 Xi3] + + [cr0] [1 0 2 0 1 0 0 0] + [cr1] [1 0 0 0 -1 0 -2 0] + [cr2] [1 0 -2 0 1 0 0 0] + [cr3] [1 0 0 0 -1 0 2 0] + [ci0] [0 2 0 2 0 0 0 0] + [ci1] [0 s 0 -s 0 -s 0 -s] + [ci2] [0 0 0 0 0 -2 0 2] + [ci3] [0 -s 0 s 0 -s 0 -s] + */ + for (k=1; k < dk; ++k) { + pffft_real_preprocess_4x4(in+8*k, e + k*6, out-1+k*8, 0); + } + + cr0=(Xr.f[0]+Xi.f[0]) + 2*Xr.f[2]; uout[0].f[0] = cr0; + cr1=(Xr.f[0]-Xi.f[0]) - 2*Xi.f[2]; uout[0].f[1] = cr1; + cr2=(Xr.f[0]+Xi.f[0]) - 2*Xr.f[2]; uout[0].f[2] = cr2; + cr3=(Xr.f[0]-Xi.f[0]) + 2*Xi.f[2]; uout[0].f[3] = cr3; + ci0= 2*(Xr.f[1]+Xr.f[3]); uout[2*Ncvec-1].f[0] = ci0; + ci1= s*(Xr.f[1]-Xr.f[3]) - s*(Xi.f[1]+Xi.f[3]); uout[2*Ncvec-1].f[1] = ci1; + ci2= 2*(Xi.f[3]-Xi.f[1]); uout[2*Ncvec-1].f[2] = ci2; + ci3=-s*(Xr.f[1]-Xr.f[3]) - s*(Xi.f[1]+Xi.f[3]); uout[2*Ncvec-1].f[3] = ci3; +} + + +void pffft_transform_internal(PFFFT_Setup *setup, const float *finput, float *foutput, v4sf *scratch, + pffft_direction_t direction, int ordered) { + int k, Ncvec = setup->Ncvec; + int nf_odd = (setup->ifac[1] & 1); + + // temporary buffer is allocated on the stack if the scratch pointer is NULL + int stack_allocate = (scratch == 0 ? Ncvec*2 : 1); + VLA_ARRAY_ON_STACK(v4sf, scratch_on_stack, stack_allocate); + + const v4sf *vinput = (const v4sf*)finput; + v4sf *voutput = (v4sf*)foutput; + v4sf *buff[2] = { voutput, scratch ? scratch : scratch_on_stack }; + int ib = (nf_odd ^ ordered ? 1 : 0); + + assert(VALIGNED(finput) && VALIGNED(foutput)); + + //assert(finput != foutput); + if (direction == PFFFT_FORWARD) { + ib = !ib; + if (setup->transform == PFFFT_REAL) { + ib = (rfftf1_ps(Ncvec*2, vinput, buff[ib], buff[!ib], + setup->twiddle, &setup->ifac[0]) == buff[0] ? 0 : 1); + pffft_real_finalize(Ncvec, buff[ib], buff[!ib], (v4sf*)setup->e); + } else { + v4sf *tmp = buff[ib]; + for (k=0; k < Ncvec; ++k) { + UNINTERLEAVE2(vinput[k*2], vinput[k*2+1], tmp[k*2], tmp[k*2+1]); + } + ib = (cfftf1_ps(Ncvec, buff[ib], buff[!ib], buff[ib], + setup->twiddle, &setup->ifac[0], -1) == buff[0] ? 0 : 1); + pffft_cplx_finalize(Ncvec, buff[ib], buff[!ib], (v4sf*)setup->e); + } + if (ordered) { + pffft_zreorder(setup, (float*)buff[!ib], (float*)buff[ib], PFFFT_FORWARD); + } else ib = !ib; + } else { + if (vinput == buff[ib]) { + ib = !ib; // may happen when finput == foutput + } + if (ordered) { + pffft_zreorder(setup, (float*)vinput, (float*)buff[ib], PFFFT_BACKWARD); + vinput = buff[ib]; ib = !ib; + } + if (setup->transform == PFFFT_REAL) { + pffft_real_preprocess(Ncvec, vinput, buff[ib], (v4sf*)setup->e); + ib = (rfftb1_ps(Ncvec*2, buff[ib], buff[0], buff[1], + setup->twiddle, &setup->ifac[0]) == buff[0] ? 0 : 1); + } else { + pffft_cplx_preprocess(Ncvec, vinput, buff[ib], (v4sf*)setup->e); + ib = (cfftf1_ps(Ncvec, buff[ib], buff[0], buff[1], + setup->twiddle, &setup->ifac[0], +1) == buff[0] ? 0 : 1); + for (k=0; k < Ncvec; ++k) { + INTERLEAVE2(buff[ib][k*2], buff[ib][k*2+1], buff[ib][k*2], buff[ib][k*2+1]); + } + } + } + + if (buff[ib] != voutput) { + /* extra copy required -- this situation should only happen when finput == foutput */ + assert(finput==foutput); + for (k=0; k < Ncvec; ++k) { + v4sf a = buff[ib][2*k], b = buff[ib][2*k+1]; + voutput[2*k] = a; voutput[2*k+1] = b; + } + ib = !ib; + } + assert(buff[ib] == voutput); +} + +void pffft_zconvolve_accumulate(PFFFT_Setup *s, const float *a, const float *b, float *ab, float scaling) { + int Ncvec = s->Ncvec; + const v4sf * RESTRICT va = (const v4sf*)a; + const v4sf * RESTRICT vb = (const v4sf*)b; + v4sf * RESTRICT vab = (v4sf*)ab; + +#ifdef __arm__ + __builtin_prefetch(va); + __builtin_prefetch(vb); + __builtin_prefetch(vab); + __builtin_prefetch(va+2); + __builtin_prefetch(vb+2); + __builtin_prefetch(vab+2); + __builtin_prefetch(va+4); + __builtin_prefetch(vb+4); + __builtin_prefetch(vab+4); + __builtin_prefetch(va+6); + __builtin_prefetch(vb+6); + __builtin_prefetch(vab+6); +# ifndef __clang__ +# define ZCONVOLVE_USING_INLINE_NEON_ASM +# endif +#endif + + float ar0, ai0, br0, bi0, abr0, abi0; +#ifndef ZCONVOLVE_USING_INLINE_ASM + v4sf vscal = LD_PS1(scaling); + int i; +#endif + + assert(VALIGNED(a) && VALIGNED(b) && VALIGNED(ab)); + ar0 = ((v4sf_union*)va)[0].f[0]; + ai0 = ((v4sf_union*)va)[1].f[0]; + br0 = ((v4sf_union*)vb)[0].f[0]; + bi0 = ((v4sf_union*)vb)[1].f[0]; + abr0 = ((v4sf_union*)vab)[0].f[0]; + abi0 = ((v4sf_union*)vab)[1].f[0]; + +#ifdef ZCONVOLVE_USING_INLINE_ASM // inline asm version, unfortunately miscompiled by clang 3.2, at least on ubuntu.. so this will be restricted to gcc + const float *a_ = a, *b_ = b; float *ab_ = ab; + int N = Ncvec; + asm volatile("mov r8, %2 \n" + "vdup.f32 q15, %4 \n" + "1: \n" + "pld [%0,#64] \n" + "pld [%1,#64] \n" + "pld [%2,#64] \n" + "pld [%0,#96] \n" + "pld [%1,#96] \n" + "pld [%2,#96] \n" + "vld1.f32 {q0,q1}, [%0,:128]! \n" + "vld1.f32 {q4,q5}, [%1,:128]! \n" + "vld1.f32 {q2,q3}, [%0,:128]! \n" + "vld1.f32 {q6,q7}, [%1,:128]! \n" + "vld1.f32 {q8,q9}, [r8,:128]! \n" + + "vmul.f32 q10, q0, q4 \n" + "vmul.f32 q11, q0, q5 \n" + "vmul.f32 q12, q2, q6 \n" + "vmul.f32 q13, q2, q7 \n" + "vmls.f32 q10, q1, q5 \n" + "vmla.f32 q11, q1, q4 \n" + "vld1.f32 {q0,q1}, [r8,:128]! \n" + "vmls.f32 q12, q3, q7 \n" + "vmla.f32 q13, q3, q6 \n" + "vmla.f32 q8, q10, q15 \n" + "vmla.f32 q9, q11, q15 \n" + "vmla.f32 q0, q12, q15 \n" + "vmla.f32 q1, q13, q15 \n" + "vst1.f32 {q8,q9},[%2,:128]! \n" + "vst1.f32 {q0,q1},[%2,:128]! \n" + "subs %3, #2 \n" + "bne 1b \n" + : "+r"(a_), "+r"(b_), "+r"(ab_), "+r"(N) : "r"(scaling) : "r8", "q0","q1","q2","q3","q4","q5","q6","q7","q8","q9", "q10","q11","q12","q13","q15","memory"); +#else // default routine, works fine for non-arm cpus with current compilers + for (i=0; i < Ncvec; i += 2) { + v4sf ar, ai, br, bi; + ar = va[2*i+0]; ai = va[2*i+1]; + br = vb[2*i+0]; bi = vb[2*i+1]; + VCPLXMUL(ar, ai, br, bi); + vab[2*i+0] = VMADD(ar, vscal, vab[2*i+0]); + vab[2*i+1] = VMADD(ai, vscal, vab[2*i+1]); + ar = va[2*i+2]; ai = va[2*i+3]; + br = vb[2*i+2]; bi = vb[2*i+3]; + VCPLXMUL(ar, ai, br, bi); + vab[2*i+2] = VMADD(ar, vscal, vab[2*i+2]); + vab[2*i+3] = VMADD(ai, vscal, vab[2*i+3]); + } +#endif + if (s->transform == PFFFT_REAL) { + ((v4sf_union*)vab)[0].f[0] = abr0 + ar0*br0*scaling; + ((v4sf_union*)vab)[1].f[0] = abi0 + ai0*bi0*scaling; + } +} + + +#else // defined(PFFFT_SIMD_DISABLE) + +// standard routine using scalar floats, without SIMD stuff. + +#define pffft_zreorder_nosimd pffft_zreorder +void pffft_zreorder_nosimd(PFFFT_Setup *setup, const float *in, float *out, pffft_direction_t direction) { + int k, N = setup->N; + if (setup->transform == PFFFT_COMPLEX) { + for (k=0; k < 2*N; ++k) out[k] = in[k]; + return; + } + else if (direction == PFFFT_FORWARD) { + float x_N = in[N-1]; + for (k=N-1; k > 1; --k) out[k] = in[k-1]; + out[0] = in[0]; + out[1] = x_N; + } else { + float x_N = in[1]; + for (k=1; k < N-1; ++k) out[k] = in[k+1]; + out[0] = in[0]; + out[N-1] = x_N; + } +} + +#define pffft_transform_internal_nosimd pffft_transform_internal +void pffft_transform_internal_nosimd(PFFFT_Setup *setup, const float *input, float *output, float *scratch, + pffft_direction_t direction, int ordered) { + int Ncvec = setup->Ncvec; + int nf_odd = (setup->ifac[1] & 1); + + // temporary buffer is allocated on the stack if the scratch pointer is NULL + int stack_allocate = (scratch == 0 ? Ncvec*2 : 1); + VLA_ARRAY_ON_STACK(v4sf, scratch_on_stack, stack_allocate); + float *buff[2]; + int ib; + if (scratch == 0) scratch = scratch_on_stack; + buff[0] = output; buff[1] = scratch; + + if (setup->transform == PFFFT_COMPLEX) ordered = 0; // it is always ordered. + ib = (nf_odd ^ ordered ? 1 : 0); + + if (direction == PFFFT_FORWARD) { + if (setup->transform == PFFFT_REAL) { + ib = (rfftf1_ps(Ncvec*2, input, buff[ib], buff[!ib], + setup->twiddle, &setup->ifac[0]) == buff[0] ? 0 : 1); + } else { + ib = (cfftf1_ps(Ncvec, input, buff[ib], buff[!ib], + setup->twiddle, &setup->ifac[0], -1) == buff[0] ? 0 : 1); + } + if (ordered) { + pffft_zreorder(setup, buff[ib], buff[!ib], PFFFT_FORWARD); ib = !ib; + } + } else { + if (input == buff[ib]) { + ib = !ib; // may happen when finput == foutput + } + if (ordered) { + pffft_zreorder(setup, input, buff[!ib], PFFFT_BACKWARD); + input = buff[!ib]; + } + if (setup->transform == PFFFT_REAL) { + ib = (rfftb1_ps(Ncvec*2, input, buff[ib], buff[!ib], + setup->twiddle, &setup->ifac[0]) == buff[0] ? 0 : 1); + } else { + ib = (cfftf1_ps(Ncvec, input, buff[ib], buff[!ib], + setup->twiddle, &setup->ifac[0], +1) == buff[0] ? 0 : 1); + } + } + if (buff[ib] != output) { + int k; + // extra copy required -- this situation should happens only when finput == foutput + assert(input==output); + for (k=0; k < Ncvec; ++k) { + float a = buff[ib][2*k], b = buff[ib][2*k+1]; + output[2*k] = a; output[2*k+1] = b; + } + ib = !ib; + } + assert(buff[ib] == output); +} + +#define pffft_zconvolve_accumulate_nosimd pffft_zconvolve_accumulate +void pffft_zconvolve_accumulate_nosimd(PFFFT_Setup *s, const float *a, const float *b, + float *ab, float scaling) { + int i, Ncvec = s->Ncvec; + + if (s->transform == PFFFT_REAL) { + // take care of the fftpack ordering + ab[0] += a[0]*b[0]*scaling; + ab[2*Ncvec-1] += a[2*Ncvec-1]*b[2*Ncvec-1]*scaling; + ++ab; ++a; ++b; --Ncvec; + } + for (i=0; i < Ncvec; ++i) { + float ar, ai, br, bi; + ar = a[2*i+0]; ai = a[2*i+1]; + br = b[2*i+0]; bi = b[2*i+1]; + VCPLXMUL(ar, ai, br, bi); + ab[2*i+0] += ar*scaling; + ab[2*i+1] += ai*scaling; + } +} + +#endif // defined(PFFFT_SIMD_DISABLE) + +void pffft_transform(PFFFT_Setup *setup, const float *input, float *output, float *work, pffft_direction_t direction) { + pffft_transform_internal(setup, input, output, (v4sf*)work, direction, 0); +} + +void pffft_transform_ordered(PFFFT_Setup *setup, const float *input, float *output, float *work, pffft_direction_t direction) { + pffft_transform_internal(setup, input, output, (v4sf*)work, direction, 1); +} diff --git a/third-party/pffft/pffft.h b/third-party/pffft/pffft.h new file mode 100644 index 00000000..5db3d113 --- /dev/null +++ b/third-party/pffft/pffft.h @@ -0,0 +1,177 @@ +/* Copyright (c) 2013 Julien Pommier ( pommier@modartt.com ) + + Based on original fortran 77 code from FFTPACKv4 from NETLIB, + authored by Dr Paul Swarztrauber of NCAR, in 1985. + + As confirmed by the NCAR fftpack software curators, the following + FFTPACKv5 license applies to FFTPACKv4 sources. My changes are + released under the same terms. + + FFTPACK license: + + http://www.cisl.ucar.edu/css/software/fftpack5/ftpk.html + + Copyright (c) 2004 the University Corporation for Atmospheric + Research ("UCAR"). All rights reserved. Developed by NCAR's + Computational and Information Systems Laboratory, UCAR, + www.cisl.ucar.edu. + + Redistribution and use of the Software in source and binary forms, + with or without modification, is permitted provided that the + following conditions are met: + + - Neither the names of NCAR's Computational and Information Systems + Laboratory, the University Corporation for Atmospheric Research, + nor the names of its sponsors or contributors may be used to + endorse or promote products derived from this Software without + specific prior written permission. + + - Redistributions of source code must retain the above copyright + notices, this list of conditions, and the disclaimer below. + + - Redistributions in binary form must reproduce the above copyright + notice, this list of conditions, and the disclaimer below in the + documentation and/or other materials provided with the + distribution. + + THIS SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + EXPRESS OR IMPLIED, INCLUDING, BUT NOT LIMITED TO THE WARRANTIES OF + MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + NONINFRINGEMENT. IN NO EVENT SHALL THE CONTRIBUTORS OR COPYRIGHT + HOLDERS BE LIABLE FOR ANY CLAIM, INDIRECT, INCIDENTAL, SPECIAL, + EXEMPLARY, OR CONSEQUENTIAL DAMAGES OR OTHER LIABILITY, WHETHER IN AN + ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS WITH THE + SOFTWARE. +*/ + +/* + PFFFT : a Pretty Fast FFT. + + This is basically an adaptation of the single precision fftpack + (v4) as found on netlib taking advantage of SIMD instruction found + on cpus such as intel x86 (SSE1), powerpc (Altivec), and arm (NEON). + + For architectures where no SIMD instruction is available, the code + falls back to a scalar version. + + Restrictions: + + - 1D transforms only, with 32-bit single precision. + + - supports only transforms for inputs of length N of the form + N=(2^a)*(3^b)*(5^c), a >= 5, b >=0, c >= 0 (32, 48, 64, 96, 128, + 144, 160, etc are all acceptable lengths). Performance is best for + 128<=N<=8192. + + - all (float*) pointers in the functions below are expected to + have an "simd-compatible" alignment, that is 16 bytes on x86 and + powerpc CPUs. + + You can allocate such buffers with the functions + pffft_aligned_malloc / pffft_aligned_free (or with stuff like + posix_memalign..) + +*/ + +#ifndef PFFFT_H +#define PFFFT_H + +#include // for size_t + +#ifdef __cplusplus +extern "C" { +#endif + + /* opaque struct holding internal stuff (precomputed twiddle factors) + this struct can be shared by many threads as it contains only + read-only data. + */ + typedef struct PFFFT_Setup PFFFT_Setup; + + /* direction of the transform */ + typedef enum { PFFFT_FORWARD, PFFFT_BACKWARD } pffft_direction_t; + + /* type of transform */ + typedef enum { PFFFT_REAL, PFFFT_COMPLEX } pffft_transform_t; + + /* + prepare for performing transforms of size N -- the returned + PFFFT_Setup structure is read-only so it can safely be shared by + multiple concurrent threads. + */ + PFFFT_Setup *pffft_new_setup(int N, pffft_transform_t transform); + void pffft_destroy_setup(PFFFT_Setup *); + /* + Perform a Fourier transform , The z-domain data is stored in the + most efficient order for transforming it back, or using it for + convolution. If you need to have its content sorted in the + "usual" way, that is as an array of interleaved complex numbers, + either use pffft_transform_ordered , or call pffft_zreorder after + the forward fft, and before the backward fft. + + Transforms are not scaled: PFFFT_BACKWARD(PFFFT_FORWARD(x)) = N*x. + Typically you will want to scale the backward transform by 1/N. + + The 'work' pointer should point to an area of N (2*N for complex + fft) floats, properly aligned. If 'work' is NULL, then stack will + be used instead (this is probably the best strategy for small + FFTs, say for N < 16384). + + input and output may alias. + */ + void pffft_transform(PFFFT_Setup *setup, const float *input, float *output, float *work, pffft_direction_t direction); + + /* + Similar to pffft_transform, but makes sure that the output is + ordered as expected (interleaved complex numbers). This is + similar to calling pffft_transform and then pffft_zreorder. + + input and output may alias. + */ + void pffft_transform_ordered(PFFFT_Setup *setup, const float *input, float *output, float *work, pffft_direction_t direction); + + /* + call pffft_zreorder(.., PFFFT_FORWARD) after pffft_transform(..., + PFFFT_FORWARD) if you want to have the frequency components in + the correct "canonical" order, as interleaved complex numbers. + + (for real transforms, both 0-frequency and half frequency + components, which are real, are assembled in the first entry as + F(0)+i*F(n/2+1). Note that the original fftpack did place + F(n/2+1) at the end of the arrays). + + input and output should not alias. + */ + void pffft_zreorder(PFFFT_Setup *setup, const float *input, float *output, pffft_direction_t direction); + + /* + Perform a multiplication of the frequency components of dft_a and + dft_b and accumulate them into dft_ab. The arrays should have + been obtained with pffft_transform(.., PFFFT_FORWARD) and should + *not* have been reordered with pffft_zreorder (otherwise just + perform the operation yourself as the dft coefs are stored as + interleaved complex numbers). + + the operation performed is: dft_ab += (dft_a * fdt_b)*scaling + + The dft_a, dft_b and dft_ab pointers may alias. + */ + void pffft_zconvolve_accumulate(PFFFT_Setup *setup, const float *dft_a, const float *dft_b, float *dft_ab, float scaling); + + /* + the float buffers must have the correct alignment (16-byte boundary + on intel and powerpc). This function may be used to obtain such + correctly aligned buffers. + */ + void *pffft_aligned_malloc(size_t nb_bytes); + void pffft_aligned_free(void *); + + /* return 4 or 1 wether support SSE/Altivec instructions was enable when building pffft.c */ + int pffft_simd_size(void); + +#ifdef __cplusplus +} +#endif + +#endif // PFFFT_H