BodyKinematics6Dof.h

#ifndef BODY_KINEMATICS_6DOF_H
#define BODY_KINEMATICS_6DOF_H
 
#include <fhsim/simobject/ISimObjectCreator.h>
#include <string>
 
class BodyKinematics6Dof
{
   public:
   BodyKinematics6Dof(ISimObjectCreator* creator, const std::string& angularRateName, double convergenceTime);
 
   int PosIndex() const { return m_posIndex; }
 
   int QuaterIndex() const { return m_quaterIndex; }
 
   int LocalVelIndex() const { return m_localVelIndex; }
 
   int AngularRateIndex() const { return m_angularRateIndex; }
 
   int LocalIndex(int globalIndex) const { return globalIndex - m_posIndex; }
 
   void Derivatives(const double* X, double posDot[3], double quaterDot[4]) const;
 
   void AddJacobian(const double* localX, double* J, int nStates) const;
 
   static void RotationDerivatives(const double quater[4], double dRdq[4][9]);
 
   private:
   void AddPositionJacobian(const double* localX, double* J, int nStates) const;
   void AddQuaternionJacobian(const double* localX, double* J, int nStates) const;
 
   int m_posIndex           = 0;
   int m_quaterIndex        = 0;
   int m_localVelIndex      = 0;
   int m_angularRateIndex   = 0;
   double m_convergenceTime = 0.0;
};
 
#endif