|
| 1 | +/********************************************************************* |
| 2 | + * @file radiation_ss_component.cpp |
| 3 | + * @brief Implementation of radiation state-space force component. |
| 4 | + * |
| 5 | + * IMPLEMENTATION NOTES: |
| 6 | + * |
| 7 | + * 1. DOF INDEXING |
| 8 | + * For N bodies, we have 6N total DOFs. DOF indexing is: |
| 9 | + * Body 0: DOFs 0-5 (surge, sway, heave, roll, pitch, yaw) |
| 10 | + * Body 1: DOFs 6-11 |
| 11 | + * ...etc |
| 12 | + * |
| 13 | + * 2. RIRF INDEXING |
| 14 | + * HydroData provides RIRF via GetRIRFVal(body, row_dof, col, step): |
| 15 | + * - body: which body (0 to num_bodies-1) |
| 16 | + * - row_dof: output DOF within body (0-5) |
| 17 | + * - col: column DOF (0 to 6*num_bodies-1) |
| 18 | + * - step: time step (0 to rirf_steps-1) |
| 19 | + * |
| 20 | + * We convert to global (i,j) indexing: |
| 21 | + * i = body * 6 + row_dof (global output DOF) |
| 22 | + * j = col (global input DOF) |
| 23 | + * |
| 24 | + * 3. FIT QUALITY |
| 25 | + * We skip fitting for kernels that are essentially zero (norm < 1e-10). |
| 26 | + * This avoids creating models for DOF pairs with no coupling. |
| 27 | + * |
| 28 | + * 4. RUNTIME COMPUTATION |
| 29 | + * For each timestep: |
| 30 | + * 1. Extract velocity vector v from SystemState |
| 31 | + * 2. Compute dt = time - prev_time |
| 32 | + * 3. For each DOF pair (i,j): |
| 33 | + * model.Step(dt, v[j]) |
| 34 | + * f_rad[i] += model.GetForce() |
| 35 | + * 4. Apply forces to BodyForces (with negative sign for damping) |
| 36 | + *********************************************************************/ |
| 37 | + |
| 38 | +#include "radiation_ss_component.h" |
| 39 | +#include <hydroc/io/h5_reader.h> |
| 40 | +#include <hydroc/logging.h> |
| 41 | + |
| 42 | +#include <Eigen/Dense> |
| 43 | +#include <algorithm> |
| 44 | +#include <stdexcept> |
| 45 | +#include <sstream> |
| 46 | + |
| 47 | +namespace hydrochrono::hydro { |
| 48 | + |
| 49 | +RadiationStateSpaceComponent::RadiationStateSpaceComponent( |
| 50 | + const HydroData& file_info, |
| 51 | + int num_bodies, |
| 52 | + const StateSpaceOptions& options) |
| 53 | + : num_bodies_(num_bodies), |
| 54 | + num_dofs_(kDofPerBody * num_bodies), |
| 55 | + options_(options), |
| 56 | + prev_time_(0.0), |
| 57 | + has_prev_time_(false) { |
| 58 | + |
| 59 | + if (num_bodies_ <= 0) { |
| 60 | + throw std::invalid_argument( |
| 61 | + "RadiationStateSpaceComponent: num_bodies must be > 0"); |
| 62 | + } |
| 63 | + |
| 64 | + // Initialize state-space models from RIRF data |
| 65 | + InitializeFromRirf(file_info); |
| 66 | +} |
| 67 | + |
| 68 | +void RadiationStateSpaceComponent::InitializeFromRirf(const HydroData& file_info) { |
| 69 | + // Get RIRF dimensions and time vector |
| 70 | + const int rirf_steps = file_info.GetRIRFDims(2); |
| 71 | + const Eigen::VectorXd rirf_time = file_info.GetRIRFTimeVector(); |
| 72 | + |
| 73 | + if (rirf_steps <= 0 || rirf_time.size() != rirf_steps) { |
| 74 | + throw std::runtime_error( |
| 75 | + "RadiationStateSpaceComponent: Invalid RIRF data dimensions"); |
| 76 | + } |
| 77 | + |
| 78 | + // Compute dt from time vector (assume uniform spacing) |
| 79 | + const double dt = (rirf_steps > 1) |
| 80 | + ? (rirf_time(rirf_time.size() - 1) - rirf_time(0)) / (rirf_steps - 1) |
| 81 | + : 0.01; |
| 82 | + |
| 83 | + // Create fitter with user options |
| 84 | + RadiationStateSpaceFitter fitter(options_); |
| 85 | + |
| 86 | + // Reserve space for models (worst case: all pairs are non-zero) |
| 87 | + dof_pair_models_.reserve(num_dofs_ * num_dofs_); |
| 88 | + |
| 89 | + int fitted_count = 0; |
| 90 | + int skipped_count = 0; |
| 91 | + |
| 92 | + // Iterate over all DOF pairs (i,j) |
| 93 | + for (int i = 0; i < num_dofs_; ++i) { |
| 94 | + for (int j = 0; j < num_dofs_; ++j) { |
| 95 | + // Extract kernel K_ij(t) from HydroData |
| 96 | + // Convert global i to (body, row_dof) |
| 97 | + const int body_i = i / kDofPerBody; |
| 98 | + const int row_dof = i % kDofPerBody; |
| 99 | + const int col = j; // Column is already global index |
| 100 | + |
| 101 | + // Build kernel vector |
| 102 | + Eigen::VectorXd K(rirf_steps); |
| 103 | + for (int step = 0; step < rirf_steps; ++step) { |
| 104 | + K(step) = file_info.GetRIRFVal(body_i, row_dof, col, step); |
| 105 | + } |
| 106 | + |
| 107 | + // Skip if kernel is essentially zero (no coupling) |
| 108 | + const double K_norm = K.norm(); |
| 109 | + if (K_norm < 1e-10) { |
| 110 | + ++skipped_count; |
| 111 | + continue; |
| 112 | + } |
| 113 | + |
| 114 | + // Fit the kernel |
| 115 | + StateSpaceFitResult result = fitter.FitKernel(K, dt); |
| 116 | + |
| 117 | + // Skip if fit failed |
| 118 | + if (!result.IsValid()) { |
| 119 | + ++skipped_count; |
| 120 | + continue; |
| 121 | + } |
| 122 | + |
| 123 | + // Create model from fit result |
| 124 | + RadiationStateSpaceModel model = RadiationStateSpaceModel::FromFitResult(result); |
| 125 | + model.Reset(); |
| 126 | + |
| 127 | + // Store the model |
| 128 | + DofPairModel dof_model; |
| 129 | + dof_model.i = i; |
| 130 | + dof_model.j = j; |
| 131 | + dof_model.r2 = result.r2; |
| 132 | + dof_model.model = std::move(model); |
| 133 | + dof_pair_models_.push_back(std::move(dof_model)); |
| 134 | + |
| 135 | + ++fitted_count; |
| 136 | + } |
| 137 | + } |
| 138 | + |
| 139 | + // Log summary using hydroc logging system |
| 140 | + { |
| 141 | + std::ostringstream oss; |
| 142 | + oss << "[RadiationStateSpaceComponent] Initialized: " |
| 143 | + << num_bodies_ << " bodies (" << num_dofs_ << " DOFs), " |
| 144 | + << fitted_count << " pairs fitted, " |
| 145 | + << skipped_count << " skipped, " |
| 146 | + << total_modes() << " total modes"; |
| 147 | + if (fitted_count > 0) { |
| 148 | + oss << ", R² range: [" << min_r2() << ", " << max_r2() << "]"; |
| 149 | + } |
| 150 | + LOG_INFO(oss.str()); |
| 151 | + } |
| 152 | +} |
| 153 | + |
| 154 | +void RadiationStateSpaceComponent::Compute( |
| 155 | + const SystemState& state, |
| 156 | + double time, |
| 157 | + BodyForces& inout_forces) { |
| 158 | + |
| 159 | + // Validate state and forces sizes |
| 160 | + if (static_cast<int>(state.bodies.size()) != num_bodies_) { |
| 161 | + throw std::runtime_error( |
| 162 | + "RadiationStateSpaceComponent::Compute: state.bodies.size() mismatch"); |
| 163 | + } |
| 164 | + if (static_cast<int>(inout_forces.size()) != num_bodies_) { |
| 165 | + throw std::runtime_error( |
| 166 | + "RadiationStateSpaceComponent::Compute: inout_forces.size() mismatch"); |
| 167 | + } |
| 168 | + |
| 169 | + // Extract velocity vector from state |
| 170 | + Eigen::VectorXd v = ExtractVelocityVector(state); |
| 171 | + |
| 172 | + // Compute time step |
| 173 | + double dt; |
| 174 | + if (!has_prev_time_) { |
| 175 | + // First call - use a small default dt |
| 176 | + dt = 0.001; // Will be overwritten next step |
| 177 | + has_prev_time_ = true; |
| 178 | + } else { |
| 179 | + dt = time - prev_time_; |
| 180 | + } |
| 181 | + prev_time_ = time; |
| 182 | + |
| 183 | + // Handle zero or negative dt (can happen at initialization) |
| 184 | + if (dt <= 0.0) { |
| 185 | + dt = 0.001; |
| 186 | + } |
| 187 | + |
| 188 | + // Accumulate radiation forces |
| 189 | + Eigen::VectorXd f_rad = Eigen::VectorXd::Zero(num_dofs_); |
| 190 | + |
| 191 | + for (auto& dof_model : dof_pair_models_) { |
| 192 | + // Step the model with velocity from input DOF j |
| 193 | + dof_model.model.Step(dt, v(dof_model.j)); |
| 194 | + |
| 195 | + // Accumulate force contribution to output DOF i |
| 196 | + f_rad(dof_model.i) += dof_model.model.GetForce(); |
| 197 | + } |
| 198 | + |
| 199 | + // Add forces to output (with negative sign for damping) |
| 200 | + AccumulateForces(f_rad, inout_forces); |
| 201 | +} |
| 202 | + |
| 203 | +void RadiationStateSpaceComponent::Reset() { |
| 204 | + for (auto& dof_model : dof_pair_models_) { |
| 205 | + dof_model.model.Reset(); |
| 206 | + } |
| 207 | + prev_time_ = 0.0; |
| 208 | + has_prev_time_ = false; |
| 209 | +} |
| 210 | + |
| 211 | +Eigen::VectorXd RadiationStateSpaceComponent::ExtractVelocityVector( |
| 212 | + const SystemState& state) const { |
| 213 | + |
| 214 | + Eigen::VectorXd v(num_dofs_); |
| 215 | + |
| 216 | + for (int b = 0; b < num_bodies_; ++b) { |
| 217 | + const auto& body_state = state.bodies[b]; |
| 218 | + const int offset = b * kDofPerBody; |
| 219 | + |
| 220 | + // Linear velocities (surge, sway, heave) |
| 221 | + v(offset + 0) = body_state.linear_velocity[0]; |
| 222 | + v(offset + 1) = body_state.linear_velocity[1]; |
| 223 | + v(offset + 2) = body_state.linear_velocity[2]; |
| 224 | + |
| 225 | + // Angular velocities (roll, pitch, yaw) |
| 226 | + v(offset + 3) = body_state.angular_velocity[0]; |
| 227 | + v(offset + 4) = body_state.angular_velocity[1]; |
| 228 | + v(offset + 5) = body_state.angular_velocity[2]; |
| 229 | + } |
| 230 | + |
| 231 | + return v; |
| 232 | +} |
| 233 | + |
| 234 | +void RadiationStateSpaceComponent::AccumulateForces( |
| 235 | + const Eigen::VectorXd& f_rad, |
| 236 | + BodyForces& inout_forces) const { |
| 237 | + |
| 238 | + // Ensure forces are properly sized |
| 239 | + for (int b = 0; b < num_bodies_; ++b) { |
| 240 | + if (static_cast<int>(inout_forces[b].size()) != kDofPerBody) { |
| 241 | + inout_forces[b].resize(kDofPerBody); |
| 242 | + inout_forces[b].setZero(); |
| 243 | + } |
| 244 | + } |
| 245 | + |
| 246 | + // Map radiation forces to per-body forces. |
| 247 | + // Radiation damping opposes motion, so we SUBTRACT. |
| 248 | + for (int b = 0; b < num_bodies_; ++b) { |
| 249 | + const int offset = b * kDofPerBody; |
| 250 | + for (int d = 0; d < kDofPerBody; ++d) { |
| 251 | + inout_forces[b][d] -= f_rad(offset + d); |
| 252 | + } |
| 253 | + } |
| 254 | +} |
| 255 | + |
| 256 | +int RadiationStateSpaceComponent::total_modes() const { |
| 257 | + int total = 0; |
| 258 | + for (const auto& dof_model : dof_pair_models_) { |
| 259 | + total += dof_model.model.total_modes(); |
| 260 | + } |
| 261 | + return total; |
| 262 | +} |
| 263 | + |
| 264 | +double RadiationStateSpaceComponent::min_r2() const { |
| 265 | + if (dof_pair_models_.empty()) return 0.0; |
| 266 | + double min_val = dof_pair_models_[0].r2; |
| 267 | + for (const auto& dof_model : dof_pair_models_) { |
| 268 | + min_val = std::min(min_val, dof_model.r2); |
| 269 | + } |
| 270 | + return min_val; |
| 271 | +} |
| 272 | + |
| 273 | +double RadiationStateSpaceComponent::max_r2() const { |
| 274 | + if (dof_pair_models_.empty()) return 0.0; |
| 275 | + double max_val = dof_pair_models_[0].r2; |
| 276 | + for (const auto& dof_model : dof_pair_models_) { |
| 277 | + max_val = std::max(max_val, dof_model.r2); |
| 278 | + } |
| 279 | + return max_val; |
| 280 | +} |
| 281 | + |
| 282 | +} // namespace hydrochrono::hydro |
| 283 | + |
0 commit comments