RigidBodyTerms.h

#pragma once
 
namespace rigid_body
{
enum class MunkMoment
{
   Included,
   Omitted
};
 
inline void Skew(const double a[3], double S[3][3])
{
   S[0][0] = 0.0;
   S[0][1] = -a[2];
   S[0][2] = a[1];
   S[1][0] = a[2];
   S[1][1] = 0.0;
   S[1][2] = -a[0];
   S[2][0] = -a[1];
   S[2][1] = a[0];
   S[2][2] = 0.0;
}
 
inline void Cross(const double a[3], const double b[3], double axb[3])
{
   axb[0] = a[1] * b[2] - a[2] * b[1];
   axb[1] = a[2] * b[0] - a[0] * b[2];
   axb[2] = a[0] * b[1] - a[1] * b[0];
}
 
inline void Momenta(const double rigid[6], const double added[6], const double vel[3], const double waterVel[3],
      const double omega[3], double momentum[3], double angularMomentum[3], double addedMomentum[3])
{
   for (int i = 0; i < 3; i++) {
      addedMomentum[i]   = added[i] * (vel[i] - waterVel[i]);
      momentum[i]        = rigid[i] * vel[i] + addedMomentum[i];
      angularMomentum[i] = (rigid[i + 3] + added[i + 3]) * omega[i];
   }
}
 
inline void OmegaTermsWrench(const double rigid[6], const double added[6], const double vel[3], const double waterVel[3],
      const double omega[3], MunkMoment munk, double wrench[6])
{
   double momentum[3];
   double angularMomentum[3];
   double addedMomentum[3];
   Momenta(rigid, added, vel, waterVel, omega, momentum, angularMomentum, addedMomentum);
 
   double omegaCrossMomentum[3];
   double omegaCrossAngularMomentum[3];
   double omegaCrossWaterVel[3];
   Cross(omega, momentum, omegaCrossMomentum);
   Cross(omega, angularMomentum, omegaCrossAngularMomentum);
   Cross(omega, waterVel, omegaCrossWaterVel);
 
   double munkMoment[3] = {0.0, 0.0, 0.0};
   if (munk == MunkMoment::Included) {
      const double relVel[3] = {vel[0] - waterVel[0], vel[1] - waterVel[1], vel[2] - waterVel[2]};
      Cross(relVel, addedMomentum, munkMoment);
   }
 
   for (int i = 0; i < 3; i++) {
      wrench[i]     = -omegaCrossMomentum[i] - added[i] * omegaCrossWaterVel[i];
      wrench[i + 3] = -omegaCrossAngularMomentum[i] - munkMoment[i];
   }
}
 
inline void AddOmegaTermsJacobian(const double rigid[6], const double added[6], const double vel[3], const double waterVel[3],
      const double omega[3], MunkMoment munk, double dWrench[6][6])
{
   double momentum[3];
   double angularMomentum[3];
   double addedMomentum[3];
   Momenta(rigid, added, vel, waterVel, omega, momentum, angularMomentum, addedMomentum);
   const double relVel[3] = {vel[0] - waterVel[0], vel[1] - waterVel[1], vel[2] - waterVel[2]};
 
   double skewOmega[3][3];
   double skewMomentum[3][3];
   double skewAngularMomentum[3][3];
   double skewWaterVel[3][3];
   double skewAddedMomentum[3][3];
   double skewRelVel[3][3];
   Skew(omega, skewOmega);
   Skew(momentum, skewMomentum);
   Skew(angularMomentum, skewAngularMomentum);
   Skew(waterVel, skewWaterVel);
   Skew(addedMomentum, skewAddedMomentum);
   Skew(relVel, skewRelVel);
 
   for (int i = 0; i < 3; i++) {
      for (int j = 0; j < 3; j++) {
         dWrench[i][j] += -skewOmega[i][j] * (rigid[j] + added[j]);
         dWrench[i][j + 3] += skewMomentum[i][j] + added[i] * skewWaterVel[i][j];
         dWrench[i + 3][j + 3] += skewAngularMomentum[i][j] - skewOmega[i][j] * (rigid[j + 3] + added[j + 3]);
         if (munk == MunkMoment::Included)
            dWrench[i + 3][j] += skewAddedMomentum[i][j] - skewRelVel[i][j] * added[j];
      }
   }
}
} // namespace rigid_body