requested PR changes

Robotic-Decision-Making-Lab · Oct 31, 2024 · 8ada41a · 8ada41a
1 parent 1238dff
commit 8ada41a
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 12 deletions.
diff --git a/include/hydrodynamics/hydrodynamics.hpp b/include/hydrodynamics/hydrodynamics.hpp
@@ -45,13 +45,19 @@ struct Inertia
     double Zdw,
     double Kdp,
     double Mdq,
-    double Ndr,
-    const Eigen::Vector3d & center_of_gravity);
+    double Ndr);
 
   /// 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.
+  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,
@@ -81,13 +87,19 @@ struct Coriolis
     double Zdw,
     double Kdp,
     double Mdq,
-    double Ndr,
-    Eigen::Vector3d center_of_gravity);
+    double Ndr);
 
   /// 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.
+  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.

diff --git a/src/hydrodynamics.cpp b/src/hydrodynamics.cpp
@@ -47,14 +47,11 @@ Inertia::Inertia(
   double Zdw,
   double Kdp,
   double Mdq,
-  double Ndr,
-  const Eigen::Vector3d & center_of_gravity)
+  double Ndr)
 {
   // Construct the rigid body inertia matrix
   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) = Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal().toDenseMatrix();
 
   // Construct the added mass matrix
@@ -64,6 +61,20 @@ Inertia::Inertia(
   mass_matrix = rigid_body_matrix + added_mass_matrix;
 }
 
+Inertia::Inertia(double mass, const Eigen::Vector3d & moments, const Eigen::Vector6d & added_mass)
+{
+  // 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.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;
+}
+
 Inertia::Inertia(
   double mass,
   const Eigen::Vector3d & moments,
@@ -94,13 +105,19 @@ Coriolis::Coriolis(
   double Zdw,
   double Kdp,
   double Mdq,
-  double Ndr,
-  Eigen::Vector3d center_of_gravity)
+  double Ndr)
 : mass(mass),
   moments(Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal().toDenseMatrix()),
-  added_mass_coeff(Eigen::Vector6d(Xdu, Ydv, Zdw, Kdp, Mdq, Ndr)),
-  center_of_gravity(std::move(center_of_gravity))
+  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))
 {
+  center_of_gravity = Eigen::Vector3d::Zero();
 }
 
 Coriolis::Coriolis(