trigonometricSmoother.h

Go to the documentation of this file.

/*******************************************************************************
** Copyright (c) 1992-2025 MAK Technologies, Inc
** All rights reserved.
*******************************************************************************/


#pragma once

#include "deadReckoner.h"

class DT_DLL_VLPROTIND DtTrigonometricSmoother : public DtDeadReckoner
{
public:
   DtTrigonometricSmoother();
   virtual ~DtTrigonometricSmoother();
   DtTrigonometricSmoother( const DtTrigonometricSmoother& src );
   DtTrigonometricSmoother& operator=( const DtTrigonometricSmoother& src );

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

   virtual DtApproximator* clone() const;

protected:
   virtual void resetPosSmoother( DtTime t );
   virtual void resetOriSmoother( DtTime t );

public:
   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 t );
   virtual const DtVector& approxLocation( DtTime t ) const;

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

   const DtVector32& approxUnsmoothedVelocity( DtTime curTime ) const;

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

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

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

   virtual const DtDcm& approxBodyToGeoc( DtTime t ) const;

   virtual void setSmoothPeriod( DtTime t );
   virtual DtTime smoothPeriod() const;

   virtual void setOriSmoothPeriod( DtTime t );
   virtual DtTime oriSmoothPeriod() const;

   virtual void setSmoothEnable(bool onOff);
   virtual bool smoothEnable() const;

protected:
   virtual const DtVector& smoothPosition( DtTime t );
   virtual const DtVector32& smoothVelocity( DtTime t );
   virtual const DtDcm& smoothBodyToGeoc( DtTime t );
   virtual void initSmootherParams();
   virtual void initSmoothOriParams();

   inline DtTrigonometricSmoother* self() const;

public:
   static void setDfltSmoothPeriod(DtTime smoothPeriod);
   static DtTime dfltSmoothPeriod();

   static void setMasterSmoothEnable(bool onOff);
   static bool masterSmoothEnable();


   static void rotAngleAndAxisFromRotMatrix(
      DtVector& n, double& phi, const DtDcm& m );
   static void rotMatrixFromRotRatesAndTime(
      DtDcm& m, DtDcm& o, DtVector& n, double& w, const DtVector32& r, double t );

protected:
   static double theTimeTolerance;
   static bool   theMasterSmoothEnable;
   static DtTime theDfltSmoothPeriod;

protected:
   DtVector    myLastDrPos;
   DtVector32  myLastDrVel;
   DtVector    mySmoothPos;
   DtVector32  mySmoothVel;
   DtVector    myX1;
   DtVector    myV1;
   DtVector    myA1;
   DtVector    myA2;
   DtTime      myTau;
   DtTime      myTau2;
   DtTime      myTau2Inv;
   DtTime      myTauSq4;
   DtTime      myTauSq4Inv;
   DtTime      myLastSetTime;
   DtTime      myPostSmoothTime;
   DtTime      myHalfSmoothTime;
   DtTime      myOriTau;
   DtTime      myLastSetOriTime;
   DtTime      myPostSmoothOriTime;
   DtTime      myLastSmoothOriTime;
   DtTime      myLastSmoothTbTime;
   double      mySmoothRotRate;
   DtDcm       myLastDrOri;
   DtDcm       mySmoothOri;
   DtDcm       mySmoothOmegaMx;
   DtVector    mySmoothRotAxis;
   DtTaitBryan mySmoothTb;
   bool        myIsDirty;
   bool        myOriIsDirty;
   bool        myTbIsDirty;
   bool        mySmoothEnable;
};


inline DtTrigonometricSmoother* DtTrigonometricSmoother::self() const
{
   return (DtTrigonometricSmoother*)this;
}


typedef DtTrigonometricSmoother DtSmoother;