CRigidBody.h

#ifndef C_RigidBody_H
#define C_RigidBody_H
 
#include <fhsim/simobject/SimObject.h>
 
#include <Eigen/Eigen>
namespace environment
{
class EnvironmentProvider;
 
}
namespace rigidbody
{
typedef Eigen::Matrix<double, 6, 6> mat6;
typedef Eigen::Matrix<double, 6, 1> vec6;
typedef Eigen::Matrix<double, 3, 3> mat3;
typedef Eigen::Matrix<double, 3, 1> vec3;
typedef Eigen::Matrix<double, 13, 1> vec13;
typedef Eigen::Matrix<double, 13, 13> mat13;
typedef Eigen::Matrix<double, 3, 4> mat34;
typedef Eigen::Matrix<double, 6, 4> mat64;
typedef Eigen::Quaternion<double> quat;
 
class CRigidBody
{
   public:
   virtual ~CRigidBody(){};
 
   // r: global
   // q: global
   // dX: bodylocal
   // return: bodylocal
   virtual vec6 GetInternalForces(const vec6& dX, const vec3& r, const quat& q, environment::EnvironmentProvider* environment, double time, const double* states) = 0;
 
   virtual mat64 GetGravityForceDerivative(const quat& q) = 0;
 
   // return: bodylocal
   virtual mat6 GetInertiaMatrix(const vec3& r, double time, const double* states, environment::EnvironmentProvider* environment) = 0;
 
   // all global coords
   vec13 GetSecondDerivative(const vec3& r, const quat& q, const vec3& v, const vec3 w, const vec6& externalForces, environment::EnvironmentProvider* environment, double time, const double* states);
 
   mat13 GetSecondDerivativeJacobian(const vec3& r, const quat& q, const vec3& v, const vec3& w, const vec6& externalForces, environment::EnvironmentProvider* environment, double time, const double* states);
 
   void WriteStateJacobian(const double* X, int positionIndex, int quaterIndex, int velocityIndex, int omegaIndex, const vec6& externalForces, environment::EnvironmentProvider* environment, double time, double* J, int nStates);
 
   // bodylocal coords
   static vec6 GetCoriolisForce(const vec6& dX, const mat6& Inertia);
   static mat6 GetCoriolisForceDerivative(const vec6& dX, const mat6& Inertia);
 
 
#ifdef FH_VISUALIZATION
   virtual void DrawBody(Ogre::SceneNode* renderNode, Ogre::SceneManager* sceneMgr) = 0;
#endif
 
   //static mat6 reorientInertiaRotateTranslate(const mat6& Inertia, const vec6& orientation);
   static mat6 ReorientInertiaTranslateRotate(const mat6& Inertia, const vec6& orientation);
   static mat3 MakeDyadic(const vec3& vector);
 
   static constexpr double kQuaterConvergenceTime = 0.01;
   static constexpr double QuaterCorrectionRate() { return 1.0 / (2.0 * kQuaterConvergenceTime); }
   static mat34 RotationDerivative(const quat& q, const vec3& x);
   static mat34 ConjugateRotationDerivative(const quat& q, const vec3& x);
   static mat3 GetRotation(const vec3& angle);
   static mat6 GetRotation6(const vec3& angle);
 
   private:
   vec6 GetBodyAcceleration(const vec3& r, const quat& q, const vec6& dX, const vec6& bodyExternalForces, const mat6& Inertia, environment::EnvironmentProvider* environment, double time, const double* states);
   static vec6 RotateToBody(const quat& q, const vec6& globalVector);
   static mat6 GetCoriolisMatrix(const vec6& dX);
};
}; // namespace rigidbody
#endif