sparse_components_pointers_3d.hpp

#pragma once

#include "components_pointers_3d.hpp"

namespace Cvb
{

CVB_BEGIN_INLINE_NS


template<class T>
class SparseComponentsPointers3D final
{
  friend class SparsePointCloud;

  public:

    SparseComponentsPointers3D() = default;


    std::uintptr_t BasePtrX() const noexcept
    {
      return components_.BasePtrX();
    }


    std::uintptr_t BasePtrY() const noexcept
    {
      return components_.BasePtrY();
    }


    std::uintptr_t BasePtrZ() const noexcept
    {
      return components_.BasePtrZ();
    }


    std::uintptr_t BasePtrW() const noexcept
    {
      return components_.BasePtrW();
    }


    std::uintptr_t BasePtrConfidence() const noexcept
    {
      return components_.BasePtrConfidence();
    }


    std::intptr_t XInc() const noexcept
    {
      return components_.XInc();
    }


    std::intptr_t YInc() const noexcept
    {
      return components_.YInc();
    }


    std::intptr_t ZInc() const noexcept
    {
      return components_.ZInc();
    }


    std::intptr_t WInc() const noexcept
    {
      return components_.WInc();
    }


    std::intptr_t ConfidenceInc() const noexcept
    {
      return components_.ConfidenceInc();
    }


    std::size_t NumPoints() const noexcept
    {
      return components_.NumPoints();
    }


    T PointAt(std::size_t index) const noexcept
    {
      return InternalPointAt(index, TypeTag<T>{});
    }


    void SetPointAt(std::size_t index, T point) noexcept
    {
      return InternalSetPointAt(index, point);
    }

  private:

    explicit SparseComponentsPointers3D(const ComponentsPointers3D& components) noexcept
      : components_(components)
    {
    }

    template<class TYPE>
    struct TypeTag {};

    template <class TYPE>
    Point3DH<TYPE> InternalPointAt(std::size_t index, TypeTag<Point3DH<TYPE>>) const noexcept
    {
      auto x = *reinterpret_cast<TYPE*>(BasePtrX() + index * XInc());
      auto y = *reinterpret_cast<TYPE*>(BasePtrY() + index * YInc());
      auto z = *reinterpret_cast<TYPE*>(BasePtrZ() + index * ZInc());
      auto w = *reinterpret_cast<TYPE*>(BasePtrW() + index * WInc());
      return Point3DH<TYPE>(x, y, z, w);
    }

    template <class TYPE>
    Point3D<TYPE> InternalPointAt(std::size_t index, TypeTag<Point3D<TYPE>>) const noexcept
    {
      auto x = *reinterpret_cast<TYPE*>(BasePtrX() + index * XInc());
      auto y = *reinterpret_cast<TYPE*>(BasePtrY() + index * YInc());
      auto z = *reinterpret_cast<TYPE*>(BasePtrZ() + index * ZInc());
      return Point3D<TYPE>(x, y, z);
    }

    template <class TYPE>
    void InternalSetPointAt(std::size_t index, Point3DH<TYPE> point) noexcept
    {
      *reinterpret_cast<TYPE*>(BasePtrX() + index * XInc()) = point.X();
      *reinterpret_cast<TYPE*>(BasePtrY() + index * YInc()) = point.Y();
      *reinterpret_cast<TYPE*>(BasePtrZ() + index * ZInc()) = point.Z();
      *reinterpret_cast<TYPE*>(BasePtrW() + index * WInc()) = point.W();
      
    }

    template <class TYPE>
    void InternalSetPointAt(std::size_t index, Point3D<TYPE> point) noexcept
    {
      *reinterpret_cast<TYPE*>(BasePtrX() + index * XInc()) = point.X();
      *reinterpret_cast<TYPE*>(BasePtrY() + index * YInc()) = point.Y();
      *reinterpret_cast<TYPE*>(BasePtrZ() + index * ZInc()) = point.Z();
    }

    ComponentsPointers3D components_;
};

CVB_END_INLINE_NS

}