dense_components_pointers_3d.hpp

#pragma once

#include "components_pointers_3d.hpp"
#include  "size_2d.hpp"

namespace Cvb
{

  CVB_BEGIN_INLINE_NS


    template<class T>
  class DenseComponentsPointers3D final
  {
    friend class DensePointCloud;

  public:

    DenseComponentsPointers3D() = 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();
    }


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


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


    Size2D<int> LatticeSize() const noexcept
    {
      return latticeSize_;
    }

  private:

    explicit DenseComponentsPointers3D(const ComponentsPointers3D& components, Size2D<int> latticeSize)
      : components_(components)
      , latticeSize_(latticeSize)
    {
    }

    template<class TYPE>
    struct TypeTag {};

    template <class TYPE>
    Point3DC<TYPE> InternalPointAt(std::size_t indexX, std::size_t indexY, TypeTag<Point3DC<TYPE>>) const noexcept
    {
      auto x = *reinterpret_cast<TYPE*>(BasePtrX() + indexY * latticeSize_.Width() * XInc() + indexX * XInc());
      auto y = *reinterpret_cast<TYPE*>(BasePtrY() + indexY * latticeSize_.Width() * YInc() + indexX * YInc());
      auto z = *reinterpret_cast<TYPE*>(BasePtrZ() + indexY * latticeSize_.Width() * ZInc() + indexX * ZInc());
      auto c = *reinterpret_cast<TYPE*>(BasePtrConfidence() + indexY * latticeSize_.Width() * ConfidenceInc() + indexX * ConfidenceInc());
      return Point3DC<TYPE>(x, y, z, c);
    }

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

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

    template <class TYPE>
    void InternalSetPointAt(std::size_t indexX, std::size_t indexY, Point3DC<TYPE> point) noexcept
    {
      *reinterpret_cast<TYPE*>(BasePtrX() + indexY * latticeSize_.Width() * XInc() + indexX * XInc()) = point.X();
      *reinterpret_cast<TYPE*>(BasePtrY() + indexY * latticeSize_.Width() * YInc() + indexX * YInc()) = point.Y();
      *reinterpret_cast<TYPE*>(BasePtrZ() + indexY * latticeSize_.Width() * ZInc() + indexX * ZInc()) = point.Z();
      *reinterpret_cast<TYPE*>(BasePtrConfidence() + indexY * latticeSize_.Width() * ConfidenceInc() + indexX * ConfidenceInc()) = point.Confidence();
    }

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

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

    ComponentsPointers3D components_;
    Size2D<int> latticeSize_;
  };

  CVB_END_INLINE_NS

}