FhSim  3.1.0
Marine systems simulation
Loading...
Searching...
No Matches
BodyKinematics6Dof.h
1#ifndef BODY_KINEMATICS_6DOF_H
2#define BODY_KINEMATICS_6DOF_H
3
9#include <fhsim/simobject/ISimObjectCreator.h>
10#include <string>
11
42{
43 public:
50 BodyKinematics6Dof(ISimObjectCreator* creator, const std::string& angularRateName, double convergenceTime);
51
53 int PosIndex() const { return m_posIndex; }
54
56 int QuaterIndex() const { return m_quaterIndex; }
57
59 int LocalVelIndex() const { return m_localVelIndex; }
60
62 int AngularRateIndex() const { return m_angularRateIndex; }
63
65 int LocalIndex(int globalIndex) const { return globalIndex - m_posIndex; }
66
73 void Derivatives(const double* X, double posDot[3], double quaterDot[4]) const;
74
81 void AddJacobian(const double* localX, double* J, int nStates) const;
82
88 static void RotationDerivatives(const double quater[4], double dRdq[4][9]);
89
90 private:
91 void AddPositionJacobian(const double* localX, double* J, int nStates) const;
92 void AddQuaternionJacobian(const double* localX, double* J, int nStates) const;
93
94 int m_posIndex = 0;
95 int m_quaterIndex = 0;
96 int m_localVelIndex = 0;
97 int m_angularRateIndex = 0;
98 double m_convergenceTime = 0.0;
99};
100
101#endif
Definition BodyKinematics6Dof.h:42
int LocalIndex(int globalIndex) const
Index within the object's state slice of the state with global index globalIndex.
Definition BodyKinematics6Dof.h:65
void Derivatives(const double *X, double posDot[3], double quaterDot[4]) const
int AngularRateIndex() const
Global index of the body-frame angular-rate state.
Definition BodyKinematics6Dof.h:62
BodyKinematics6Dof(ISimObjectCreator *creator, const std::string &angularRateName, double convergenceTime)
int QuaterIndex() const
Global index of the attitude quaternion state.
Definition BodyKinematics6Dof.h:56
int PosIndex() const
Global index of the position state.
Definition BodyKinematics6Dof.h:53
void AddJacobian(const double *localX, double *J, int nStates) const
static void RotationDerivatives(const double quater[4], double dRdq[4][9])
int LocalVelIndex() const
Global index of the body-frame velocity state.
Definition BodyKinematics6Dof.h:59