diff --git a/gz_waves_provider_fft/CMakeLists.txt b/gz_waves_provider_fft/CMakeLists.txt new file mode 100644 index 00000000..c5e81a80 --- /dev/null +++ b/gz_waves_provider_fft/CMakeLists.txt @@ -0,0 +1,134 @@ +cmake_minimum_required(VERSION 3.16) +project(gz_waves_provider_fft) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +# gz_waves's exported library links gz-sim PUBLIC, so gz-sim::core must be +# resolvable before find_package(gz_waves). The core also provides +# WavesSystemBase, the base for the fft system plugin below. +find_package(gz_sim_vendor REQUIRED) +find_package(gz-sim REQUIRED) +find_package(gz_waves REQUIRED) # IWaveField + WaveParameters + base +find_package(gz_plugin_vendor REQUIRED) +find_package(gz-plugin REQUIRED COMPONENTS register) +find_package(gz_math_vendor REQUIRED) +find_package(gz-math REQUIRED) # gz::math::Vector3d in the interface +find_package(gz_common_vendor REQUIRED) +find_package(gz-common REQUIRED) # gzerr/gzmsg console +# FFTWaveSimulation's public header returns Eigen::MatrixXd grids, so Eigen is +# a direct dependency. +find_package(Eigen3 REQUIRED) +# Apache-2.0 Horvath spectrum library — REQUIRED, consumed as a regular system +# package (installed from HonuRobotics/encinowaves, not vendored in-tree). The +# fft engine is the EncinoWaves spectral synthesizer (TMA/JONSWAP/PM spectra + +# directional spreading + dispersion), selected via +# //. Its CMake config transitively pulls in +# Eigen3 / TBB / Imath. +find_package(EncinoWaves REQUIRED) + +# -------------------------------------------------------------------------- +# FFT wave-field engine (libgz-waves-provider-fft): a plain IWaveField +# implementation backed by the EncinoWaves spectral library. +# It is LINKED directly by the fft system plugin (server) and the water visual +# (GUI), not discovered by a runtime loader, so it carries no GZ_ADD_PLUGIN and +# is exported as a normal library target. MakeFFTWaveField() is the factory the +# consumers register under the "fft" token. +# -------------------------------------------------------------------------- +add_library(gz-waves-provider-fft SHARED + src/FFTWaveSimulation.cc +) +target_include_directories(gz-waves-provider-fft PUBLIC + $ + $) +target_include_directories(gz-waves-provider-fft PRIVATE + ${gz_waves_INCLUDE_DIRS}) +# Eigen is PUBLIC: the engine's public header returns Eigen::MatrixXd grids, so +# anything that includes it (the system plugin, the water visual) needs Eigen. +target_link_libraries(gz-waves-provider-fft + PUBLIC Eigen3::Eigen gz-math::gz-math + PRIVATE EncinoWaves::EncinoWaves gz-common::gz-common) + +# -------------------------------------------------------------------------- +# FFT wave source system (gz-sim-waves-fft-system): a WavesSystemBase subclass +# that builds the FFT engine. THIS is the gz-plugin SDF loads by filename; it +# links the engine directly, so there is no runtime engine loader on the server. +# -------------------------------------------------------------------------- +add_library(gz-sim-waves-fft-system SHARED + src/FftWavesSystem.cc +) +target_include_directories(gz-sim-waves-fft-system PRIVATE + $ + ${gz_waves_INCLUDE_DIRS}) +target_link_libraries(gz-sim-waves-fft-system + PRIVATE + gz_waves::gz_waves + gz-waves-provider-fft + gz-sim::core + gz-plugin::register) + +# -------------------------------------------------------------------------- +# FFT GUI registrar (gz-sim-waves-fft-gui): loaded in the GUI process alongside +# WaterVisual. It registers the "fft" factory so the visual can rebuild its own +# thread-private engine from the replicated recipe via CreateWaveSimulation. +# This is what lets gz_waves_rendering stay provider-agnostic (core-only): the +# engine dependency lives here, the GUI-side mirror of gz-sim-waves-fft-system. +# -------------------------------------------------------------------------- +add_library(gz-sim-waves-fft-gui SHARED + src/FftWavesGui.cc +) +target_include_directories(gz-sim-waves-fft-gui PRIVATE + $ + ${gz_waves_INCLUDE_DIRS}) +target_link_libraries(gz-sim-waves-fft-gui + PRIVATE + gz_waves::gz_waves + gz-waves-provider-fft + gz-sim::core + gz-plugin::register) + +install( + TARGETS gz-waves-provider-fft + EXPORT export_gz_waves_provider_fft + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) +install( + TARGETS gz-sim-waves-fft-system gz-sim-waves-fft-gui + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(DIRECTORY include/ DESTINATION include) + +ament_export_targets(export_gz_waves_provider_fft HAS_LIBRARY_TARGET) +ament_export_include_directories(include) +ament_export_dependencies(gz_math_vendor Eigen3) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + # Links the engine directly and registers its "fft" factory in-binary (see + # kFftRegistered in the test), so the CreateWaveSimulation tests resolve via + # the registry — no dlopen, no GzPluginHook. + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(fft_test test/fft_test.cc) + target_include_directories(fft_test PRIVATE + ${gz_waves_INCLUDE_DIRS}) + target_link_libraries(fft_test + gz_waves::gz_waves gz-waves-provider-fft) +endif() + +ament_package() diff --git a/gz_waves_provider_fft/include/gz/sim/waves/FFTWaveSimulation.hh b/gz_waves_provider_fft/include/gz/sim/waves/FFTWaveSimulation.hh new file mode 100644 index 00000000..48c27c48 --- /dev/null +++ b/gz_waves_provider_fft/include/gz/sim/waves/FFTWaveSimulation.hh @@ -0,0 +1,189 @@ +/* + * Copyright (C) 2026 Honu Robotics + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + */ + +#ifndef GZ_SIM_WAVES_FFTWAVESIMULATION_HH_ +#define GZ_SIM_WAVES_FFTWAVESIMULATION_HH_ + +#include +#include +#include + +#include + +#include "gz/sim/waves/WaveSimulation.hh" + +namespace gz::sim::waves +{ + +struct WaveParameters; + +/// \brief Stochastic FFT-based wave field engine backed by the Apache-2.0 EncinoWaves +/// spectral library (Horvath 2015): the inverse 2D FFT of an empirically +/// modelled directional spectrum (TMA/JONSWAP/PM + directional spreading + +/// dispersion), selected via the // params. +/// +/// The output is a periodic tile of size `tileSize × tileSize` metres. +/// Queries outside the tile are wrapped via `fmod`. Each call to `Update` +/// regenerates the grid for that time; per-point queries (`Elevation`, +/// `ParticleVelocity`, ...) bilinear-sample the stored grid. +class FFTWaveSimulation final : public IWaveField +{ + /// \brief Default-construct an unconfigured field. Call `SetParameters` + /// before `Update`/sampling. Used by the engine factory (`MakeFFTWaveField`). + public: FFTWaveSimulation(); + + /// \brief Construct from spectrum / wind parameters. + /// \param[in] _params Wave parameters; uses `direction` as the wind heading + /// and derives wind speed from `period` (deep-water PMS relation: + /// V19 ≈ 0.879·g/omegaP). `gain` scales spectrum amplitudes uniformly. + /// \param[in] _tileSize Physical tile extent in metres; the wave field is + /// periodic with this period along both x and y. + /// \param[in] _gridSize Resolution per axis (must be a power of two for + /// the FFT path; 64, 128, 256 typical). + /// \param[in] _seed RNG seed for the Gaussian-distributed amplitudes; same + /// seed → same wave field bit-for-bit across runs. + public: FFTWaveSimulation(const WaveParameters &_params, + double _tileSize, + std::size_t _gridSize, + std::uint32_t _seed); + + /// \brief Destructor (out-of-line so the EncinoState pimpl stays in the .cc). + public: ~FFTWaveSimulation() override; + + // Documentation inherited + public: void SetParameters(const WaveParameters &_params) override; + // Documentation inherited + public: double Elevation(double _x, double _y, double _t) const override; + // Documentation inherited + public: gz::math::Vector3d ParticleVelocity( + double _x, double _y, double _t) const override; + // Documentation inherited + public: gz::math::Vector3d Normal( + double _x, double _y, double _t) const override; + // Documentation inherited + public: double Jacobian(double _x, double _y, double _t) const override; + // Documentation inherited + public: void Update(double _simTime) override; + // Documentation inherited + public: std::string_view Kind() const override { return "fft"; } + // Documentation inherited + public: std::optional Bounds() const override + { + return TileSize{this->tileSize, this->tileSize}; + } + // Documentation inherited + public: const WaveField2D *Field() const override; + + // ---- Accessors for unit tests + visual heightmap upload ---------------- + + /// \brief Grid resolution per axis (power of two). + public: std::size_t GridSize() const { return this->gridSize; } + /// \brief Physical tile extent per axis [m]; the field is periodic with it. + public: double TileSizeMeters() const { return this->tileSize; } + + /// \brief Surface elevation field η(x, y, t), refreshed by Update(). + public: const Eigen::MatrixXd &HeightGrid() const + { + return this->heightGrid; + } + /// \brief Horizontal x-displacement field Dx(x, y, t), refreshed by Update(). + /// Multiplied by a "choppiness" factor in the visual shader to sharpen + /// crests (Tessendorf 2001, eq. 29). Same grid layout as `HeightGrid()`. + public: const Eigen::MatrixXd &DispXGrid() const + { + return this->dispXGrid; + } + /// \brief Horizontal y-displacement field Dy(x, y, t). + public: const Eigen::MatrixXd &DispYGrid() const + { + return this->dispYGrid; + } + + /// \brief Bilinear sample of `_grid` at the given world (x, y), wrapping + /// queries outside the tile to its periodic image. + /// \param[in] _grid The grid to sample. + /// \param[in] _x World x coordinate [m]. + /// \param[in] _y World y coordinate [m]. + /// \return The bilinearly-interpolated grid value. + private: double BilinearSample(const Eigen::MatrixXd &_grid, + double _x, double _y) const; + + // Configuration. Default member-inits keep a default-constructed instance + // benign until SetParameters() runs (the factory path). + + /// \brief Physical tile extent per axis [m]. + private: double tileSize{200.0}; + /// \brief Grid resolution per axis (power of two). + private: std::size_t gridSize{128}; + /// \brief Wind speed [m/s], derived from `period`. + private: double windSpeed{0.0}; + /// \brief Spectrum amplitude gain. + private: double gain{1.0}; + /// \brief Startup-ramp time constant τ [s]. + private: double tau{2.0}; + + /// \brief Surface elevation grid η (real domain), regenerated by Update(). + private: Eigen::MatrixXd heightGrid; + /// \brief Horizontal x-displacement grid Dx (Tessendorf eq. 29); used by the + /// visual shader for choppy, asymmetric crests. + private: Eigen::MatrixXd dispXGrid; + /// \brief Horizontal y-displacement grid Dy. + private: Eigen::MatrixXd dispYGrid; + /// \brief Folding/whitecap metric (Encino path): per-cell minimum eigenvalue + /// of the displacement Jacobian, 1 = flat. Sampled by Jacobian() → FoamMask(). + private: Eigen::MatrixXd minEGrid; + + /// \brief Water-particle velocity grids [m/s] — the Eulerian time derivative + /// of the displacement field (∂Dx/∂t, ∂Dy/∂t, ∂η/∂t), computed each Update by + /// finite-differencing a scratch propagation at t+dt. Sampled by + /// ParticleVelocity(). + private: Eigen::MatrixXd velXGrid; + /// \brief Water-particle velocity grid, y component [m/s]. + private: Eigen::MatrixXd velYGrid; + /// \brief Water-particle velocity grid, z (vertical) component [m/s]. + private: Eigen::MatrixXd velZGrid; + + /// \brief Column-major view into the grids above, returned by Field() as the + /// engine-agnostic rendering contract. Repopulated on each call from the + /// current grid data() — Update reassigns the grids, so a cached pointer + /// would dangle. Mutable because Field() is const. + private: mutable WaveField2D field; + + /// \brief Forward declaration of the EncinoWaves Update state. Defined + /// entirely in FFTWaveSimulation.cc so the vendored EncinoWaves headers don't + /// leak into this public include surface. + private: struct EncinoState; + /// \brief The EncinoWaves-backed Update state (pimpl). + private: std::unique_ptr encino; + + /// \brief Physics-based amplitude calibration for the Encino path. Encino's + /// intrinsic field variance is far larger than a physical sea state at our + /// wind speeds, so we measure its intrinsic RMS once at construction and + /// store the factor that rescales the significant wave height to the + /// fully-developed Pierson-Moskowitz relation (Hs = 0.21·V19.5²/g). Applied + /// to η/Dx/Dy every Update. + private: double encinoScale{1.0}; + + /// \brief Sim time of the last Update(). The field is deterministic in time, + /// so Update() short-circuits on a repeat call — letting several consumers + /// (the source system plus each WaveBuoyancy that advances its own instance) + /// converge on one FFT per tick. -1 = never updated (sim time is always ≥ 0). + private: double lastUpdateT{-1.0}; +}; + +/// \brief Factory: a default-constructed FFT wave-field engine (apply +/// `SetParameters` before use). Registered under the "fft" token so +/// `CreateWaveSimulation` can rebuild the engine from a serialized `Wavefield` +/// component on the GUI side. +std::shared_ptr MakeFFTWaveField(); + +} // namespace gz::sim::waves + +#endif // GZ_SIM_WAVES_FFTWAVESIMULATION_HH_ diff --git a/gz_waves_provider_fft/package.xml b/gz_waves_provider_fft/package.xml new file mode 100644 index 00000000..d3b1f1ac --- /dev/null +++ b/gz_waves_provider_fft/package.xml @@ -0,0 +1,40 @@ + + + + gz_waves_provider_fft + 0.0.0 + + Stochastic FFT wave-field engine for gz_waves, backed by the Apache-2.0 + EncinoWaves spectral library: TMA/JONSWAP/Pierson-Moskowitz spectra with + directional spreading and dispersion, selected via the spectrum, spreading + and dispersion SDF parameters. Provides the IWaveField engine library and + the gz-sim-waves-fft-system plugin. + + Carlos Agüero + Apache-2.0 + + ament_cmake + + gz_waves + gz_sim_vendor + gz_math_vendor + gz_common_vendor + gz_plugin_vendor + eigen + + libtbb-dev + libimath-dev + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/gz_waves_provider_fft/src/FFTWaveSimulation.cc b/gz_waves_provider_fft/src/FFTWaveSimulation.cc new file mode 100644 index 00000000..8dc99fd0 --- /dev/null +++ b/gz_waves_provider_fft/src/FFTWaveSimulation.cc @@ -0,0 +1,511 @@ +/* + * Copyright (C) 2026 Honu Robotics + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + */ + +#include "gz/sim/waves/FFTWaveSimulation.hh" + +#include +#include +#include + +#include + +#include "gz/sim/waves/Wavefield.hh" + +#include "EncinoWaves/All.h" + +namespace gz::sim::waves +{ + +namespace +{ +constexpr double k2Pi = 6.28318530717958647692; + +////////////////////////////////////////////////// +/// Returns the integer log2 of n if n is a positive power of two; -1 otherwise. +int Log2Pow2(std::size_t _n) +{ + if (_n == 0 || (_n & (_n - 1)) != 0) return -1; + int r = 0; + while ((static_cast(1u) << r) < _n) ++r; + return r; +} + +////////////////////////////////////////////////// +/// Smallest power of two >= n (>= 1). +std::size_t CeilPow2(std::size_t _n) +{ + std::size_t p = 1; + while (p < _n) p <<= 1; + return p; +} + +////////////////////////////////////////////////// +// ---- Human-readable names for the EncinoWaves model enums (logging) -------- +const char *SpectrumName(EncinoWaves::SpectrumType _t) +{ + switch (_t) + { + case EncinoWaves::kPiersonMoskowitzSpectrum: return "pms"; + case EncinoWaves::kJONSWAPSpectrum: return "jonswap"; + case EncinoWaves::kTMASpectrum: return "tma"; + default: return "?"; + } +} + +////////////////////////////////////////////////// +const char *DispersionName(EncinoWaves::DispersionType _t) +{ + switch (_t) + { + case EncinoWaves::kDeepDispersion: return "deep"; + case EncinoWaves::kFiniteDepthDispersion: return "finite"; + case EncinoWaves::kCapillaryDispersion: return "capillary"; + default: return "?"; + } +} + +////////////////////////////////////////////////// +const char *SpreadingName(EncinoWaves::DirectionalSpreadingType _t) +{ + switch (_t) + { + case EncinoWaves::kPosCosThetaSqrDirectionalSpreading: return "poscos2"; + case EncinoWaves::kMitsuyasuDirectionalSpreading: return "mitsuyasu"; + case EncinoWaves::kHasselmannDirectionalSpreading: return "hasselmann"; + case EncinoWaves::kDonelanBannerDirectionalSpreading: return "donelanbanner"; + default: return "?"; + } +} + +////////////////////////////////////////////////// +const char *FilterName(EncinoWaves::FilterType _t) +{ + switch (_t) + { + case EncinoWaves::kNullFilter: return "none"; + case EncinoWaves::kSmoothInvertibleBandPassFilter: return "bandpass"; + default: return "?"; + } +} + +////////////////////////////////////////////////// +// ---- // SDF strings -> EncinoWaves enums --- +// Names match the *Name() helpers above and the SDF tag values. Return false on +// an unrecognised value so the caller can warn and keep the Encino default. +bool SpectrumFromString(const std::string &_s, EncinoWaves::SpectrumType &_out) +{ + if (_s == "pms" || _s == "pm") _out = EncinoWaves::kPiersonMoskowitzSpectrum; + else if (_s == "jonswap") _out = EncinoWaves::kJONSWAPSpectrum; + else if (_s == "tma") _out = EncinoWaves::kTMASpectrum; + else return false; + return true; +} + +////////////////////////////////////////////////// +bool DispersionFromString(const std::string &_s, + EncinoWaves::DispersionType &_out) +{ + if (_s == "deep") _out = EncinoWaves::kDeepDispersion; + else if (_s == "finite" || _s == "finite_depth") + _out = EncinoWaves::kFiniteDepthDispersion; + else if (_s == "capillary") _out = EncinoWaves::kCapillaryDispersion; + else return false; + return true; +} + +////////////////////////////////////////////////// +bool SpreadingFromString(const std::string &_s, + EncinoWaves::DirectionalSpreadingType &_out) +{ + if (_s == "poscos2" || _s == "poscossqr") + _out = EncinoWaves::kPosCosThetaSqrDirectionalSpreading; + else if (_s == "mitsuyasu") + _out = EncinoWaves::kMitsuyasuDirectionalSpreading; + else if (_s == "hasselmann") + _out = EncinoWaves::kHasselmannDirectionalSpreading; + else if (_s == "donelanbanner" || _s == "donelan") + _out = EncinoWaves::kDonelanBannerDirectionalSpreading; + else return false; + return true; +} + +////////////////////////////////////////////////// +// Map the SDF spectrum selectors and numeric knobs onto `ep`. Unknown selector +// values warn and leave the Encino default in place. The numeric knobs default +// (via WaveParameters) to Encino's own defaults, so an SDF that sets none of +// them reproduces the stock Horvath "good ocean" config. +void ApplyEncinoParams(EncinoWaves::Parametersf &_ep, const WaveParameters &_p) +{ + if (!SpectrumFromString(_p.spectrum, _ep.spectrum.type)) + gzerr << "[FFTWaveSimulation] ignoring unknown ='" + << _p.spectrum << "' (want pms|jonswap|tma)" << '\n'; + if (!DispersionFromString(_p.dispersion, _ep.dispersion.type)) + gzerr << "[FFTWaveSimulation] ignoring unknown ='" + << _p.dispersion << "' (want deep|finite|capillary)" << '\n'; + if (!SpreadingFromString(_p.spreading, _ep.directionalSpreading.type)) + gzerr << "[FFTWaveSimulation] ignoring unknown ='" + << _p.spreading + << "' (want poscos2|mitsuyasu|hasselmann|donelanbanner)" + << '\n'; + + _ep.gravity = static_cast(_p.gravity); + _ep.depth = static_cast(_p.depth); + _ep.fetch = static_cast(_p.fetch); + _ep.directionalSpreading.swell = static_cast(_p.swell); + _ep.troughDamping = static_cast(_p.troughDamping); + + // Optional spectral band-pass over wavelength: keep wavelengths within + // [filter_min_wl, filter_max_wl] m and roll off outside over filter_soft. + // filter_min raises the suppression floor (0 = full cut); filter_invert turns + // the band into a notch. Enabled when a band edge is given. The amplitude + // calibration renormalises total energy, so the filter reshapes *which* + // wavelengths survive rather than the overall sea height. + if (_p.filterMinWavelength > 0.0 || _p.filterMaxWavelength > 0.0) + { + _ep.filter.type = EncinoWaves::kSmoothInvertibleBandPassFilter; + _ep.filter.smallWavelength = static_cast(_p.filterMinWavelength); + if (_p.filterMaxWavelength > 0.0) + _ep.filter.bigWavelength = static_cast(_p.filterMaxWavelength); + _ep.filter.min = static_cast(_p.filterMin); + _ep.filter.invert = _p.filterInvert; + // A zero soft width collapses the smoothstep edges (NaN spectrum); default + // a positive transition from the lower cutoff when none was supplied. + _ep.filter.softWidth = (_p.filterSoftWidth > 0.0) + ? static_cast(_p.filterSoftWidth) + : std::max(0.25f * _ep.filter.smallWavelength, 1.0f); + } +} + +} // namespace + +//----------------------------------------------------------------------------- +// EncinoState — pimpl that holds the vendored Horvath-spectrum library's +// per-instance state. Defined here (not in the header) so EncinoWaves headers +// stay out of the public include surface. +//----------------------------------------------------------------------------- +struct FFTWaveSimulation::EncinoState +{ + EncinoWaves::Parametersf params; + std::unique_ptr initial; + std::unique_ptr propagation; + std::unique_ptr state; + /// Scratch state propagated at t+dt to finite-difference particle velocity. + std::unique_ptr scratch; +}; + +FFTWaveSimulation::~FFTWaveSimulation() = default; + +FFTWaveSimulation::FFTWaveSimulation() = default; + +////////////////////////////////////////////////// +FFTWaveSimulation::FFTWaveSimulation(const WaveParameters &_p, + double _tile, + std::size_t _grid, + std::uint32_t _seed) +{ + // The constructor's tile/grid/seed arguments override whatever the params + // struct carries (callers and tests rely on this). Fold them into a params + // copy and route through the single SetParameters setup path. + WaveParameters q = _p; + q.tileSize = _tile; + q.gridSize = _grid; + q.seed = _seed; + this->SetParameters(q); +} + +////////////////////////////////////////////////// +void FFTWaveSimulation::SetParameters(const WaveParameters &_params) +{ + // Resolve (if set) into period/gain before configuring. + const WaveParameters p = WithSeaState(_params); + this->tileSize = p.tileSize; + this->gridSize = p.gridSize; + this->gain = p.gain; + this->tau = p.tau; + const std::uint32_t seed = p.seed; + + // EncinoWaves requires a power-of-two grid; round up if the SDF asks for + // something else. + if (Log2Pow2(this->gridSize) < 0) + { + const std::size_t adjusted = CeilPow2(this->gridSize); + gzerr << "[FFTWaveSimulation] grid_size=" << this->gridSize + << " is not a power of two (required by the FFT spectrum); using " + << adjusted << '\n'; + this->gridSize = adjusted; + } + + // PMS relation: peak omega <-> wind speed at 19.5 m. period -> omegaP -> V19. + const double omegaP = k2Pi / p.period; + this->windSpeed = 0.879 * p.gravity / omegaP; + + const int N = static_cast(this->gridSize); + this->heightGrid = Eigen::MatrixXd::Zero(N, N); + this->dispXGrid = Eigen::MatrixXd::Zero(N, N); + this->dispYGrid = Eigen::MatrixXd::Zero(N, N); + this->minEGrid = Eigen::MatrixXd::Constant(N, N, 1.0); // 1 = no foam + this->velXGrid = Eigen::MatrixXd::Zero(N, N); + this->velYGrid = Eigen::MatrixXd::Zero(N, N); + this->velZGrid = Eigen::MatrixXd::Zero(N, N); + + // Build the EncinoWaves spectral state -- the spectral engine the fft system + // is built on. The // SDF selectors choose + // the spectral models (default TMA + Hasselmann + capillary). + this->encino = std::make_unique(); + auto &ep = this->encino->params; + ep.resolutionPowerOfTwo = Log2Pow2(this->gridSize); + ep.domain = static_cast(this->tileSize); + ep.windSpeed = static_cast(this->windSpeed); + ep.amplitudeGain = static_cast(this->gain); + ep.random.seed = static_cast(seed); + + // Map the SDF spectrum selectors + numeric/filter knobs onto Encino. + ApplyEncinoParams(ep, p); + + this->encino->initial = + std::make_unique(ep); + this->encino->propagation = + std::make_unique(ep, /*nthreads=*/-1); + this->encino->state = + std::make_unique(ep); + this->encino->scratch = + std::make_unique(ep); + + // --- Physics-based amplitude calibration ------------------------------- + // EncinoWaves' amplitudeGain only feeds its (here unused) normal computation, + // not the height field, and its intrinsic variance does not correspond to a + // physical sea state at our wind speeds. Measure the intrinsic RMS once (one + // propagation past the ramp) and rescale so the significant wave height + // follows the standard fully-developed Pierson-Moskowitz relation + // Hs = 0.21 * V19.5^2 / g, i.e. sigma = Hs/4. The selected spectrum still + // sets the spectral *shape*; this only fixes the overall energy. + { + using RowMatF = Eigen::Matrix; + this->encino->propagation->propagate( + this->encino->params, *this->encino->initial, + *this->encino->state, 10.0f); + const int M = ep.resolution(); + const double sigmaEncino = std::sqrt( + Eigen::Map(this->encino->state->Height.cdata(), + M, M).cast().array() + .square().mean()); + const double sigmaTarget = + 0.21 / (4.0 * p.gravity) * this->windSpeed * this->windSpeed; + this->encinoScale = + (sigmaEncino > 1e-9) ? (sigmaTarget / sigmaEncino) : 1.0; + } + + gzmsg << "[FFTWaveSimulation] EncinoWaves spectrum library active " + << "(res=" << ep.resolution() << " domain=" << ep.domain + << "m wind=" << ep.windSpeed << "m/s seed=" << ep.random.seed + << " spectrum=" << SpectrumName(ep.spectrum.type) + << " dispersion=" << DispersionName(ep.dispersion.type) + << " spreading=" << SpreadingName(ep.directionalSpreading.type) + << " depth=" << ep.depth << "m fetch=" << ep.fetch << "km" + << " swell=" << ep.directionalSpreading.swell + << " troughDamp=" << ep.troughDamping + << " filter=" << FilterName(ep.filter.type) + << " ampCalib=" << this->encinoScale + << " targetHs=" << (4.0 * 0.21 / (4.0 * p.gravity) * + this->windSpeed * this->windSpeed) + << "m)" << '\n'; + + this->Update(0.0); +} + +////////////////////////////////////////////////// +void FFTWaveSimulation::Update(double _simTime) +{ + // Idempotent: the field at a given time is deterministic, so a repeat call + // for the same time is a no-op. Lets the source system and any number of + // WaveBuoyancy consumers that advance the same instance to the same tick + // share a single recompute instead of each paying for a full propagation. + if (_simTime == this->lastUpdateT) + return; + this->lastUpdateT = _simTime; + + if (!this->encino || !this->encino->propagation) + return; // default-constructed, not yet configured + + const int N = static_cast(this->gridSize); + + this->encino->propagation->propagate( + this->encino->params, + *this->encino->initial, + *this->encino->state, + static_cast(_simTime)); + + // Combined output scale per Update: + // * ramp — fade the field in over `tau`; + // * this->encinoScale — physics-based amplitude calibration to a PM sea state + // (Encino's amplitudeGain doesn't scale the height); + // * this->gain — the SDF user multiplier (a no-op via Encino's + // amplitudeGain, so we apply it here to make it work). + const double scale = + StartupRamp(_simTime, this->tau) * this->encinoScale * this->gain; + + // Encino stores its spatial fields row-major in float; our grids are + // column-major in double. Map+cast assignment lets Eigen vectorize the + // conversion + layout swap; the scale folds into the same expression. + using RowMatF = Eigen::Matrix; + this->heightGrid = Eigen::Map( + this->encino->state->Height.cdata(), N, N).cast() * scale; + this->dispXGrid = Eigen::Map( + this->encino->state->Dx.cdata(), N, N).cast() * scale; + this->dispYGrid = Eigen::Map( + this->encino->state->Dy.cdata(), N, N).cast() * scale; + + // Foam: Encino computes MinE = -(min eigenvalue of the displacement + // Jacobian) at its internal amplitude. At our calibrated amplitude the + // minimum eigenvalue is 1 - scale*(MinE + 1). Jacobian() bilinear-samples + // this; Eval::FoamMask turns values below its threshold into whitecaps. At + // t=0 (scale=0) the surface is flat -> eigenvalue 1 -> no foam. + this->minEGrid = (1.0 - scale * + (Eigen::Map(this->encino->state->MinE.cdata(), N, N) + .cast().array() + 1.0)).matrix(); + + // Particle velocity = Eulerian time derivative of the displacement field + // (∂Dx/∂t, ∂Dy/∂t, ∂η/∂t). EncinoWaves exposes no velocity field and no + // public spectral coefficients, so finite-difference a scratch propagation a + // small step ahead. The same amplitude `scale` is applied to both samples, so + // it factors out as the wave motion's velocity (the startup ramp's own + // d/dt is intentionally excluded — it's a transient, not water motion). + constexpr double kVelDt = 0.05; // [s] forward-difference step + this->encino->propagation->propagate( + this->encino->params, + *this->encino->initial, + *this->encino->scratch, + static_cast(_simTime + kVelDt)); + const double velK = scale / kVelDt; + this->velZGrid = velK * + (Eigen::Map(this->encino->scratch->Height.cdata(), N, N) + - Eigen::Map(this->encino->state->Height.cdata(), N, N)) + .cast(); + this->velXGrid = velK * + (Eigen::Map(this->encino->scratch->Dx.cdata(), N, N) + - Eigen::Map(this->encino->state->Dx.cdata(), N, N)) + .cast(); + this->velYGrid = velK * + (Eigen::Map(this->encino->scratch->Dy.cdata(), N, N) + - Eigen::Map(this->encino->state->Dy.cdata(), N, N)) + .cast(); +} + +////////////////////////////////////////////////// +double FFTWaveSimulation::BilinearSample(const Eigen::MatrixXd &_grid, + double _x, double _y) const +{ + const int N = static_cast(this->gridSize); + const double L = this->tileSize; + + // Wrap query into [0, L). Grid cell (i, j) corresponds to world position + // (i·L/N, j·L/N) — the IFFT output's natural layout, no centring shift. + auto wrap = [L](double v) { + double r = std::fmod(v, L); + if (r < 0.0) r += L; + return r; + }; + const double wx = wrap(_x); + const double wy = wrap(_y); + + // Grid index in [0, N). + const double fi = wx / L * N; + const double fj = wy / L * N; + int i0 = static_cast(std::floor(fi)) % N; + int j0 = static_cast(std::floor(fj)) % N; + if (i0 < 0) i0 += N; + if (j0 < 0) j0 += N; + const int i1 = (i0 + 1) % N; + const int j1 = (j0 + 1) % N; + const double fx = fi - std::floor(fi); + const double fy = fj - std::floor(fj); + + return _grid(i0, j0) * (1 - fx) * (1 - fy) + + _grid(i1, j0) * fx * (1 - fy) + + _grid(i0, j1) * (1 - fx) * fy + + _grid(i1, j1) * fx * fy; +} + +////////////////////////////////////////////////// +double FFTWaveSimulation::Elevation(double _x, double _y, double /*_t*/) const +{ + // Caller is expected to have called Update(t) ≤ this tick (the source + // system does this in PreUpdate). The bilinear sample is time-free. + return this->BilinearSample(this->heightGrid, _x, _y); +} + +////////////////////////////////////////////////// +gz::math::Vector3d FFTWaveSimulation::ParticleVelocity( + double _x, double _y, double /*_t*/) const +{ + // Bilinear-sample the velocity grids that Update() finite-differenced from the + // displacement field. Caller is expected to have called Update(t) ≤ this tick. + return gz::math::Vector3d( + this->BilinearSample(this->velXGrid, _x, _y), + this->BilinearSample(this->velYGrid, _x, _y), + this->BilinearSample(this->velZGrid, _x, _y)); +} + +////////////////////////////////////////////////// +gz::math::Vector3d FFTWaveSimulation::Normal( + double _x, double _y, double /*_t*/) const +{ + // Finite-difference normal from the height grid (a derivative-grid path via + // FFT would be more accurate but is not yet wired up). + const double L = this->tileSize; + const double h = L / static_cast(this->gridSize); + const double dx = + (this->BilinearSample(this->heightGrid, _x + h, _y) - + this->BilinearSample(this->heightGrid, _x - h, _y)) / (2.0 * h); + const double dy = + (this->BilinearSample(this->heightGrid, _x, _y + h) - + this->BilinearSample(this->heightGrid, _x, _y - h)) / (2.0 * h); + gz::math::Vector3d n(-dx, -dy, 1.0); + n.Normalize(); + return n; +} + +////////////////////////////////////////////////// +double FFTWaveSimulation::Jacobian(double _x, double _y, double /*_t*/) const +{ + // Bilinear-sample the per-cell minimum eigenvalue of the displacement + // Jacobian (1 = flat, < 1 → folding). Eval::FoamMask turns values below its + // threshold into whitecap intensity, so foam appears on the pinched crests. + return this->BilinearSample(this->minEGrid, _x, _y); +} + +////////////////////////////////////////////////// +const WaveField2D *FFTWaveSimulation::Field() const +{ + // Repopulate the view from the current grids each call: Update reallocates + // them, so cached data() pointers would dangle. Eigen is column-major, + // matching WaveField2D's documented (i + j*N) layout. The renderer + // finite-diffs η for normals; foam is the Encino MinE folding metric. + this->field.n = this->gridSize; + this->field.tile = this->tileSize; + this->field.dz = this->heightGrid.data(); + this->field.dx = this->dispXGrid.data(); + this->field.dy = this->dispYGrid.data(); + this->field.foam = this->minEGrid.data(); + return &this->field; +} + +////////////////////////////////////////////////// +/// \brief Factory used to register the FFT engine under the "fft" token (see +/// RegisterWaveEngineFactory). Returns a default-constructed engine; the caller +/// applies SetParameters. +std::shared_ptr MakeFFTWaveField() +{ + return std::make_shared(); +} + +} // namespace gz::sim::waves diff --git a/gz_waves_provider_fft/src/FftWavesGui.cc b/gz_waves_provider_fft/src/FftWavesGui.cc new file mode 100644 index 00000000..1022c6bc --- /dev/null +++ b/gz_waves_provider_fft/src/FftWavesGui.cc @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2026 Honu Robotics + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + */ + +#include + +#include "gz/sim/System.hh" +#include "gz/sim/waves/FFTWaveSimulation.hh" // MakeFFTWaveField +#include "gz/sim/waves/WaveSimulation.hh" // RegisterWaveEngineFactory + +namespace gz::sim::systems +{ +/// \brief GUI-side registrar for the FFT wave engine. Loaded in the GUI process +/// alongside WaterVisual so the in-process engine registry knows the "fft" +/// token, letting WaterVisual rebuild its own (thread-private) engine from the +/// replicated recipe via `CreateWaveSimulation`. The server builds its engine +/// directly (`FftWaves::MakeEngine`) and does not need this. Keeping +/// registration here — rather than in `gz_waves_rendering` — lets the rendering +/// package stay provider-agnostic (it depends only on the gz_waves core). The +/// GUI-side mirror of the server's `gz-sim-waves-fft-system`. +/// +/// The factory is registered by the file-scope initializer below, which runs +/// the moment the GUI dlopens this library — so the plugin deliberately carries +/// no ISystem interface and parses no SDF, avoiding gz-sim's empty-plugin SDF +/// re-parse warning. The empty System body exists only so gz-sim has a plugin +/// to load (which is what triggers the dlopen). +class FftWavesGui : public System +{ +}; +} // namespace gz::sim::systems + +namespace +{ +/// \brief Register the "fft" engine factory when this library is loaded. +const bool kFftGuiRegistered = [] +{ + gz::sim::waves::RegisterWaveEngineFactory( + "fft", &gz::sim::waves::MakeFFTWaveField); + return true; +}(); +} // namespace + +////////////////////////////////////////////////// +GZ_ADD_PLUGIN(gz::sim::systems::FftWavesGui, gz::sim::System) + +GZ_ADD_PLUGIN_ALIAS(gz::sim::systems::FftWavesGui, + "gz::sim::systems::FftWavesGui") diff --git a/gz_waves_provider_fft/src/FftWavesSystem.cc b/gz_waves_provider_fft/src/FftWavesSystem.cc new file mode 100644 index 00000000..b0daa9ef --- /dev/null +++ b/gz_waves_provider_fft/src/FftWavesSystem.cc @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2026 Honu Robotics + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + */ + +#include +#include + +#include + +#include "gz/sim/systems/WavesSystemBase.hh" +#include "gz/sim/waves/FFTWaveSimulation.hh" // MakeFFTWaveField +#include "gz/sim/waves/Wavefield.hh" // WaveParameters + +namespace gz::sim::systems +{ +/// \brief Wave source system backed by the stochastic FFT engine — the +/// Apache-2.0 EncinoWaves spectral library (TMA/JONSWAP/PM spectra, default +/// TMA + Hasselmann spreading + capillary dispersion). Loaded by SDF as +/// `gz-sim-waves-fft-system`. The ECM/source plumbing lives in WavesSystemBase; +/// this names the engine and owns the FFT parameter surface below. +/// +/// ## SDF parameters +/// +/// `` is at plugin level; the rest live in a `` block. All +/// are also runtime-settable via the `/world//wave/set_parameters` +/// service (`gz.msgs.Param`). +/// +/// | Tag | Type | Default | Meaning | +/// |---|---|---|---| +/// | `` | double [Hz] | 30.0 | Throttle for the FFT recompute (plugin level) | +/// | `` | double [s] | 5.0 | Wave period -> wind speed (PM relation) | +/// | `` | double | 1.0 | Amplitude multiplier | +/// | `` | double [s] | 2.0 | Startup-ramp time constant, (1 - exp(-t/tau)) | +/// | `` | int [0-9] | -1 (off) | WMO sea-state code; when >= 0 it OVERRIDES ``/`` — see "Sea state" below | +/// | `` | double [m] | 200.0 | Periodic tile extent per axis | +/// | `` | uint | 128 | Grid samples/axis (power of two; rounded up) | +/// | `` | uint | 0 | RNG seed for the spectrum amplitudes | +/// | `` | double | -1.0 | Tessendorf horizontal-displacement multiplier (~[-2, 0]); applied in the visual shader | +/// | `` | string | tma | EncinoWaves spectrum: `pms`, `jonswap`, `tma` | +/// | `` | string | hasselmann | Directional spreading: `poscos2`, `mitsuyasu`, `hasselmann`, `donelanbanner` | +/// | `` | string | capillary | Dispersion relation: `deep`, `finite`, `capillary` | +/// | `` | double [m] | 100 | Water depth (EncinoWaves dispersion input) | +/// | `` | double [km] | 300 | Wind fetch (EncinoWaves spectrum input) | +/// | `` | double | 0 | Swell elongation (directional spreading) | +/// | `` | double [0,1] | 0 | Breaking-wave trough damping | +/// | `` | double [m] | 0 | Band-pass lower edge; >0 enables the filter | +/// | `` | double [m] | 0 | Band-pass upper edge (0 = no upper bound) | +/// | `` | double [m] | 0 | Band-pass transition width (0 = auto) | +/// | `` | double [0,1] | 0 | Band-pass suppression floor | +/// | `` | bool | false | Band-stop (notch) instead of band-pass | +/// +/// ### Sea state vs. manual height +/// +/// `` is the one-knob control: a WMO code 0-9 (table 3700) that sets +/// the sea by deriving its significant wave height and peak period. Precedence: +/// - When `>= 0` it **overrides `` and ``** — setting those +/// alongside it has no effect, so use one approach or the other. +/// - `0` is calm/glassy (flat) water; `-1` (the default, or simply omitting the +/// tag) means "manual": drive the sea with `` + `` instead. +/// - Every other knob (``, ``, ``, the +/// spectrum/spreading/dispersion tags, ...) is independent of `` +/// and applies in both modes — combine freely. +/// +/// `` is parsed but not applied — EncinoWaves assumes wind along +X. +/// +/// \verbatim +/// +/// 30 +/// +/// +/// 3.2 +/// 1.0 +/// 2.0 +/// 256 +/// 128 +/// 42 +/// -2.0 +/// tma +/// hasselmann +/// capillary +/// +/// +/// \endverbatim +class FftWaves : public WavesSystemBase +{ + // Documentation inherited + protected: std::string EngineToken() const override { return "fft"; } + + // Documentation inherited + protected: std::shared_ptr MakeEngine( + const waves::WaveParameters &_params) const override + { + auto engine = waves::MakeFFTWaveField(); + engine->SetParameters(_params); + return engine; + } +}; +} // namespace gz::sim::systems + +GZ_ADD_PLUGIN(gz::sim::systems::FftWaves, + gz::sim::System, + gz::sim::systems::FftWaves::ISystemConfigure, + gz::sim::systems::FftWaves::ISystemPreUpdate, + gz::sim::systems::FftWaves::ISystemReset) + +GZ_ADD_PLUGIN_ALIAS(gz::sim::systems::FftWaves, "gz::sim::systems::FftWaves") diff --git a/gz_waves_provider_fft/test/fft_test.cc b/gz_waves_provider_fft/test/fft_test.cc new file mode 100644 index 00000000..62c0e20d --- /dev/null +++ b/gz_waves_provider_fft/test/fft_test.cc @@ -0,0 +1,487 @@ +/* + * Copyright (C) 2026 Honu Robotics + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + */ + +#include +#include +#include +#include + +#include + +#include "gz/sim/waves/Eval.hh" +#include "gz/sim/waves/FFTWaveSimulation.hh" +#include "gz/sim/waves/WaveSimulation.hh" +#include "gz/sim/waves/Wavefield.hh" + +namespace gsw = gz::sim::waves; + +namespace +{ +// The fft engine is no longer a dlopen'd gz-plugin; this binary links it +// directly, so register its factory for the CreateWaveSimulation tests. +const bool kFftRegistered = [] { + gsw::RegisterWaveEngineFactory("fft", &gsw::MakeFFTWaveField); + return true; +}(); + +/// \brief Default FFT wave parameters (PMS model, V19 ≈ 8 m/s). +/// \return The default WaveParameters. +gsw::WaveParameters DefaultParams() +{ + gsw::WaveParameters p; + p.model = "PMS"; + p.number = 3; // ignored by FFT engine (uses gridSize) + p.period = 6.0; // ω_P ≈ 1.05 rad/s, V19 ≈ 8 m/s + p.gain = 1.0; + p.direction = 0.0; + p.tau = 2.0; + return p; +} + +/// \brief Build a 64² / 100 m FFT simulation from DefaultParams(). +/// \param[in] _seed RNG seed for the spectrum amplitudes. +/// \return The constructed FFTWaveSimulation. +gsw::FFTWaveSimulation MakeSim(std::uint32_t _seed = 1) +{ + return gsw::FFTWaveSimulation( + DefaultParams(), /*tile=*/100.0, /*grid=*/64, _seed); +} + +/// \brief Root-mean-square of a grid's values. +/// \param[in] _g The grid to measure. +/// \return The RMS value. +double RmsHeight(const Eigen::MatrixXd &_g) +{ + return std::sqrt(_g.squaredNorm() / static_cast(_g.size())); +} + +/// \brief Largest |elevation| over a few scattered sample points. +/// \param[in] _d Wave-field state to query. +/// \param[in] _t Simulation time [s]. +/// \return The maximum absolute elevation [m]. +double MaxAbsElevation(const gsw::WavefieldData &_d, double _t) +{ + const double xs[] = {10.0, 33.0, 5.0, 61.0}; + const double ys[] = {12.0, 47.0, 80.0, 23.0}; + double m = 0.0; + for (int i = 0; i < 4; ++i) + m = std::max(m, std::abs(gsw::SurfaceElevation(_d, xs[i], ys[i], _t))); + return m; +} +} // namespace + +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, FactoryRoute) +{ + auto sim = gsw::CreateWaveSimulation("fft", DefaultParams()); + ASSERT_NE(sim, nullptr); + EXPECT_EQ(sim->Kind(), "fft"); + ASSERT_TRUE(sim->Bounds().has_value()); + EXPECT_GT(sim->Bounds()->x, 0.0); +} + +// The Wavefield component serializes only the *recipe* (algorithm + params), +// not a live engine: operator>> restores the params and leaves `simulation` +// null. A consumer rebuilds its own engine from the recipe (what WaterVisual +// does), so each consumer owns a private field instance — no shared +// process-global engine. Same seed → the rebuilt field matches the original. +////////////////////////////////////////////////// +TEST(Wavefield, FftComponentSerializesRecipeNotEngine) +{ + gsw::WavefieldData live; + live.algorithm = "fft"; + live.generation = 918273; + live.params.model = "PMS"; + live.params.period = 3.2; + live.params.gain = 1.0; + live.params.tileSize = 100.0; + live.params.gridSize = 64; + live.params.seed = 7; + live.simulation = gsw::CreateWaveSimulation(live.algorithm, live.params); + ASSERT_NE(live.simulation, nullptr); + live.simulation->Update(20.0); + EXPECT_GT(MaxAbsElevation(live, 20.0), 1e-3) + << "a live, time-advanced FFT field must be non-flat"; + + // Round-trip through the component serialization (what replication does). + std::stringstream ss; + ss << live; + gsw::WavefieldData round; + ss >> round; + + // Deserialization restores the recipe but builds no engine. + EXPECT_EQ(round.simulation, nullptr) + << "operator>> must not build a (process-global) engine"; + EXPECT_EQ(round.algorithm, "fft"); + EXPECT_EQ(round.generation, 918273u); + EXPECT_EQ(round.params.seed, 7u); + EXPECT_NEAR(round.params.period, 3.2, 1e-9); + + // A consumer rebuilds its own engine from the recipe; same seed → same field. + round.simulation = gsw::CreateWaveSimulation(round.algorithm, round.params); + ASSERT_NE(round.simulation, nullptr); + round.simulation->Update(20.0); + EXPECT_NEAR(MaxAbsElevation(round, 20.0), MaxAbsElevation(live, 20.0), 1e-9) + << "a privately rebuilt field matches the original (deterministic by seed)"; +} + +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, HeightsAreFinite) +{ + auto sim = MakeSim(); + sim.Update(5.0); + const auto &g = sim.HeightGrid(); + for (int i = 0; i < g.rows(); ++i) + for (int j = 0; j < g.cols(); ++j) + EXPECT_TRUE(std::isfinite(g(i, j))) << "(" << i << "," << j << ")"; +} + +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, HeightsAreBounded) +{ + auto sim = MakeSim(); + sim.Update(20.0); // past startup ramp + const auto &g = sim.HeightGrid(); + const double sigma = RmsHeight(g); + EXPECT_GT(sigma, 0.0); // not all zero + // 99.99% of a Gaussian is within ±4σ; allow a margin. + EXPECT_LE(g.cwiseAbs().maxCoeff(), 6.0 * sigma); +} + +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, RampUpFromZero) +{ + auto sim = MakeSim(); + sim.Update(0.0); + const double rms0 = RmsHeight(sim.HeightGrid()); + sim.Update(20.0); + const double rmsLong = RmsHeight(sim.HeightGrid()); + EXPECT_NEAR(rms0, 0.0, 1e-9); + // Phillips amplitudes at 100 m tile / V≈8 m/s wind give an RMS height in + // the low-cm range; bound generously to avoid spectrum-tuning regressions. + EXPECT_GT(rmsLong, 1e-4); +} + +// Encino's MinE folding metric is hooked through Jacobian() → Eval::FoamMask: +// pinched crests report Jacobian < 1 and produce whitecaps. +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, FoamFromJacobian) +{ + gsw::WaveParameters p; + p.model = "PMS"; + p.period = 3.2; + p.gain = 20.0; // large amplitude so folding is unambiguous + p.tileSize = 100.0; + p.gridSize = 64; + p.seed = 7; + gsw::FFTWaveSimulation sim(p, 100.0, 64, 7); + const double L = sim.TileSizeMeters(); + + // Flat surface at t=0 (ramp 0): no folding → Jacobian 1, no foam, either way. + sim.Update(0.0); + for (int i = 0; i < 32; ++i) + for (int j = 0; j < 32; ++j) + EXPECT_NEAR(sim.Jacobian(i * L / 32.0, j * L / 32.0, 0.0), 1.0, 1e-9); + + // Past the ramp: sample the folding metric and the derived foam. + sim.Update(20.0); + double minJac = 1e9, maxFoam = 0.0; + for (int i = 0; i < 32; ++i) + for (int j = 0; j < 32; ++j) + { + const double jac = sim.Jacobian(i * L / 32.0, j * L / 32.0, 20.0); + minJac = std::min(minJac, jac); + const double foam = + jac < 0.6 ? std::clamp(1.0 - jac / 0.6, 0.0, 1.0) : 0.0; + maxFoam = std::max(maxFoam, foam); + } + + EXPECT_LT(minJac, 0.6) << "Encino crests should fold below the foam threshold"; + EXPECT_GT(maxFoam, 0.0) << "Encino path should produce whitecaps"; +} + +// The band-pass reshapes which wavelengths survive in the spectrum. +// Same seed → without the filter the field is one thing; with a narrow band it +// differs. (The amplitude calibration renormalises total energy, so we compare +// the field shape, not its RMS.) +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, BandPassFilterReshapesField) +{ + gsw::WaveParameters p; + p.model = "PMS"; + p.period = 3.2; + p.gain = 1.0; + p.tileSize = 200.0; + p.gridSize = 64; + p.seed = 11; + + gsw::FFTWaveSimulation plain(p, 200.0, 64, 11); + plain.Update(20.0); + const Eigen::MatrixXd a = plain.HeightGrid(); + + // Keep only 30–100 m waves (suppresses the ~16 m peak and the ripples). + p.filterMinWavelength = 30.0; + p.filterMaxWavelength = 100.0; + gsw::FFTWaveSimulation filtered(p, 200.0, 64, 11); + filtered.Update(20.0); + const Eigen::MatrixXd b = filtered.HeightGrid(); + + EXPECT_GT((a - b).cwiseAbs().maxCoeff(), 1e-3) + << "band-pass filter should reshape the field"; +} + +// The // SDF selectors feed EncinoWaves. Two +// different spectra with the same seed produce different fields. +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, SpectrumSelectorChangesField) +{ + gsw::WaveParameters p; + p.model = "PMS"; + p.period = 3.2; + p.tileSize = 200.0; + p.gridSize = 64; + p.seed = 11; + + p.spectrum = "tma"; + gsw::FFTWaveSimulation tma; + tma.SetParameters(p); + tma.Update(20.0); + const Eigen::MatrixXd a = tma.HeightGrid(); + + p.spectrum = "pms"; + gsw::FFTWaveSimulation pms; + pms.SetParameters(p); + pms.Update(20.0); + const Eigen::MatrixXd b = pms.HeightGrid(); + + EXPECT_GT((a - b).cwiseAbs().maxCoeff(), 1e-3) + << "different selectors should produce different fields"; +} + +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, TimeEvolutionChangesField) +{ + auto sim = MakeSim(); + sim.Update(10.0); + const Eigen::MatrixXd snapshotA = sim.HeightGrid(); + sim.Update(11.0); + const Eigen::MatrixXd snapshotB = sim.HeightGrid(); + EXPECT_GT((snapshotB - snapshotA).cwiseAbs().maxCoeff(), 1e-3); +} + +// The Field() rendering view must alias the engine grids with the documented +// column-major layout (element (i,j) at i + j*N), so the renderer can upload +// it without knowing the concrete engine. +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, FieldExposesGridViews) +{ + auto sim = MakeSim(); + sim.Update(8.0); + const auto *f = sim.Field(); + ASSERT_NE(f, nullptr); + EXPECT_EQ(f->n, sim.GridSize()); + EXPECT_NEAR(f->tile, sim.TileSizeMeters(), 1e-12); + ASSERT_NE(f->dz, nullptr); + ASSERT_NE(f->dx, nullptr); + ASSERT_NE(f->dy, nullptr); + ASSERT_NE(f->foam, nullptr); + + const int N = static_cast(f->n); + const auto &eta = sim.HeightGrid(); + const auto &dxG = sim.DispXGrid(); + const auto &dyG = sim.DispYGrid(); + for (int j = 0; j < N; j += N / 4) + { + for (int i = 0; i < N; i += N / 4) + { + EXPECT_DOUBLE_EQ(f->dz[i + j * N], eta(i, j)) << "i=" << i << " j=" << j; + EXPECT_DOUBLE_EQ(f->dx[i + j * N], dxG(i, j)); + EXPECT_DOUBLE_EQ(f->dy[i + j * N], dyG(i, j)); + EXPECT_TRUE(std::isfinite(f->foam[i + j * N])); + } + } +} + +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, DeterministicAcrossRunsWithSameSeed) +{ + auto a = MakeSim(/*seed=*/42); + auto b = MakeSim(/*seed=*/42); + a.Update(7.0); + b.Update(7.0); + EXPECT_NEAR( + (a.HeightGrid() - b.HeightGrid()).cwiseAbs().maxCoeff(), + 0.0, 1e-12); +} + +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, DifferentSeedsProduceDifferentFields) +{ + auto a = MakeSim(/*seed=*/1); + auto b = MakeSim(/*seed=*/2); + a.Update(7.0); + b.Update(7.0); + EXPECT_GT( + (a.HeightGrid() - b.HeightGrid()).cwiseAbs().maxCoeff(), + 1e-3); +} + +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, BilinearSampleMatchesGrid) +{ + auto sim = MakeSim(); + sim.Update(8.0); + const auto &g = sim.HeightGrid(); + const double L = sim.TileSizeMeters(); + const std::size_t N = sim.GridSize(); + + // Bilinear at the exact grid corners should return the grid value. + // World coordinates of grid cell (i,j): x = i*L/N - L/2 + 0, + // y = j*L/N - L/2 + 0 (we wrap into [0, L) internally, so the offset + // doesn't matter as long as it's consistent). + for (std::size_t i = 0; i < N; i += N / 4) + { + for (std::size_t j = 0; j < N; j += N / 4) + { + const double x = static_cast(i) * L / N; + const double y = static_cast(j) * L / N; + EXPECT_NEAR(sim.Elevation(x, y, 8.0), g(i, j), 1e-9) + << "i=" << i << " j=" << j; + } + } +} + +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, ElevationIsPeriodic) +{ + auto sim = MakeSim(); + sim.Update(5.0); + const double L = sim.TileSizeMeters(); + // Sampling at (x + L, y) should give the same height as (x, y). + for (double x = -20.0; x <= 20.0; x += 3.7) + { + for (double y = -20.0; y <= 20.0; y += 4.1) + { + EXPECT_NEAR(sim.Elevation(x, y, 5.0), + sim.Elevation(x + L, y, 5.0), 1e-9); + EXPECT_NEAR(sim.Elevation(x, y, 5.0), + sim.Elevation(x, y + L, 5.0), 1e-9); + } + } +} + +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, NormalIsUnitAndPointsUp) +{ + auto sim = MakeSim(); + sim.Update(8.0); + for (double t = 1.0; t < 5.0; t += 0.7) + { + const auto n = sim.Normal(3.0 * t, 1.0 * t, 8.0); + EXPECT_NEAR(n.Length(), 1.0, 1e-9); + EXPECT_GT(n.Z(), 0.0); + } +} + +////////////////////////////////////////////////// +// ParticleVelocity is the time-derivative of the displacement field. On a +// moving sea it must be finite, physically bounded, and non-zero somewhere. +TEST(FFTWaveSimulation, ParticleVelocityIsFiniteBoundedAndNonzero) +{ + auto sim = MakeSim(); + sim.Update(8.0); + double maxSpeed = 0.0; + for (double x = 0.0; x < 100.0; x += 9.0) + { + for (double y = 0.0; y < 100.0; y += 9.0) + { + const auto v = sim.ParticleVelocity(x, y, 8.0); + ASSERT_TRUE(std::isfinite(v.X()) && std::isfinite(v.Y()) && + std::isfinite(v.Z())); + maxSpeed = std::max(maxSpeed, v.Length()); + } + } + EXPECT_GT(maxSpeed, 1e-3) << "velocity should be non-zero on a moving sea"; + EXPECT_LT(maxSpeed, 25.0) << "velocity should be physically bounded"; +} + +////////////////////////////////////////////////// +// At t = 0 the startup ramp is zero, so the field is flat and still. +TEST(FFTWaveSimulation, ParticleVelocityZeroAtRest) +{ + auto sim = MakeSim(); + sim.Update(0.0); + const auto v = sim.ParticleVelocity(12.0, 34.0, 0.0); + EXPECT_NEAR(v.Length(), 0.0, 1e-9); +} + +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, WindDirectionBiasesAmplitude) +{ + // Encino's directional spreading concentrates energy along the wind axis + // (assumed +x). We expect a higher RMS along x than perpendicular when + // sampled over a long enough strip. + gsw::WaveParameters p = DefaultParams(); + p.direction = 0.0; // wind along +x + gsw::FFTWaveSimulation sim(p, 100.0, 64, 7); + sim.Update(20.0); + + double sxx = 0, syy = 0; + int cxx = 0, cyy = 0; + for (double s = -40.0; s <= 40.0; s += 1.0) + { + sxx += std::abs(sim.Elevation(s, 0.0, 20.0)); ++cxx; + syy += std::abs(sim.Elevation(0.0, s, 20.0)); ++cyy; + } + EXPECT_GT(sxx / cxx, syy / cyy); +} + +// WMO sea state code -> representative wave parameters (the canonical table). +////////////////////////////////////////////////// +TEST(SeaState, CodeMapsToCanonicalHeights) +{ + gsw::SeaStateSpec prev{}; + for (int c = 1; c <= 9; ++c) + { + gsw::SeaStateSpec s; + ASSERT_TRUE(gsw::SeaStateFromCode(c, s)); + EXPECT_GT(s.significantWaveHeight, prev.significantWaveHeight); // increasing + EXPECT_GT(s.peakPeriod, 0.0); + EXPECT_GT(s.windSpeed, 0.0); + prev = s; + } + gsw::SeaStateSpec dummy; + EXPECT_FALSE(gsw::SeaStateFromCode(-1, dummy)); + EXPECT_FALSE(gsw::SeaStateFromCode(10, dummy)); + gsw::SeaStateSpec calm; + ASSERT_TRUE(gsw::SeaStateFromCode(0, calm)); + EXPECT_NEAR(calm.significantWaveHeight, 0.0, 1e-9); // sea state 0 = flat +} + +// drives the field to that WMO sea state's significant wave height. +////////////////////////////////////////////////// +TEST(FFTWaveSimulation, SeaStateSetsSignificantWaveHeight) +{ + gsw::WaveParameters p; + p.model = "PMS"; + p.gridSize = 64; + p.tileSize = 256.0; + p.seed = 11; + p.seaState = 5; // "rough", representative Hs ~3.25 m + gsw::FFTWaveSimulation sim; + sim.SetParameters(p); + sim.Update(50.0); // past the startup ramp + + // Hs = 4 * RMS(eta). Encino calibrates to the PM Hs derived from the + // sea-state wind/period, so it should land near the canonical value. + const double hs = 4.0 * RmsHeight(sim.HeightGrid()); + gsw::SeaStateSpec s; + ASSERT_TRUE(gsw::SeaStateFromCode(5, s)); + EXPECT_NEAR(hs, s.significantWaveHeight, 0.30 * s.significantWaveHeight) + << "sea state 5 should give Hs ~= " << s.significantWaveHeight + << " m, got " << hs; +} diff --git a/gz_waves_rendering/share/models/water_surface/model.sdf b/gz_waves_rendering/share/models/water_surface/model.sdf index 6b6a1371..0f49185a 100644 --- a/gz_waves_rendering/share/models/water_surface/model.sdf +++ b/gz_waves_rendering/share/models/water_surface/model.sdf @@ -46,10 +46,11 @@ static initializer that runs when the GUI dlopens the library), so order is irrelevant and they add no plugin warnings. One registrar per available engine; gz_waves_rendering itself depends on no engine, - so these plugins carry that dependency. The FFT registrar - (gz-sim-waves-fft-gui) is added alongside when the FFT backend lands. --> + so these plugins carry that dependency. --> + diff --git a/vrx_gazebo/worlds/open_water.sdf b/vrx_gazebo/worlds/open_water.sdf index 38f556b3..dbef0ed3 100644 --- a/vrx_gazebo/worlds/open_water.sdf +++ b/vrx_gazebo/worlds/open_water.sdf @@ -88,12 +88,12 @@ the comment directly above each block. ================================================================== --> - - + + 30 5 - + + + + + + + -->