sbg_vector3.h

Go to the documentation of this file.

#ifndef SBG_VECTOR_3_H
#define SBG_VECTOR_3_H

// ROS headers
#include <geometry_msgs/Vector3.h>

// Sbg headers
#include <sbgDefines.h>

namespace sbg
{
template <typename T>
bool areEquals(T firstValue, T secondValue)
{
  //
  // The machine epsilon has to be scaled to the magnitude of the values used.
  //
  return std::fabs(firstValue - secondValue) <= (std::numeric_limits<T>::epsilon() * std::fabs(firstValue + secondValue));
}

template <class T>
class SbgVector3
{
private:

  //---------------------------------------------------------------------//
  //- Private variables                                                 -//
  //---------------------------------------------------------------------//

  std::array<T, 3> m_data;

public:

  //---------------------------------------------------------------------//
  //- Constructor                                                       -//
  //---------------------------------------------------------------------//

  SbgVector3(void)
  {
    m_data[0] = static_cast<T>(0.0);
    m_data[1] = static_cast<T>(0.0);
    m_data[2] = static_cast<T>(0.0);
  }

  SbgVector3(T x_value, T y_value, T z_value)
  {
    m_data[0] = x_value;
    m_data[1] = y_value;
    m_data[2] = z_value;
  };

  SbgVector3(const T* p_raw_data, size_t array_size)
  {
    assert(array_size == 3);

    m_data[0] = p_raw_data[0];
    m_data[1] = p_raw_data[1];
    m_data[2] = p_raw_data[2];
  };

  //---------------------------------------------------------------------//
  //- Parameters                                                        -//
  //---------------------------------------------------------------------//

  //---------------------------------------------------------------------//
  //- Operations                                                        -//
  //---------------------------------------------------------------------//

  bool operator==(const SbgVector3<T> &ref_vector)
  {
    return ((areEquals(m_data[0], ref_vector.m_data[0]))
          && (areEquals(m_data[1], ref_vector.m_data[1]))
          && (areEquals(m_data[2], ref_vector.m_data[2])));
  };

  bool operator!=(const SbgVector3<T> &ref_vector)
  {
    return !(*this == ref_vector);
  };
  
  const T operator()(size_t index) const
  {
          assert(index < 3);

          return m_data[index];
  }

  const T *data(void) const
  {  
    return static_cast<const T*>(m_data.data());
  };
};

typedef SbgVector3<float>  SbgVector3f;
typedef SbgVector3<double> SbgVector3d;

}

#endif // SBG_VECTOR_3_H