deadReckoner.h

Go to the documentation of this file.

/****************************************************************************
 * Copyright (c) 2014 VT MAK
 * All rights reserved.
 ****************************************************************************/


#pragma once

#include <vlpi/approximator.h>
#include <vlpi/disEnums.h>

#include <matrix/vlTaitBryan.h>
#include <matrix/vlVector.h>
#include <matrix/vlDcm.h>

class DT_DLL_VLPROTIND DtDeadReckoner : public DtApproximator
{
public:

   DtDeadReckoner();

   DtDeadReckoner( const DtDeadReckoner& orig );

   virtual ~DtDeadReckoner();

   DtDeadReckoner& operator = ( const DtDeadReckoner& orig );

   bool operator == (const DtDeadReckoner& orig ) const;

   virtual DtApproximator* clone() const;

   virtual void setAll(
      const DtVector& loc,
      const DtVector32& vel,
      const DtTaitBryan& ori,
      const DtVector32& acc,
      const DtVector32& rotVel,
      DtTime t);

   virtual void setLocation( const DtVector& loc, DtTime curTime );

   virtual const DtVector& approxLocation( DtTime curTime ) const;

   virtual const DtVector& deadReckonedLocation( DtTime curTime ) const;

   virtual void setVelocity( const DtVector32& vel, DtTime curTime );

   virtual const DtVector32& approxVelocity( DtTime curTime ) const;

   virtual const DtVector32& deadReckonedVelocity( DtTime curTime ) const;

   virtual void setOrientation( const DtTaitBryan& ori, DtTime curTime );

   virtual const DtTaitBryan& approxEuler( DtTime curTime ) const;

   virtual const DtTaitBryan& deadReckonedEuler( DtTime curTime ) const;

   virtual const DtTime& locationUpdateTime() const;

   virtual const DtTime& velocityUpdateTime() const;

   virtual const DtTime& orientationUpdateTime() const;

   virtual void setAcceleration( const DtVector32& acc, DtTime curTime );
   virtual const DtVector32& acceleration( DtTime curTime ) const;

   virtual void setRotationalVelocity( const DtVector32& vel, DtTime curTime );
   virtual const DtVector32& rotationalVelocity( DtTime curTime ) const;

   virtual const DtDcm& approxBodyToGeoc( DtTime curTime ) const;

   virtual const DtDcm& deadReckonedBodyToGeoc( DtTime curTime ) const;

   virtual void setAlgorithm( DtDeadReckonTypes alg );
   virtual DtDeadReckonTypes algorithm() const;


   virtual int isRotational() const;

   virtual void deadReckonPosition( DtTime dt, const DtVector& oldPos,
      const DtVector32& oldVel, const DtVector32& oldAccel,
      DtVector32& newVel, DtVector& newPos ) const;

   virtual void deadReckonOrientation( DtTime dt, const DtDcm& old_b2w,
      const DtDirectedRotRate& drr, DtDcm& new_b2w ) const;



   static int isRotational( DtDeadReckonTypes algorithm );

   static void deadReckonPosition( DtTime dt, DtDeadReckonTypes algorithm,
      const DtVector& oldPos, const DtVector32& oldVel, const DtVector32& oldAccel,
      DtVector32& newVel, DtVector& newPos );

   static void deadReckonOrientation( DtTime dt, DtDeadReckonTypes algorithm,
      const DtDcm& old_b2w, const DtDirectedRotRate& drr, DtDcm& new_b2w );

protected:

   virtual void deadReckonLoc( DtTime curTime ) const;

   virtual void deadReckonBodyToGeoc( DtTime curTime ) const;

   virtual void deadReckonEuler( DtTime curTime ) const;

protected:

   DtVector myLocAtTimeOfValidity;
   DtVector32 myVelAtTimeOfValidity;
   DtTime mySimTimePosDataValid;
   DtDcm myBodyToGeocAtTimeOfValidity;
   DtTime mySimTimeOrientDataValid;

   mutable DtVector myDRLoc;
   mutable DtVector32 myDRVel;
   mutable DtTime myDRPosTime;
   mutable DtDcm myDRBodyToGeoc;
   mutable DtTime myDRBodyToGeocTime;
   mutable DtTaitBryan myDREuler;
   mutable DtTime myDREulerTime;

   DtDirectedRotRate myDirectedRotRate;
   DtDeadReckonTypes myAlgorithm;
   DtVector32 myAccel;
   DtBodyRates myRotVel;

};

inline const DtVector& DtDeadReckoner::deadReckonedLocation( DtTime curTime ) const
{
   deadReckonLoc( curTime );
   return myDRLoc;
}

inline const DtVector& DtDeadReckoner::approxLocation( DtTime curTime ) const
{
   return deadReckonedLocation( curTime );
}

inline const DtVector32& DtDeadReckoner::deadReckonedVelocity( DtTime curTime ) const
{
   deadReckonLoc( curTime );
   return myDRVel;
}

inline const DtVector32& DtDeadReckoner::approxVelocity( DtTime curTime ) const
{
   return deadReckonedVelocity( curTime );
}

inline const DtDcm& DtDeadReckoner::deadReckonedBodyToGeoc( DtTime curTime ) const
{
   deadReckonBodyToGeoc( curTime );
   return myDRBodyToGeoc;
}

inline const DtDcm& DtDeadReckoner::approxBodyToGeoc( DtTime curTime ) const
{
   return deadReckonedBodyToGeoc( curTime );
}

inline const DtTaitBryan& DtDeadReckoner::deadReckonedEuler( DtTime curTime ) const
{
   deadReckonEuler( curTime );
   return myDREuler;
}

inline const DtTaitBryan& DtDeadReckoner::approxEuler( DtTime curTime ) const
{
   return deadReckonedEuler( curTime );
}

inline const DtTime& DtDeadReckoner::locationUpdateTime() const
{
   return mySimTimePosDataValid;
}

inline const DtTime& DtDeadReckoner::velocityUpdateTime() const
{
   // This is the same as Position, because velocity is recalculated
   // jointly in this implementation of deadreckoning.
   return mySimTimePosDataValid;
}

inline const DtTime& DtDeadReckoner::orientationUpdateTime() const
{
   return mySimTimeOrientDataValid;
}

inline void DtDeadReckoner::setAlgorithm( DtDeadReckonTypes alg )
{
   myAlgorithm = alg;
}

inline DtDeadReckonTypes DtDeadReckoner::algorithm() const
{
   return myAlgorithm;
}

inline const DtVector32& DtDeadReckoner::acceleration( DtTime curTime ) const
{
   return myAccel;
}

inline const DtVector32& DtDeadReckoner::rotationalVelocity( DtTime curTime ) const
{
   return myRotVel;
}