RigidBodyEnvironmentForces.h

#ifndef RigidBodyEnvironmentForces_H
#define RigidBodyEnvironmentForces_H
#include "CRigidBody.h"
 
#include <fhsim_environment/SeafloorForcesInterface.h>
 
namespace rigidbody
{
namespace soilforces
{
inline environment::seabed::RigidMotion<double> RigidMotionOf(const vec6& dX, const vec3& r, const quat& q)
{
   environment::seabed::RigidMotion<double> motion;
   const vec3 velocity        = q * dX.head<3>();
   const vec3 angularVelocity = q * dX.tail<3>();
   for (int i = 0; i < 3; i++) {
      motion.pos[i]   = r(i);
      motion.vel[i]   = velocity(i);
      motion.omega[i] = angularVelocity(i);
      const vec3 axis = q * vec3::Unit(i);
      for (int j = 0; j < 3; j++)
         motion.R[3 * i + j] = axis(j);
   }
   return motion;
}
 
inline vec6 BodyLocalLoad(const environment::seabed::ContactLoad<double>& load, const quat& q)
{
   const quat inverse = q.conjugate();
   vec6 force;
   force << inverse * vec3(load.force[0], load.force[1], load.force[2]), inverse * vec3(load.moment[0], load.moment[1], load.moment[2]);
   return force;
}
 
class SoilElement
{
   public:
   void SetSeafloorForces(environment::SeafloorForcesInterface* seafloorForces) { m_seafloorForces = seafloorForces; }
   bool InSeabedContact() const { return m_inSeabedContact; }
 
   protected:
   environment::SeafloorForcesInterface* m_seafloorForces = nullptr;
   bool m_inSeabedContact                                 = false;
};
} // namespace soilforces
} // namespace rigidbody
 
#endif