C6DBody.h

#ifndef C_6D_BODY_H
#define C_6D_BODY_H
 
#include "BodyKinematics6Dof.h"
 
#include <fhsim/simobject/SimObject.h>
 
class C6DBody : public SimObject
{
   public:
   C6DBody(std::string sSimObjectName, ISimObjectCreator* pCreator);
 
#ifdef FH_VISUALIZATION
   virtual void RenderInit(Ogre::Root* const ogreRoot, ISimObjectCreator* const creator);
 
   virtual void RenderUpdate(const double T, const double* const X);
 
#endif
 
   virtual void OdeFcn(const double dT, const double* const adX,
       double* const adXDot) const;
 
   bool HasJacobians() const override;
   void OdeJacobian(double T, const double* X, double* J, int nStates) override;
   int GetJacobianSparsity(int nStates, int* rowPtr, int* colIdx) override;
 
   virtual void CalcDerivatives(const double adLocalForces[3],
       const double adLocalMoments[3],
       const double adGlobalForces[3],
       const double adGlobalMoments[3],
       const double adVelLocal[3],
       const double adOmegaLocal[3],
       const double adQuater[4],
       double adVelLocalDot[3],
       double adOmegaLocalDot[3]) const;
 
#ifdef USE_FULL_MASS_MATRIX
   void CalcDerivatives(const double adSumForces[6],
       const double adXVel_d[3],
       const double adXOmega[3],
       const double adXQuater[4],
       const double R[3][3],
       double adXPos_n_DotOut[3],
       double adXVel_d_DotOut[3],
       double adXOmega_DotOut[3],
       double adXQuater_DotOut[4]);
#endif
 
   protected:
   ISignalPort* m_pInMoment; 
   ISignalPort* m_pInForce; 
   virtual const double* Position(const double dT, const double* const adX);
   virtual const double* Velocity(const double dT, const double* const adX);
   virtual const double* Quater(const double dT, const double* const adX);
   virtual const double* LocalVel(const double dT, const double* const adX);
   virtual const double* AngVel(const double dT, const double* const adX);
 
   double m_dMass; 
   double m_dGravity; 
   double m_adVelGlobal[3]; 
   double m_adDiagonalMass[6]; 
   BodyKinematics6Dof m_kinematics; 
   int m_IStatePos; 
   int m_IStateQuater; 
   int m_IStateLocalVel; 
   int m_IStateOmega; 
 
   double m_adBodySize[3]; 
#ifdef USE_FULL_MASS_MATRIX
   bool m_bHasFullMassMatrix; 
   double m_mdMassInverse[6][6]; 
#endif
 
#ifdef FH_VISUALIZATION
   std::string m_sMaterial; 
   std::string m_sMeshName; 
 
   double m_adMeshScale[3]; 
   double m_adMeshOrigin[3]; 
   double m_dVisScale; 
   double m_dLengthScale;
   double m_dHeightScale;
   double m_dCurvatureScale;
   Ogre::Quaternion m_InitOrientation;
   Ogre::Entity* m_pRenderEntity; 
   Ogre::SceneNode* m_pRenderNode; 
   Ogre::SceneManager* m_pSceneMgr; 
#endif
};
 
 
#endif