point_3d_c.hpp

#pragma once

#include <cmath>
#include <cassert>

#include "global.hpp"

#include "angle.hpp"
#include "point_3d.hpp"

namespace Cvb
{

CVB_BEGIN_INLINE_NS

#pragma pack ( push, 4 )


template< class T, class ENABLE = void>
class Point3DC final
{

#ifndef CVB_DOXYGEN
  static_assert(std::is_arithmetic<T>::value, "CVB: Unsupported data type - must be arithmetic!");
};


template <class T>
class Point3DC<T,
  typename EnableIfArithmetic<T>::type> final
{

#endif

  public:

    using ValueType = T;


    Point3DC() noexcept = default;



    Point3DC(T x, T y, T z) noexcept
      : x_(x)
      , y_(y)
      , z_(z)
    {
    }


    Point3DC(T x, T y, T z, T c) noexcept
      : x_(x)
      , y_(y)
      , z_(z)
      , c_(c)
    {
    }



    Point3DC(Point3D<T> point3d) noexcept
      : Point3DC(point3d.X(), point3d.Y(), point3d.Z())
    {
    }


    template<std::size_t N>
    Point3DC(const T(&list)[N]) noexcept
    {
      static_assert(N == 4, "CVB: Point3DC must have 4 elements");
      std::copy(std::begin(list), std::end(list), &x_);
    }




    T X() const noexcept
    {
      return x_;
    }


    void SetX(T x) noexcept
    {
      x_ = x;
    }


    T Y() const noexcept
    {
      return y_;
    }


    void SetY(T y)
    {
      y_ = y;
    }


    T Z() const noexcept
    {
      return z_;
    }


    void SetZ(T z)
    {
      z_ = z;
    }


    T Confidence() const noexcept
    {
      return c_;
    }


    void SetConfidence(T c)
    {
      c_ = c;
    }


    T Length() const noexcept
    {
      return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
    }



    bool operator==(const Point3DC<T>& other) const noexcept
    {
      return (x_ == other.x_ && y_ == other.y_ && z_ == other.z_ && c_ == other.c_);
    }



    bool operator!=(const Point3DC<T>& other) const noexcept
    {
      return !(*this == other);
    }

    template <class C>
    explicit operator Point3DC<C>() const noexcept
    {
      static_assert(std::is_floating_point<C>::value, "CVB: Unsupported data type - must be floating point!");
      return Point3DC<C>(static_cast<C>(X()), static_cast<C>(Y()), static_cast<C>(Z()), static_cast<C>(C()));
    }


    Point3DC<T>& operator +=(const Point3DC<T> & point) noexcept
    {
      x_ += point.x_;
      y_ += point.y_;
      z_ += point.z_;  
      c_ = std::min(c_, point.c_);

      return *this;
    }


    Point3DC<T>& operator -=(const Point3DC<T> & point) noexcept
    {
      x_ -= point.x_;
      y_ -= point.y_;
      z_ -= point.z_;
      c_ = std::min(c_, point.c_);

      return *this;
    }


    Point3DC<T>& operator *=(const T & value) noexcept
    {
      x_ *= value;
      y_ *= value;
      z_ *= value;
      return *this;
    }


    Point3DC<T>& operator /=(const T & value) noexcept
    {
      x_ /= value;
      y_ /= value;
      z_ /= value;
      return *this;
    }


    const T& operator[](int index) const noexcept
    {
      assert(index < 4 && index >= 0);
      return *(&x_ + index);
    }


    T& operator[](int index) noexcept
    {
      assert(index < 4 && index >= 0);
      return *(&x_ + index);
    }

  private:

    T x_ = static_cast<T>(0.0);
    T y_ = static_cast<T>(0.0);
    T z_ = static_cast<T>(0.0);
    T c_ = static_cast<T>(1.0);

};


template <class T>
inline Point3DC<T> operator+(const Point3DC<T> & lhs, const Point3DC<T> & rhs)
{
  Point3DC<T> retval(lhs);
  retval += rhs;
  return retval;
}


template <class T>
inline Point3DC<T> operator-(const Point3DC<T> & lhs, const Point3DC<T> & rhs)
{
  Point3DC<T> retval(lhs);
  retval -= rhs;
  return retval;
}


template <class T>
inline T operator*(const Point3DC<T> & lhs, const Point3DC<T> & rhs)
{
  return lhs.X() * rhs.X() + lhs.Y() * rhs.Y() + lhs.Z() * rhs.Z() ;
}


template <class T>
inline Point3DC<T> operator*(const Point3DC<T> & lhs, const T & rhs)
{
  Point3DC<T> retval(lhs);
  retval *= rhs;
  return retval;
}


template <class T>
inline Point3DC<T> operator*(const T & lhs, const Point3DC<T> & rhs)
{
  return rhs * lhs;
}


template <class T>
inline Point3DC<T> operator/(const Point3DC<T> & lhs, const T & rhs)
{
  Point3DC<T> retval(lhs);
  retval /= rhs;
  return retval;
}


template<class T>
using Vector3DC = Point3DC<T>;

#pragma pack ( pop )


CVB_END_INLINE_NS


}