Merge pull request #50 from tridelat/rad_interp

Interpolate velocity history for radiation convolution

Author: tridelat

doc/user/_theory/theory.rst

@@ -114,6 +114,7 @@ The :math:`K_{rad}(t)` function is derived by implementing the inverse continuou
 
 This transform allows the frequency domain coefficients, :math:`B(\omega)`, to be remapped into the time domain, thus producing the radiation impulse response function, :math:`K_{rad}(t)`. The :math:`B(\omega)` values can be sourced using open-source BEM software.
 
+In HydroChrono, the force is computed through trapezoidal integration at the time values given by the RIRF time array relative to the current simulation time step. Linear interpolation is done for the velocity history if a given time value is between two values of the time series of the stored velocity history.
 Wave excitation force, :math:`F_{exc}(t)`
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 

include/hydroc/hydro_forces.h

@@ -153,7 +153,6 @@ class ChLoadAddedMass;
 // TODO: Split TestHydro class from its helper classes for clearer code structure.
 class TestHydro {
   public:
-    bool convTrapz_;  // for use in ComputeForceRadiationDampingConv()
     TestHydro() = delete;
 
     /**
@@ -193,6 +192,13 @@ class TestHydro {
     /**
      * @brief Computes the Radiation Damping force with convolution history for a 6N dimensional system.
      *
+     * The discretization uses the time series of the the RIRF relative to the current step.
+     * Linear interpolation is done on the velocity history if time_sim-time_rirf is between two values of the time
+     * history. Trapezoidal integration is used to compute the force.
+     *
+     * Time history is automatically added in this function (so it should only be called once per time step), and
+     * history that is older than the maximum RIRF time value is automatically removed.
+     *
      * @return 6N dimensional force for 6 DOF and N bodies in system.
      */
     std::vector<double> ComputeForceRadiationDampingConv();
@@ -244,12 +250,12 @@ class TestHydro {
     // Additional properties related to equilibrium and hydrodynamics
     std::vector<double> equilibrium_;
     std::vector<double> cb_minus_cg_;
-    double rirf_timestep_;
     Eigen::VectorXd rirf_time_vector;  // Assumed consistent for each body
-    int offset_rirf;                   // For managing the circular nature of velocity history in convolution
+    Eigen::VectorXd rirf_width_vector;
 
     // Properties for velocity history management and time tracking
-    std::vector<double> velocity_history_;
+    std::vector<std::vector<std::vector<double>>> velocity_history_;
+    std::vector<double> time_history_;
     double prev_time;
 
     // Added mass related properties

src/h5fileinfo.cpp

@@ -7,12 +7,11 @@
 // TODO: this include statement list looks good
 #include <H5Cpp.h>
 #include <hydroc/h5fileinfo.h>
-#include <filesystem> // std::filesystem::absolute
+#include <filesystem>  // std::filesystem::absolute
 
 using namespace chrono;  // TODO narrow this using namespace to specify what we use from chrono or put chrono:: in front
                          // of it all?
 
-
 H5FileInfo::H5FileInfo(std::string file, int num_bod) {
     h5_file_name_ = file;
     num_bodies_   = num_bod;
@@ -41,7 +40,7 @@ HydroData H5FileInfo::ReadH5Data() {
     for (int i = 0; i < num_bodies_; i++) {
         // body data
         data_to_init.body_data_[i].body_name = "body" + std::to_string(i + 1);
-        std::string bodyName                = data_to_init.body_data_[i].body_name;  // shortcut for reading later
+        std::string bodyName                 = data_to_init.body_data_[i].body_name;  // shortcut for reading later
         data_to_init.body_data_[i].body_num  = i;
 
         InitScalar(userH5File, bodyName + "/properties/disp_vol", data_to_init.body_data_[i].disp_vol);
@@ -54,7 +53,8 @@ HydroData H5FileInfo::ReadH5Data() {
 
         Init1D(userH5File, bodyName + "/properties/cg", data_to_init.body_data_[i].cg);
         Init1D(userH5File, bodyName + "/properties/cb", data_to_init.body_data_[i].cb);
-        Init2D(userH5File, bodyName + "/hydro_coeffs/linear_restoring_stiffness", data_to_init.body_data_[i].lin_matrix);
+        Init2D(userH5File, bodyName + "/hydro_coeffs/linear_restoring_stiffness",
+               data_to_init.body_data_[i].lin_matrix);
         Init2D(userH5File, bodyName + "/hydro_coeffs/added_mass/inf_freq", data_to_init.body_data_[i].inf_added_mass);
         data_to_init.body_data_[i].inf_added_mass *= rho;
         Init3D(userH5File, bodyName + "/hydro_coeffs/radiation_damping/impulse_response_fun/K",
@@ -242,5 +242,17 @@ int HydroData::GetRIRFDims(int i) const {
 }
 
 Eigen::VectorXd HydroData::GetRIRFTimeVector() const {
-    return body_data_[0].rirf_time_vector;
+    double tol = 1e-10;
+    // check if all time vectors are the same within tolerance
+    auto& rirf_time_vector = body_data_[0].rirf_time_vector;
+    for (size_t ii = 1; ii < body_data_.size(); ii++) {
+        for (size_t jj = 0; jj < body_data_[ii].rirf_time_vector.size(); jj++) {
+            if (abs(body_data_[ii].rirf_time_vector[jj] - rirf_time_vector[jj]) > tol) {
+                throw std::runtime_error(
+                    "RIRF time vectors have to be exactly the same for all bodies. Difference found in body " +
+                    std::to_string(jj) + " at time index " + std::to_string(jj) + ".");
+            }
+        }
+    }
+    return rirf_time_vector;
 }

src/hydro_forces.cpp

@@ -14,16 +14,16 @@
 #include <chrono/physics/ChLoad.h>
 #include <unsupported/Eigen/Splines>
 
+#include <Eigen/Dense>
 #include <algorithm>
 #include <cmath>
+#include <fstream>
+#include <iostream>
 #include <memory>
 #include <numeric>  // std::accumulate
 #include <random>
-#include <vector>
-#include <Eigen/Dense>
-#include <fstream>
-#include <iostream>
 #include <stdexcept>
+#include <vector>
 
 const int kDofPerBody  = 6;
 const int kDofLinOrRot = 3;
@@ -96,7 +96,7 @@ ForceFunc6d::ForceFunc6d() : forces_{{this, 0}, {this, 1}, {this, 2}, {this, 3},
 
 ForceFunc6d::ForceFunc6d(std::shared_ptr<ChBody> object, TestHydro* user_all_forces) : ForceFunc6d() {
     body_             = object;
-    std::string temp = body_->GetNameString();   // remove "body" from "bodyN", convert N to int, get body num
+    std::string temp  = body_->GetNameString();  // remove "body" from "bodyN", convert N to int, get body num
     b_num_            = stoi(temp.erase(0, 4));  // 1 indexed TODO: fix b_num starting here to be 0 indexed
     all_hydro_forces_ = user_all_forces;         // TODO switch to smart pointers? does this use = ?
     if (all_hydro_forces_ == NULL) {
@@ -166,19 +166,31 @@ TestHydro::TestHydro(std::vector<std::shared_ptr<ChBody>> user_bodies,
     : bodies_(user_bodies),
       num_bodies_(bodies_.size()),
       file_info_(H5FileInfo(h5_file_name, num_bodies_).ReadH5Data()) {
-
-    prev_time   = -1;
-    offset_rirf = 0;
+    prev_time = -1;
 
     // Set up time vector
     rirf_time_vector = file_info_.GetRIRFTimeVector();
-    rirf_timestep_   = rirf_time_vector[1] - rirf_time_vector[0];
+    // width array
+    rirf_width_vector.resize(rirf_time_vector.size());
+    for (int ii = 0; ii < rirf_width_vector.size(); ii++) {
+        rirf_width_vector[ii] = 0.0;
+        if (ii < rirf_time_vector.size() - 1) {
+            rirf_width_vector[ii] += 0.5 * abs(rirf_time_vector[ii + 1] - rirf_time_vector[ii]);
+        }
+        if (ii > 0) {
+            rirf_width_vector[ii] += 0.5 * abs(rirf_time_vector[ii] - rirf_time_vector[ii - 1]);
+        }
+    }
 
     // Total degrees of freedom
     int total_dofs = kDofPerBody * num_bodies_;
 
     // Initialize vectors
-    velocity_history_.assign(file_info_.GetRIRFDims(2) * total_dofs, 0.0);
+    time_history_.clear();
+    velocity_history_.clear();
+    for (int b = 0; b < num_bodies_; ++b) {
+        velocity_history_.push_back(std::vector<std::vector<double>>(0));
+    }
     force_hydrostatic_.assign(total_dofs, 0.0);
     force_radiation_damping_.assign(total_dofs, 0.0);
     total_force_.assign(total_dofs, 0.0);
@@ -238,48 +250,12 @@ void TestHydro::AddWaves(std::shared_ptr<WaveBase> waves) {
     user_waves_->Initialize();
 }
 
-double TestHydro::GetVelHistoryVal(int step, int c) const {
-    if (step < 0 || step >= file_info_.GetRIRFDims(2) || c < 0 || c >= num_bodies_ * kDofPerBody) {
-        std::cout << "wrong vel history index " << std::endl;
-        return 0;
-    }
-
-    int index            = c % kDofPerBody;
-    int b                = c / kDofPerBody;  // 0 indexed
-    int calculated_index = index + (kDofPerBody * b) + (kDofPerBody * num_bodies_ * step);
-
-    if (calculated_index >= num_bodies_ * kDofPerBody * file_info_.GetRIRFDims(2) || calculated_index < 0) {
-        std::cout << "bad vel history math" << std::endl;
-        return 0;
-    }
-
-    return velocity_history_[calculated_index];
-}
-
-double TestHydro::SetVelHistory(double val, int step, int b_num, int index) {
-    if (step < 0 || step >= file_info_.GetRIRFDims(2) || b_num < 1 || b_num > num_bodies_ || index < 0 ||
-        index >= kDofPerBody) {
-        std::cout << "bad set vel history indexing" << std::endl;
-        return 0;
-    }
-
-    int calculated_index = index + (kDofPerBody * (b_num - 1)) + (kDofPerBody * num_bodies_ * step);
-
-    if (calculated_index < 0 || calculated_index >= num_bodies_ * kDofPerBody * file_info_.GetRIRFDims(2)) {
-        std::cout << "bad set vel history math" << std::endl;
-        return 0;
-    }
-
-    velocity_history_[calculated_index] = val;
-    return val;
-}
-
 std::vector<double> TestHydro::ComputeForceHydrostatics() {
     assert(num_bodies_ > 0);
 
-    const double rho      = file_info_.GetRhoVal();
-    const auto g_acc      = bodies_[0]->GetSystem()->Get_G_acc();  // assuming all bodies in same system
-    const double gg       = g_acc.Length();
+    const double rho = file_info_.GetRhoVal();
+    const auto g_acc = bodies_[0]->GetSystem()->Get_G_acc();  // assuming all bodies in same system
+    const double gg  = g_acc.Length();
 
     for (int b = 0; b < num_bodies_; b++) {
         std::shared_ptr<chrono::ChBody> body = bodies_[b];
@@ -294,7 +270,7 @@ std::vector<double> TestHydro::ComputeForceHydrostatics() {
 
         chrono::ChVectorN<double, kDofPerBody> body_displacement;
         for (int ii = 0; ii < kDofLinOrRot; ii++) {
-            body_displacement[ii]              = body_position[ii] - body_equilibrium[ii];
+            body_displacement[ii]                = body_position[ii] - body_equilibrium[ii];
             body_displacement[ii + kDofLinOrRot] = body_rotation[ii] - body_equilibrium[ii + kDofLinOrRot];
         }
 
@@ -328,69 +304,92 @@ std::vector<double> TestHydro::ComputeForceRadiationDampingConv() {
     const int numRows = kDofPerBody * num_bodies_;
     const int numCols = kDofPerBody * num_bodies_;
 
-    // "Shift" everything left 1
-    offset_rirf--;  // Starts as 0 before timestep change
-
-    // Keep offset close to 0, avoids small chance of overflow errors in long simulations
-    if (offset_rirf < -1 * size) {
-        offset_rirf += size;
-    }
-
     assert(numRows * size > 0 && numCols > 0);
 
-    std::vector<double> timeseries(numRows * numCols * size, 0.0);
-    std::vector<double> tmp_s(numRows * size, 0.0);
-
-    // Helper function for timeseries indexing
-    auto TimeseriesIndex = [&](int row, int col, int step) { return (row * numCols * size) + (col * size) + step; };
+    // time history
+    auto t_sim = bodies_[0]->GetChTime();
+    auto t_min = t_sim - rirf_time_vector.tail<1>()[0];
+    if (time_history_.size() > 0 && t_sim == time_history_.front()) {
+        throw std::runtime_error("Tried to compute the radiation damping convolution twice within the same time step!");
+    }
+    time_history_.insert(time_history_.begin(), t_sim);
 
-    // Helper function for tmp_s indexing
-    auto TmpSIndex = [&](int row, int step) { return (row * size) + step; };
+    // velocity history
+    for (int b = 0; b < num_bodies_; b++) {
+        auto& body                  = bodies_[b];
+        auto& velocity_history_body = velocity_history_[b];
 
-    // Helper function for circular indexing
-    auto CircularIndex = [&](int value) { return ((value % size) + size) % size; };
+        auto vel                    = body->GetPos_dt();
+        auto wvel                   = body->GetWvel_par();
+        std::vector<double> vel_vec = {vel[0], vel[1], vel[2], wvel[0], wvel[1], wvel[2]};
+        velocity_history_body.insert(velocity_history_body.begin(), vel_vec);
+    }
 
-    // Set last entry as velocity
-    for (int i = 0; i < 3; i++) {
-        for (int b = 1; b <= num_bodies_; b++) {
-            int vi = CircularIndex(size + offset_rirf);
-            SetVelHistory(bodies_[b - 1]->GetPos_dt()[i], vi, b, i);
-            SetVelHistory(bodies_[b - 1]->GetWvel_par()[i], vi, b, i + 3);
+    // remove unnecessary history
+    if (time_history_.size() > 1) {
+        while (time_history_[time_history_.size() - 2] < t_min) {
+            time_history_.pop_back();
+            for (int b = 0; b < num_bodies_; b++) {
+                auto& velocity_history_body = velocity_history_[b];
+                velocity_history_body.pop_back();
+            }
         }
     }
 
-    if (convTrapz_) {
-        for (int row = 0; row < numRows; row++) {
-            for (int st = 0; st < size; st++) {
-                int vi                    = CircularIndex(st + offset_rirf);
-                tmp_s[TmpSIndex(row, st)] = 0;
-                for (int col = 0; col < numCols; col++) {
-                    timeseries[TimeseriesIndex(row, col, st)] = GetRIRFval(row, col, st) * GetVelHistoryVal(vi, col);
-                    tmp_s[TmpSIndex(row, st)] += timeseries[TimeseriesIndex(row, col, st)];
+    if (time_history_.size() > 1) {
+        int idx_history = 0;
+
+        // iterate over RIRF steps
+        for (int step = 0; step < size; step++) {
+            auto t_rirf = t_sim - rirf_time_vector[step];
+            while (time_history_[idx_history + 1] > t_rirf && idx_history < time_history_.size() - 1) {
+                idx_history += 1;
+            }
+            if (idx_history >= time_history_.size() - 1) {
+                break;
+            }
+
+            // iterate over bodies
+            for (int idx_body = 0; idx_body < num_bodies_; idx_body++) {
+                auto& velocity_history_body = velocity_history_[idx_body];
+                std::vector<double> vel(kDofPerBody);
+
+                // interpolate velocity at t_rirf from recorded velocity history
+                // time values
+                auto t1 = time_history_[idx_history + 1];
+                auto t2 = time_history_[idx_history];
+                if (t_rirf == t1) {
+                    vel = velocity_history_body[idx_history + 1];
+                } else if (t_rirf == t2) {
+                    vel = velocity_history_body[idx_history];
+                } else if (t_rirf > t1 && t_rirf < t2) {
+                    // weights
+                    auto w1 = (t2 - t_rirf) / (t2 - t1);
+                    auto w2 = 1.0 - w1;
+                    // velocity values
+                    auto vel1 = velocity_history_body[idx_history + 1];
+                    auto vel2 = velocity_history_body[idx_history];
+                    for (int dof = 0; dof < kDofPerBody; dof++) {
+                        vel[dof] = w1 * vel1[dof] + w2 * vel2[dof];
+                    }
+                } else {
+                    throw std::runtime_error("Radiation convolution: wrong interpolation: " + std::to_string(t_rirf) +
+                                             " not between " + std::to_string(t1) + " and " + std::to_string(t2) + ".");
                 }
-                if (st > 0) {
-                    // Integrate tmp_s
-                    force_radiation_damping_[row] += (tmp_s[TmpSIndex(row, st - 1)] + tmp_s[TmpSIndex(row, st)]) / 2.0 *
-                                                     (rirf_time_vector[st] - rirf_time_vector[st - 1]);
+
+                for (int dof = 0; dof < kDofPerBody; dof++) {
+                    // get column index
+                    int col = dof + idx_body * kDofPerBody;
+
+                    // iterate over rows
+                    for (int row = 0; row < numRows; row++) {
+                        force_radiation_damping_[row] +=
+                            GetRIRFval(row, col, step) * vel[dof] * rirf_width_vector[step];
+                    }
                 }
             }
         }
     }
-    //else {
-    //    // Convolution integral assuming fixed dt
-    //    for (int row = 0; row < numRows; row++) {
-    //        double sumVelHistoryAndRIRF = 0.0;
-    //        for (int col = 0; col < numCols; col++) {
-    //            for (int st = 0; st < size; st++) {
-    //                int vi                                    = CircularIndex(st + offset_rirf);
-    //                timeseries[TimeseriesIndex(row, col, st)] = GetRIRFval(row, col, st) * GetVelHistoryVal(vi, col);
-    //                sumVelHistoryAndRIRF += timeseries[TimeseriesIndex(row, col, st)];
-    //            }
-    //        }
-    //        force_radiation_damping_[row] -= sumVelHistoryAndRIRF * rirf_timestep;
-    //    }
-    //}
-
     return force_radiation_damping_;
 }
 
@@ -450,8 +449,6 @@ double TestHydro::CoordinateFuncForBody(int b, int dof_index) {
     std::fill(force_radiation_damping_.begin(), force_radiation_damping_.end(), 0.0);
     std::fill(force_waves_.begin(), force_waves_.end(), 0.0);
 
-    // Compute forces using the trapezoidal rule (or other methods in the future)
-    convTrapz_               = true;
     force_hydrostatic_       = ComputeForceHydrostatics();
     force_radiation_damping_ = ComputeForceRadiationDampingConv();
     force_waves_             = ComputeForceWaves();