diff --git a/include/hydrodynamics/hydrodynamics.hpp b/include/hydrodynamics/hydrodynamics.hpp index 7356ede..a55d204 100644 --- a/include/hydrodynamics/hydrodynamics.hpp +++ b/include/hydrodynamics/hydrodynamics.hpp @@ -53,6 +53,17 @@ struct Inertia /// - the added mass coefficients in the surge, sway, heave, roll, pitch, and yaw directions. Inertia(double mass, const Eigen::Vector3d & moments, const Eigen::Vector6d & added_mass); + /// Create a new wrapper for inertial parameters (including added mass and rigid body inertia) using: + /// - the mass of the vehicle, + /// - the moments of inertia about the x, y, and z axes, + /// - the added mass coefficients in the surge, sway, heave, roll, pitch, and yaw directions. + /// - the center of gravity of the vehicle. + Inertia( + double mass, + const Eigen::Vector3d & moments, + const Eigen::Vector6d & added_mass, + const Eigen::Vector3d & center_of_gravity); + Eigen::Matrix6d rigid_body_matrix; Eigen::Matrix6d added_mass_matrix; Eigen::Matrix6d mass_matrix; @@ -84,6 +95,13 @@ struct Coriolis /// - the added mass coefficients in the surge, sway, heave, roll, pitch, and yaw directions. Coriolis(double mass, const Eigen::Vector3d & moments, Eigen::Vector6d added_mass); + /// Create a new wrapper for the Coriolis and centripetal force parameters using: + /// - the mass of the vehicle, + /// - the moments of inertia about the x, y, and z axes, + /// - the added mass coefficients in the surge, sway, heave, roll, pitch, and yaw directions. + /// - the center of gravity of the vehicle. + Coriolis(double mass, const Eigen::Vector3d & moments, Eigen::Vector6d added_mass, Eigen::Vector3d center_of_gravity); + /// Calculate the rigid body Coriolis matrix using the angular velocity of the vehicle. [[nodiscard]] auto calculate_rigid_body_coriolis_matrix(const Eigen::Vector3d & angular_velocity) const -> Eigen::Matrix6d; @@ -97,6 +115,7 @@ struct Coriolis double mass; Eigen::Matrix3d moments; Eigen::Vector6d added_mass_coeff; + Eigen::Vector3d center_of_gravity; }; struct Damping diff --git a/src/hydrodynamics.cpp b/src/hydrodynamics.cpp index dd44217..cac333a 100644 --- a/src/hydrodynamics.cpp +++ b/src/hydrodynamics.cpp @@ -75,6 +75,26 @@ Inertia::Inertia(double mass, const Eigen::Vector3d & moments, const Eigen::Vect mass_matrix = rigid_body_matrix + added_mass_matrix; } +Inertia::Inertia( + double mass, + const Eigen::Vector3d & moments, + const Eigen::Vector6d & added_mass, + const Eigen::Vector3d & center_of_gravity) +{ + // Construct the rigid body inertia matrix + Eigen::Matrix6d rigid_body_matrix = Eigen::Matrix6d::Zero(); + rigid_body_matrix.topLeftCorner(3, 3) = mass * Eigen::Matrix3d::Identity(); + rigid_body_matrix.topRightCorner(3, 3) = -mass * make_skew_symmetric_matrix(center_of_gravity); + rigid_body_matrix.bottomLeftCorner(3, 3) = mass * make_skew_symmetric_matrix(center_of_gravity); + rigid_body_matrix.bottomRightCorner(3, 3) = moments.asDiagonal().toDenseMatrix(); + + // Construct the added mass matrix + added_mass_matrix = -added_mass.asDiagonal().toDenseMatrix(); + + // The complete mass matrix is the sum of the rigid body and added mass matrices + mass_matrix = rigid_body_matrix + added_mass_matrix; +} + Coriolis::Coriolis( double mass, double Ixx, @@ -88,14 +108,28 @@ Coriolis::Coriolis( double Ndr) : mass(mass), moments(Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal().toDenseMatrix()), - added_mass_coeff(Eigen::Vector6d(Xdu, Ydv, Zdw, Kdp, Mdq, Ndr)) + added_mass_coeff(Eigen::Vector6d(Xdu, Ydv, Zdw, Kdp, Mdq, Ndr)), + center_of_gravity(Eigen::Vector3d::Zero()) { } Coriolis::Coriolis(double mass, const Eigen::Vector3d & moments, Eigen::Vector6d added_mass) : mass(mass), moments(moments.asDiagonal().toDenseMatrix()), - added_mass_coeff(std::move(added_mass)) + added_mass_coeff(std::move(added_mass)), + center_of_gravity(Eigen::Vector3d::Zero()) +{ +} + +Coriolis::Coriolis( + double mass, + const Eigen::Vector3d & moments, + Eigen::Vector6d added_mass, + Eigen::Vector3d center_of_gravity) +: mass(mass), + moments(moments.asDiagonal().toDenseMatrix()), + added_mass_coeff(std::move(added_mass)), + center_of_gravity(std::move(center_of_gravity)) { } @@ -106,6 +140,10 @@ auto Coriolis::calculate_rigid_body_coriolis_matrix(const Eigen::Vector3d & angu const Eigen::Vector3d moments_v2 = moments * angular_velocity; coriolis.topLeftCorner(3, 3) = mass * make_skew_symmetric_matrix(angular_velocity); + coriolis.topRightCorner(3, 3) = + -mass * make_skew_symmetric_matrix(angular_velocity) * make_skew_symmetric_matrix(center_of_gravity); + coriolis.bottomLeftCorner(3, 3) = + mass * make_skew_symmetric_matrix(center_of_gravity) * make_skew_symmetric_matrix(angular_velocity); coriolis.bottomRightCorner(3, 3) = -make_skew_symmetric_matrix(moments_v2); return coriolis;