Position.h

Go to the documentation of this file.

#ifndef _GAMS_UTILITY_POSITION_H_
#define _GAMS_UTILITY_POSITION_H_

#include <vector>

#include "gams/GamsExport.h"
#include "madara/knowledge/containers/DoubleVector.h"
#include "madara/knowledge/containers/NativeDoubleVector.h"

#include "gams/pose/Position.h"
#include "gams/pose/ReferenceFrame.h"

namespace gams
{
  namespace utility
  {
    class GAMS_EXPORT Position
    {
    public:
      Position (
        double init_x = DBL_MAX, double init_y = 0.0, double init_z = 0.0);

      Position(const Position & source);

      Position (const pose::Position & pos)
        : x(pos.x()), y(pos.y()), z(pos.z()) {}

      pose::Position to_pos (const pose::ReferenceFrame &frame) const {
        return pose::Position (frame, x, y, z);
      }

      virtual ~Position ();

      void operator= (const Position & rhs);

      bool operator== (const Position & rhs) const;
      
      bool operator== (const
        madara::knowledge::containers::DoubleArray & rhs) const;

      bool operator== (const
        madara::knowledge::containers::NativeDoubleArray & rhs) const;
      
      bool operator!= (const Position & rhs) const;
      
      bool operator!= (const
        madara::knowledge::containers::DoubleArray & rhs) const;

      bool operator!= (const
        madara::knowledge::containers::NativeDoubleArray & rhs) const;

      bool operator< (const Position& rhs) const;

      virtual double dot (const Position& p_2) const;

      bool approximately_equal (
        const Position & rhs, const double & epsilon) const;

      bool approximately_equal_2d (
        const Position & rhs, const double & epsilon) const;

      void direction_to (
        const Position& rhs, double& phi, double& theta) const;

      double distance_to (const Position & rhs) const;

      virtual double distance_to_2d (const Position & rhs) const;

      virtual double distance_to_2d (
        const Position& end, const Position& check) const;

      virtual bool slope_2d (
        const Position & p, double & slope) const;

      virtual bool is_between_2d (const Position & end, const Position & check) const;
      
      std::string to_string (const std::string & delimiter = ",") const;

      static Position from_string (const std::string & delimiter = ",");

      static Position from_record (const madara::knowledge::KnowledgeRecord & record);

      virtual void to_container (
        madara::knowledge::containers::DoubleArray & target) const;
      
      virtual void from_container (
        madara::knowledge::containers::DoubleArray & source);
      
      virtual void to_container (
        madara::knowledge::containers::NativeDoubleArray & target)
        const;
      
      virtual void from_container (
        madara::knowledge::containers::NativeDoubleArray & source);

      double x;

      double y;

      double z;

    protected:
      Position operator- (const Position & rhs) const;

      Position operator+ (const Position & rhs) const;

      Position operator* (const double& scale) const;
    };

    // helpful typedef for vector of positions
    typedef std::vector <Position>    Positions;
  }
}

#endif // _GAMS_UTILITY_POSITION_H_