Homogeneous.h

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
// SPDX-License-Identifier: MPL-2.0
 
#ifndef EIGEN_HOMOGENEOUS_H
#define EIGEN_HOMOGENEOUS_H
 
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
 
namespace Eigen {

 
namespace internal {
 
template <typename MatrixType, int Direction>
struct traits<Homogeneous<MatrixType, Direction> > : traits<MatrixType> {
  using StorageKind = typename traits<MatrixType>::StorageKind;
  using MatrixTypeNested = typename ref_selector<MatrixType>::type;
  using MatrixTypeNested_ = std::remove_reference_t<MatrixTypeNested>;
  enum {
    RowsPlusOne = (MatrixType::RowsAtCompileTime != Dynamic) ? int(MatrixType::RowsAtCompileTime) + 1 : Dynamic,
    ColsPlusOne = (MatrixType::ColsAtCompileTime != Dynamic) ? int(MatrixType::ColsAtCompileTime) + 1 : Dynamic,
    RowsAtCompileTime = Direction == Vertical ? RowsPlusOne : MatrixType::RowsAtCompileTime,
    ColsAtCompileTime = Direction == Horizontal ? ColsPlusOne : MatrixType::ColsAtCompileTime,
    MaxRowsAtCompileTime = RowsAtCompileTime,
    MaxColsAtCompileTime = ColsAtCompileTime,
    TmpFlags = MatrixTypeNested_::Flags & HereditaryBits,
    Flags = ColsAtCompileTime == 1   ? (TmpFlags & ~RowMajorBit)
            : RowsAtCompileTime == 1 ? (TmpFlags | RowMajorBit)
                                     : TmpFlags
  };
};
 
template <typename MatrixType, typename Lhs>
struct homogeneous_left_product_impl;
template <typename MatrixType, typename Rhs>
struct homogeneous_right_product_impl;
 
}  // end namespace internal
 
template <typename MatrixType, int Direction_>
class Homogeneous : public MatrixBase<Homogeneous<MatrixType, Direction_> >, internal::no_assignment_operator {
 public:
  using NestedExpression = MatrixType;
  enum { Direction = Direction_ };
 
  using Base = MatrixBase<Homogeneous>;
  EIGEN_DENSE_PUBLIC_INTERFACE(Homogeneous)
 
  EIGEN_DEVICE_FUNC explicit inline Homogeneous(const MatrixType& matrix) : m_matrix(matrix) {}
 
  EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept {
    return m_matrix.rows() + (int(Direction) == Vertical ? 1 : 0);
  }
  EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept {
    return m_matrix.cols() + (int(Direction) == Horizontal ? 1 : 0);
  }
 
  EIGEN_DEVICE_FUNC const NestedExpression& nestedExpression() const { return m_matrix; }
 
  template <typename Rhs>
  EIGEN_DEVICE_FUNC inline const Product<Homogeneous, Rhs> operator*(const MatrixBase<Rhs>& rhs) const {
    return Product<Homogeneous, Rhs>(*this, rhs.derived());
  }
 
  template <typename Lhs>
  friend EIGEN_DEVICE_FUNC inline const Product<Lhs, Homogeneous> operator*(const MatrixBase<Lhs>& lhs,
                                                                            const Homogeneous& rhs) {
    return Product<Lhs, Homogeneous>(lhs.derived(), rhs);
  }
 
  template <typename Scalar, int Dim, int Mode, int Options>
  friend EIGEN_DEVICE_FUNC inline const Product<Transform<Scalar, Dim, Mode, Options>, Homogeneous> operator*(
      const Transform<Scalar, Dim, Mode, Options>& lhs, const Homogeneous& rhs) {
    eigen_assert(int(Direction) == Vertical);
    return Product<Transform<Scalar, Dim, Mode, Options>, Homogeneous>(lhs, rhs);
  }
 
  template <typename Func>
  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::result_of<Func(Scalar, Scalar)>::type redux(
      const Func& func) const {
    return func(m_matrix.redux(func), Scalar(1));
  }
 
 protected:
  typename MatrixType::Nested m_matrix;
};

template <typename Derived>
EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::HomogeneousReturnType MatrixBase<Derived>::homogeneous() const {
  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
  return HomogeneousReturnType(derived());
}

template <typename ExpressionType, int Direction>
EIGEN_DEVICE_FUNC inline Homogeneous<ExpressionType, Direction> VectorwiseOp<ExpressionType, Direction>::homogeneous()
    const {
  return HomogeneousReturnType(_expression());
}

template <typename Derived>
EIGEN_DEVICE_FUNC inline const typename MatrixBase<Derived>::HNormalizedReturnType MatrixBase<Derived>::hnormalized()
    const {
  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
  return ConstStartMinusOne(derived(), 0, 0, ColsAtCompileTime == 1 ? size() - 1 : 1,
                            ColsAtCompileTime == 1 ? 1 : size() - 1) /
         coeff(size() - 1);
}

template <typename ExpressionType, int Direction>
EIGEN_DEVICE_FUNC inline const typename VectorwiseOp<ExpressionType, Direction>::HNormalizedReturnType
VectorwiseOp<ExpressionType, Direction>::hnormalized() const {
  return HNormalized_Block(_expression(), 0, 0, Direction == Vertical ? _expression().rows() - 1 : _expression().rows(),
                           Direction == Horizontal ? _expression().cols() - 1 : _expression().cols())
      .cwiseQuotient(Replicate < HNormalized_Factors, Direction == Vertical ? HNormalized_SizeMinusOne : 1,
                     Direction == Horizontal
                         ? HNormalized_SizeMinusOne
                         : 1 > (HNormalized_Factors(_expression(), Direction == Vertical ? _expression().rows() - 1 : 0,
                                                    Direction == Horizontal ? _expression().cols() - 1 : 0,
                                                    Direction == Vertical ? 1 : _expression().rows(),
                                                    Direction == Horizontal ? 1 : _expression().cols()),
                                Direction == Vertical ? _expression().rows() - 1 : 1,
                                Direction == Horizontal ? _expression().cols() - 1 : 1));
}
 
namespace internal {
 
template <typename MatrixOrTransformType>
struct take_matrix_for_product {
  using type = MatrixOrTransformType;
  EIGEN_DEVICE_FUNC static const type& run(const type& x) { return x; }
};
 
template <typename Scalar, int Dim, int Mode, int Options>
struct take_matrix_for_product<Transform<Scalar, Dim, Mode, Options> > {
  using TransformType = Transform<Scalar, Dim, Mode, Options>;
  using type = std::add_const_t<typename TransformType::ConstAffinePart>;
  EIGEN_DEVICE_FUNC static type run(const TransformType& x) { return x.affine(); }
};
 
template <typename Scalar, int Dim, int Options>
struct take_matrix_for_product<Transform<Scalar, Dim, Projective, Options> > {
  using TransformType = Transform<Scalar, Dim, Projective, Options>;
  using type = typename TransformType::MatrixType;
  EIGEN_DEVICE_FUNC static const type& run(const TransformType& x) { return x.matrix(); }
};
 
template <typename MatrixType, typename Lhs>
struct traits<homogeneous_left_product_impl<Homogeneous<MatrixType, Vertical>, Lhs> > {
  using LhsMatrixType = typename take_matrix_for_product<Lhs>::type;
  using MatrixTypeCleaned = remove_all_t<MatrixType>;
  using LhsMatrixTypeCleaned = remove_all_t<LhsMatrixType>;
  using ReturnType = typename make_proper_matrix_type<
      typename traits<MatrixTypeCleaned>::Scalar, LhsMatrixTypeCleaned::RowsAtCompileTime,
      MatrixTypeCleaned::ColsAtCompileTime, MatrixTypeCleaned::PlainObject::Options,
      LhsMatrixTypeCleaned::MaxRowsAtCompileTime, MatrixTypeCleaned::MaxColsAtCompileTime>::type;
};
 
template <typename MatrixType, typename Lhs>
struct homogeneous_left_product_impl<Homogeneous<MatrixType, Vertical>, Lhs>
    : public ReturnByValue<homogeneous_left_product_impl<Homogeneous<MatrixType, Vertical>, Lhs> > {
  using LhsMatrixType = typename traits<homogeneous_left_product_impl>::LhsMatrixType;
  using LhsMatrixTypeCleaned = remove_all_t<LhsMatrixType>;
  using LhsMatrixTypeNested = remove_all_t<typename LhsMatrixTypeCleaned::Nested>;
  EIGEN_DEVICE_FUNC homogeneous_left_product_impl(const Lhs& lhs, const MatrixType& rhs)
      : m_lhs(take_matrix_for_product<Lhs>::run(lhs)), m_rhs(rhs) {}
 
  EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_lhs.rows(); }
  EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_rhs.cols(); }
 
  template <typename Dest>
  EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const {
    // FIXME: investigate how to allow lazy evaluation of this product when possible.
    dst = Block < const LhsMatrixTypeNested, LhsMatrixTypeNested::RowsAtCompileTime,
    LhsMatrixTypeNested::ColsAtCompileTime == Dynamic
        ? Dynamic
        : LhsMatrixTypeNested::ColsAtCompileTime - 1 > (m_lhs, 0, 0, m_lhs.rows(), m_lhs.cols() - 1) * m_rhs;
    dst += m_lhs.col(m_lhs.cols() - 1).rowwise().template replicate<MatrixType::ColsAtCompileTime>(m_rhs.cols());
  }
 
  typename LhsMatrixTypeCleaned::Nested m_lhs;
  typename MatrixType::Nested m_rhs;
};
 
template <typename MatrixType, typename Rhs>
struct traits<homogeneous_right_product_impl<Homogeneous<MatrixType, Horizontal>, Rhs> > {
  using ReturnType =
      typename make_proper_matrix_type<typename traits<MatrixType>::Scalar, MatrixType::RowsAtCompileTime,
                                       Rhs::ColsAtCompileTime, MatrixType::PlainObject::Options,
                                       MatrixType::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime>::type;
};
 
template <typename MatrixType, typename Rhs>
struct homogeneous_right_product_impl<Homogeneous<MatrixType, Horizontal>, Rhs>
    : public ReturnByValue<homogeneous_right_product_impl<Homogeneous<MatrixType, Horizontal>, Rhs> > {
  using RhsNested = remove_all_t<typename Rhs::Nested>;
  EIGEN_DEVICE_FUNC homogeneous_right_product_impl(const MatrixType& lhs, const Rhs& rhs) : m_lhs(lhs), m_rhs(rhs) {}
 
  EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_lhs.rows(); }
  EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_rhs.cols(); }
 
  template <typename Dest>
  EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const {
    // FIXME: investigate how to allow lazy evaluation of this product when possible.
    dst = m_lhs * Block < const RhsNested,
    RhsNested::RowsAtCompileTime == Dynamic ? Dynamic : RhsNested::RowsAtCompileTime - 1,
    RhsNested::ColsAtCompileTime > (m_rhs, 0, 0, m_rhs.rows() - 1, m_rhs.cols());
    dst += m_rhs.row(m_rhs.rows() - 1).colwise().template replicate<MatrixType::RowsAtCompileTime>(m_lhs.rows());
  }
 
  typename MatrixType::Nested m_lhs;
  typename Rhs::Nested m_rhs;
};
 
template <typename ArgType, int Direction>
struct evaluator_traits<Homogeneous<ArgType, Direction> > {
  using Kind = typename storage_kind_to_evaluator_kind<typename ArgType::StorageKind>::Kind;
  using Shape = HomogeneousShape;
};
 
template <>
struct AssignmentKind<DenseShape, HomogeneousShape> {
  using Kind = Dense2Dense;
};
 
template <typename ArgType, int Direction>
struct unary_evaluator<Homogeneous<ArgType, Direction>, IndexBased>
    : evaluator<typename Homogeneous<ArgType, Direction>::PlainObject> {
  using XprType = Homogeneous<ArgType, Direction>;
  using PlainObject = typename XprType::PlainObject;
  using Base = evaluator<PlainObject>;
 
  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& op) : Base(), m_temp(op) {
    internal::construct_at<Base>(this, m_temp);
  }
 
 protected:
  PlainObject m_temp;
};
 
// dense = homogeneous
template <typename DstXprType, typename ArgType, typename Scalar>
struct Assignment<DstXprType, Homogeneous<ArgType, Vertical>, internal::assign_op<Scalar, typename ArgType::Scalar>,
                  Dense2Dense> {
  using SrcXprType = Homogeneous<ArgType, Vertical>;
  EIGEN_DEVICE_FUNC static void run(DstXprType& dst, const SrcXprType& src,
                                    const internal::assign_op<Scalar, typename ArgType::Scalar>&) {
    Index dstRows = src.rows();
    Index dstCols = src.cols();
    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
 
    dst.template topRows<ArgType::RowsAtCompileTime>(src.nestedExpression().rows()) = src.nestedExpression();
    dst.row(dst.rows() - 1).setOnes();
  }
};
 
// dense = homogeneous
template <typename DstXprType, typename ArgType, typename Scalar>
struct Assignment<DstXprType, Homogeneous<ArgType, Horizontal>, internal::assign_op<Scalar, typename ArgType::Scalar>,
                  Dense2Dense> {
  using SrcXprType = Homogeneous<ArgType, Horizontal>;
  EIGEN_DEVICE_FUNC static void run(DstXprType& dst, const SrcXprType& src,
                                    const internal::assign_op<Scalar, typename ArgType::Scalar>&) {
    Index dstRows = src.rows();
    Index dstCols = src.cols();
    if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols);
 
    dst.template leftCols<ArgType::ColsAtCompileTime>(src.nestedExpression().cols()) = src.nestedExpression();
    dst.col(dst.cols() - 1).setOnes();
  }
};
 
template <typename LhsArg, typename Rhs, int ProductTag>
struct generic_product_impl<Homogeneous<LhsArg, Horizontal>, Rhs, HomogeneousShape, DenseShape, ProductTag> {
  template <typename Dest>
  EIGEN_DEVICE_FUNC static void evalTo(Dest& dst, const Homogeneous<LhsArg, Horizontal>& lhs, const Rhs& rhs) {
    homogeneous_right_product_impl<Homogeneous<LhsArg, Horizontal>, Rhs>(lhs.nestedExpression(), rhs).evalTo(dst);
  }
};
 
template <typename Lhs, typename Rhs>
struct homogeneous_right_product_refactoring_helper {
  enum { Dim = Lhs::ColsAtCompileTime, Rows = Lhs::RowsAtCompileTime };
  using LinearBlockConst = typename Rhs::template ConstNRowsBlockXpr<Dim>::Type;
  using LinearBlock = std::remove_const_t<LinearBlockConst>;
  using ConstantColumn = typename Rhs::ConstRowXpr;
  using ConstantBlock = Replicate<const ConstantColumn, Rows, 1>;
  using LinearProduct = Product<Lhs, LinearBlock, LazyProduct>;
  using Xpr = CwiseBinaryOp<internal::scalar_sum_op<typename Lhs::Scalar, typename Rhs::Scalar>, const LinearProduct,
                            const ConstantBlock>;
};
 
template <typename Lhs, typename Rhs, int ProductTag>
struct product_evaluator<Product<Lhs, Rhs, LazyProduct>, ProductTag, HomogeneousShape, DenseShape>
    : public evaluator<
          typename homogeneous_right_product_refactoring_helper<typename Lhs::NestedExpression, Rhs>::Xpr> {
  using XprType = Product<Lhs, Rhs, LazyProduct>;
  using helper = homogeneous_right_product_refactoring_helper<typename Lhs::NestedExpression, Rhs>;
  using ConstantBlock = typename helper::ConstantBlock;
  using RefactoredXpr = typename helper::Xpr;
  using Base = evaluator<RefactoredXpr>;
 
  EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr)
      : Base(xpr.lhs().nestedExpression().lazyProduct(
                 xpr.rhs().template topRows<helper::Dim>(xpr.lhs().nestedExpression().cols())) +
             ConstantBlock(xpr.rhs().row(xpr.rhs().rows() - 1), xpr.lhs().rows(), 1)) {}
};
 
template <typename Lhs, typename RhsArg, int ProductTag>
struct generic_product_impl<Lhs, Homogeneous<RhsArg, Vertical>, DenseShape, HomogeneousShape, ProductTag> {
  template <typename Dest>
  EIGEN_DEVICE_FUNC static void evalTo(Dest& dst, const Lhs& lhs, const Homogeneous<RhsArg, Vertical>& rhs) {
    homogeneous_left_product_impl<Homogeneous<RhsArg, Vertical>, Lhs>(lhs, rhs.nestedExpression()).evalTo(dst);
  }
};
 
template <typename Lhs, typename RhsArg, int ProductTag>
struct generic_product_impl<Lhs, Homogeneous<RhsArg, Vertical>, TriangularShape, HomogeneousShape, ProductTag> {
  template <typename Dest>
  static void evalTo(Dest& dst, const Lhs& lhs, const Homogeneous<RhsArg, Vertical>& rhs) {
    dst.noalias() = lhs * rhs.eval();
  }
};
 
template <typename Lhs, typename Rhs>
struct homogeneous_left_product_refactoring_helper {
  enum { Dim = Rhs::RowsAtCompileTime, Cols = Rhs::ColsAtCompileTime };
  using LinearBlockConst = typename Lhs::template ConstNColsBlockXpr<Dim>::Type;
  using LinearBlock = std::remove_const_t<LinearBlockConst>;
  using ConstantColumn = typename Lhs::ConstColXpr;
  using ConstantBlock = Replicate<const ConstantColumn, 1, Cols>;
  using LinearProduct = Product<LinearBlock, Rhs, LazyProduct>;
  using Xpr = CwiseBinaryOp<internal::scalar_sum_op<typename Lhs::Scalar, typename Rhs::Scalar>, const LinearProduct,
                            const ConstantBlock>;
};
 
template <typename Lhs, typename Rhs, int ProductTag>
struct product_evaluator<Product<Lhs, Rhs, LazyProduct>, ProductTag, DenseShape, HomogeneousShape>
    : public evaluator<typename homogeneous_left_product_refactoring_helper<Lhs, typename Rhs::NestedExpression>::Xpr> {
  using XprType = Product<Lhs, Rhs, LazyProduct>;
  using helper = homogeneous_left_product_refactoring_helper<Lhs, typename Rhs::NestedExpression>;
  using ConstantBlock = typename helper::ConstantBlock;
  using RefactoredXpr = typename helper::Xpr;
  using Base = evaluator<RefactoredXpr>;
 
  EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr)
      : Base(xpr.lhs()
                 .template leftCols<helper::Dim>(xpr.rhs().nestedExpression().rows())
                 .lazyProduct(xpr.rhs().nestedExpression()) +
             ConstantBlock(xpr.lhs().col(xpr.lhs().cols() - 1), 1, xpr.rhs().cols())) {}
};
 
template <typename Scalar, int Dim, int Mode, int Options, typename RhsArg, int ProductTag>
struct generic_product_impl<Transform<Scalar, Dim, Mode, Options>, Homogeneous<RhsArg, Vertical>, DenseShape,
                            HomogeneousShape, ProductTag> {
  using TransformType = Transform<Scalar, Dim, Mode, Options>;
  template <typename Dest>
  EIGEN_DEVICE_FUNC static void evalTo(Dest& dst, const TransformType& lhs, const Homogeneous<RhsArg, Vertical>& rhs) {
    homogeneous_left_product_impl<Homogeneous<RhsArg, Vertical>, TransformType>(lhs, rhs.nestedExpression())
        .evalTo(dst);
  }
};
 
template <typename ExpressionType, int Side, bool Transposed>
struct permutation_matrix_product<ExpressionType, Side, Transposed, HomogeneousShape>
    : public permutation_matrix_product<ExpressionType, Side, Transposed, DenseShape> {};
 
}  // end namespace internal
 
}  // end namespace Eigen
 
#endif  // EIGEN_HOMOGENEOUS_H