Line data    Source code

       1              : #ifndef FE49A15E_40AF_485A_A47C_F00025FCB4E2
       2              : #define FE49A15E_40AF_485A_A47C_F00025FCB4E2
       3              : 
       4              : #include "math/linalg/vector.h"
       5              : 
       6              : #include "math/linalg/matrix.hpp" // IWYU pragma: keep
       7              : #include <cmath>                  // for sqrt
       8              : 
       9              : namespace tracking
      10              : {
      11              : namespace math
      12              : {
      13              : 
      14              : template <typename ValueType_, sint32 Size_>
      15            8 : inline auto Vector<ValueType_, Size_>::Zeros() -> Vector
      16              : {
      17            8 :   return Vector{BaseMatrix::Zeros()};
      18              : }
      19              : 
      20              : template <typename ValueType_, sint32 Size_>
      21            1 : inline auto Vector<ValueType_, Size_>::Ones() -> Vector
      22              : {
      23            1 :   return Vector{BaseMatrix::Ones()};
      24              : }
      25              : 
      26              : template <typename ValueType_, sint32 Size_>
      27              : template <sint32 Row_>
      28            2 : inline auto Vector<ValueType_, Size_>::UnitVector() -> Vector
      29              : {
      30              :   static_assert(Row_ >= 0 && Row_ < Size_);
      31              : 
      32            2 :   auto tmp{Vector::Zeros()};
      33            2 :   tmp.at_unsafe(Row_) = static_cast<ValueType_>(1.0);
      34            2 :   return tmp;
      35              : }
      36              : 
      37              : template <typename ValueType_, sint32 Size_>
      38          138 : inline auto Vector<ValueType_, Size_>::FromList(const std::initializer_list<ValueType_>& list) -> Vector
      39              : {
      40          138 :   assert(list.size() == Size_);
      41              : 
      42          138 :   Vector tmp;
      43          138 :   auto   iter = tmp.data().begin();
      44          138 :   std::copy(list.begin(), list.end(), iter);
      45          138 :   return tmp;
      46              : }
      47              : 
      48              : template <typename ValueType_, sint32 Size_>
      49            2 : inline auto Vector<ValueType_, Size_>::operator[](sint32 idx) const -> tl::expected<ValueType_, Errors>
      50              : {
      51            2 :   return BaseMatrix::operator()(idx, 0);
      52              : }
      53              : 
      54              : template <typename ValueType_, sint32 Size_>
      55            1 : inline auto Vector<ValueType_, Size_>::operator[](sint32 idx) -> tl::expected<std::reference_wrapper<ValueType_>, Errors>
      56              : {
      57            1 :   return BaseMatrix::operator()(idx, 0);
      58              : }
      59              : 
      60              : template <typename ValueType_, sint32 Size_>
      61          636 : inline auto Vector<ValueType_, Size_>::operator*(const Vector& other) const -> ValueType_
      62              : {
      63          636 :   auto sum = static_cast<ValueType_>(0);
      64         3867 :   for (auto idx = 0; idx < Size_; ++idx)
      65              :   {
      66         3231 :     sum += at_unsafe(idx) * other.at_unsafe(idx);
      67              :   }
      68          636 :   return sum;
      69              : }
      70              : 
      71              : template <typename ValueType_, sint32 Size_>
      72          634 : inline auto Vector<ValueType_, Size_>::normSq() const -> ValueType_
      73              : {
      74          629 :   return this->operator*(*this);
      75              : }
      76              : 
      77              : template <typename ValueType_, sint32 Size_>
      78              : template <typename U, std::enable_if_t<std::is_floating_point<U>::value, int>>
      79          632 : inline auto Vector<ValueType_, Size_>::norm() const -> ValueType_
      80              : {
      81              :   if constexpr (Size_ == 1)
      82              :   {
      83              :     return std::abs(Matrix<ValueType_, 1, 1, true>::at_unsafe(0, 0));
      84              :   }
      85              :   else
      86              :   {
      87          632 :     return std::sqrt(normSq());
      88              :   }
      89              : }
      90              : 
      91              : template <typename ValueType_, sint32 Size_>
      92              : template <typename U, std::enable_if_t<std::is_floating_point<U>::value, int>>
      93            2 : inline void Vector<ValueType_, Size_>::normalize()
      94              : {
      95              :   if constexpr (Size_ == 1)
      96              :   {
      97              :     // the normalized vector of dim 1 is always 1 or -1, so we just set it accordingly
      98              :     at_unsafe(0) = std::copysign(static_cast<ValueType_>(1.0), at_unsafe(0));
      99              :   }
     100              :   else
     101              :   {
     102            2 :     const auto length = norm();
     103            2 :     this->operator/=(length);
     104              :   }
     105            2 : }
     106              : 
     107              : template <typename ValueType_, sint32 Size_>
     108              : template <typename U, std::enable_if_t<std::is_floating_point<U>::value, int>>
     109            2 : inline auto Vector<ValueType_, Size_>::normalize() const -> Vector
     110              : {
     111            2 :   Vector tmp(*this);
     112            2 :   tmp.normalize();
     113              :   return tmp;
     114              : }
     115              : 
     116              : } // namespace math
     117              : } // namespace tracking
     118              : 
     119              : #endif // FE49A15E_40AF_485A_A47C_F00025FCB4E2