From 1dfd8129f880f90fd34d9ea9d03239e05bd014c7 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 23 Feb 2024 17:41:55 -0800 Subject: [PATCH] Removed current effects and fixed weird const refs in constructor --- .clang-format | 5 +- .clang-tidy | 4 + .devcontainer/devcontainer.json | 2 +- include/eigen.hpp | 1 - include/hydrodynamics.hpp | 246 +++++++++++++++++++------------- src/hydrodynamics.cpp | 141 ++++-------------- 6 files changed, 184 insertions(+), 215 deletions(-) diff --git a/.clang-format b/.clang-format index 52dd477..e2b7a20 100644 --- a/.clang-format +++ b/.clang-format @@ -10,9 +10,8 @@ BraceWrapping: AfterNamespace: true AfterStruct: true AfterEnum: true -ColumnLimit: 100 -BinPackParameters: false -BinPackArguments: false +ColumnLimit: 120 +BinPackParameters: true ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 DerivePointerAlignment: false diff --git a/.clang-tidy b/.clang-tidy index 6c0e4dd..0cdf02f 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -45,3 +45,7 @@ CheckOptions: value: lower_case - key: readability-identifier-naming.ClassMemberCase value: lower_case + - key: readability-identifier-naming.ProtectedMemberSuffix + value: "_" + - key: readability-identifier-naming.PrivateMemberSuffix + value: "_" diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index d90845a..753e418 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -16,7 +16,7 @@ "C_Cpp.codeAnalysis.clangTidy.enabled": true, "C_Cpp.codeAnalysis.clangTidy.runAutomatically": true, "clang-format.executable": "/usr/bin/clang-format-14", - "editor.rulers": [100], + "editor.rulers": [120], "editor.tabSize": 2, "[cpp]": { "editor.defaultFormatter": "xaver.clang-format" diff --git a/include/eigen.hpp b/include/eigen.hpp index 3cd984b..98a3fb1 100644 --- a/include/eigen.hpp +++ b/include/eigen.hpp @@ -21,7 +21,6 @@ #pragma once #include -#include namespace Eigen { diff --git a/include/hydrodynamics.hpp b/include/hydrodynamics.hpp index 6a2cec3..fe2e4b1 100644 --- a/include/hydrodynamics.hpp +++ b/include/hydrodynamics.hpp @@ -1,4 +1,4 @@ -// Copyright 2023, Evan Palmer +// Copyright 2024, Evan Palmer // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -21,35 +21,66 @@ #pragma once #include -#include +#include #include "eigen.hpp" namespace hydrodynamics { +/** + * @brief A wrapper for inertial parameters including added mass and rigid body inertia. + */ class Inertia { public: - Inertia() = default; - - Inertia(double mass, - double Ixx, - double Iyy, - double Izz, - double Xdu, - double Ydv, - double Zdw, - double Kdp, - double Mdq, + /** + * @brief Construct a new wrapper for inertial parameters including added mass and rigid body inertia. + * + * @param mass The mass of the vehicle. + * @param Ixx The moment of inertia about the x-axis. + * @param Iyy The moment of inertia about the y-axis. + * @param Izz The moment of inertia about the z-axis. + * @param Xdu The added mass coefficient in the surge direction. + * @param Ydv The added mass coefficient in the sway direction. + * @param Zdw The added mass coefficient in the heave direction. + * @param Kdp The added mass coefficient in the roll direction. + * @param Mdq The added mass coefficient in the pitch direction. + * @param Ndr The added mass coefficient in the yaw direction. + */ + Inertia(double mass, double Ixx, double Iyy, double Izz, double Xdu, double Ydv, double Zdw, double Kdp, double Mdq, double Ndr); - Inertia(double mass, const Eigen::Vector3d & moments, const Eigen::Vector6d & added_mass); - + /** + * @brief Construct a new wrapper for inertial parameters including added mass and rigid body inertia. + * + * @param mass The mass of the vehicle. + * @param moments The moments of inertia about the x, y, and z axes. + * @param added_mass The added mass coefficients in the surge, sway, heave, roll, pitch, and yaw directions. + */ + Inertia(double mass, Eigen::Vector3d moments, Eigen::Vector6d added_mass); + + /** + * @brief Get the rigid body inertia matrix. + * + * @return Eigen::Matrix6d + */ [[nodiscard]] Eigen::Matrix6d getRigidBodyInertiaMatrix() const; + /** + * @brief Get the added mass matrix. + * + * @return Eigen::Matrix6d + */ [[nodiscard]] Eigen::Matrix6d getAddedMassMatrix() const; + /** + * @brief Get the mass matrix. + * + * @note This is the sum of the rigid body and added mass matrices. + * + * @return Eigen::Matrix6d + */ [[nodiscard]] Eigen::Matrix6d getMassMatrix() const; private: @@ -58,30 +89,62 @@ class Inertia Eigen::Matrix6d mass_mat_; }; +/** + * @brief A wrapper for Coriolis and centripetal force parameters. + */ class Coriolis { public: - Coriolis() = default; - - Coriolis(double mass, - double Ixx, - double Iyy, - double Izz, - double Xdu, - double Ydv, - double Zdw, - double Kdp, - double Mdq, + /** + * @brief Construct a new wrapper for the Coriolis and centripetal force parameters. + * + * @param mass The mass of the vehicle. + * @param Ixx The moment of inertia about the x-axis. + * @param Iyy The moment of inertia about the y-axis. + * @param Izz The moment of inertia about the z-axis. + * @param Xdu The added mass coefficient in the surge direction. + * @param Ydv The added mass coefficient in the sway direction. + * @param Zdw The added mass coefficient in the heave direction. + * @param Kdp The added mass coefficient in the roll direction. + * @param Mdq The added mass coefficient in the pitch direction. + * @param Ndr The added mass coefficient in the yaw direction. + */ + Coriolis(double mass, double Ixx, double Iyy, double Izz, double Xdu, double Ydv, double Zdw, double Kdp, double Mdq, double Ndr); - Coriolis(double mass, const Eigen::Vector3d & moments, Eigen::Vector6d added_mass); - - [[nodiscard]] Eigen::Matrix6d calculateRigidBodyCoriolisMatrix( - const Eigen::Vector3d & angular_velocity) const; - - [[nodiscard]] Eigen::Matrix6d calculateAddedCoriolisMatrix( - const Eigen::Vector6d & velocity) const; - + /** + * @brief Construct a new wrapper for the Coriolis and centripetal force parameters. + * + * @param mass The mass of the vehicle. + * @param moments The moments of inertia about the x, y, and z axes. + * @param added_mass The added mass coefficients in the surge, sway, heave, roll, pitch, and yaw directions. + */ + Coriolis(double mass, Eigen::Vector3d moments, Eigen::Vector6d added_mass); + + /** + * @brief Calculate the rigid body Coriolis matrix. + * + * @param angular_velocity The angular velocity of the vehicle. + * @return Eigen::Matrix6d + */ + [[nodiscard]] Eigen::Matrix6d calculateRigidBodyCoriolisMatrix(const Eigen::Vector3d & angular_velocity) const; + + /** + * @brief Calculate the added mass Coriolis matrix. + * + * @param velocity The velocity of the vehicle. + * @return Eigen::Matrix6d + */ + [[nodiscard]] Eigen::Matrix6d calculateAddedCoriolisMatrix(const Eigen::Vector6d & velocity) const; + + /** + * @brief Calculate the Coriolis matrix. + * + * @note This is the sum of the rigid body and added mass Coriolis matrices. + * + * @param velocity The velocity of the vehicle. + * @return Eigen::Matrix6d + */ [[nodiscard]] Eigen::Matrix6d calculateCoriolisMatrix(const Eigen::Vector6d & velocity) const; private: @@ -90,26 +153,45 @@ class Coriolis Eigen::Vector6d added_mass_coeff_; }; +/** + * @brief A wrapper for the damping forces acting on the vehicle, including linear and quadratic damping coefficients. + */ class Damping { public: - Damping() = default; - - Damping(double Xu, - double Yv, - double Zw, - double Kp, - double Mq, - double Nr, - double Xuu, - double Yvv, - double Zww, - double Kpp, - double Mqq, - double Nrr); - + /** + * @brief Construct a new wrapper for the damping coefficients. + * + * @param Xu The linear damping coefficient in the surge direction. + * @param Yv The linear damping coefficient in the sway direction. + * @param Zw The linear damping coefficient in the heave direction. + * @param Kp The linear damping coefficient in the roll direction. + * @param Mq The linear damping coefficient in the pitch direction. + * @param Nr The linear damping coefficient in the yaw direction. + * @param Xuu The quadratic damping coefficient in the surge direction. + * @param Yvv The quadratic damping coefficient in the sway direction. + * @param Zww The quadratic damping coefficient in the heave direction. + * @param Kpp The quadratic damping coefficient in the roll direction. + * @param Mqq The quadratic damping coefficient in the pitch direction. + * @param Nrr The quadratic damping coefficient in the yaw direction. + */ + Damping(double Xu, double Yv, double Zw, double Kp, double Mq, double Nr, double Xuu, double Yvv, double Zww, + double Kpp, double Mqq, double Nrr); + + /** + * @brief Construct a new wrapper for the damping coefficients. + * + * @param linear The linear damping coefficients. + * @param quadratic The quadratic damping coefficients. + */ Damping(Eigen::Vector6d linear, Eigen::Vector6d quadratic); + /** + * @brief Calculate the damping matrix. + * + * @param velocity The current velocity of the vehicle. + * @return Eigen::Matrix6d + */ [[nodiscard]] Eigen::Matrix6d calculateDampingMatrix(const Eigen::Vector6d & velocity) const; private: @@ -117,17 +199,30 @@ class Damping Eigen::Vector6d quadratic_coeff_; }; +/** + * @brief A wrapper for the restoring forces acting on the vehicle, including gravity and bouyancy. + */ class RestoringForces { public: - RestoringForces() = default; - - RestoringForces(double weight, - double buoyancy, - Eigen::Vector3d center_of_buoyancy, + /** + * @brief Construct a wrapper for restoring force parameters. + * + * @param weight The weight of the vehicle. + * @param buoyancy The buoyancy of the vehicle. + * @param center_of_buoyancy The center of buoyancy of the vehicle. + * @param center_of_gravity The center of gravity of the vehicle. + */ + RestoringForces(double weight, double buoyancy, Eigen::Vector3d center_of_buoyancy, Eigen::Vector3d center_of_gravity); - [[nodiscard]] Eigen::Vector6d calculateRestoringForcesVector(const Eigen::Matrix3d & rot) const; + /** + * @brief Calculate the restoring forces acting on the vehicle. + * + * @param rotation The current rotation of the vehicle with respect to the inertial frame. + * @return Eigen::Vector6d + */ + [[nodiscard]] Eigen::Vector6d calculateRestoringForcesVector(const Eigen::Matrix3d & rotation) const; private: double weight_; @@ -136,45 +231,4 @@ class RestoringForces Eigen::Vector3d center_of_gravity_; }; -class CurrentEffects -{ -public: - CurrentEffects() = default; - - explicit CurrentEffects(Eigen::Vector6d velocity); - - [[nodiscard]] Eigen::Vector6d calculateCurrentEffectsVector(const Eigen::Matrix3d & rot) const; - -private: - Eigen::Vector6d current_; -}; - -struct HydrodynamicParameters -{ - Inertia inertia; - Coriolis coriolis; - Damping damping; - RestoringForces restoring_forces; - CurrentEffects current_effects; - - HydrodynamicParameters() = default; - - HydrodynamicParameters(Inertia inertia, - Coriolis coriolis, - Damping damping, - RestoringForces restoring_forces, - CurrentEffects current_effects); - - HydrodynamicParameters(double mass, - double weight, - double buoyancy, - const Eigen::Vector3d & moments, - const Eigen::Vector6d & added_mass, - Eigen::Vector6d linear_damping, - Eigen::Vector6d quadratic_damping, - Eigen::Vector3d center_of_buoyancy, - Eigen::Vector3d center_of_gravity, - Eigen::Vector6d current_velocity); -}; - } // namespace hydrodynamics diff --git a/src/hydrodynamics.cpp b/src/hydrodynamics.cpp index 3798813..b3ede62 100644 --- a/src/hydrodynamics.cpp +++ b/src/hydrodynamics.cpp @@ -20,6 +20,8 @@ #include "hydrodynamics.hpp" +#include + namespace hydrodynamics { @@ -35,35 +37,32 @@ Eigen::Matrix3d makeSkewSymmetricMatrix(const Eigen::Vector3d & v) } // namespace -Inertia::Inertia(double mass, - double Ixx, - double Iyy, - double Izz, - double Xdu, - double Ydv, - double Zdw, - double Kdp, - double Mdq, - double Ndr) +Inertia::Inertia(double mass, double Ixx, double Iyy, double Izz, double Xdu, double Ydv, double Zdw, double Kdp, + double Mdq, double Ndr) { + // Construct the rigid body inertia matrix Eigen::Matrix6d rigid_body_mat_ = Eigen::Matrix6d::Zero(); rigid_body_mat_.topLeftCorner(3, 3) = mass * Eigen::Matrix3d::Identity(); - rigid_body_mat_.bottomRightCorner(3, 3) = - Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal().toDenseMatrix(); + rigid_body_mat_.bottomRightCorner(3, 3) = Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal().toDenseMatrix(); + // Construct the added mass matrix added_mass_mat_ = -Eigen::Vector6d(Xdu, Ydv, Zdw, Kdp, Mdq, Ndr).asDiagonal().toDenseMatrix(); + // The complete mass matrix is the sum of the rigid body and added mass matrices mass_mat_ = rigid_body_mat_ + added_mass_mat_; } -Inertia::Inertia(double mass, const Eigen::Vector3d & moments, const Eigen::Vector6d & added_mass) +Inertia::Inertia(double mass, Eigen::Vector3d moments, Eigen::Vector6d added_mass) { + // Construct the rigid body inertia matrix Eigen::Matrix6d rigid_body_mat_ = Eigen::Matrix6d::Zero(); rigid_body_mat_.topLeftCorner(3, 3) = mass * Eigen::Matrix3d::Identity(); rigid_body_mat_.bottomRightCorner(3, 3) = moments.asDiagonal().toDenseMatrix(); + // Construct the added mass matrix added_mass_mat_ = -added_mass.asDiagonal().toDenseMatrix(); + // The complete mass matrix is the sum of the rigid body and added mass matrices mass_mat_ = rigid_body_mat_ + added_mass_mat_; } @@ -73,31 +72,20 @@ Eigen::Matrix6d Inertia::getAddedMassMatrix() const { return added_mass_mat_; } Eigen::Matrix6d Inertia::getMassMatrix() const { return mass_mat_; } -Coriolis::Coriolis(double mass, - double Ixx, - double Iyy, - double Izz, - double Xdu, - double Ydv, - double Zdw, - double Kdp, - double Mdq, - double Ndr) +Coriolis::Coriolis(double mass, double Ixx, double Iyy, double Izz, double Xdu, double Ydv, double Zdw, double Kdp, + double Mdq, double Ndr) : mass_(mass), moments_(Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal().toDenseMatrix()), added_mass_coeff_(Eigen::Vector6d(Xdu, Ydv, Zdw, Kdp, Mdq, Ndr)) { } -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)) +Coriolis::Coriolis(double mass, Eigen::Vector3d moments, Eigen::Vector6d added_mass) +: mass_(mass), moments_(moments.asDiagonal().toDenseMatrix()), added_mass_coeff_(std::move(added_mass)) { } -Eigen::Matrix6d Coriolis::calculateRigidBodyCoriolisMatrix( - const Eigen::Vector3d & angular_velocity) const +Eigen::Matrix6d Coriolis::calculateRigidBodyCoriolisMatrix(const Eigen::Vector3d & angular_velocity) const { Eigen::Matrix6d coriolis = Eigen::Matrix6d::Zero(); @@ -113,15 +101,11 @@ Eigen::Matrix6d Coriolis::calculateAddedCoriolisMatrix(const Eigen::Vector6d & v { Eigen::Matrix6d coriolis = Eigen::Matrix6d::Zero(); - Eigen::Matrix3d linear_vel = - makeSkewSymmetricMatrix(Eigen::Vector3d(added_mass_coeff_(0) * velocity(0), - added_mass_coeff_(1) * velocity(1), - added_mass_coeff_(2) * velocity(2))); + Eigen::Matrix3d linear_vel = makeSkewSymmetricMatrix(Eigen::Vector3d( + added_mass_coeff_(0) * velocity(0), added_mass_coeff_(1) * velocity(1), added_mass_coeff_(2) * velocity(2))); - Eigen::Matrix3d angular_vel = - makeSkewSymmetricMatrix(Eigen::Vector3d(added_mass_coeff_(3) * velocity(3), - added_mass_coeff_(4) * velocity(4), - added_mass_coeff_(5) * velocity(5))); + Eigen::Matrix3d angular_vel = makeSkewSymmetricMatrix(Eigen::Vector3d( + added_mass_coeff_(3) * velocity(3), added_mass_coeff_(4) * velocity(4), added_mass_coeff_(5) * velocity(5))); coriolis.topRightCorner(3, 3) = linear_vel; coriolis.bottomLeftCorner(3, 3) = linear_vel; @@ -132,22 +116,11 @@ Eigen::Matrix6d Coriolis::calculateAddedCoriolisMatrix(const Eigen::Vector6d & v Eigen::Matrix6d Coriolis::calculateCoriolisMatrix(const Eigen::Vector6d & velocity) const { - return calculateRigidBodyCoriolisMatrix(velocity.bottomRows(3)) + - calculateAddedCoriolisMatrix(velocity); + return calculateRigidBodyCoriolisMatrix(velocity.bottomRows(3)) + calculateAddedCoriolisMatrix(velocity); } -Damping::Damping(double Xu, - double Yv, - double Zw, - double Kp, - double Mq, - double Nr, - double Xuu, - double Yvv, - double Zww, - double Kpp, - double Mqq, - double Nrr) +Damping::Damping(double Xu, double Yv, double Zw, double Kp, double Mq, double Nr, double Xuu, double Yvv, double Zww, + double Kpp, double Mqq, double Nrr) : linear_coeff_(Eigen::Vector6d(Xu, Yv, Zw, Kp, Mq, Nr)), quadratic_coeff_(Eigen::Vector6d(Xuu, Yvv, Zww, Kpp, Mqq, Nrr)) { @@ -160,16 +133,13 @@ Damping::Damping(Eigen::Vector6d linear, Eigen::Vector6d quadratic) Eigen::Matrix6d Damping::calculateDampingMatrix(const Eigen::Vector6d & velocity) const { - Eigen::Matrix6d quadratic = -(quadratic_coeff_.asDiagonal().toDenseMatrix() * velocity.cwiseAbs()) - .asDiagonal() - .toDenseMatrix(); + Eigen::Matrix6d quadratic = + -(quadratic_coeff_.asDiagonal().toDenseMatrix() * velocity.cwiseAbs()).asDiagonal().toDenseMatrix(); return -linear_coeff_.asDiagonal().toDenseMatrix() + quadratic; } -RestoringForces::RestoringForces(double weight, - double buoyancy, - Eigen::Vector3d center_of_buoyancy, +RestoringForces::RestoringForces(double weight, double buoyancy, Eigen::Vector3d center_of_buoyancy, Eigen::Vector3d center_of_gravity) : weight_(weight), buoyancy_(buoyancy), @@ -178,61 +148,4 @@ RestoringForces::RestoringForces(double weight, { } -Eigen::Vector6d RestoringForces::calculateRestoringForcesVector(const Eigen::Matrix3d & rot) const -{ - const Eigen::Vector3d fg(0, 0, weight_); - const Eigen::Vector3d fb(0, 0, -buoyancy_); - - Eigen::Vector6d g_rb; - - g_rb.topRows(3) = rot * (fg + fb); - g_rb.bottomRows(3) = center_of_gravity_.cross(rot * fg) + center_of_buoyancy_.cross(rot * fb); - - g_rb *= -1; - - return g_rb; -} - -CurrentEffects::CurrentEffects(Eigen::Vector6d velocity) : current_(std::move(velocity)) {} - -Eigen::Vector6d CurrentEffects::calculateCurrentEffectsVector(const Eigen::Matrix3d & rot) const -{ - Eigen::Vector6d rotated_current_effects; - rotated_current_effects.topRows(3) = rot * current_.topRows(3); - rotated_current_effects.bottomRows(3) = rot * current_.bottomRows(3); - - return rotated_current_effects; -} - -HydrodynamicParameters::HydrodynamicParameters(Inertia inertia, - Coriolis coriolis, - Damping damping, - RestoringForces restoring_forces, - CurrentEffects current_effects) -: inertia(std::move(inertia)), - coriolis(std::move(coriolis)), - damping(std::move(damping)), - restoring_forces(std::move(restoring_forces)), - current_effects(std::move(current_effects)) -{ -} - -HydrodynamicParameters::HydrodynamicParameters(double mass, - double weight, - double buoyancy, - const Eigen::Vector3d & moments, - const Eigen::Vector6d & added_mass, - Eigen::Vector6d linear_damping, - Eigen::Vector6d quadratic_damping, - Eigen::Vector3d center_of_buoyancy, - Eigen::Vector3d center_of_gravity, - Eigen::Vector6d current_velocity) -{ - inertia = Inertia(mass, moments, added_mass); - coriolis = Coriolis(mass, moments, added_mass); - damping = Damping(linear_damping, quadratic_damping); - restoring_forces = RestoringForces(weight, buoyancy, center_of_buoyancy, center_of_gravity); - current_effects = CurrentEffects(current_velocity); -} - } // namespace hydrodynamics