rotgen/include/rotgen/functions/functions.hpp

//==================================================================================================
/*
  ROTGEN - Runtime Overlay for Eigen
  Copyright : CODE RECKONS
  SPDX-License-Identifier: BSL-1.0
*/
//==================================================================================================
#pragma once

#include <rotgen/detail/helpers.hpp>

namespace rotgen
{
  //-----------------------------------------------------------------------------------------------
  // Infos & Shape
  //-----------------------------------------------------------------------------------------------
  std::size_t rows(auto const& m) requires(requires{ m.rows(); }){ return m.rows(); }
  std::size_t cols(auto const& m) requires(requires{ m.cols(); }){ return m.cols(); }
  std::size_t size(auto const& m) requires(requires{ m.size(); }){ return m.size(); }

  void resize(auto& a, int s)         requires requires{a.resize(s);}   { a.resize(s);    }
  void resize(auto& a, int r, int c)  requires requires{a.resize(r,c);} { a.resize(r,c);  }

  void conservativeResize(auto& a, int s) requires requires{a.conservativeResize(s);}
  {
    a.conservativeResize(s);
  }

  void conservativeResize(auto& a, int r, int c) requires requires{a.conservativeResize(r,c);}
  {
    a.conservativeResize(r, c);
  }

  //-----------------------------------------------------------------------------------------------
  // Global operations
  //-----------------------------------------------------------------------------------------------
  decltype(auto) normalized (auto const& m) requires(requires{ m.normalized(); }) { return m.normalized();  }
  decltype(auto) transpose  (auto const& m) requires(requires{ m.transpose();  }) { return m.transpose();   }
  decltype(auto) conjugate  (auto const& m) requires(requires{ m.conjugate();  }) { return m.conjugate();   }
  decltype(auto) adjoint    (auto const& m) requires(requires{ m.adjoint();    }) { return m.adjoint();     }

  void normalize(auto& a)         requires(requires{ a.normalize(); })        { a.normalize(); }
  void transposeInPlace(auto& a)  requires(requires{ a.transposeInPlace(); }) { a.transposeInPlace(); }
  void adjointInPlace(auto& a)    requires(requires{ a.adjointInPlace(); })   { a.adjointInPlace(); }

  //-----------------------------------------------------------------------------------------------
  // Component-wise functions
  //-----------------------------------------------------------------------------------------------
  auto abs (auto const& arg) requires(requires{arg.cwiseAbs();}    ) { return arg.cwiseAbs();     }
  auto abs2(auto const& arg) requires(requires{arg.cwiseAbs2();}   ) { return arg.cwiseAbs2();    }
  auto rec (auto const& arg) requires(requires{arg.cwiseInverse();}) { return arg.cwiseInverse(); }
  auto sqrt(auto const& arg) requires(requires{arg.cwiseSqrt();}   ) { return arg.cwiseSqrt();    }

  template<concepts::entity A, concepts::entity B>
  auto min(A const& a, B const& b)
  {
    if constexpr(!use_expression_templates) return min(generalize_t<A const>(a), generalize_t<B const>(b));
    else                                    return base_of(a).cwiseMin(base_of(b));
  }

  template<concepts::entity A>
  auto min(A const& a, std::convertible_to<typename A::value_type> auto b)
  {
    if constexpr(!use_expression_templates) return min(generalize_t<A const>(a), b);
    else                                    return base_of(a).cwiseMin(b);
  }

  template<concepts::entity B>
  auto min(std::convertible_to<typename B::value_type> auto a, B const& b)
  {
    if constexpr(!use_expression_templates) return min(a,generalize_t<B const>(b));
    else                                    return base_of(b).cwiseMin(a);
  }

  template<concepts::entity A, concepts::entity B>
  auto max(A const& a, B const& b)
  {
    if constexpr(!use_expression_templates) return max(generalize_t<A const>(a), generalize_t<B const>(b));
    else                                    return base_of(a).cwiseMax(base_of(b));
  }

  template<concepts::entity A>
  auto max(A const& a, std::convertible_to<typename A::value_type> auto b)
  {
    if constexpr(!use_expression_templates) return max(generalize_t<A const>(a), b);
    else                                    return base_of(a).cwiseMax(b);
  }

  template<concepts::entity B>
  auto max(std::convertible_to<typename B::value_type> auto a, B const& b)
  {
    if constexpr(!use_expression_templates) return max(a, generalize_t<B const>(b));
    else                                    return base_of(b).cwiseMax(a);
  }

  template<concepts::entity A, concepts::entity B>
  auto mul(A const& a, B const& b)
  {
    if constexpr(!use_expression_templates) return mul(generalize_t<A const>(a), generalize_t<B const>(b));
    else                                    return base_of(a).cwiseProduct(base_of(b));
  }

  template<concepts::entity A>
  auto mul(A const& a, std::convertible_to<typename A::value_type> auto b)
  {
    return a * b;
  }

  template<concepts::entity B>
  auto mul(std::convertible_to<typename B::value_type> auto a, B const& b)
  {
    return a * b;
  }

  template<concepts::entity A, concepts::entity B>
  auto div(A const& a, B const& b)
  {
    if constexpr(!use_expression_templates) return div(generalize_t<A const>(a), generalize_t<B const>(b));
    else                                    return base_of(a).array() / base_of(b).array();
  }

  template<concepts::entity A>
  auto div(A const& a, std::convertible_to<typename A::value_type> auto b)
  {
    return a / b;
  }

  //-----------------------------------------------------------------------------------------------
  // Reductions
  //-----------------------------------------------------------------------------------------------
  auto trace(concepts::entity auto const& arg)        { return arg.trace(); }
  auto squaredNorm(concepts::entity auto const& arg)  { return arg.squaredNorm(); }
  auto norm(concepts::entity auto const& arg)         { return arg.norm(); }
  auto sum(auto const& arg) requires requires{ arg.sum(); }  { return arg.sum(); }
  auto prod(concepts::entity auto const& arg)         { return arg.prod(); }
  auto mean(concepts::entity auto const& arg)         { return arg.mean(); }

  template<concepts::entity A, concepts::entity B>
  auto dot(A const& a, B const& b)
  requires(detail::has_same_vector_size<A,B> && std::same_as<typename A::value_type, typename B::value_type>)
  {
    if constexpr(!use_expression_templates) return dot(generalize_t<A const>(a), generalize_t<B const>(b));
    else                                    return base_of(a).dot(base_of(b));
  }

  auto maxCoeff(auto const& arg) requires( requires{ arg.maxCoeff(); } ) { return arg.maxCoeff(); }
  auto minCoeff(auto const& arg) requires( requires{ arg.minCoeff(); } ) { return arg.minCoeff(); }

  template<std::integral IndexType>
  auto maxCoeff(concepts::entity auto const& arg, IndexType* row, IndexType* col)
  {
    return arg.maxCoeff(row, col);
  }

  template<std::integral IndexType>
  auto minCoeff(concepts::entity auto const& arg, IndexType* row, IndexType* col)
  {
    return arg.minCoeff(row, col);
  }

  template<int P, typename T>
  auto lpNorm(T const& arg) requires( requires{arg.template lpNorm<P>();} )
  {
    static_assert(P == 1 || P == 2 || P == Infinity);
    return arg.template lpNorm<P>();
  }

  //-----------------------------------------------------------------------------------------------
  // Expression handling
  //-----------------------------------------------------------------------------------------------
  template<typename T>
  decltype(auto) noalias(T&& t) requires( requires{std::forward<T>(t).noalias();} )
  {
    return std::forward<T>(t).noalias();
  }

  template<typename T>
  auto evaluate(T&& t) requires( requires{std::forward<T>(t).evaluate();} )
  {
    return std::forward<T>(t).evaluate();
  }

  template<typename T>
  auto evaluate(T&& t) requires( requires{std::forward<T>(t).eval();} )
  {
    return std::forward<T>(t).eval();
  }

  //-----------------------------------------------------------------------------------------------
  // Generators
  //-----------------------------------------------------------------------------------------------
  template<typename T>
  auto setZero(T&& t) requires( requires{std::forward<T>(t).setZero();} )
  {
    return std::forward<T>(t).setZero();
  }

  template<typename T>
  auto setZero() requires( requires{T::Zero();} )
  {
    return T::Zero();
  }

  template<typename T>
  auto setZero(std::integral auto n) requires( requires{T::Zero(n);} )
  {
     return T::Zero(n);
  }

  template<typename T>
  auto setZero(std::integral auto r,std::integral auto c) requires( requires{T::Zero(r,c);} )
  {
    return T::Zero(r,c);
  }

  template<typename T>
  auto setOnes(T&& t) requires( requires{std::forward<T>(t).setOnes();} )
  {
    return std::forward<T>(t).setOnes();
  }

  template<typename T>
  auto setOnes() requires( requires{T::Ones();} )
  {
    return T::Ones();
  }

  template<typename T>
  auto setOnes(std::integral auto n) requires( requires{T::Ones(n);} )
  {
     return T::Ones(n);
  }

  template<typename T>
  auto setOnes(std::integral auto r,std::integral auto c) requires( requires{T::Ones(r,c);} )
  {
    return T::Ones(r,c);
  }

  template<typename T>
  auto setIdentity(T&& t) requires( requires{std::forward<T>(t).setIdentity();} )
  {
    return std::forward<T>(t).setIdentity();
  }

  template<typename T>
  auto setIdentity() requires( requires{T::Identity();} )
  {
    return T::Identity();
  }

  template<typename T>
  auto setIdentity(std::integral auto n) requires( requires{T::Identity(n);} )
  {
     return T::Identity(n);
  }

  template<typename T>
  auto setIdentity(std::integral auto r,std::integral auto c) requires( requires{T::Identity(r,c);} )
  {
    return T::Identity(r,c);
  }

  template<typename T>
  auto setRandom(T&& t) requires( requires{std::forward<T>(t).setRandom();} )
  {
    return std::forward<T>(t).setRandom();
  }

  template<typename T>
  auto setRandom() requires( requires{T::Random();} )
  {
    return T::Random();
  }

  template<typename T>
  auto setRandom(std::integral auto n) requires( requires{T::Random(n);} )
  {
     return T::Random(n);
  }

  template<typename T>
  auto setRandom(std::integral auto r,std::integral auto c) requires( requires{T::Random(r,c);} )
  {
    return T::Random(r,c);
  }

  template<typename T>
  auto setConstant(T&& t, auto v) requires( requires{std::forward<T>(t).setConstant(v);} )
  {
    return std::forward<T>(t).setConstant(v);
  }

  template<typename T>
  auto setConstant(auto v) requires( requires{T::Constant(v);} )
  {
    return T::Constant(v);
  }

  template<typename T>
  auto setConstant(std::integral auto n, auto v) requires( requires{T::Constant(n,v);} )
  {
     return T::Constant(n,v);
  }

  template<typename T>
  auto setConstant(std::integral auto r,std::integral auto c, auto v) requires( requires{T::Constant(r,c,v);} )
  {
    return T::Constant(r,c,v);
  }

  //-----------------------------------------------------------------------------------------------
  // Others
  // TODO: Adapt other functions ot behave as inverse and limit code in non-ref/non-map containers
  //-----------------------------------------------------------------------------------------------
  template<typename A> auto inverse(A const& a)
  {
    if constexpr(!use_expression_templates)               return inverse(generalize_t<A const>(a));
    else if constexpr(requires{typename A::rotgen_tag;})  return base_of(a).inverse();
    else                                                  return a.inverse();
  }

  template<concepts::vectorND<3> L, concepts::vectorND<3> R> auto cross(L const& l, R const& r)
  {
    if constexpr(!use_expression_templates)
      return cross(generalize_t<R const>(l),generalize_t<R const>(r));
    else if constexpr(requires{typename L::rotgen_tag;} || requires{typename R::rotgen_tag;} )
      return base_of(l).cross(base_of(r));
    else  return l.cross(r);
  }
}