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_