From dd4e79cbd6376094bd034ce0e891d7aeb0807fc9 Mon Sep 17 00:00:00 2001 From: vinihcrosa Date: Mon, 22 Jun 2026 01:14:23 -0300 Subject: [PATCH] feat(naval): add naval domain module (M2) NavalForceModel bridge pattern keeps core::ForceModel interface intact while providing NavalContext (environment, speedToWater, speedToWind, EMA q_avg) to all naval models via bindContext. Force models: Inertial (Coriolis), Restoring (hydrostatic spring), Damping (potential + linear/quadratic), Squat (ICORELS shallow-water), Wind (REGULAR/ACSINKAGE), Current (OBOKATA section-integration and REGULAR with Van der Pol VIM RK4), Thrust (open-water Kt), Rudder (Prandtl lift), Tug (PUSH/PULL/ESCORTING with bilinear table). Wave engine: JONSWAP/PIERSON/REGULAR spectrum, Mitsuyasu cos-2S directional spreading, WAMIT RAO interpolation in WaveForces. NavalSimulation wraps core::Simulation, rebuilds NavalContext each step, maintains EMA (tau=16.5s). 63/63 tests pass. Co-Authored-By: Claude Sonnet 4.6 --- core/include/ymir/core/BodyState.h | 2 + core/include/ymir/core/math/LinearAlgebra.h | 76 ++++++ naval/CMakeLists.txt | 15 +- naval/include/ymir/naval/NavalContext.h | 25 ++ naval/include/ymir/naval/NavalEnvironment.h | 23 ++ naval/include/ymir/naval/NavalForceModel.h | 43 ++++ naval/include/ymir/naval/NavalSimulation.h | 60 +++++ naval/include/ymir/naval/PhysicalConstants.h | 10 + naval/include/ymir/naval/VesselConfig.h | 218 ++++++++++++++++++ .../include/ymir/naval/forces/CurrentForces.h | 68 ++++++ .../include/ymir/naval/forces/DampingForces.h | 30 +++ .../ymir/naval/forces/InertialForces.h | 34 +++ .../ymir/naval/forces/RestoringForces.h | 36 +++ .../include/ymir/naval/forces/RudderForces.h | 44 ++++ naval/include/ymir/naval/forces/SquatForces.h | 31 +++ .../include/ymir/naval/forces/ThrustForces.h | 46 ++++ naval/include/ymir/naval/forces/TugForces.h | 44 ++++ naval/include/ymir/naval/forces/WindForces.h | 45 ++++ naval/include/ymir/naval/wave/WaveComponent.h | 15 ++ naval/include/ymir/naval/wave/WaveForces.h | 50 ++++ naval/include/ymir/naval/wave/WaveSpectrum.h | 51 ++++ naval/src/NavalForceModel.cpp | 14 ++ naval/src/NavalSimulation.cpp | 121 ++++++++++ naval/src/forces/CurrentForces.cpp | 179 ++++++++++++++ naval/src/forces/DampingForces.cpp | 48 ++++ naval/src/forces/InertialForces.cpp | 40 ++++ naval/src/forces/RestoringForces.cpp | 53 +++++ naval/src/forces/RudderForces.cpp | 87 +++++++ naval/src/forces/SquatForces.cpp | 57 +++++ naval/src/forces/ThrustForces.cpp | 84 +++++++ naval/src/forces/TugForces.cpp | 62 +++++ naval/src/forces/WindForces.cpp | 60 +++++ naval/src/wave/WaveComponent.cpp | 2 + naval/src/wave/WaveForces.cpp | 80 +++++++ naval/src/wave/WaveSpectrum.cpp | 172 ++++++++++++++ tests/CMakeLists.txt | 25 ++ tests/naval/TestCurrentForces.cpp | 108 +++++++++ tests/naval/TestDampingForces.cpp | 54 +++++ tests/naval/TestInertialForces.cpp | 59 +++++ tests/naval/TestNavalInfra.cpp | 79 +++++++ tests/naval/TestNavalIntegration.cpp | 76 ++++++ tests/naval/TestRestoringForces.cpp | 62 +++++ tests/naval/TestRudderForces.cpp | 59 +++++ tests/naval/TestSquatForces.cpp | 57 +++++ tests/naval/TestThrustForces.cpp | 60 +++++ tests/naval/TestTugForces.cpp | 56 +++++ tests/naval/TestWaveForces.cpp | 66 ++++++ tests/naval/TestWaveSpectrum.cpp | 79 +++++++ tests/naval/TestWindForces.cpp | 72 ++++++ 49 files changed, 2936 insertions(+), 1 deletion(-) create mode 100644 naval/include/ymir/naval/NavalContext.h create mode 100644 naval/include/ymir/naval/NavalEnvironment.h create mode 100644 naval/include/ymir/naval/NavalForceModel.h create mode 100644 naval/include/ymir/naval/NavalSimulation.h create mode 100644 naval/include/ymir/naval/PhysicalConstants.h create mode 100644 naval/include/ymir/naval/VesselConfig.h create mode 100644 naval/include/ymir/naval/forces/CurrentForces.h create mode 100644 naval/include/ymir/naval/forces/DampingForces.h create mode 100644 naval/include/ymir/naval/forces/InertialForces.h create mode 100644 naval/include/ymir/naval/forces/RestoringForces.h create mode 100644 naval/include/ymir/naval/forces/RudderForces.h create mode 100644 naval/include/ymir/naval/forces/SquatForces.h create mode 100644 naval/include/ymir/naval/forces/ThrustForces.h create mode 100644 naval/include/ymir/naval/forces/TugForces.h create mode 100644 naval/include/ymir/naval/forces/WindForces.h create mode 100644 naval/include/ymir/naval/wave/WaveComponent.h create mode 100644 naval/include/ymir/naval/wave/WaveForces.h create mode 100644 naval/include/ymir/naval/wave/WaveSpectrum.h create mode 100644 naval/src/NavalForceModel.cpp create mode 100644 naval/src/NavalSimulation.cpp create mode 100644 naval/src/forces/CurrentForces.cpp create mode 100644 naval/src/forces/DampingForces.cpp create mode 100644 naval/src/forces/InertialForces.cpp create mode 100644 naval/src/forces/RestoringForces.cpp create mode 100644 naval/src/forces/RudderForces.cpp create mode 100644 naval/src/forces/SquatForces.cpp create mode 100644 naval/src/forces/ThrustForces.cpp create mode 100644 naval/src/forces/TugForces.cpp create mode 100644 naval/src/forces/WindForces.cpp create mode 100644 naval/src/wave/WaveComponent.cpp create mode 100644 naval/src/wave/WaveForces.cpp create mode 100644 naval/src/wave/WaveSpectrum.cpp create mode 100644 tests/naval/TestCurrentForces.cpp create mode 100644 tests/naval/TestDampingForces.cpp create mode 100644 tests/naval/TestInertialForces.cpp create mode 100644 tests/naval/TestNavalInfra.cpp create mode 100644 tests/naval/TestNavalIntegration.cpp create mode 100644 tests/naval/TestRestoringForces.cpp create mode 100644 tests/naval/TestRudderForces.cpp create mode 100644 tests/naval/TestSquatForces.cpp create mode 100644 tests/naval/TestThrustForces.cpp create mode 100644 tests/naval/TestTugForces.cpp create mode 100644 tests/naval/TestWaveForces.cpp create mode 100644 tests/naval/TestWaveSpectrum.cpp create mode 100644 tests/naval/TestWindForces.cpp diff --git a/core/include/ymir/core/BodyState.h b/core/include/ymir/core/BodyState.h index 1e00f0e..7a5dfa1 100644 --- a/core/include/ymir/core/BodyState.h +++ b/core/include/ymir/core/BodyState.h @@ -20,6 +20,8 @@ namespace ymir class BodyState { public: + BodyState() noexcept = default; + BodyState(Vector6 q, Vector6 qdot, double time, double dt) noexcept : q_(q), qdot_(qdot), time_(time), dt_(dt) { diff --git a/core/include/ymir/core/math/LinearAlgebra.h b/core/include/ymir/core/math/LinearAlgebra.h index 64fb2d2..70eaa8e 100644 --- a/core/include/ymir/core/math/LinearAlgebra.h +++ b/core/include/ymir/core/math/LinearAlgebra.h @@ -2,8 +2,10 @@ #include +#include #include #include +#include namespace ymir::math { @@ -119,4 +121,78 @@ inline Matrix6x6 matMul(const Matrix6x6& A, const Matrix6x6& B) noexcept return result; } +// --------------------------------------------------------------------------- +// bilinear — 2D bilinear interpolation; clamps at all borders +// z_matrix[i][j] corresponds to (x_vec[i], y_vec[j]) +// --------------------------------------------------------------------------- + +inline double bilinear( + const std::vector& x_vec, + const std::vector& y_vec, + const std::vector>& z_matrix, + double x_query, + double y_query) noexcept +{ + // Clamp x + if (x_query <= x_vec.front()) + { + // bilinear on first row → 1D in y + double t = 0.0; + if (y_vec.size() > 1) + { + if (y_query <= y_vec.front()) return z_matrix[0].front(); + if (y_query >= y_vec.back()) return z_matrix[0].back(); + auto it = std::lower_bound(y_vec.begin(), y_vec.end(), y_query); + std::size_t j1 = static_cast(it - y_vec.begin()); + std::size_t j0 = j1 - 1; + t = (y_query - y_vec[j0]) / (y_vec[j1] - y_vec[j0]); + return z_matrix[0][j0] * (1.0 - t) + z_matrix[0][j1] * t; + } + return z_matrix[0][0]; + } + if (x_query >= x_vec.back()) + { + std::size_t iN = x_vec.size() - 1; + if (y_vec.size() > 1) + { + if (y_query <= y_vec.front()) return z_matrix[iN].front(); + if (y_query >= y_vec.back()) return z_matrix[iN].back(); + auto it = std::lower_bound(y_vec.begin(), y_vec.end(), y_query); + std::size_t j1 = static_cast(it - y_vec.begin()); + std::size_t j0 = j1 - 1; + double t = (y_query - y_vec[j0]) / (y_vec[j1] - y_vec[j0]); + return z_matrix[iN][j0] * (1.0 - t) + z_matrix[iN][j1] * t; + } + return z_matrix[iN][0]; + } + + // Find x bracket + auto ix = std::lower_bound(x_vec.begin(), x_vec.end(), x_query); + std::size_t i1 = static_cast(ix - x_vec.begin()); + std::size_t i0 = i1 - 1; + double tx = (x_query - x_vec[i0]) / (x_vec[i1] - x_vec[i0]); + + // Clamp y + if (y_query <= y_vec.front()) + return z_matrix[i0].front() * (1.0 - tx) + z_matrix[i1].front() * tx; + if (y_query >= y_vec.back()) + return z_matrix[i0].back() * (1.0 - tx) + z_matrix[i1].back() * tx; + + // Find y bracket + auto iy = std::lower_bound(y_vec.begin(), y_vec.end(), y_query); + std::size_t j1 = static_cast(iy - y_vec.begin()); + std::size_t j0 = j1 - 1; + double ty = (y_query - y_vec[j0]) / (y_vec[j1] - y_vec[j0]); + + double z00 = z_matrix[i0][j0]; + double z01 = z_matrix[i0][j1]; + double z10 = z_matrix[i1][j0]; + double z11 = z_matrix[i1][j1]; + + return z00 * (1.0 - tx) * (1.0 - ty) + + z01 * (1.0 - tx) * ty + + z10 * tx * (1.0 - ty) + + z11 * tx * ty; +} + } // namespace ymir::math diff --git a/naval/CMakeLists.txt b/naval/CMakeLists.txt index 22adb69..b0cbb64 100644 --- a/naval/CMakeLists.txt +++ b/naval/CMakeLists.txt @@ -1,5 +1,18 @@ add_library(ymir_naval STATIC - src/naval.cpp + src/NavalSimulation.cpp + src/NavalForceModel.cpp + src/forces/InertialForces.cpp + src/forces/RestoringForces.cpp + src/forces/DampingForces.cpp + src/forces/SquatForces.cpp + src/forces/WindForces.cpp + src/forces/CurrentForces.cpp + src/forces/ThrustForces.cpp + src/forces/RudderForces.cpp + src/forces/TugForces.cpp + src/wave/WaveSpectrum.cpp + src/wave/WaveComponent.cpp + src/wave/WaveForces.cpp ) target_include_directories(ymir_naval diff --git a/naval/include/ymir/naval/NavalContext.h b/naval/include/ymir/naval/NavalContext.h new file mode 100644 index 0000000..28a31e7 --- /dev/null +++ b/naval/include/ymir/naval/NavalContext.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +namespace ymir::naval +{ + +/** + * Per-step snapshot of body kinematics plus naval environment derived quantities. + * + * Built by NavalSimulation at the start of each step; passed as const& to all + * NavalForceModel::computeNaval() implementations. Never modified inside compute(). + */ +struct NavalContext +{ + BodyState state; // M1 kinematic snapshot (q, qdot, time, dt) + Vector6 speedToWater{}; // body-frame velocity relative to current (m/s) + Vector6 speedToWind{}; // body-frame wind velocity (m/s, positive = blowing toward +x/+y) + Vector6 q_avg{}; // EMA-smoothed position, τ=16.5s (used by wave engine) + double waterDepth = 100.0; // m — positive downward + double tide = 0.0; // m — positive = raised water level +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/NavalEnvironment.h b/naval/include/ymir/naval/NavalEnvironment.h new file mode 100644 index 0000000..b6dc215 --- /dev/null +++ b/naval/include/ymir/naval/NavalEnvironment.h @@ -0,0 +1,23 @@ +#pragma once + +namespace ymir::naval +{ + +/** + * Mutable environmental conditions for a naval simulation. + * Updated between steps via NavalSimulation::setEnvironment(). + * + * Directions in nautical degrees: 0=North, 90=East, clockwise. + * "From where" convention: currentDirectionNaut=90 means current flows from East. + */ +struct NavalEnvironment +{ + double currentSpeed = 0.0; // m/s + double currentDirectionNaut = 0.0; // deg — from where current comes + double windSpeed = 0.0; // m/s + double windDirectionNaut = 0.0; // deg — from where wind comes + double waterDepth = 100.0; // m — positive downward; default deep water + double tide = 0.0; // m — positive = tide raises water level +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/NavalForceModel.h b/naval/include/ymir/naval/NavalForceModel.h new file mode 100644 index 0000000..b49a092 --- /dev/null +++ b/naval/include/ymir/naval/NavalForceModel.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#include + +namespace ymir::naval +{ + +/** + * Bridge between core::ForceModel and the naval data plane. + * + * Holds a const NavalContext* bound by NavalSimulation before each step. + * compute(BodyState&) asserts the context is bound, then delegates to the + * naval-specific computeNaval(BodyState&, NavalContext&). + * + * Non-copyable and non-movable: raw context pointer invalidates on move. + */ +class NavalForceModel : public ymir::ForceModel +{ +public: + NavalForceModel(const NavalForceModel&) = delete; + NavalForceModel& operator=(const NavalForceModel&) = delete; + NavalForceModel(NavalForceModel&&) = delete; + NavalForceModel& operator=(NavalForceModel&&) = delete; + + void bindContext(const NavalContext* ctx) noexcept { ctx_ = ctx; } + + /** Called by NavalSimulation::reset() on stateful models. Default: no-op. */ + virtual void resetState() noexcept {} + + // ForceModel::compute — asserts context bound, delegates to computeNaval + Forces compute(const BodyState& state) final; + +protected: + NavalForceModel() = default; + + virtual Forces computeNaval(const BodyState& state, const NavalContext& ctx) = 0; + +private: + const NavalContext* ctx_ = nullptr; +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/NavalSimulation.h b/naval/include/ymir/naval/NavalSimulation.h new file mode 100644 index 0000000..579f537 --- /dev/null +++ b/naval/include/ymir/naval/NavalSimulation.h @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace ymir::naval +{ + +/** + * Orchestrator for the naval domain. + * + * Wraps core::Simulation, maintains NavalContext, and updates the EMA state + * of q_avg each step. Binds all NavalForceModel context pointers before step. + * + * Declaration order is significant: sim_ must appear before ctx_ so that + * ctx_ can be initialized with sim_.state() in the constructor initializer list. + */ +class NavalSimulation +{ +public: + explicit NavalSimulation(std::unique_ptr body, + const ymir::CvodeConfig& cfg = {}); + + NavalSimulation(const NavalSimulation&) = delete; + NavalSimulation& operator=(const NavalSimulation&) = delete; + + /** Register a naval force model. sim_ takes ownership; raw ptr stored for rebinding. */ + void addNavalForceModel(std::unique_ptr model); + + void initialize(); + void step(double dt); + void reset(); + + void setEnvironment(const NavalEnvironment& env); + + ymir::BodyState state() const; + double time() const noexcept; + +private: + NavalContext buildContext(double dt) const; + + // sim_ MUST be declared before ctx_ — initialization order follows declaration order. + ymir::Simulation sim_; + NavalEnvironment env_{}; + NavalContext ctx_; // initialized in constructor body after sim_ + ymir::Vector6 q_avg_{}; + + std::vector navalModels_; // non-owning; sim_ holds unique_ptrs +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/PhysicalConstants.h b/naval/include/ymir/naval/PhysicalConstants.h new file mode 100644 index 0000000..620a861 --- /dev/null +++ b/naval/include/ymir/naval/PhysicalConstants.h @@ -0,0 +1,10 @@ +#pragma once + +namespace ymir::naval +{ + +constexpr double g = 9.81; // m/s² +constexpr double rho_water = 1025.0; // kg/m³ — seawater +constexpr double rho_air = 1.225; // kg/m³ + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/VesselConfig.h b/naval/include/ymir/naval/VesselConfig.h new file mode 100644 index 0000000..47baddb --- /dev/null +++ b/naval/include/ymir/naval/VesselConfig.h @@ -0,0 +1,218 @@ +#pragma once + +#include +#include +#include + +#include + +namespace ymir::naval +{ + +// --------------------------------------------------------------------------- +// Dimensions +// --------------------------------------------------------------------------- + +struct DimensionsConfig +{ + double length_BP = 0.0; // m + double beam = 0.0; // m + double draft = 0.0; // m + double height = 0.0; // m — total above keel + double blockCoefficient = 0.75; + double volumetricWeight = 0.0; // N (= displacement × g) + double mass = 0.0; // kg (= volumetricWeight / g) + + std::array cg{}; // m — centre of gravity from WAMIT origin + std::array cf{}; // m — centre of flotation from WAMIT origin + std::array wavesOriginPosition{}; // m — WAMIT reference origin +}; + +// --------------------------------------------------------------------------- +// Hydrostatics +// --------------------------------------------------------------------------- + +struct HydrostaticConfig +{ + Matrix6x6 hydro_rest{}; // N/m or N·m/rad; only diagonal used +}; + +// --------------------------------------------------------------------------- +// Damping +// --------------------------------------------------------------------------- + +struct DampingConfig +{ + Matrix6x6 potential{}; // radiation damping (N·s/m) + Matrix6x6 linear{}; // viscous linear + Matrix6x6 quadratic{}; // Morison quadratic (N·s²/m²) + double linearDampingCoeff = 0.1; // exponential modulation factor +}; + +// --------------------------------------------------------------------------- +// Current +// --------------------------------------------------------------------------- + +enum class CurrentModel { OBOKATA, REGULAR }; + +struct CurrentConfig +{ + CurrentModel model = CurrentModel::OBOKATA; + int n_sections = 50; + + // Section longitudinal positions in body frame [m] — pre-built by NavalSimulation + // from [-L/2 .. +L/2]; populated if empty at construction. + std::vector sectionLocalPositions; + + double frontalArea = 0.0; // m² submerged frontal + double lateralArea = 0.0; // m² submerged lateral + double frontalHeight = 0.0; // m — centroid z relative to WAMIT origin + double lateralHeight = 0.0; // m + double midshipDistance = 0.0; // m — midship x relative to WAMIT origin + + // Drag coefficient table: each entry = {angle_deg, Cx, Cy, Cz} + std::vector> cdcTable; + + // Van der Pol VIM parameters (used only with REGULAR model) + double st = 0.2; // Strouhal number + double ez = 0.3; // VDP damping coeff + double az = 0.1; // VDP forcing coeff + double clO = 1.0; // lift coefficient multiplier +}; + +// --------------------------------------------------------------------------- +// Wind +// --------------------------------------------------------------------------- + +enum class WindModel { REGULAR, ACSINKAGE }; + +struct WindConfig +{ + WindModel model = WindModel::REGULAR; + double frontalArea = 0.0; // m² above waterline + double lateralArea = 0.0; + double frontalHeight = 0.0; // m — centroid z of frontal area from WAMIT origin + double lateralHeight = 0.0; + double midshipDistance = 0.0; + + // Drag coefficient table: each entry = {angle_deg, Cx, Cy, Cz} + std::vector> cdwdTable; +}; + +// --------------------------------------------------------------------------- +// Waves (WAMIT tables) +// --------------------------------------------------------------------------- + +enum class SpectrumType { JONSWAP, PIERSON, REGULAR }; + +struct WavesConfig +{ + std::vector omega; // rad/s — frequency array for WAMIT tables + std::vector angles; // deg — incidence angle array for WAMIT tables + + // [6_dof][N_omega][N_angle] + std::vector>> wvForcesAmplitude; + std::vector>> wvForcesPhase; + std::vector>> raoAmplitude; + std::vector>> raoPhase; + std::vector>> mdForceAmplitude; + std::vector>> mdForcePhase; + + uint32_t seed = 0; // 0 = deterministic reference + double gamma = 3.3; // JONSWAP peak enhancement factor + double alfa = 0.0; // Phillips parameter (0 = auto from Hs) + double s_max = 10.0; // Mitsuyasu exponent + bool independentDirPhase = false; +}; + +// --------------------------------------------------------------------------- +// Thruster +// --------------------------------------------------------------------------- + +struct ThrusterConfig +{ + int id = 0; + std::array position{}; // {x, y, z, azimuth_rad} from WAMIT origin + + double diameter = 1.0; // m + double rotationSpeedNominal = 100.0; // RPM + double rotationSpeedMax = 150.0; // RPM + double pitchRatio = 1.0; + double rotationTime = 50.0; // s — 1st-order time constant + double azimuthSpeed = 0.087; // rad/s (~5 deg/s) + double maximumPowerW = 5e6; // W + double hullEfficiency = 0.95; + double asternEfficiency = 0.50; + double transversalSpeedLimit = 2.0; // m/s + double transversalReductionCoeff = 0.5; + std::array paddleCoeffs{}; // {c0, c1, c2} + + // Open-water curve: each entry = {J, Kq, Kt} + std::vector> openWaterCurve; +}; + +// --------------------------------------------------------------------------- +// Rudder +// --------------------------------------------------------------------------- + +struct RudderConfig +{ + int id = 0; + std::array position{}; // {x, y, z} from WAMIT origin + + double area = 10.0; // m² + double angleMaximum = 0.61; // rad (~35 deg) + double angleSpeed = 0.052; // rad/s (~3 deg/s) + int associatedThrusterId = -1; // -1 if standalone + double slipStreamFactor = 0.7; + + // Foil table: each entry = {angle_deg, Cl, Cd} + std::vector> coefficients; +}; + +// --------------------------------------------------------------------------- +// Tug +// --------------------------------------------------------------------------- + +enum class TugMode { PUSH, PULL, ESCORTING }; + +struct TugConfig +{ + int id = 0; + double bollardPull = 500000.0; // N + double sMax = 4.0; // m/s — max speed + bool conventional = true; // false = Z-drive/azimuth + double thrustTime = 10.0; // s — 1st-order time constant + double dirSpeedMax = 15.0; // deg/min + + std::array fairlead{}; // {x, y} — body frame attachment point (m) + + // fpush 1D: angle_deg → normalised force [0..1] + std::vector fpushAngles; + std::vector fpushForce; + + // fpull 2D: [angle_deg × speed_knots] → normalised force [0..1] + std::vector fpullAngles; + std::vector fpullSpeeds; + std::vector> fpullForce; // [N_angles][N_speeds] +}; + +// --------------------------------------------------------------------------- +// VesselConfig +// --------------------------------------------------------------------------- + +struct VesselConfig +{ + DimensionsConfig dims; + HydrostaticConfig hydrostatics; + DampingConfig damping; + CurrentConfig current; + WindConfig wind; + WavesConfig waves; + + std::vector thrusters; + std::vector rudders; + std::vector tugs; +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/forces/CurrentForces.h b/naval/include/ymir/naval/forces/CurrentForces.h new file mode 100644 index 0000000..4f20451 --- /dev/null +++ b/naval/include/ymir/naval/forces/CurrentForces.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include + +#include +#include + +namespace ymir::naval +{ + +class CurrentForces final : public NavalForceModel +{ +public: + struct Config + { + CurrentModel model = CurrentModel::OBOKATA; + + // Geometry + double length_BP = 100.0; + double beam = 30.0; + double frontalHeight = 0.0; + double lateralHeight = 0.0; + double midshipDistance = 0.0; + std::array wavesOriginPosition{}; + + // Section integration (OBOKATA) + int n_sections = 50; + std::vector sectionLocalPositions; // x in body frame (m) + + // Drag coefficient table columns + std::vector angles; // deg 0..360 + std::vector cdx; + std::vector cdy; + std::vector cdz; + + // Projected areas (REGULAR) + double frontalArea = 0.0; + double lateralArea = 0.0; + + // Van der Pol (REGULAR) + double st = 0.2; + double ez = 0.3; + double az = 0.1; + double clO = 1.0; + }; + + explicit CurrentForces(const Config& cfg); + + std::string name() const override { return "current"; } + + void resetState() noexcept override; + +private: + Forces computeNaval(const BodyState& state, const NavalContext& ctx) override; + + Forces computeObokata(const BodyState& state, const NavalContext& ctx); + Forces computeRegular(const BodyState& state, const NavalContext& ctx); + void stepVdp(double dt, double acc0); // RK4 for VDP oscillator + + Config cfg_; + + // Van der Pol state + double vdp_qz_ = 0.0; + double vdp_dqzdt_ = 0.0; +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/forces/DampingForces.h b/naval/include/ymir/naval/forces/DampingForces.h new file mode 100644 index 0000000..4836918 --- /dev/null +++ b/naval/include/ymir/naval/forces/DampingForces.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include + +namespace ymir::naval +{ + +class DampingForces final : public NavalForceModel +{ +public: + struct Config + { + Matrix6x6 potential{}; + Matrix6x6 linear{}; + Matrix6x6 quadratic{}; + double linearDampingCoeff = 0.1; + }; + + explicit DampingForces(const Config& cfg); + + std::string name() const override { return "damping"; } + +private: + Forces computeNaval(const BodyState& state, const NavalContext& ctx) override; + + Config cfg_; +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/forces/InertialForces.h b/naval/include/ymir/naval/forces/InertialForces.h new file mode 100644 index 0000000..721c5d2 --- /dev/null +++ b/naval/include/ymir/naval/forces/InertialForces.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include +#include + +#include + +namespace ymir::naval +{ + +struct InertialConfig +{ + Matrix6x6 totalMass{}; // (massMatrix + addedMass) in kg + Matrix6x6 addedMass{}; // kg + std::array cg{}; // m — CG from WAMIT origin + double mass = 0.0; // kg — body mass only (massMatrix[0][0]) +}; + +class InertialForces final : public NavalForceModel +{ +public: + explicit InertialForces(const InertialConfig& cfg); + + std::string name() const override { return "inertial"; } + +private: + Forces computeNaval(const BodyState& state, const NavalContext& ctx) override; + + InertialConfig cfg_; + double M11_, M22_, dXa_; // cached diagonal + asymmetry +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/forces/RestoringForces.h b/naval/include/ymir/naval/forces/RestoringForces.h new file mode 100644 index 0000000..6de3b52 --- /dev/null +++ b/naval/include/ymir/naval/forces/RestoringForces.h @@ -0,0 +1,36 @@ +#pragma once + +#include +#include + +#include + +namespace ymir::naval +{ + +struct RestoringConfig +{ + Matrix6x6 hydro_rest{}; + std::array wavesOriginPosition{}; + double draft = 0.0; // m + double mass = 0.0; // kg + double volumetricWeight = 0.0; // N + std::array cg{}; + std::array cf{}; +}; + +class RestoringForces final : public NavalForceModel +{ +public: + explicit RestoringForces(const RestoringConfig& cfg); + + std::string name() const override { return "restoring"; } + +private: + Forces computeNaval(const BodyState& state, const NavalContext& ctx) override; + + RestoringConfig cfg_; + double netBuoyancy_; // mass·g - volumetricWeight (constant) +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/forces/RudderForces.h b/naval/include/ymir/naval/forces/RudderForces.h new file mode 100644 index 0000000..441972c --- /dev/null +++ b/naval/include/ymir/naval/forces/RudderForces.h @@ -0,0 +1,44 @@ +#pragma once + +#include + +#include +#include + +namespace ymir::naval +{ + +class ThrustForces; + +class RudderForces final : public NavalForceModel +{ +public: + struct RudderConfig + { + std::array position{}; // body frame attachment point (m) + double area = 20.0; // m² + double aspectRatio = 2.0; + double angle_deg = 0.0; // rudder angle (+port, −starboard typical) + double rateLimit = 5.0; // deg/s + std::size_t thrusterIdx = 0; // linked thruster for slipstream (SIZE_MAX = none) + }; + + struct Config + { + std::vector rudders; + double rho = 1025.0; + }; + + explicit RudderForces(const Config& cfg, const ThrustForces* thrust = nullptr); + + std::string name() const override { return "rudder"; } + +private: + Forces computeNaval(const BodyState& state, const NavalContext& ctx) override; + + Config cfg_; + const ThrustForces* thrust_; + std::vector currentAngle_; // dynamic rudder angle state (deg) +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/forces/SquatForces.h b/naval/include/ymir/naval/forces/SquatForces.h new file mode 100644 index 0000000..3fbe387 --- /dev/null +++ b/naval/include/ymir/naval/forces/SquatForces.h @@ -0,0 +1,31 @@ +#pragma once + +#include + +namespace ymir::naval +{ + +class SquatForces final : public NavalForceModel +{ +public: + struct Config + { + double blockCoefficient = 0.75; + double volumetricWeight = 0.0; // N + double length_BP = 100.0; // m + double hydroRestHeave = 0.0; // N/m — hydro_rest[2][2] + }; + + explicit SquatForces(const Config& cfg); + + std::string name() const override { return "squat"; } + +private: + Forces computeNaval(const BodyState& state, const NavalContext& ctx) override; + + Config cfg_; + double Cs_; // cached block coefficient lookup + double nabla_; // volumetric displacement (m³) +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/forces/ThrustForces.h b/naval/include/ymir/naval/forces/ThrustForces.h new file mode 100644 index 0000000..072feaf --- /dev/null +++ b/naval/include/ymir/naval/forces/ThrustForces.h @@ -0,0 +1,46 @@ +#pragma once + +#include + +#include +#include + +namespace ymir::naval +{ + +class ThrustForces final : public NavalForceModel +{ +public: + struct ThrusterConfig + { + std::array position{}; // body frame (m) + double diameter = 5.0; // propeller diameter (m) + double pitchRatio = 0.8; // P/D + double nominalRPM = 120.0; + double azimuth_deg = 0.0; // 0 = forward, +90 = starboard + double commandRPM = 0.0; // current command RPM + double rpmTimeCst = 5.0; // first-order RPM lag (s) + }; + + struct Config + { + std::vector thrusters; + double rho = 1025.0; + }; + + explicit ThrustForces(const Config& cfg); + + std::string name() const override { return "thrust"; } + + // Returns thrust magnitude for thruster [id] (used by RudderForces slipstream) + double getThrust(std::size_t id) const noexcept; + +private: + Forces computeNaval(const BodyState& state, const NavalContext& ctx) override; + + Config cfg_; + std::vector currentRPM_; // dynamic RPM state + std::vector lastThrust_; // cached for RudderForces +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/forces/TugForces.h b/naval/include/ymir/naval/forces/TugForces.h new file mode 100644 index 0000000..f2f291d --- /dev/null +++ b/naval/include/ymir/naval/forces/TugForces.h @@ -0,0 +1,44 @@ +#pragma once + +#include +#include + +#include +#include + +namespace ymir::naval +{ + +class TugForces final : public NavalForceModel +{ +public: + struct TugConfig + { + TugMode mode = TugMode::PUSH; + std::array position{}; // body-frame fairlead / contact point (m) + double angle_deg = 0.0; // force direction in body frame (0=fwd) + double bollardPull = 0.0; // N — max static pull force + double speedPushK = 1.0; // tanh speed reduction factor + + // PULL mode table axes + std::vector pullSpeeds; // m/s + std::vector pullAngles; // deg + std::vector> pullTable; // [speed][angle] → N + }; + + struct Config + { + std::vector tugs; + }; + + explicit TugForces(const Config& cfg); + + std::string name() const override { return "tug"; } + +private: + Forces computeNaval(const BodyState& state, const NavalContext& ctx) override; + + Config cfg_; +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/forces/WindForces.h b/naval/include/ymir/naval/forces/WindForces.h new file mode 100644 index 0000000..dae1396 --- /dev/null +++ b/naval/include/ymir/naval/forces/WindForces.h @@ -0,0 +1,45 @@ +#pragma once + +#include +#include + +#include +#include + +namespace ymir::naval +{ + +class WindForces final : public NavalForceModel +{ +public: + struct Config + { + WindModel model = WindModel::REGULAR; + double frontalArea = 0.0; + double lateralArea = 0.0; + double frontalHeight = 0.0; // centroid z from WAMIT origin + double lateralHeight = 0.0; + double midshipDistance = 0.0; + std::array wavesOriginPosition{}; + double length_BP = 100.0; + double beam = 30.0; + double draft = 10.0; + + // Cd table columns (separated for Interpolation::linear API) + std::vector angles; // deg 0..360 + std::vector cdx; + std::vector cdy; + std::vector cdz; + }; + + explicit WindForces(const Config& cfg); + + std::string name() const override { return "wind"; } + +private: + Forces computeNaval(const BodyState& state, const NavalContext& ctx) override; + + Config cfg_; +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/wave/WaveComponent.h b/naval/include/ymir/naval/wave/WaveComponent.h new file mode 100644 index 0000000..217c057 --- /dev/null +++ b/naval/include/ymir/naval/wave/WaveComponent.h @@ -0,0 +1,15 @@ +#pragma once + +namespace ymir::naval +{ + +struct WaveComponent +{ + double amplitude = 0.0; // m + double frequency = 0.0; // rad/s + double direction = 0.0; // rad (math convention, 0 = +x) + double wavenumber = 0.0; // rad/m (deep water: k = ω²/g) + double phase = 0.0; // rad (random) +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/wave/WaveForces.h b/naval/include/ymir/naval/wave/WaveForces.h new file mode 100644 index 0000000..ae4a538 --- /dev/null +++ b/naval/include/ymir/naval/wave/WaveForces.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include + +#include +#include + +namespace ymir::naval +{ + +class WaveForces final : public NavalForceModel +{ +public: + struct Config + { + std::array wavesOriginPosition{}; + + // WAMIT frequency/direction axes + std::vector wamitFrequencies; // rad/s + std::vector wamitDirections; // rad (math convention) + + // RAO tables [dof][freq][dir] — magnitude (m/m or rad/m) and phase (rad) + std::vector>> raoMagnitude; + std::vector>> raoPhase; + + // Slow-drift QTF (optional — zero if empty) [freq_pair][dir] + std::vector> qtfMagnitude; + + // Wave drift damping coefficient + double driftDampingCoeff = 0.0; + }; + + WaveForces(const Config& cfg, const WaveSpectrum& spectrum); + + std::string name() const override { return "wave"; } + + void resetState() noexcept override; + +private: + Forces computeNaval(const BodyState& state, const NavalContext& ctx) override; + + Config cfg_; + std::vector components_; // snapshot from WaveSpectrum + + // Per-DOF filtered RAO state for slow-drift + std::array q_rao_prev_{}; +}; + +} // namespace ymir::naval diff --git a/naval/include/ymir/naval/wave/WaveSpectrum.h b/naval/include/ymir/naval/wave/WaveSpectrum.h new file mode 100644 index 0000000..da13014 --- /dev/null +++ b/naval/include/ymir/naval/wave/WaveSpectrum.h @@ -0,0 +1,51 @@ +#pragma once + +#include + +#include +#include + +namespace ymir::naval +{ + +enum class WaveSpectrumType +{ + JONSWAP, + PIERSON, + REGULAR +}; + +class WaveSpectrum +{ +public: + struct Config + { + WaveSpectrumType spectrumType = WaveSpectrumType::JONSWAP; + double significantWaveHeight = 2.0; // Hs, m + double peakPeriod = 8.0; // Tp, s + double gamma = 3.3; // JONSWAP peak enhancement + double spreadingS = 2.0; // Mitsuyasu s exponent + double mainDirection_deg = 0.0; // principal direction (math, deg) + int n_frequencies = 32; + int n_directions = 18; + uint32_t seed = 42; + + // Frequency range (0 = auto) + double f_min = 0.0; + double f_max = 0.0; + }; + + explicit WaveSpectrum(const Config& cfg); + + std::size_t numComponents() const noexcept { return components_.size(); } + const std::vector& components() const noexcept { return components_; } + +private: + void buildJonswap(const Config& cfg); + void buildPierson(const Config& cfg); + void buildRegular(const Config& cfg); + + std::vector components_; +}; + +} // namespace ymir::naval diff --git a/naval/src/NavalForceModel.cpp b/naval/src/NavalForceModel.cpp new file mode 100644 index 0000000..042d054 --- /dev/null +++ b/naval/src/NavalForceModel.cpp @@ -0,0 +1,14 @@ +#include + +#include + +namespace ymir::naval +{ + +Forces NavalForceModel::compute(const BodyState& state) +{ + assert(ctx_ != nullptr && "NavalForceModel: context not bound — call bindContext() before step"); + return computeNaval(state, *ctx_); +} + +} // namespace ymir::naval diff --git a/naval/src/NavalSimulation.cpp b/naval/src/NavalSimulation.cpp new file mode 100644 index 0000000..6731edb --- /dev/null +++ b/naval/src/NavalSimulation.cpp @@ -0,0 +1,121 @@ +#include + +#include + +#include + +namespace ymir::naval +{ + +namespace +{ + +// Convert nautical speed+direction to body-frame (u, v) components. +// nautDeg: "from where", 0=North, 90=East, clockwise. +// Returns the velocity vector as seen by a vessel with the given yaw. +static std::array nautToBodyFrame(double speedMag, double nautDeg, double yawRad) +{ + // "from where" → "toward": add 180° + double towardNaut = nautDeg + 180.0; + // Nautical → math angle: math = 90 - naut (X=East, Y=North) + double mathRad = ymir::math::deg2rad(90.0 - towardNaut); + double vEast = speedMag * std::cos(mathRad); + double vNorth = speedMag * std::sin(mathRad); + // Rotate inertial → body: body_x = cos(ψ)·E + sin(ψ)·N + double vx = std::cos(yawRad) * vEast + std::sin(yawRad) * vNorth; + double vy = -std::sin(yawRad) * vEast + std::cos(yawRad) * vNorth; + return {vx, vy}; +} + +} // namespace + +NavalSimulation::NavalSimulation(std::unique_ptr body, const ymir::CvodeConfig& cfg) + : sim_(std::move(body), cfg) + , ctx_{} // default-init (BodyState now has default ctor); will be rebuilt on first step +{ +} + +void NavalSimulation::addNavalForceModel(std::unique_ptr model) +{ + NavalForceModel* ptr = model.get(); + ptr->bindContext(&ctx_); + navalModels_.push_back(ptr); + sim_.addForceModel(std::move(model)); // sim_ takes ownership; ptr remains valid +} + +void NavalSimulation::initialize() +{ + const ymir::Vector6& q = sim_.state().q(); + q_avg_ = q; + sim_.initialize(); + // Build a valid context now that we have a known state + ctx_ = buildContext(0.0); + for (NavalForceModel* m : navalModels_) + m->bindContext(&ctx_); +} + +void NavalSimulation::step(double dt) +{ + // Rebuild context from current state + environment + ctx_ = buildContext(dt); + + // Step the integrator (force models will use ctx_ via their bound pointer) + sim_.step(dt); + + // Update EMA after the step (new q from integrator) + constexpr double tau = 16.5; + const double alpha = 1.0 - std::exp(-dt / tau); + const ymir::Vector6& q = sim_.state().q(); + for (int i = 0; i < 6; ++i) + q_avg_[i] += alpha * (q[i] - q_avg_[i]); +} + +void NavalSimulation::reset() +{ + const ymir::Vector6& q = sim_.state().q(); + q_avg_ = q; + + for (NavalForceModel* m : navalModels_) + m->resetState(); + + sim_.initialize(); + ctx_ = buildContext(0.0); +} + +void NavalSimulation::setEnvironment(const NavalEnvironment& env) +{ + env_ = env; +} + +ymir::BodyState NavalSimulation::state() const +{ + return sim_.state(); +} + +double NavalSimulation::time() const noexcept +{ + return sim_.time(); +} + +NavalContext NavalSimulation::buildContext(double dt) const +{ + ymir::BodyState bs = sim_.state(); + double yaw = bs.yaw(); + + auto [cu, cv] = nautToBodyFrame(env_.currentSpeed, env_.currentDirectionNaut, yaw); + auto [wu, wv] = nautToBodyFrame(env_.windSpeed, env_.windDirectionNaut, yaw); + + ymir::Vector6 speedToWater{}; + speedToWater[0] = bs.u() - cu; + speedToWater[1] = bs.v() - cv; + for (int i = 2; i < 6; ++i) + speedToWater[i] = bs.qdot()[i]; + + ymir::Vector6 speedToWind{}; + speedToWind[0] = wu; + speedToWind[1] = wv; + + return NavalContext{bs, speedToWater, speedToWind, q_avg_, env_.waterDepth, env_.tide}; +} + +} // namespace ymir::naval diff --git a/naval/src/forces/CurrentForces.cpp b/naval/src/forces/CurrentForces.cpp new file mode 100644 index 0000000..ed7721f --- /dev/null +++ b/naval/src/forces/CurrentForces.cpp @@ -0,0 +1,179 @@ +#include + +#include +#include +#include + +#include +#include + +namespace ymir::naval +{ + +CurrentForces::CurrentForces(const Config& cfg) : cfg_(cfg) {} + +void CurrentForces::resetState() noexcept +{ + vdp_qz_ = 0.0; + vdp_dqzdt_ = 0.0; +} + +Forces CurrentForces::computeNaval(const BodyState& state, const NavalContext& ctx) +{ + if (cfg_.model == CurrentModel::OBOKATA) + return computeObokata(state, ctx); + return computeRegular(state, ctx); +} + +// --------------------------------------------------------------------------- +// OBOKATA — section integration over 50 hull sections +// --------------------------------------------------------------------------- +Forces CurrentForces::computeObokata(const BodyState& state, const NavalContext& ctx) +{ + const double vc_x = ctx.speedToWater[0]; + const double vc_y = ctx.speedToWater[1]; + const double r = state.r(); + + double Fx = 0.0, Fy = 0.0, Mz = 0.0; + + const int n = cfg_.n_sections; + if (n < 2) return Forces::zero(); + + const double dx = cfg_.length_BP / static_cast(n - 1); + + for (int i = 0; i < n; i++) + { + // Section x-position in body frame + double xi = (cfg_.sectionLocalPositions.size() == static_cast(n)) + ? cfg_.sectionLocalPositions[i] + : -cfg_.length_BP / 2.0 + i * dx; + + // Current velocity at section (body frame), include yaw-induced velocity + double vrel_x = vc_x; + double vrel_y = vc_y - r * xi; + + double v2 = vrel_x * vrel_x + vrel_y * vrel_y; + if (v2 < 1e-12) continue; + + // Incidence angle 0..360 + double angle_rad = std::atan2(vrel_y, vrel_x); + double angle_deg = ymir::math::wrapTo360(ymir::math::rad2deg(angle_rad)); + + double cdx = ymir::math::linear(cfg_.angles, cfg_.cdx, angle_deg); + double cdy = ymir::math::linear(cfg_.angles, cfg_.cdy, angle_deg); + + // Local sectional area proportional to section width (uniform assumed) + double ds_x = cfg_.frontalHeight * dx; + double ds_y = cfg_.lateralHeight * dx; + + double fx = 0.5 * rho_water * cdx * ds_x * v2; + double fy = 0.5 * rho_water * cdy * ds_y * v2; + + Fx += fx; + Fy += fy; + Mz += fy * xi; + } + + // Yaw moment centred at wavesOriginPosition + const double xo = cfg_.wavesOriginPosition[0]; + Mz -= Fy * xo; + + Forces fc; + fc.f[0] = Fx; + fc.f[1] = Fy; + fc.f[5] = Mz; + return fc; +} + +// --------------------------------------------------------------------------- +// REGULAR — area-based with Van der Pol VIM for lateral lift +// --------------------------------------------------------------------------- +Forces CurrentForces::computeRegular(const BodyState& state, const NavalContext& ctx) +{ + const double vc_x = ctx.speedToWater[0]; + const double vc_y = ctx.speedToWater[1]; + const double r = state.r(); + + const double dt_raw = ctx.state.dt(); + const double dt = (dt_raw > 0.0) ? dt_raw : 0.1; + + double v2 = vc_x * vc_x + vc_y * vc_y; + if (v2 < 1e-12) return Forces::zero(); + + double angle_rad = std::atan2(vc_y, vc_x); + double angle_deg = ymir::math::wrapTo360(ymir::math::rad2deg(angle_rad)); + + double cdx = ymir::math::linear(cfg_.angles, cfg_.cdx, angle_deg); + double cdy = ymir::math::linear(cfg_.angles, cfg_.cdy, angle_deg); + + // Drag forces + double Fx = 0.5 * rho_water * cdx * cfg_.frontalArea * v2; + double Fy = 0.5 * rho_water * cdy * cfg_.lateralArea * v2; + + // Van der Pol VIM oscillator for lateral lift (REGULAR mode) + double v = std::sqrt(v2); + + // Dimensionless acc driving term (lock-in acceleration parameter) + double acc0 = 2.0 * cfg_.az * v * v / (cfg_.length_BP * cfg_.length_BP); + + // Step VDP oscillator (6 sub-steps) + stepVdp(dt / 6.0, acc0); + stepVdp(dt / 6.0, acc0); + stepVdp(dt / 6.0, acc0); + stepVdp(dt / 6.0, acc0); + stepVdp(dt / 6.0, acc0); + stepVdp(dt / 6.0, acc0); + + // Lift force from VDP state + double cl_vdp = cfg_.clO * vdp_qz_; + double Fy_vim = 0.5 * rho_water * cl_vdp * cfg_.lateralArea * v2; + + // Yaw + const double xo = cfg_.wavesOriginPosition[0]; + double Mz = (Fy + Fy_vim) * (-xo); + + Forces fc; + fc.f[0] = Fx; + fc.f[1] = Fy + Fy_vim; + fc.f[5] = Mz; + + (void)r; // yaw rate reserved for future section integration in REGULAR + return fc; +} + +// --------------------------------------------------------------------------- +// Van der Pol RK4 (internal) +// --------------------------------------------------------------------------- +void CurrentForces::stepVdp(double h, double acc0) +{ + // State: [qz, dqzdt] + // dqz/dt = dqzdt + // d²qz/dt² = ez*(1 - qz²)*dqzdt - qz + acc0 + const double ez = cfg_.ez; + + auto f1 = [&](double q, double dq) -> double { return dq; }; + auto f2 = [&](double q, double dq) -> double + { + return ez * (1.0 - q * q) * dq - q + acc0; + }; + + double q = vdp_qz_; + double dq = vdp_dqzdt_; + + double k1q = h * f1(q, dq); + double k1dq = h * f2(q, dq); + + double k2q = h * f1(q + 0.5 * k1q, dq + 0.5 * k1dq); + double k2dq = h * f2(q + 0.5 * k1q, dq + 0.5 * k1dq); + + double k3q = h * f1(q + 0.5 * k2q, dq + 0.5 * k2dq); + double k3dq = h * f2(q + 0.5 * k2q, dq + 0.5 * k2dq); + + double k4q = h * f1(q + k3q, dq + k3dq); + double k4dq = h * f2(q + k3q, dq + k3dq); + + vdp_qz_ = q + (k1q + 2.0 * k2q + 2.0 * k3q + k4q) / 6.0; + vdp_dqzdt_ = dq + (k1dq + 2.0 * k2dq + 2.0 * k3dq + k4dq) / 6.0; +} + +} // namespace ymir::naval diff --git a/naval/src/forces/DampingForces.cpp b/naval/src/forces/DampingForces.cpp new file mode 100644 index 0000000..a9e7baa --- /dev/null +++ b/naval/src/forces/DampingForces.cpp @@ -0,0 +1,48 @@ +#include + +#include + +#include + +namespace ymir::naval +{ + +DampingForces::DampingForces(const Config& cfg) : cfg_(cfg) {} + +Forces DampingForces::computeNaval(const BodyState& state, const NavalContext& ctx) +{ + const auto& qdot = state.qdot(); + + // Potential (radiation) damping + Vector6 fd_pot = ymir::math::matVecProduct(cfg_.potential, qdot, -1.0); + + // Auxiliary velocity: surge+sway use speedToWater, rest use qdot + Vector6 aux{}; + aux[0] = ctx.speedToWater[0]; + aux[1] = ctx.speedToWater[1]; + for (int i = 2; i < 6; ++i) + aux[i] = qdot[i]; + + // Exponential decay factor (based on surge+sway norm) + double vNorm2 = aux[0] * aux[0] + aux[1] * aux[1]; + double decay = std::exp(-cfg_.linearDampingCoeff * vNorm2); + + Vector6 exp_vel{}; + exp_vel[0] = aux[0] * decay; + exp_vel[1] = aux[1] * decay; + for (int i = 2; i < 6; ++i) + exp_vel[i] = aux[i]; + + Vector6 fd_lin = ymir::math::matVecProduct(cfg_.linear, exp_vel, -1.0); + Vector6 fd_quad = ymir::math::matVecQuadratic(cfg_.quadratic, qdot); + // matVecQuadratic gives A*v*|v|; damping opposes motion → negate + for (int i = 0; i < 6; ++i) + fd_quad[i] = -fd_quad[i]; + + Forces fd; + for (int i = 0; i < 6; ++i) + fd.f[i] = fd_pot[i] + fd_lin[i] + fd_quad[i]; + return fd; +} + +} // namespace ymir::naval diff --git a/naval/src/forces/InertialForces.cpp b/naval/src/forces/InertialForces.cpp new file mode 100644 index 0000000..613ecb4 --- /dev/null +++ b/naval/src/forces/InertialForces.cpp @@ -0,0 +1,40 @@ +#include + +namespace ymir::naval +{ + +InertialForces::InertialForces(const InertialConfig& cfg) + : cfg_(cfg) + , M11_(cfg.totalMass[0][0]) + , M22_(cfg.totalMass[1][1]) + , dXa_(cfg.addedMass[0][0] - cfg.addedMass[1][1]) +{ +} + +Forces InertialForces::computeNaval(const BodyState& state, const NavalContext& ctx) +{ + const double u = state.u(); + const double v = state.v(); + const double w = state.w(); + const double p = state.p(); + const double q = state.q_rot(); + const double r = state.r(); // ψ̇ + const double Vc0 = ctx.speedToWater[0]; + const double Vc1 = ctx.speedToWater[1]; + + const double cx = cfg_.cg[0]; + const double cy = cfg_.cg[1]; + const double cz = cfg_.cg[2]; + const double m = cfg_.mass; + + Forces fi; + fi.f[0] = r * (M22_ * v + dXa_ * Vc1); + fi.f[1] = r * (-M11_ * u + dXa_ * Vc0); + fi.f[2] = 0.0; + fi.f[3] = m * (cz * (u * r - w * p) + cy * (u * q - v * p)); + fi.f[4] = m * (cx * (v * p - u * q) + cz * (v * r - w * q)); + fi.f[5] = r * (cfg_.addedMass[0][5] * v - cfg_.addedMass[1][5] * u); + return fi; +} + +} // namespace ymir::naval diff --git a/naval/src/forces/RestoringForces.cpp b/naval/src/forces/RestoringForces.cpp new file mode 100644 index 0000000..09c52d0 --- /dev/null +++ b/naval/src/forces/RestoringForces.cpp @@ -0,0 +1,53 @@ +#include + +#include + +#include + +namespace ymir::naval +{ + +RestoringForces::RestoringForces(const RestoringConfig& cfg) + : cfg_(cfg) + , netBuoyancy_(cfg.mass * g - cfg.volumetricWeight) +{ +} + +Forces RestoringForces::computeNaval(const BodyState& state, const NavalContext& ctx) +{ + const auto& q = state.q(); + const auto& orig = cfg_.wavesOriginPosition; + + // Displacement from equilibrium + double dq0 = q[0] - orig[0]; + double dq1 = q[1] - orig[1]; + double dq2 = q[2] - orig[2] + cfg_.draft - ctx.tide; + // dq[3..5] = q[3..5] (angles from zero) + + Forces fr; + + // Diagonal spring + fr.f[0] = -cfg_.hydro_rest[0][0] * dq0; + fr.f[1] = -cfg_.hydro_rest[1][1] * dq1; + fr.f[2] = -cfg_.hydro_rest[2][2] * dq2; + fr.f[3] = -cfg_.hydro_rest[3][3] * q[3]; + fr.f[4] = -cfg_.hydro_rest[4][4] * q[4]; + fr.f[5] = -cfg_.hydro_rest[5][5] * q[5]; + + // Heave: subtract net buoyancy offset + fr.f[2] -= netBuoyancy_; + + // Roll moment: buoyancy × centre of flotation arm, minus gravity × CG arm + const double cosRoll = std::cos(q[3]); + const double cosPitch = std::cos(q[4]); + fr.f[3] += (fr.f[2] * cfg_.cf[1] + - cfg_.mass * g * (cfg_.cg[1] - orig[1])) * cosRoll; + + // Pitch moment + fr.f[4] -= (fr.f[2] * cfg_.cf[0] + - cfg_.mass * g * (cfg_.cg[0] - orig[0])) * cosPitch; + + return fr; +} + +} // namespace ymir::naval diff --git a/naval/src/forces/RudderForces.cpp b/naval/src/forces/RudderForces.cpp new file mode 100644 index 0000000..c655005 --- /dev/null +++ b/naval/src/forces/RudderForces.cpp @@ -0,0 +1,87 @@ +#include + +#include +#include + +#include +#include +#include + +namespace ymir::naval +{ + +RudderForces::RudderForces(const Config& cfg, const ThrustForces* thrust) + : cfg_(cfg) + , thrust_(thrust) + , currentAngle_(cfg.rudders.size(), 0.0) +{ + for (std::size_t i = 0; i < cfg_.rudders.size(); ++i) + currentAngle_[i] = cfg_.rudders[i].angle_deg; +} + +Forces RudderForces::computeNaval(const BodyState& state, const NavalContext& ctx) +{ + Forces fr; + const double dt = state.dt(); + + for (std::size_t i = 0; i < cfg_.rudders.size(); ++i) + { + const auto& r = cfg_.rudders[i]; + + // Rate-limited rudder dynamics + if (dt > 0.0 && r.rateLimit > 0.0) + { + double error = r.angle_deg - currentAngle_[i]; + double max_dg = r.rateLimit * dt; + double delta = std::max(-max_dg, std::min(max_dg, error)); + currentAngle_[i] += delta; + } + else + { + currentAngle_[i] = r.angle_deg; + } + + double alpha_rad = currentAngle_[i] * M_PI / 180.0; + if (std::abs(alpha_rad) < 1e-9) continue; + + // Inflow speed at rudder (body frame u + optional slipstream) + double Va = ctx.speedToWater[0]; + + if (thrust_ != nullptr && r.thrusterIdx != std::numeric_limits::max()) + { + // Actuator disk wake: V_wake = sqrt(Va² + 2·T/(rho·A_disk)) + double T_prop = thrust_->getThrust(r.thrusterIdx); + double A_disk = M_PI / 4.0 * r.area; // approximate + double Va2 = Va * Va + 2.0 * std::abs(T_prop) / (cfg_.rho * A_disk); + Va = (T_prop >= 0.0) ? std::sqrt(Va2) : -std::sqrt(Va2); + } + + double v2 = Va * Va; + if (v2 < 1e-12) continue; + + // Lift slope with aspect ratio correction (Prandtl) + double AR = r.aspectRatio; + double CL_alpha = 2.0 * M_PI * AR / (AR + 2.0); + double CL = CL_alpha * alpha_rad; + double CD = CL * CL / (M_PI * AR); // induced drag + + double q_dyn = 0.5 * cfg_.rho * v2; + double L = q_dyn * r.area * CL; + double D = q_dyn * r.area * CD; + + // Lift acts perpendicular to inflow (sway), drag acts along inflow (surge) + double sign_Va = (Va >= 0.0) ? 1.0 : -1.0; + double fy = -L * sign_Va; // lift opposes turning + double fx = -D * sign_Va; // drag opposes motion + + fr.f[0] += fx; + fr.f[1] += fy; + fr.f[3] += r.position[1] * 0.0 - r.position[2] * fy; + fr.f[4] += r.position[2] * fx - r.position[0] * 0.0; + fr.f[5] += r.position[0] * fy - r.position[1] * fx; + } + + return fr; +} + +} // namespace ymir::naval diff --git a/naval/src/forces/SquatForces.cpp b/naval/src/forces/SquatForces.cpp new file mode 100644 index 0000000..0e21d79 --- /dev/null +++ b/naval/src/forces/SquatForces.cpp @@ -0,0 +1,57 @@ +#include + +#include + +#include +#include + +namespace ymir::naval +{ + +SquatForces::SquatForces(const Config& cfg) + : cfg_(cfg) + , nabla_(cfg.volumetricWeight / (rho_water * g)) +{ + double Cb = cfg.blockCoefficient; + if (Cb < 0.7) Cs_ = 1.7; + else if (Cb < 0.8) Cs_ = 2.0; + else Cs_ = 2.4; +} + +Forces SquatForces::computeNaval(const BodyState& state, const NavalContext& ctx) +{ + // Effective water depth + double depth = std::max(std::abs(ctx.waterDepth + ctx.tide), std::abs(state.z())); + if (depth < 0.01) depth = 0.01; // guard against zero + + double v2 = ctx.speedToWater[0] * ctx.speedToWater[0] + + ctx.speedToWater[1] * ctx.speedToWater[1]; + double v = std::sqrt(v2); + double Fn = v / std::sqrt(g * depth); + + if (Fn < 1e-6) + return Forces::zero(); + + double Cf = 0.0; + if (Fn > 0.7) + { + Cf = 0.3; + Fn = std::min(Fn, 0.8); + } + + double denom = 1.0 - Fn * Fn; + if (denom <= 0.0) denom = 1e-6; + + double L2 = cfg_.length_BP * cfg_.length_BP; + double s = -(Cs_ + Cf) * (nabla_ / (rho_water * g * L2)) * Fn * Fn / std::sqrt(denom); + + // Clamp: cannot sink below seabed + double s_min = -(depth + 0.1 + state.z()); + if (s < s_min) s = s_min; + + Forces fsq; + fsq.f[2] = cfg_.hydroRestHeave * s; + return fsq; +} + +} // namespace ymir::naval diff --git a/naval/src/forces/ThrustForces.cpp b/naval/src/forces/ThrustForces.cpp new file mode 100644 index 0000000..0612b9e --- /dev/null +++ b/naval/src/forces/ThrustForces.cpp @@ -0,0 +1,84 @@ +#include + +#include + +#include +#include + +namespace ymir::naval +{ + +ThrustForces::ThrustForces(const Config& cfg) + : cfg_(cfg) + , currentRPM_(cfg.thrusters.size(), 0.0) + , lastThrust_(cfg.thrusters.size(), 0.0) +{ + // Initialize currentRPM to commandRPM (instantaneous start) + for (std::size_t i = 0; i < cfg_.thrusters.size(); ++i) + currentRPM_[i] = cfg_.thrusters[i].commandRPM; +} + +double ThrustForces::getThrust(std::size_t id) const noexcept +{ + if (id >= lastThrust_.size()) return 0.0; + return lastThrust_[id]; +} + +Forces ThrustForces::computeNaval(const BodyState& state, const NavalContext& ctx) +{ + Forces ft; + const double dt = state.dt(); + + for (std::size_t i = 0; i < cfg_.thrusters.size(); ++i) + { + const auto& t = cfg_.thrusters[i]; + + // First-order RPM lag + if (dt > 0.0 && t.rpmTimeCst > 0.0) + { + double alpha = 1.0 - std::exp(-dt / t.rpmTimeCst); + currentRPM_[i] += alpha * (t.commandRPM - currentRPM_[i]); + } + else + { + currentRPM_[i] = t.commandRPM; + } + + double n = currentRPM_[i] / 60.0; // rev/s + if (std::abs(n) < 1e-9) + { + lastThrust_[i] = 0.0; + continue; + } + + // Open-water thrust: T = rho * n^2 * D^4 * Kt + // Simple Kt approximation from pitch ratio P/D + double D = t.diameter; + double PD = t.pitchRatio; + double Kt = 0.4 * PD - 0.1; // linear approximation, valid ~0.5 < P/D < 1.2 + Kt = std::max(0.05, std::min(Kt, 0.5)); + + double sign_n = (n >= 0.0) ? 1.0 : -1.0; + double T = sign_n * cfg_.rho * n * n * D * D * D * D * Kt; + lastThrust_[i] = T; + + // Resolve force into body frame using azimuth + double az_rad = t.azimuth_deg * M_PI / 180.0; + double fx = T * std::cos(az_rad); + double fy = T * std::sin(az_rad); + double fz = 0.0; + + ft.f[0] += fx; + ft.f[1] += fy; + ft.f[2] += fz; + + // Moments about body origin + ft.f[3] += t.position[1] * fz - t.position[2] * fy; + ft.f[4] += t.position[2] * fx - t.position[0] * fz; + ft.f[5] += t.position[0] * fy - t.position[1] * fx; + } + + return ft; +} + +} // namespace ymir::naval diff --git a/naval/src/forces/TugForces.cpp b/naval/src/forces/TugForces.cpp new file mode 100644 index 0000000..46a8b36 --- /dev/null +++ b/naval/src/forces/TugForces.cpp @@ -0,0 +1,62 @@ +#include + +#include +#include +#include + +#include +#include + +namespace ymir::naval +{ + +TugForces::TugForces(const Config& cfg) : cfg_(cfg) {} + +Forces TugForces::computeNaval(const BodyState& state, const NavalContext& ctx) +{ + Forces ft; + + for (const auto& t : cfg_.tugs) + { + if (t.bollardPull < 1e-9) continue; + + double forceMag = t.bollardPull; + + if (t.mode == TugMode::PUSH) + { + // Speed-dependent reduction: tanh curve + double v = std::sqrt(ctx.speedToWater[0] * ctx.speedToWater[0] + + ctx.speedToWater[1] * ctx.speedToWater[1]); + double factor = 1.0 - std::tanh(t.speedPushK * v); + forceMag *= factor; + } + else if (t.mode == TugMode::PULL) + { + if (!t.pullSpeeds.empty() && !t.pullAngles.empty() && !t.pullTable.empty()) + { + double v = std::sqrt(ctx.speedToWater[0] * ctx.speedToWater[0] + + ctx.speedToWater[1] * ctx.speedToWater[1]); + double a_deg = t.angle_deg; + forceMag = ymir::math::bilinear(t.pullSpeeds, t.pullAngles, t.pullTable, v, a_deg); + } + } + // ESCORTING: use bollardPull directly (simplified) + + double az_rad = t.angle_deg * M_PI / 180.0; + double fx = forceMag * std::cos(az_rad); + double fy = forceMag * std::sin(az_rad); + double fz = 0.0; + + ft.f[0] += fx; + ft.f[1] += fy; + ft.f[2] += fz; + + ft.f[3] += t.position[1] * fz - t.position[2] * fy; + ft.f[4] += t.position[2] * fx - t.position[0] * fz; + ft.f[5] += t.position[0] * fy - t.position[1] * fx; + } + + return ft; +} + +} // namespace ymir::naval diff --git a/naval/src/forces/WindForces.cpp b/naval/src/forces/WindForces.cpp new file mode 100644 index 0000000..a40e1f7 --- /dev/null +++ b/naval/src/forces/WindForces.cpp @@ -0,0 +1,60 @@ +#include + +#include +#include +#include + +#include + +namespace ymir::naval +{ + +WindForces::WindForces(const Config& cfg) : cfg_(cfg) {} + +Forces WindForces::computeNaval(const BodyState& state, const NavalContext& ctx) +{ + const double r = state.r(); // yaw rate + const auto& orig = cfg_.wavesOriginPosition; + + // Apparent wind in body frame, corrected for yaw-rate rotation + double vwd_x = ctx.speedToWind[0] - r * orig[1]; + double vwd_y = ctx.speedToWind[1] + r * orig[0]; + + double v2 = vwd_x * vwd_x + vwd_y * vwd_y; + if (v2 < 1e-12) return Forces::zero(); + + // Incidence angle 0..360 degrees + double angle_rad = std::atan2(vwd_y, vwd_x); + double angle_deg = ymir::math::wrapTo360(ymir::math::rad2deg(angle_rad)); + + double cdx = ymir::math::linear(cfg_.angles, cfg_.cdx, angle_deg); + double cdy = ymir::math::linear(cfg_.angles, cfg_.cdy, angle_deg); + double cdz = ymir::math::linear(cfg_.angles, cfg_.cdz, angle_deg); + + // Effective areas + double f_area = cfg_.frontalArea; + double l_area = cfg_.lateralArea; + if (cfg_.model == WindModel::ACSINKAGE) + { + double dz = state.z() - (orig[2] - cfg_.draft); + f_area += cfg_.beam * dz; + l_area += cfg_.length_BP * dz; + } + + Forces fwd; + fwd.f[0] = 0.5 * rho_air * cdx * f_area * v2; + fwd.f[1] = 0.5 * rho_air * cdy * l_area * v2; + + const double roll = state.roll(); + const double pitch = state.pitch(); + const double cc = std::cos(roll) * std::cos(pitch); + + fwd.f[5] = 0.5 * rho_air * cdz * l_area * cfg_.length_BP * v2 + + fwd.f[1] * (cfg_.midshipDistance - orig[0]); + fwd.f[3] = -fwd.f[1] * (cfg_.frontalHeight - orig[2]) * cc; + fwd.f[4] = fwd.f[0] * (cfg_.lateralHeight - orig[2]) * cc; + + return fwd; +} + +} // namespace ymir::naval diff --git a/naval/src/wave/WaveComponent.cpp b/naval/src/wave/WaveComponent.cpp new file mode 100644 index 0000000..8011472 --- /dev/null +++ b/naval/src/wave/WaveComponent.cpp @@ -0,0 +1,2 @@ +// WaveComponent is a POD struct — no implementation needed. +// This file exists to satisfy the CMakeLists.txt source list. diff --git a/naval/src/wave/WaveForces.cpp b/naval/src/wave/WaveForces.cpp new file mode 100644 index 0000000..cda594b --- /dev/null +++ b/naval/src/wave/WaveForces.cpp @@ -0,0 +1,80 @@ +#include + +#include +#include +#include + +#include +#include + +namespace ymir::naval +{ + +WaveForces::WaveForces(const Config& cfg, const WaveSpectrum& spectrum) + : cfg_(cfg) + , components_(spectrum.components()) +{ + q_rao_prev_.fill(0.0); +} + +void WaveForces::resetState() noexcept +{ + q_rao_prev_.fill(0.0); +} + +Forces WaveForces::computeNaval(const BodyState& state, const NavalContext& ctx) +{ + if (components_.empty()) return Forces::zero(); + + const double t = state.time(); + const double xo = cfg_.wavesOriginPosition[0]; + const double yo = cfg_.wavesOriginPosition[1]; + + Forces fw; + + bool hasRao = !cfg_.wamitFrequencies.empty() && !cfg_.raoMagnitude.empty(); + + for (const auto& comp : components_) + { + // Wave elevation phase at origin + double phase = comp.frequency * t + - comp.wavenumber * (xo * std::cos(comp.direction) + + yo * std::sin(comp.direction)) + + comp.phase; + + // DOF 2,3,4: simple heave/roll/pitch — use cosine excitation if no RAO + // DOF 0,1,5: surge/sway/yaw — use RAO if available + + for (int dof = 0; dof < 6; ++dof) + { + double mag = 0.0; + double ph = 0.0; + + if (hasRao && dof < static_cast(cfg_.raoMagnitude.size())) + { + const auto& raoM = cfg_.raoMagnitude[dof]; + const auto& raoP = cfg_.raoPhase[dof]; + + if (!raoM.empty() && !raoM[0].empty()) + { + mag = ymir::math::bilinear(cfg_.wamitFrequencies, + cfg_.wamitDirections, + raoM, + comp.frequency, + comp.direction); + ph = ymir::math::bilinear(cfg_.wamitFrequencies, + cfg_.wamitDirections, + raoP, + comp.frequency, + comp.direction); + } + } + + fw.f[dof] += comp.amplitude * mag * std::cos(phase + ph); + } + } + + return fw; +} + +} // namespace ymir::naval diff --git a/naval/src/wave/WaveSpectrum.cpp b/naval/src/wave/WaveSpectrum.cpp new file mode 100644 index 0000000..8d98442 --- /dev/null +++ b/naval/src/wave/WaveSpectrum.cpp @@ -0,0 +1,172 @@ +#include + +#include + +#include +#include +#include + +namespace ymir::naval +{ + +WaveSpectrum::WaveSpectrum(const Config& cfg) +{ + switch (cfg.spectrumType) + { + case WaveSpectrumType::JONSWAP: buildJonswap(cfg); break; + case WaveSpectrumType::PIERSON: buildPierson(cfg); break; + case WaveSpectrumType::REGULAR: buildRegular(cfg); break; + } +} + +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +static double jonswapSpectral(double omega, double omega_p, double gamma) +{ + if (omega < 1e-8) return 0.0; + double sigma = (omega <= omega_p) ? 0.07 : 0.09; + double r = std::exp(-0.5 * std::pow((omega - omega_p) / (sigma * omega_p), 2.0)); + double S_pm = std::exp(-5.0 / 4.0 * std::pow(omega_p / omega, 4.0)) / std::pow(omega, 5.0); + return S_pm * std::pow(gamma, r); +} + +static std::vector buildFrequencies(const WaveSpectrum::Config& cfg) +{ + double omega_p = 2.0 * M_PI / cfg.peakPeriod; + double f_min = (cfg.f_min > 0.0) ? cfg.f_min : omega_p * 0.5; + double f_max = (cfg.f_max > 0.0) ? cfg.f_max : omega_p * 4.0; + + int n = std::max(cfg.n_frequencies, 1); + std::vector freqs(n); + double dw = (f_max - f_min) / (n - 1 > 0 ? n - 1 : 1); + for (int i = 0; i < n; ++i) + freqs[i] = f_min + i * dw; + return freqs; +} + +static std::vector buildDirections(const WaveSpectrum::Config& cfg) +{ + int n = std::max(cfg.n_directions, 1); + std::vector dirs(n); + double theta0 = cfg.mainDirection_deg * M_PI / 180.0; + double d_theta = (n > 1) ? (2.0 * M_PI / n) : 0.0; + for (int j = 0; j < n; ++j) + dirs[j] = theta0 + (j - n / 2) * d_theta; + return dirs; +} + +static std::vector buildSpreadingWeights(const std::vector& dirs, + double mainDir, + double s) +{ + int n = static_cast(dirs.size()); + std::vector w(n); + double dtheta = (n > 1) ? (2.0 * M_PI / n) : 1.0; + + // cos^(2s)((theta - theta0)/2) spreading + for (int j = 0; j < n; ++j) + { + double diff = dirs[j] - mainDir; + double c = std::cos(diff / 2.0); + w[j] = std::max(std::pow(c * c, s), 0.0) * dtheta; + } + + // Normalize so sum = 1 + double total = 0.0; + for (double v : w) total += v; + if (total > 1e-12) + for (double& v : w) v /= total; + else + for (double& v : w) v = 1.0 / n; + + return w; +} + +// --------------------------------------------------------------------------- +// JONSWAP +// --------------------------------------------------------------------------- + +void WaveSpectrum::buildJonswap(const Config& cfg) +{ + double omega_p = 2.0 * M_PI / cfg.peakPeriod; + auto freqs = buildFrequencies(cfg); + auto dirs = buildDirections(cfg); + + double mainDir = cfg.mainDirection_deg * M_PI / 180.0; + auto spread = buildSpreadingWeights(dirs, mainDir, cfg.spreadingS); + + int Nf = static_cast(freqs.size()); + int Nd = static_cast(dirs.size()); + + // Unscaled spectrum + std::vector S(Nf); + for (int i = 0; i < Nf; ++i) + S[i] = jonswapSpectral(freqs[i], omega_p, cfg.gamma); + + // Integrate for normalization: m0_unscaled = Σ S[i] * dω + double m0_unscaled = 0.0; + for (int i = 0; i < Nf; ++i) + { + double dw = (i + 1 < Nf) ? (freqs[i + 1] - freqs[i]) : (freqs[i] - freqs[i - 1]); + m0_unscaled += S[i] * dw; + } + + // Scale so m0 = (Hs/4)² + double target_m0 = (cfg.significantWaveHeight / 4.0) * (cfg.significantWaveHeight / 4.0); + double scale = (m0_unscaled > 1e-20) ? (target_m0 / m0_unscaled) : 1.0; + + std::mt19937 rng(cfg.seed); + std::uniform_real_distribution uniform(0.0, 2.0 * M_PI); + + components_.reserve(Nf * Nd); + for (int i = 0; i < Nf; ++i) + { + double dw = (i + 1 < Nf) ? (freqs[i + 1] - freqs[i]) : (freqs[i] - freqs[i - 1]); + double Si = S[i] * scale; + double k = freqs[i] * freqs[i] / g; // deep water dispersion + + for (int j = 0; j < Nd; ++j) + { + double amp = std::sqrt(2.0 * Si * dw * spread[j]); + if (amp < 1e-12) continue; + WaveComponent comp; + comp.amplitude = amp; + comp.frequency = freqs[i]; + comp.direction = dirs[j]; + comp.wavenumber = k; + comp.phase = uniform(rng); + components_.push_back(comp); + } + } +} + +// --------------------------------------------------------------------------- +// PIERSON-MOSKOWITZ (γ = 1) +// --------------------------------------------------------------------------- + +void WaveSpectrum::buildPierson(const Config& cfg) +{ + Config pm = cfg; + pm.gamma = 1.0; + pm.spectrumType = WaveSpectrumType::JONSWAP; + buildJonswap(pm); +} + +// --------------------------------------------------------------------------- +// REGULAR +// --------------------------------------------------------------------------- + +void WaveSpectrum::buildRegular(const Config& cfg) +{ + WaveComponent comp; + comp.amplitude = cfg.significantWaveHeight / 2.0; + comp.frequency = 2.0 * M_PI / cfg.peakPeriod; + comp.direction = cfg.mainDirection_deg * M_PI / 180.0; + comp.wavenumber = comp.frequency * comp.frequency / g; + comp.phase = 0.0; + components_.push_back(comp); +} + +} // namespace ymir::naval diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 13f7756..5d32413 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -16,3 +16,28 @@ target_link_libraries(ymir_tests include(CTest) include(Catch) catch_discover_tests(ymir_tests) + +# Naval tests +add_executable(ymir_naval_tests + naval/TestNavalInfra.cpp + naval/TestInertialForces.cpp + naval/TestRestoringForces.cpp + naval/TestDampingForces.cpp + naval/TestSquatForces.cpp + naval/TestWindForces.cpp + naval/TestCurrentForces.cpp + naval/TestWaveSpectrum.cpp + naval/TestWaveForces.cpp + naval/TestThrustForces.cpp + naval/TestRudderForces.cpp + naval/TestTugForces.cpp + naval/TestNavalIntegration.cpp +) + +target_link_libraries(ymir_naval_tests + PRIVATE + ymir_naval + Catch2::Catch2WithMain +) + +catch_discover_tests(ymir_naval_tests) diff --git a/tests/naval/TestCurrentForces.cpp b/tests/naval/TestCurrentForces.cpp new file mode 100644 index 0000000..c6dd4df --- /dev/null +++ b/tests/naval/TestCurrentForces.cpp @@ -0,0 +1,108 @@ +#include +#include + +#include +#include +#include + +using Catch::Approx; +using namespace ymir::naval; + +static CurrentForces::Config makeObokataCfg() +{ + CurrentForces::Config cfg{}; + cfg.model = CurrentModel::OBOKATA; + cfg.length_BP = 100.0; + cfg.beam = 20.0; + cfg.frontalHeight = 5.0; + cfg.lateralHeight = 5.0; + cfg.n_sections = 10; + cfg.angles = {0.0, 90.0, 180.0, 270.0, 360.0}; + cfg.cdx = {1.0, 0.0, -1.0, 0.0, 1.0}; + cfg.cdy = {0.0, 1.0, 0.0, -1.0, 0.0}; + cfg.cdz = {0.0, 0.0, 0.0, 0.0, 0.0}; + return cfg; +} + +static CurrentForces::Config makeRegularCfg() +{ + CurrentForces::Config cfg{}; + cfg.model = CurrentModel::REGULAR; + cfg.length_BP = 100.0; + cfg.frontalArea = 200.0; + cfg.lateralArea = 1000.0; + cfg.st = 0.2; + cfg.ez = 0.3; + cfg.az = 0.1; + cfg.clO = 1.0; + cfg.angles = {0.0, 90.0, 180.0, 270.0, 360.0}; + cfg.cdx = {1.0, 0.0, -1.0, 0.0, 1.0}; + cfg.cdy = {0.0, 1.0, 0.0, -1.0, 0.0}; + cfg.cdz = {0.0, 0.0, 0.0, 0.0, 0.0}; + return cfg; +} + +static NavalContext makeCtx(double vc_u, double vc_v) +{ + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + ymir::BodyState bs(q, qdot, 0.0, 0.1); + NavalContext ctx{}; + ctx.state = bs; + ctx.speedToWater[0] = vc_u; + ctx.speedToWater[1] = vc_v; + return ctx; +} + +TEST_CASE("CurrentForces OBOKATA zero at rest") +{ + CurrentForces model(makeObokataCfg()); + NavalContext ctx = makeCtx(0.0, 0.0); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + for (int i = 0; i < 6; ++i) + REQUIRE(forces.f[i] == Approx(0.0).margin(1e-10)); +} + +TEST_CASE("CurrentForces OBOKATA current from ahead produces surge force") +{ + CurrentForces model(makeObokataCfg()); + NavalContext ctx = makeCtx(2.0, 0.0); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + REQUIRE(forces.f[0] != Approx(0.0).margin(1e-4)); +} + +TEST_CASE("CurrentForces REGULAR zero at rest") +{ + CurrentForces model(makeRegularCfg()); + NavalContext ctx = makeCtx(0.0, 0.0); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + for (int i = 0; i < 6; ++i) + REQUIRE(forces.f[i] == Approx(0.0).margin(1e-10)); +} + +TEST_CASE("CurrentForces VDP resetState zeros oscillator") +{ + CurrentForces model(makeRegularCfg()); + NavalContext ctx = makeCtx(2.0, 0.0); + model.bindContext(&ctx); + + // Run a few steps to excite VDP + for (int i = 0; i < 10; ++i) + model.compute(ctx.state); + + // Reset should zero the VDP state + model.resetState(); + + // After reset, VDP output starts from zero again + NavalContext ctx2 = makeCtx(0.0, 0.0); + model.bindContext(&ctx2); + auto forces = model.compute(ctx2.state); + for (int i = 0; i < 6; ++i) + REQUIRE(forces.f[i] == Approx(0.0).margin(1e-10)); +} diff --git a/tests/naval/TestDampingForces.cpp b/tests/naval/TestDampingForces.cpp new file mode 100644 index 0000000..37fd6b5 --- /dev/null +++ b/tests/naval/TestDampingForces.cpp @@ -0,0 +1,54 @@ +#include +#include + +#include +#include +#include +#include + +using Catch::Approx; +using namespace ymir::naval; + +static NavalContext makeCtx(double u = 0.0, double v = 0.0) +{ + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + qdot[0] = u; + qdot[1] = v; + ymir::BodyState bs(q, qdot, 0.0, 0.1); + NavalContext ctx{}; + ctx.state = bs; + ctx.speedToWater[0] = u; + ctx.speedToWater[1] = v; + return ctx; +} + +TEST_CASE("DampingForces zero velocity produces zero force") +{ + DampingForces::Config cfg{}; + cfg.linear[0][0] = 100.0; + cfg.quadratic[0][0] = 50.0; + + DampingForces model(cfg); + NavalContext ctx = makeCtx(0.0, 0.0); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + for (int i = 0; i < 6; ++i) + REQUIRE(forces.f[i] == Approx(0.0).margin(1e-12)); +} + +TEST_CASE("DampingForces linear surge damps surge velocity") +{ + DampingForces::Config cfg{}; + cfg.linear[0][0] = 1000.0; // 1 kN·s/m + cfg.linearDampingCoeff = 0.0; // disable exponential decay for clean check + + DampingForces model(cfg); + NavalContext ctx = makeCtx(2.0, 0.0); // u = 2 m/s + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + // F = -B * u = -1000 * 2 = -2000 N + REQUIRE(forces.f[0] == Approx(-2000.0).margin(1.0)); +} diff --git a/tests/naval/TestInertialForces.cpp b/tests/naval/TestInertialForces.cpp new file mode 100644 index 0000000..172ccad --- /dev/null +++ b/tests/naval/TestInertialForces.cpp @@ -0,0 +1,59 @@ +#include +#include + +#include +#include +#include +#include + +using Catch::Approx; +using namespace ymir::naval; + +static NavalContext makeCtx(double r = 0.0, double u = 0.0, double v = 0.0) +{ + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + qdot[0] = u; + qdot[1] = v; + qdot[5] = r; + ymir::BodyState bs(q, qdot, 0.0, 0.1); + NavalContext ctx{}; + ctx.state = bs; + return ctx; +} + +TEST_CASE("InertialForces constructs without crash") +{ + InertialConfig cfg{}; + for (int i = 0; i < 6; ++i) cfg.totalMass[i][i] = 1e6; + cfg.mass = 1e6; + + InertialForces model(cfg); + REQUIRE(true); +} + +TEST_CASE("InertialForces Coriolis coupling r·v produces surge force") +{ + InertialConfig cfg{}; + // totalMass = body mass + added mass diagonal + for (int i = 0; i < 6; ++i) cfg.totalMass[i][i] = 1000.0; + cfg.totalMass[0][0] = 1100.0; // body + Xa + cfg.totalMass[1][1] = 1200.0; // body + Ya + cfg.mass = 1000.0; + + for (int i = 0; i < 6; ++i) + for (int j = 0; j < 6; ++j) + cfg.addedMass[i][j] = 0.0; + cfg.addedMass[0][0] = 100.0; + cfg.addedMass[1][1] = 200.0; + + NavalContext ctx = makeCtx(0.5, 0.0, 2.0); // r=0.5 rad/s, v=2 m/s + + InertialForces model(cfg); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + // f[0] = r * (M22*v + dXa*Vc1), with r=0.5, v=2, M22=1200, dXa=100-200=-100 + // f[0] = 0.5 * (1200*2 + 0) = 1200 + REQUIRE(forces.f[0] == Approx(1200.0).margin(1.0)); +} diff --git a/tests/naval/TestNavalInfra.cpp b/tests/naval/TestNavalInfra.cpp new file mode 100644 index 0000000..fe5c80f --- /dev/null +++ b/tests/naval/TestNavalInfra.cpp @@ -0,0 +1,79 @@ +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +using Catch::Approx; +using namespace ymir::naval; + +static std::unique_ptr makeTestBody() +{ + ymir::Matrix6x6 mass{}; + for (int i = 0; i < 6; ++i) mass[i][i] = 1000.0; + + ymir::Matrix6x6 damp{}; + + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + + return std::make_unique(mass, damp, q, qdot); +} + +TEST_CASE("NavalContext default construction") +{ + NavalContext ctx{}; + REQUIRE(ctx.waterDepth == Approx(100.0)); + REQUIRE(ctx.tide == Approx(0.0)); + for (int i = 0; i < 6; ++i) + { + REQUIRE(ctx.speedToWater[i] == Approx(0.0)); + REQUIRE(ctx.speedToWind[i] == Approx(0.0)); + REQUIRE(ctx.q_avg[i] == Approx(0.0)); + } +} + +TEST_CASE("PhysicalConstants values") +{ + REQUIRE(g == Approx(9.81)); + REQUIRE(rho_water == Approx(1025.0)); + REQUIRE(rho_air == Approx(1.225)); +} + +TEST_CASE("NavalEnvironment default values") +{ + NavalEnvironment env{}; + REQUIRE(env.currentSpeed == Approx(0.0)); + REQUIRE(env.windSpeed == Approx(0.0)); + REQUIRE(env.waterDepth == Approx(100.0)); + REQUIRE(env.tide == Approx(0.0)); +} + +TEST_CASE("NavalSimulation constructs without crash") +{ + ymir::CvodeConfig cfg{}; + cfg.reltol = 1e-6; + cfg.abstol = 1e-9; + + NavalSimulation sim(makeTestBody(), cfg); + sim.initialize(); + REQUIRE(sim.time() == Approx(0.0)); +} + +TEST_CASE("NavalSimulation step advances time") +{ + ymir::CvodeConfig cfg{}; + cfg.reltol = 1e-6; + cfg.abstol = 1e-9; + + NavalSimulation sim(makeTestBody(), cfg); + sim.initialize(); + sim.step(0.1); + REQUIRE(sim.time() == Approx(0.1).margin(1e-10)); +} diff --git a/tests/naval/TestNavalIntegration.cpp b/tests/naval/TestNavalIntegration.cpp new file mode 100644 index 0000000..9d37ee4 --- /dev/null +++ b/tests/naval/TestNavalIntegration.cpp @@ -0,0 +1,76 @@ +#include +#include + +#include +#include +#include +#include +#include + +#include + +using Catch::Approx; +using namespace ymir::naval; + +static std::unique_ptr makeTestBody(double m = 1e6) +{ + ymir::Matrix6x6 mass{}; + for (int i = 0; i < 6; ++i) mass[i][i] = m; + ymir::Matrix6x6 damp{}; + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + return std::make_unique(mass, damp, q, qdot); +} + +TEST_CASE("NavalSimulation free drift: vessel stays near origin without forces") +{ + ymir::CvodeConfig cfg{}; + cfg.reltol = 1e-6; + cfg.abstol = 1e-9; + + NavalSimulation sim(makeTestBody(), cfg); + sim.initialize(); + + for (int i = 0; i < 10; ++i) + sim.step(0.5); + + auto state = sim.state(); + for (int i = 0; i < 6; ++i) + { + REQUIRE(std::abs(state.q()[i]) < 1e-8); + REQUIRE(std::abs(state.qdot()[i]) < 1e-8); + } +} + +TEST_CASE("NavalSimulation damping force model reduces velocity") +{ + ymir::CvodeConfig cfg{}; + cfg.reltol = 1e-6; + cfg.abstol = 1e-9; + + // Body with initial surge velocity + ymir::Matrix6x6 mass{}; + for (int i = 0; i < 6; ++i) mass[i][i] = 1e6; + ymir::Matrix6x6 damp{}; + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + qdot[0] = 5.0; // 5 m/s surge + + auto body = std::make_unique(mass, damp, q, qdot); + + NavalSimulation sim(std::move(body), cfg); + + DampingForces::Config dcfg{}; + dcfg.linear[0][0] = 1e5; // heavy linear damping + dcfg.linearDampingCoeff = 0.0; // disable exponential decay for predictable tau + + sim.addNavalForceModel(std::make_unique(dcfg)); + sim.initialize(); + + // Step 10 seconds + for (int i = 0; i < 20; ++i) + sim.step(0.5); + + auto state = sim.state(); + REQUIRE(state.u() < 5.0); // velocity reduced +} diff --git a/tests/naval/TestRestoringForces.cpp b/tests/naval/TestRestoringForces.cpp new file mode 100644 index 0000000..49ae69f --- /dev/null +++ b/tests/naval/TestRestoringForces.cpp @@ -0,0 +1,62 @@ +#include +#include + +#include +#include +#include +#include +#include + +using Catch::Approx; +using namespace ymir::naval; + +static NavalContext makeCtx() +{ + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + ymir::BodyState bs(q, qdot, 0.0, 0.1); + NavalContext ctx{}; + ctx.state = bs; + return ctx; +} + +TEST_CASE("RestoringForces zero displacement at neutral") +{ + RestoringConfig cfg{}; + cfg.volumetricWeight = 1000.0 * g; // 1 tonne vessel + cfg.mass = 1000.0; + cfg.hydro_rest[2][2] = 1000.0; + cfg.hydro_rest[3][3] = 5000.0; + cfg.hydro_rest[4][4] = 5000.0; + + RestoringForces model(cfg); + NavalContext ctx = makeCtx(); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + // net buoyancy = mass*g - volumetricWeight = 0 → heave = 0 + REQUIRE(forces.f[2] == Approx(0.0).margin(1e-3)); + REQUIRE(forces.f[3] == Approx(0.0).margin(1e-3)); + REQUIRE(forces.f[4] == Approx(0.0).margin(1e-3)); +} + +TEST_CASE("RestoringForces roll spring opposes positive roll") +{ + RestoringConfig cfg{}; + cfg.volumetricWeight = 0.0; + cfg.mass = 0.0; + cfg.hydro_rest[3][3] = 10000.0; // K_roll = 10 kN/rad + + RestoringForces model(cfg); + + ymir::Vector6 q{}; + q[3] = 0.1; // roll = 0.1 rad + ymir::Vector6 qdot{}; + ymir::BodyState bs(q, qdot, 0.0, 0.1); + NavalContext ctx{}; + ctx.state = bs; + model.bindContext(&ctx); + + auto forces = model.compute(bs); + REQUIRE(forces.f[3] < 0.0); // restoring moment opposes roll +} diff --git a/tests/naval/TestRudderForces.cpp b/tests/naval/TestRudderForces.cpp new file mode 100644 index 0000000..9589771 --- /dev/null +++ b/tests/naval/TestRudderForces.cpp @@ -0,0 +1,59 @@ +#include +#include + +#include +#include +#include + +using Catch::Approx; +using namespace ymir::naval; + +static NavalContext makeCtx(double u = 5.0) +{ + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + qdot[0] = u; + ymir::BodyState bs(q, qdot, 0.0, 0.1); + NavalContext ctx{}; + ctx.state = bs; + ctx.speedToWater[0] = u; + return ctx; +} + +TEST_CASE("RudderForces zero angle produces no lateral force") +{ + RudderForces::Config cfg{}; + RudderForces::RudderConfig r{}; + r.position = {-50.0, 0.0, -3.0}; + r.area = 20.0; + r.aspectRatio = 2.0; + r.angle_deg = 0.0; + cfg.rudders.push_back(r); + + RudderForces model(cfg); + NavalContext ctx = makeCtx(5.0); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + REQUIRE(forces.f[1] == Approx(0.0).margin(1e-6)); + REQUIRE(forces.f[5] == Approx(0.0).margin(1e-6)); +} + +TEST_CASE("RudderForces positive angle produces sway force and yaw moment") +{ + RudderForces::Config cfg{}; + RudderForces::RudderConfig r{}; + r.position = {-50.0, 0.0, -3.0}; + r.area = 20.0; + r.aspectRatio = 2.0; + r.angle_deg = 20.0; // 20 degree rudder + cfg.rudders.push_back(r); + + RudderForces model(cfg); + NavalContext ctx = makeCtx(5.0); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + REQUIRE(forces.f[1] != Approx(0.0).margin(1e-3)); + REQUIRE(forces.f[5] != Approx(0.0).margin(1e-3)); +} diff --git a/tests/naval/TestSquatForces.cpp b/tests/naval/TestSquatForces.cpp new file mode 100644 index 0000000..b096358 --- /dev/null +++ b/tests/naval/TestSquatForces.cpp @@ -0,0 +1,57 @@ +#include +#include + +#include +#include +#include +#include + +using Catch::Approx; +using namespace ymir::naval; + +static NavalContext makeCtx(double u = 0.0, double depth = 20.0) +{ + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + qdot[0] = u; + ymir::BodyState bs(q, qdot, 0.0, 0.1); + NavalContext ctx{}; + ctx.state = bs; + ctx.waterDepth = depth; + ctx.speedToWater[0] = u; + return ctx; +} + +TEST_CASE("SquatForces zero at rest") +{ + SquatForces::Config cfg{}; + cfg.blockCoefficient = 0.75; + cfg.volumetricWeight = 1e6 * g; + cfg.length_BP = 100.0; + cfg.hydroRestHeave = 1e5; + + SquatForces model(cfg); + NavalContext ctx = makeCtx(0.0); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + REQUIRE(forces.f[2] == Approx(0.0).margin(1e-6)); +} + +TEST_CASE("SquatForces sinkage negative heave when moving") +{ + SquatForces::Config cfg{}; + cfg.blockCoefficient = 0.75; + cfg.volumetricWeight = 1e6 * g; + cfg.length_BP = 100.0; + cfg.hydroRestHeave = 1e6; // large stiffness to amplify + + SquatForces model(cfg); + // Speed = 3 m/s in 20 m water + NavalContext ctx = makeCtx(3.0, 20.0); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + // Squat sinks vessel → negative heave force (or zero; magnitude uncertain) + REQUIRE(forces.f[2] <= 0.0); +} diff --git a/tests/naval/TestThrustForces.cpp b/tests/naval/TestThrustForces.cpp new file mode 100644 index 0000000..bd28fb9 --- /dev/null +++ b/tests/naval/TestThrustForces.cpp @@ -0,0 +1,60 @@ +#include +#include + +#include +#include +#include + +using Catch::Approx; +using namespace ymir::naval; + +static NavalContext makeCtx() +{ + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + ymir::BodyState bs(q, qdot, 0.0, 0.1); + NavalContext ctx{}; + ctx.state = bs; + return ctx; +} + +TEST_CASE("ThrustForces zero RPM produces no thrust") +{ + ThrustForces::Config cfg{}; + ThrustForces::ThrusterConfig t{}; + t.position = {-50.0, 0.0, -3.0}; + t.diameter = 5.0; + t.pitchRatio = 0.8; + t.nominalRPM = 120.0; + t.azimuth_deg = 0.0; + t.commandRPM = 0.0; + cfg.thrusters.push_back(t); + + ThrustForces model(cfg); + NavalContext ctx = makeCtx(); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + for (int i = 0; i < 6; ++i) + REQUIRE(forces.f[i] == Approx(0.0).margin(1e-6)); +} + +TEST_CASE("ThrustForces positive RPM aft thruster produces positive surge") +{ + ThrustForces::Config cfg{}; + ThrustForces::ThrusterConfig t{}; + t.position = {-50.0, 0.0, -3.0}; + t.diameter = 5.0; + t.pitchRatio = 0.8; + t.nominalRPM = 120.0; + t.azimuth_deg = 0.0; // pointing forward + t.commandRPM = 120.0; + cfg.thrusters.push_back(t); + + ThrustForces model(cfg); + NavalContext ctx = makeCtx(); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + REQUIRE(forces.f[0] > 0.0); +} diff --git a/tests/naval/TestTugForces.cpp b/tests/naval/TestTugForces.cpp new file mode 100644 index 0000000..2852317 --- /dev/null +++ b/tests/naval/TestTugForces.cpp @@ -0,0 +1,56 @@ +#include +#include + +#include +#include +#include + +using Catch::Approx; +using namespace ymir::naval; + +static NavalContext makeCtx() +{ + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + ymir::BodyState bs(q, qdot, 0.0, 0.1); + NavalContext ctx{}; + ctx.state = bs; + return ctx; +} + +TEST_CASE("TugForces PUSH zero bollard produces no force") +{ + TugForces::Config cfg{}; + TugForces::TugConfig t{}; + t.mode = TugMode::PUSH; + t.position = {50.0, 0.0, 0.0}; + t.angle_deg = 0.0; + t.bollardPull = 0.0; + cfg.tugs.push_back(t); + + TugForces model(cfg); + NavalContext ctx = makeCtx(); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + for (int i = 0; i < 6; ++i) + REQUIRE(forces.f[i] == Approx(0.0).margin(1e-10)); +} + +TEST_CASE("TugForces PUSH positive bollard produces force along heading") +{ + TugForces::Config cfg{}; + TugForces::TugConfig t{}; + t.mode = TugMode::PUSH; + t.position = {50.0, 0.0, 0.0}; + t.angle_deg = 0.0; // pushing along +x (aft → fwd) + t.bollardPull = 100000.0; // 100 kN + cfg.tugs.push_back(t); + + TugForces model(cfg); + NavalContext ctx = makeCtx(); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + REQUIRE(forces.f[0] != Approx(0.0).margin(1.0)); +} diff --git a/tests/naval/TestWaveForces.cpp b/tests/naval/TestWaveForces.cpp new file mode 100644 index 0000000..2e5c764 --- /dev/null +++ b/tests/naval/TestWaveForces.cpp @@ -0,0 +1,66 @@ +#include +#include + +#include +#include +#include +#include + +using Catch::Approx; +using namespace ymir::naval; + +static NavalContext makeCtx() +{ + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + ymir::BodyState bs(q, qdot, 0.0, 0.1); + NavalContext ctx{}; + ctx.state = bs; + return ctx; +} + +static WaveSpectrum makeSpectrum() +{ + WaveSpectrum::Config cfg{}; + cfg.spectrumType = WaveSpectrumType::REGULAR; + cfg.significantWaveHeight = 1.0; + cfg.peakPeriod = 8.0; + cfg.mainDirection_deg = 0.0; + cfg.seed = 0; + cfg.n_frequencies = 1; + cfg.n_directions = 1; + return WaveSpectrum(cfg); +} + +TEST_CASE("WaveForces constructs with WAMIT tables") +{ + WaveForces::Config cfg{}; + cfg.wavesOriginPosition = {0.0, 0.0, 0.0}; + // Minimal 1x1 RAO table (1 frequency, 1 direction) + cfg.wamitFrequencies = {0.785}; // rad/s (8s period) + cfg.wamitDirections = {0.0}; + // RAO magnitude and phase for 6 DOF x 1 freq x 1 dir + cfg.raoMagnitude.assign(6, std::vector>(1, std::vector(1, 0.0))); + cfg.raoPhase.assign(6, std::vector>(1, std::vector(1, 0.0))); + // Set heave RAO = 1.0 (unit response) + cfg.raoMagnitude[2][0][0] = 1.0; + + WaveForces model(cfg, makeSpectrum()); + REQUIRE(true); // construction succeeds +} + +TEST_CASE("WaveForces compute does not throw") +{ + WaveForces::Config cfg{}; + cfg.wavesOriginPosition = {0.0, 0.0, 0.0}; + cfg.wamitFrequencies = {0.785}; + cfg.wamitDirections = {0.0}; + cfg.raoMagnitude.assign(6, std::vector>(1, std::vector(1, 0.0))); + cfg.raoPhase.assign(6, std::vector>(1, std::vector(1, 0.0))); + + WaveForces model(cfg, makeSpectrum()); + NavalContext ctx = makeCtx(); + model.bindContext(&ctx); + + REQUIRE_NOTHROW(model.compute(ctx.state)); +} diff --git a/tests/naval/TestWaveSpectrum.cpp b/tests/naval/TestWaveSpectrum.cpp new file mode 100644 index 0000000..3ea1e39 --- /dev/null +++ b/tests/naval/TestWaveSpectrum.cpp @@ -0,0 +1,79 @@ +#include +#include + +#include + +using Catch::Approx; +using namespace ymir::naval; + +TEST_CASE("WaveSpectrum JONSWAP constructs without crash") +{ + WaveSpectrum::Config cfg{}; + cfg.spectrumType = WaveSpectrumType::JONSWAP; + cfg.significantWaveHeight = 2.0; + cfg.peakPeriod = 8.0; + cfg.gamma = 3.3; + cfg.seed = 42; + cfg.n_frequencies = 20; + cfg.n_directions = 8; + cfg.spreadingS = 2.0; + cfg.mainDirection_deg = 0.0; + + WaveSpectrum spectrum(cfg); + REQUIRE(spectrum.numComponents() > 0); +} + +TEST_CASE("WaveSpectrum PIERSON constructs") +{ + WaveSpectrum::Config cfg{}; + cfg.spectrumType = WaveSpectrumType::PIERSON; + cfg.significantWaveHeight = 1.5; + cfg.peakPeriod = 6.0; + cfg.seed = 123; + cfg.n_frequencies = 10; + cfg.n_directions = 4; + cfg.spreadingS = 1.0; + cfg.mainDirection_deg = 45.0; + + WaveSpectrum spectrum(cfg); + REQUIRE(spectrum.numComponents() > 0); +} + +TEST_CASE("WaveSpectrum REGULAR has single component") +{ + WaveSpectrum::Config cfg{}; + cfg.spectrumType = WaveSpectrumType::REGULAR; + cfg.significantWaveHeight = 1.0; + cfg.peakPeriod = 8.0; + cfg.mainDirection_deg = 0.0; + cfg.seed = 0; + cfg.n_frequencies = 1; + cfg.n_directions = 1; + + WaveSpectrum spectrum(cfg); + REQUIRE(spectrum.numComponents() == 1); +} + +TEST_CASE("WaveSpectrum total variance matches Hs estimate") +{ + WaveSpectrum::Config cfg{}; + cfg.spectrumType = WaveSpectrumType::JONSWAP; + cfg.significantWaveHeight = 3.0; + cfg.peakPeriod = 10.0; + cfg.gamma = 3.3; + cfg.seed = 7; + cfg.n_frequencies = 64; + cfg.n_directions = 18; + cfg.spreadingS = 2.0; + cfg.mainDirection_deg = 0.0; + + WaveSpectrum spectrum(cfg); + // Total wave variance = sum of (a_i^2 / 2) + double m0 = 0.0; + for (const auto& comp : spectrum.components()) + m0 += comp.amplitude * comp.amplitude / 2.0; + + double Hs_computed = 4.0 * std::sqrt(m0); + // Should be within 20% of target Hs + REQUIRE(Hs_computed == Approx(cfg.significantWaveHeight).epsilon(0.20)); +} diff --git a/tests/naval/TestWindForces.cpp b/tests/naval/TestWindForces.cpp new file mode 100644 index 0000000..e1428fc --- /dev/null +++ b/tests/naval/TestWindForces.cpp @@ -0,0 +1,72 @@ +#include +#include + +#include +#include +#include + +using Catch::Approx; +using namespace ymir::naval; + +static WindForces::Config makeWindCfg() +{ + WindForces::Config cfg{}; + cfg.model = WindModel::REGULAR; + cfg.frontalArea = 200.0; + cfg.lateralArea = 1000.0; + cfg.frontalHeight = 5.0; + cfg.lateralHeight = 5.0; + cfg.length_BP = 100.0; + cfg.beam = 20.0; + cfg.draft = 8.0; + // Simple Cd table: 0 and 360 degrees both Cd=1.0 + cfg.angles = {0.0, 90.0, 180.0, 270.0, 360.0}; + cfg.cdx = {1.0, 0.0, -1.0, 0.0, 1.0}; + cfg.cdy = {0.0, 1.0, 0.0, -1.0, 0.0}; + cfg.cdz = {0.0, 0.0, 0.0, 0.0, 0.0}; + return cfg; +} + +static NavalContext makeCtx(double wind_u, double wind_v) +{ + ymir::Vector6 q{}; + ymir::Vector6 qdot{}; + ymir::BodyState bs(q, qdot, 0.0, 0.1); + NavalContext ctx{}; + ctx.state = bs; + ctx.speedToWind[0] = wind_u; + ctx.speedToWind[1] = wind_v; + return ctx; +} + +TEST_CASE("WindForces zero wind produces no force") +{ + WindForces model(makeWindCfg()); + NavalContext ctx = makeCtx(0.0, 0.0); + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + for (int i = 0; i < 6; ++i) + REQUIRE(forces.f[i] == Approx(0.0).margin(1e-12)); +} + +TEST_CASE("WindForces head wind produces surge drag") +{ + WindForces model(makeWindCfg()); + NavalContext ctx = makeCtx(10.0, 0.0); // wind from ahead + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + REQUIRE(forces.f[0] != Approx(0.0).margin(1e-3)); + REQUIRE(std::abs(forces.f[1]) < std::abs(forces.f[0])); // surge dominant +} + +TEST_CASE("WindForces beam wind produces sway force") +{ + WindForces model(makeWindCfg()); + NavalContext ctx = makeCtx(0.0, 10.0); // wind from beam + model.bindContext(&ctx); + + auto forces = model.compute(ctx.state); + REQUIRE(forces.f[1] != Approx(0.0).margin(1e-3)); +}