common/vecmat.h

   1 #ifndef COMMON_VECMAT_H
   2 #define COMMON_VECMAT_H
   3
   4 #include <array>
   5 #include <cmath>
   6 #include <cstddef>
   7 #include <limits>
   8
   9 #include "alspan.h"
  10
  11
  12 namespace alu {
  13
  14 template<typename T>
  15 class VectorR {
  16     static_assert(std::is_floating_point<T>::value, "Must use floating-point types");
  17     alignas(16) std::array<T,4> mVals;
  18
  19 public:
  20     constexpr VectorR() noexcept = default;
  21     constexpr VectorR(const VectorR&) noexcept = default;
  22     constexpr VectorR(T a, T b, T c, T d) noexcept : mVals{{a, b, c, d}} { }
  23
  24     constexpr VectorR& operator=(const VectorR&) noexcept = default;
  25
  26     T& operator[](size_t idx) noexcept { return mVals[idx]; }
  27     constexpr const T& operator[](size_t idx) const noexcept { return mVals[idx]; }
  28
  29     VectorR& operator+=(const VectorR &rhs) noexcept
  30     {
  31         mVals[0] += rhs.mVals[0];
  32         mVals[1] += rhs.mVals[1];
  33         mVals[2] += rhs.mVals[2];
  34         mVals[3] += rhs.mVals[3];
  35         return *this;
  36     }
  37
  38     VectorR operator-(const VectorR &rhs) const noexcept
  39     {
  40         const VectorR ret{mVals[0] - rhs.mVals[0], mVals[1] - rhs.mVals[1],
  41             mVals[2] - rhs.mVals[2], mVals[3] - rhs.mVals[3]};
  42         return ret;
  43     }
  44
  45     T normalize(T limit = std::numeric_limits<T>::epsilon())
  46     {
  47         limit = std::max(limit, std::numeric_limits<T>::epsilon());
  48         const T length_sqr{mVals[0]*mVals[0] + mVals[1]*mVals[1] + mVals[2]*mVals[2]};
  49         if(length_sqr > limit*limit)
  50         {
  51             const T length{std::sqrt(length_sqr)};
  52             T inv_length{T{1}/length};
  53             mVals[0] *= inv_length;
  54             mVals[1] *= inv_length;
  55             mVals[2] *= inv_length;
  56             return length;
  57         }
  58         mVals[0] = mVals[1] = mVals[2] = T{0};
  59         return T{0};
  60     }
  61
  62     constexpr VectorR cross_product(const alu::VectorR<T> &rhs) const
  63     {
  64         return VectorR{
  65             (*this)[1]*rhs[2] - (*this)[2]*rhs[1],
  66             (*this)[2]*rhs[0] - (*this)[0]*rhs[2],
  67             (*this)[0]*rhs[1] - (*this)[1]*rhs[0],
  68             T{0}};
  69     }
  70
  71     constexpr T dot_product(const alu::VectorR<T> &rhs) const
  72     { return (*this)[0]*rhs[0] + (*this)[1]*rhs[1] + (*this)[2]*rhs[2]; }
  73 };
  74 using Vector = VectorR<float>;
  75
  76 template<typename T>
  77 class MatrixR {
  78     static_assert(std::is_floating_point<T>::value, "Must use floating-point types");
  79     alignas(16) std::array<T,16> mVals;
  80
  81 public:
  82     constexpr MatrixR() noexcept = default;
  83     constexpr MatrixR(const MatrixR&) noexcept = default;
  84     constexpr MatrixR(T aa, T ab, T ac, T ad,
  85                       T ba, T bb, T bc, T bd,
  86                       T ca, T cb, T cc, T cd,
  87                       T da, T db, T dc, T dd) noexcept
  88       : mVals{{aa,ab,ac,ad, ba,bb,bc,bd, ca,cb,cc,cd, da,db,dc,dd}}
  89     { }
  90
  91     constexpr MatrixR& operator=(const MatrixR&) noexcept = default;
  92
  93     auto operator[](size_t idx) noexcept { return al::span<T,4>{&mVals[idx*4], 4}; }
  94     constexpr auto operator[](size_t idx) const noexcept
  95     { return al::span<const T,4>{&mVals[idx*4], 4}; }
  96
  97     static constexpr MatrixR Identity() noexcept
  98     {
  99         return MatrixR{
 100             T{1}, T{0}, T{0}, T{0},
 101             T{0}, T{1}, T{0}, T{0},
 102             T{0}, T{0}, T{1}, T{0},
 103             T{0}, T{0}, T{0}, T{1}};
 104     }
 105 };
 106 using Matrix = MatrixR<float>;
 107
 108 template<typename T>
 109 inline VectorR<T> operator*(const MatrixR<T> &mtx, const VectorR<T> &vec) noexcept
 110 {
 111     return VectorR<T>{
 112         vec[0]*mtx[0][0] + vec[1]*mtx[1][0] + vec[2]*mtx[2][0] + vec[3]*mtx[3][0],
 113         vec[0]*mtx[0][1] + vec[1]*mtx[1][1] + vec[2]*mtx[2][1] + vec[3]*mtx[3][1],
 114         vec[0]*mtx[0][2] + vec[1]*mtx[1][2] + vec[2]*mtx[2][2] + vec[3]*mtx[3][2],
 115         vec[0]*mtx[0][3] + vec[1]*mtx[1][3] + vec[2]*mtx[2][3] + vec[3]*mtx[3][3]};
 116 }
 117
 118 template<typename U, typename T>
 119 inline VectorR<U> cast_to(const VectorR<T> &vec) noexcept
 120 {
 121     return VectorR<U>{static_cast<U>(vec[0]), static_cast<U>(vec[1]),
 122         static_cast<U>(vec[2]), static_cast<U>(vec[3])};
 123 }
 124
 125 } // namespace alu
 126
 127 #endif /* COMMON_VECMAT_H */