Trajectory.hpp

Go to the documentation of this file.

/***********************************
   Copyright 2018 Ravishankar Mathur

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
***********************************/

#ifndef _OF_TRAJECTORY_
#define _OF_TRAJECTORY_

#include <OpenFrames/Export.h>
#include <osg/Referenced>
#include <OpenThreads/ReadWriteMutex>
#include <vector>

namespace OpenFrames
{
  class TrajectorySubscriber; // Forward-declare trajectory subscriber

  class OF_EXPORT Trajectory : public osg::Referenced
  {
  public:
    typedef double DataType;
    typedef std::vector<DataType> DataArray;
  typedef std::vector<TrajectorySubscriber*> SubscriberArray;

        enum SourceType
    {
      ZERO = 0, // Use 0 for the component's value
      TIME,     // Use the time value
      POSOPT,   // Use a position/optional value
      ATTITUDE  // Use an attitude value
    };

    struct DataSource
    {
      DataSource()
      {
        _src = ZERO;
        _element = _opt = 0;
        _scale = 1.0;
      }

      bool operator == (const DataSource &rhs)
      {
        if(_src == rhs._src && _element == rhs._element
           && _opt == rhs._opt && _scale == rhs._scale) return true;
        else return false;
      }

      SourceType   _src;
      unsigned int _element;
      unsigned int _opt;
      double       _scale;
    };

    Trajectory(unsigned int dof = 3, unsigned int nopt = 0 );
  
  void reserveMemory(unsigned int numPoints, bool usePos = true, bool useAtt = true);

    inline const DataArray& getTimeList() const { return _time; }
    inline const DataArray& getPosOptList() const { return _posopt; }
    inline const DataArray& getAttList() const { return _att; }

    inline unsigned int getNumPos() const { return _numPos; }

    void setNumOptionals(unsigned int nopt);
    inline unsigned int getNumOptionals() const { return _nopt; }

    inline unsigned int getNumAtt() const { return _numAtt; }

    void setDOF(unsigned int dof);
    inline unsigned int getDOF() const { return _dof; }

    inline unsigned int getBase() const { return _base; }

    virtual int getTimeIndex( const DataType &t, int &index ) const;

    inline bool isEmpty() const { return _time.empty(); }
    inline unsigned int getNumTimes() const { return _time.size(); }
    virtual bool getTimeRange( DataType &begin, DataType &end ) const;

        double getTimeDistance(const DataType &t) const;

    virtual bool addTime( const DataType &t );

    virtual bool addPosition( const DataType &x, const DataType &y, 
                const DataType z = 0.0 );
    virtual bool addPosition( const DataType* const pos );
    virtual bool getPosition( unsigned int n, DataType &x, DataType &y ) const;
    virtual bool getPosition( unsigned int n, DataType &x, DataType &y, DataType &z ) const;
    
    virtual bool addAttitude( const DataType &x, const DataType &y,
                const DataType &z, const DataType &w );
    virtual bool addAttitude( const DataType* const att );
    virtual bool getAttitude( unsigned int n, DataType &x, DataType &y,
                     DataType &z, DataType &w ) const;

    virtual bool setOptional( unsigned int index, const DataType &x,
                const DataType &y, const DataType z = 0.0 );
    virtual bool setOptional( unsigned int index, const DataType* const opt );
    virtual bool getOptional( unsigned int n, unsigned int index, DataType &x,
                  DataType &y ) const;
    virtual bool getOptional( unsigned int n, unsigned int index, DataType &x,
                  DataType &y, DataType &z ) const;

    virtual void clear();

    virtual void getPoint(unsigned int i, const DataSource source[], DataType val[]) const;

    virtual bool verifyData(const DataSource source[]) const;

    virtual unsigned int getNumPoints(const DataSource source[]) const;

  virtual void addSubscriber(TrajectorySubscriber* subscriber) const;
  virtual void removeSubscriber(TrajectorySubscriber* subscriber) const;
  virtual void informSubscribers();
  inline void autoInformSubscribers(bool autoinform) { _autoInformSubscribers = autoinform; }
  
  enum DataLockType
  {
    READ_LOCK,
    WRITE_LOCK
  };
  
    virtual void lockData(DataLockType lockType = READ_LOCK) const;   // Block the data from being changed
    virtual void unlockData(DataLockType lockType = READ_LOCK) const; // Allow the data to be changed

  protected:
    virtual ~Trajectory();

    DataArray _time;   // Times
    DataArray _posopt; // Positions & optionals
    DataArray _att;    // Attitudes
  
  mutable SubscriberArray _subscribers; // Subscribers of this Trajectory
  bool _autoInformSubscribers;

    unsigned int _nopt; // Number of optionals for each time.
    unsigned int _base; // Number of data elements taken up by each
                // position/optionals group.  Each data element
                // is of type DataType, so _base=_dof*(1+_nopt)

    unsigned int _numPos; // Current number of pos/opt groups
    unsigned int _numAtt; // Current number of attitudes

    unsigned int _dof; // Degrees of freedom for position and optionals

    // Synchronization variables
#define USERWMUTEX

#ifdef USERWMUTEX
  mutable OpenThreads::ReadWriteMutex _readWriteMutex;
#else
  mutable OpenThreads::Mutex _mutex;
#endif
  };

  class TrajectorySubscriber
  {
  public:
    virtual void dataCleared(Trajectory *traj) = 0;
  
    virtual void dataAdded(Trajectory *traj) = 0;
  };
  
} // !namespace OpenFrames

#endif // !define _OF_TRAJECTORY_