smoothUtil.h

Go to the documentation of this file.

/*********************************************************************
** Copyright (c) 1992-2010 VT MAK
** All rights reserved.
*********************************************************************/



#ifndef DtSmoothPos_H_
#define DtSmoothPos_H_

#include <matrix/vlMath.h>
#include <matrix/vlDcm.h>
#include <matrix/vlVector.h>
#include <vlutil/vlTime.h>
#include <vlutil/vlPrint.h>

class DT_DLL_VLPROTIND DtSmoothPos
{
public:

   DtSmoothPos(void *usr, DtTime smoothT, 
      const DtVector& pos, const DtVector32& vel, const DtVector32& acc);
   DtSmoothPos(void *usr, DtTime smoothT);

   virtual ~DtSmoothPos() {}

   DtSmoothPos(const DtSmoothPos&);

   DtSmoothPos& operator=(const DtSmoothPos&);

public:

   void input(const DtVector& pos, const DtVector32& vel, const DtVector32& acc,
      DtTime simTime, int freeze=0);
   void tick(DtTime simTime);

   const DtVector& position() const { return ps; }
   const DtVector32& velocity() const { return vs; }

   virtual void setSmoothEnable(int on); 
   virtual int smoothEnable() const;

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

   virtual void reset();

public:

   static void setDfltSmoothPeriod(DtTime t); 
   static DtTime dfltSmoothPeriod();

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

protected:


   virtual void dfltAction(DtTime simTime, void *usr, 
      DtVector32& vs, DtVector& ps) = 0;
   void init(void *u, DtTime t);
   void setT1data();    

protected:

   int myFrozen;

   DtTime myT0;
   DtTime myT1;
   DtTime myT2;
   DtTime myLastT;

   DtTime myLocalTau;
   DtTime myLocalHtau;
   DtTime myLocalTauInv;
   DtTime* myTau;
   DtTime* myHtau;
   DtTime* myTauInv;

   DtVector ps, p0, dp, p1;
   DtVector32 vs, v0, v1, dv;
   DtVector32 a1, a2, ap;
   int myEnable;
   int myNeedT1Data;
   int myNeedT2Data;
   void *myUsr;

protected:

   static DtTime theDfltTau;
   static DtTime theDfltHtau;
   static DtTime theDfltTauInv;
   static bool theMasterSmoothEnable;
};


class DT_DLL_VLPROTIND DtSmoothOrient
{
public:

   DtSmoothOrient(void *usr, DtTime smoothT, 
      const DtDcm & orient, const DtDirectedRotRate &drr);
   DtSmoothOrient(void *usr, DtTime smoothT);

   virtual ~DtSmoothOrient() {} 

   DtSmoothOrient(const DtSmoothOrient&);

   DtSmoothOrient& operator=(const DtSmoothOrient&);

   void input(const DtDcm & orient, const DtDirectedRotRate &drr, 
      DtTime simTime, int freeze=0);
   void tick(DtTime simTime);

   const DtDcm & orientation() const { return myOs; }

   virtual void setSmoothEnable(int on); 
   virtual int smoothEnable() const;

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

   virtual void reset();

public:

   static void setDfltSmoothPeriod(DtTime t); 
   static DtTime dfltSmoothPeriod();

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

   static void setDfltSmoothPrecision(double t);
   static double dfltSmoothPrecision();

protected:

   virtual void dfltAction(DtTime simTime, void *usr, DtDcm& os) = 0;
   void init(void *u, DtTime t);
   void setT1data();    

protected:

   int myFrozen;
   DtTime myT0, myT2, myLastT;
   DtTime* myTau;
   DtTime myLocalTau;
   DtDcm myOs, myO0;
   DtVector myWs;
   int myEnable;
   void *myUsr;
   double myPrecision;

protected:
   
   static DtTime theDfltTau;
   static bool theMasterSmoothEnable;
   static double theDfltPrecision;
};


inline DtTime DtSmoothPos::smoothPeriod() const 
{ 
   return *myTau;
}

inline DtTime DtSmoothOrient::smoothPeriod() const 
{ 
   return *myTau;
}

#endif /* _SMOOTHUTIL_H_ */