HydrodynamicsUtils.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2026 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#ifndef GZ_SIM_SYSTEMS_HYDRODYNAMICS_UTILS_HH_
#define GZ_SIM_SYSTEMS_HYDRODYNAMICS_UTILS_HH_
 
#include <cmath>
 
#include <Eigen/Eigen>
 
#include "gz/sim/config.hh"
 
namespace gz
{
namespace sim
{
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace systems
{
namespace hydrodynamics
{
  inline Eigen::Matrix<double, 6, 6> buildCoriolisMatrix(
    const Eigen::Matrix<double, 6, 6> &_Ma,
    const Eigen::Matrix<double, 6, 1> &_state)
  {
    Eigen::Matrix<double, 6, 6> Cmat =
      Eigen::Matrix<double, 6, 6>::Zero();
 
    Cmat(0, 4) = - _Ma(2, 2) * _state(2);
    Cmat(0, 5) =   _Ma(1, 1) * _state(1);
    Cmat(1, 3) =   _Ma(2, 2) * _state(2);
    Cmat(1, 5) = - _Ma(0, 0) * _state(0);
    Cmat(2, 3) = - _Ma(1, 1) * _state(1);
    Cmat(2, 4) =   _Ma(0, 0) * _state(0);
    Cmat(3, 1) = - _Ma(2, 2) * _state(2);
    Cmat(3, 2) =   _Ma(1, 1) * _state(1);
    Cmat(3, 4) = - _Ma(5, 5) * _state(5);
    Cmat(3, 5) =   _Ma(4, 4) * _state(4);
    Cmat(4, 0) =   _Ma(2, 2) * _state(2);
    Cmat(4, 2) = - _Ma(0, 0) * _state(0);
    Cmat(4, 3) =   _Ma(5, 5) * _state(5);
    Cmat(4, 5) = - _Ma(3, 3) * _state(3);
    Cmat(5, 0) = - _Ma(1, 1) * _state(1);
    Cmat(5, 1) =   _Ma(0, 0) * _state(0);
    Cmat(5, 3) = - _Ma(4, 4) * _state(4);
    Cmat(5, 4) =   _Ma(3, 3) * _state(3);
 
    return Cmat;
  }
 
  inline Eigen::Matrix<double, 6, 6> buildDampingMatrix(
    const double (&_linearTerms)[36],
    const double (&_quadAbsDerivs)[216],
    const double (&_quadDerivs)[216],
    const Eigen::Matrix<double, 6, 1> &_state)
  {
    Eigen::Matrix<double, 6, 6> Dmat =
      Eigen::Matrix<double, 6, 6>::Zero();
 
    for (int i = 0; i < 6; i++)
    {
      for (int j = 0; j < 6; j++)
      {
        auto coeff = - _linearTerms[i * 6 + j];
        for (int k = 0; k < 6; k++)
        {
          auto index = i * 36 + j * 6 + k;
          coeff -= _quadAbsDerivs[index] * std::abs(_state(k));
          coeff -= _quadDerivs[index] * _state(k);
        }
        Dmat(i, j) = coeff;
      }
    }
 
    return Dmat;
  }
 
  inline Eigen::Matrix3d skew3(const Eigen::Vector3d &_v)
  {
    Eigen::Matrix3d S;
    S <<       0, -_v(2),  _v(1),
          _v(2),       0, -_v(0),
         -_v(1),  _v(0),       0;
    return S;
  }
 
  inline Eigen::Matrix<double, 6, 6> buildFullCoriolisMatrix(
    const Eigen::Matrix<double, 6, 6> &_Ma,
    const Eigen::Matrix<double, 6, 1> &_state)
  {
    Eigen::Vector3d v1 = _state.head<3>();
    Eigen::Vector3d v2 = _state.tail<3>();
 
    Eigen::Matrix3d M11 = _Ma.block<3, 3>(0, 0);
    Eigen::Matrix3d M12 = _Ma.block<3, 3>(0, 3);
    Eigen::Matrix3d M21 = _Ma.block<3, 3>(3, 0);
    Eigen::Matrix3d M22 = _Ma.block<3, 3>(3, 3);
 
    Eigen::Vector3d a1 = M11 * v1 + M12 * v2;
    Eigen::Vector3d a2 = M21 * v1 + M22 * v2;
 
    Eigen::Matrix<double, 6, 6> C =
      Eigen::Matrix<double, 6, 6>::Zero();
    C.block<3, 3>(0, 3) = -skew3(a1);
    C.block<3, 3>(3, 0) = -skew3(a1);
    C.block<3, 3>(3, 3) = -skew3(a2);
 
    return C;
  }
 
}  // namespace hydrodynamics
}  // namespace systems
}  // namespace GZ_SIM_VERSION_NAMESPACE
}  // namespace sim
}  // namespace gz
 
#endif