SkewSymmetricMatrix3.h

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2007-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// 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_SKEWSYMMETRICMATRIX3_H
#define EIGEN_SKEWSYMMETRICMATRIX3_H
 
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
 
namespace Eigen {

template <typename Derived>
class SkewSymmetricBase : public EigenBase<Derived> {
 public:
  using SkewSymmetricVectorType = typename internal::traits<Derived>::SkewSymmetricVectorType;
  using Scalar = typename SkewSymmetricVectorType::Scalar;
  using RealScalar = typename SkewSymmetricVectorType::RealScalar;
  using StorageKind = typename internal::traits<Derived>::StorageKind;
  using StorageIndex = typename internal::traits<Derived>::StorageIndex;
 
  enum {
    RowsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime,
    ColsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime,
    MaxRowsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime,
    MaxColsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime,
    IsVectorAtCompileTime = 0,
    Flags = NoPreferredStorageOrderBit
  };
 
  using DenseMatrixType =
      Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime, 0, MaxRowsAtCompileTime, MaxColsAtCompileTime>;
  using DenseType = DenseMatrixType;
  using PlainObject = SkewSymmetricMatrix3<Scalar>;

  EIGEN_DEVICE_FUNC inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
  EIGEN_DEVICE_FUNC inline Derived& derived() { return *static_cast<Derived*>(this); }

  EIGEN_DEVICE_FUNC DenseMatrixType toDenseMatrix() const { return derived(); }

  EIGEN_DEVICE_FUNC constexpr Scalar determinant() const { return 0; }

  EIGEN_DEVICE_FUNC PlainObject transpose() const { return (-vector()).asSkewSymmetric(); }

  EIGEN_DEVICE_FUNC DenseMatrixType exponential() const {
    DenseMatrixType retVal = DenseMatrixType::Identity();
    const SkewSymmetricVectorType& v = vector();
    if (v.isZero()) {
      return retVal;
    }
    const Scalar norm2 = v.squaredNorm();
    const Scalar norm = numext::sqrt(norm2);
    retVal += ((((1 - numext::cos(norm)) / norm2) * derived()) * derived()) +
              (numext::sin(norm) / norm) * derived().toDenseMatrix();
    return retVal;
  }

  EIGEN_DEVICE_FUNC inline const SkewSymmetricVectorType& vector() const { return derived().vector(); }
  EIGEN_DEVICE_FUNC inline SkewSymmetricVectorType& vector() { return derived().vector(); }

  EIGEN_DEVICE_FUNC constexpr Index rows() const { return 3; }
  EIGEN_DEVICE_FUNC constexpr Index cols() const { return 3; }

  template <typename MatrixDerived>
  EIGEN_DEVICE_FUNC Product<Derived, MatrixDerived, LazyProduct> operator*(
      const MatrixBase<MatrixDerived>& matrix) const {
    return Product<Derived, MatrixDerived, LazyProduct>(derived(), matrix.derived());
  }

  template <typename MatrixDerived>
  EIGEN_DEVICE_FUNC Product<Derived, MatrixDerived, LazyProduct> operator*(
      const SkewSymmetricBase<MatrixDerived>& matrix) const {
    return Product<Derived, MatrixDerived, LazyProduct>(derived(), matrix.derived());
  }

  template <typename DiagonalDerived>
  EIGEN_DEVICE_FUNC Product<Derived, DiagonalDerived, LazyProduct> operator*(
      const DiagonalBase<DiagonalDerived>& diagonal) const {
    return Product<Derived, DiagonalDerived, LazyProduct>(derived(), diagonal.derived());
  }

  template <typename DiagonalDerived>
  EIGEN_DEVICE_FUNC friend Product<DiagonalDerived, Derived, LazyProduct> operator*(
      const DiagonalBase<DiagonalDerived>& diagonal, const SkewSymmetricBase& skew) {
    return Product<DiagonalDerived, Derived, LazyProduct>(diagonal.derived(), skew.derived());
  }
 
  template <typename OtherDerived>
  using SkewSymmetricProductReturnType = SkewSymmetricWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(
      SkewSymmetricVectorType, typename OtherDerived::SkewSymmetricVectorType, internal::scalar_product_op)>;

  template <typename OtherDerived>
  EIGEN_DEVICE_FUNC SkewSymmetricProductReturnType<OtherDerived> wedge(
      const SkewSymmetricBase<OtherDerived>& other) const {
    return vector().cross(other.vector()).asSkewSymmetric();
  }
 
  using SkewSymmetricScaleReturnType = SkewSymmetricWrapper<const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(
      SkewSymmetricVectorType, Scalar, internal::scalar_product_op)>;

  EIGEN_DEVICE_FUNC inline SkewSymmetricScaleReturnType operator*(const Scalar& scalar) const {
    return (vector() * scalar).asSkewSymmetric();
  }
 
  using ScaleSkewSymmetricReturnType = SkewSymmetricWrapper<const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(
      Scalar, SkewSymmetricVectorType, internal::scalar_product_op)>;

  EIGEN_DEVICE_FUNC friend inline ScaleSkewSymmetricReturnType operator*(const Scalar& scalar,
                                                                         const SkewSymmetricBase& other) {
    return (scalar * other.vector()).asSkewSymmetric();
  }
 
  template <typename OtherDerived>
  using SkewSymmetricSumReturnType = SkewSymmetricWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(
      SkewSymmetricVectorType, typename OtherDerived::SkewSymmetricVectorType, internal::scalar_sum_op)>;

  template <typename OtherDerived>
  EIGEN_DEVICE_FUNC inline SkewSymmetricSumReturnType<OtherDerived> operator+(
      const SkewSymmetricBase<OtherDerived>& other) const {
    return (vector() + other.vector()).asSkewSymmetric();
  }
 
  template <typename OtherDerived>
  using SkewSymmetricDifferenceReturnType = SkewSymmetricWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(
      SkewSymmetricVectorType, typename OtherDerived::SkewSymmetricVectorType, internal::scalar_difference_op)>;

  template <typename OtherDerived>
  EIGEN_DEVICE_FUNC inline SkewSymmetricDifferenceReturnType<OtherDerived> operator-(
      const SkewSymmetricBase<OtherDerived>& other) const {
    return (vector() - other.vector()).asSkewSymmetric();
  }
 
  // Return type of dense +/- skew. Scalar follows ScalarBinaryOpTraits, so
  // mixed-scalar cases go through the same promotion machinery as the rest of
  // Eigen (and are rejected at compile time unless a user-provided
  // specialization makes them valid). Shape is always 3x3.
  template <typename OtherDerived, typename BinaryOp>
  using DenseSkewBinaryReturnType = Matrix<
      typename ScalarBinaryOpTraits<typename internal::traits<OtherDerived>::Scalar, Scalar, BinaryOp>::ReturnType, 3,
      3>;
 
  template <typename OtherDerived>
  using DenseSkewSumReturnType =
      DenseSkewBinaryReturnType<OtherDerived,
                                internal::scalar_sum_op<typename internal::traits<OtherDerived>::Scalar, Scalar>>;
 
  template <typename OtherDerived>
  using DenseSkewDifferenceReturnType = DenseSkewBinaryReturnType<
      OtherDerived, internal::scalar_difference_op<typename internal::traits<OtherDerived>::Scalar, Scalar>>;

  template <typename OtherDerived>
  EIGEN_DEVICE_FUNC friend EIGEN_STRONG_INLINE DenseSkewSumReturnType<OtherDerived> operator+(
      const MatrixBase<OtherDerived>& lhs, const SkewSymmetricBase& rhs) {
    EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(OtherDerived, DenseMatrixType);
    return lhs.derived() + rhs.toDenseMatrix();
  }

  template <typename OtherDerived>
  EIGEN_DEVICE_FUNC friend EIGEN_STRONG_INLINE DenseSkewSumReturnType<OtherDerived> operator+(
      const SkewSymmetricBase& lhs, const MatrixBase<OtherDerived>& rhs) {
    return rhs + lhs;
  }

  template <typename OtherDerived>
  EIGEN_DEVICE_FUNC friend EIGEN_STRONG_INLINE DenseSkewDifferenceReturnType<OtherDerived> operator-(
      const MatrixBase<OtherDerived>& lhs, const SkewSymmetricBase& rhs) {
    EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(OtherDerived, DenseMatrixType);
    return lhs.derived() - rhs.toDenseMatrix();
  }

  template <typename OtherDerived>
  EIGEN_DEVICE_FUNC friend EIGEN_STRONG_INLINE DenseSkewDifferenceReturnType<OtherDerived> operator-(
      const SkewSymmetricBase& lhs, const MatrixBase<OtherDerived>& rhs) {
    EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(OtherDerived, DenseMatrixType);
    return lhs.toDenseMatrix() - rhs.derived();
  }
};

 
namespace internal {
template <typename Scalar_>
struct traits<SkewSymmetricMatrix3<Scalar_>> : traits<Matrix<Scalar_, 3, 3, 0, 3, 3>> {
  using SkewSymmetricVectorType = Matrix<Scalar_, 3, 1, 0, 3, 1>;
  using StorageKind = SkewSymmetricShape;
  enum { Flags = LvalueBit | NoPreferredStorageOrderBit | NestByRefBit };
};
}  // namespace internal
template <typename Scalar_>
class SkewSymmetricMatrix3 : public SkewSymmetricBase<SkewSymmetricMatrix3<Scalar_>> {
 public:
#ifndef EIGEN_PARSED_BY_DOXYGEN
  using SkewSymmetricVectorType = typename internal::traits<SkewSymmetricMatrix3>::SkewSymmetricVectorType;
  using Nested = const SkewSymmetricMatrix3&;
  using Scalar = Scalar_;
  using StorageKind = typename internal::traits<SkewSymmetricMatrix3>::StorageKind;
  using StorageIndex = typename internal::traits<SkewSymmetricMatrix3>::StorageIndex;
#endif
 
 protected:
  SkewSymmetricVectorType m_vector;
 
 public:
  EIGEN_DEVICE_FUNC inline const SkewSymmetricVectorType& vector() const { return m_vector; }
  EIGEN_DEVICE_FUNC inline SkewSymmetricVectorType& vector() { return m_vector; }

  EIGEN_DEVICE_FUNC inline SkewSymmetricMatrix3() {}

  EIGEN_DEVICE_FUNC inline SkewSymmetricMatrix3(const Scalar& x, const Scalar& y, const Scalar& z)
      : m_vector(x, y, z) {}

  EIGEN_DEVICE_FUNC explicit inline SkewSymmetricMatrix3(SkewSymmetricVectorType&& vec) : m_vector(std::move(vec)) {}

  template <typename OtherDerived>
  EIGEN_DEVICE_FUNC explicit inline SkewSymmetricMatrix3(const MatrixBase<OtherDerived>& other) : m_vector(other) {}

  template <typename OtherDerived>
  EIGEN_DEVICE_FUNC inline SkewSymmetricMatrix3(const SkewSymmetricBase<OtherDerived>& other)
      : m_vector(other.vector()) {}
 
#ifndef EIGEN_PARSED_BY_DOXYGEN
  inline SkewSymmetricMatrix3(const SkewSymmetricMatrix3& other) : m_vector(other.vector()) {}
#endif

  template <typename OtherDerived>
  EIGEN_DEVICE_FUNC SkewSymmetricMatrix3& operator=(const SkewSymmetricBase<OtherDerived>& other) {
    m_vector = other.vector();
    return *this;
  }
 
#ifndef EIGEN_PARSED_BY_DOXYGEN
  EIGEN_DEVICE_FUNC SkewSymmetricMatrix3& operator=(const SkewSymmetricMatrix3& other) {
    m_vector = other.vector();
    return *this;
  }
#endif
 
  using InitializeReturnType =
      SkewSymmetricWrapper<const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, SkewSymmetricVectorType>>;

  EIGEN_DEVICE_FUNC static InitializeReturnType Zero() { return SkewSymmetricVectorType::Zero().asSkewSymmetric(); }

  EIGEN_DEVICE_FUNC inline void setZero() { m_vector.setZero(); }
};

 
namespace internal {
template <typename SkewSymmetricVectorType_>
struct traits<SkewSymmetricWrapper<SkewSymmetricVectorType_>> {
  using SkewSymmetricVectorType = SkewSymmetricVectorType_;
  using Scalar = typename SkewSymmetricVectorType::Scalar;
  using StorageIndex = typename SkewSymmetricVectorType::StorageIndex;
  using StorageKind = SkewSymmetricShape;
  using XprKind = typename traits<SkewSymmetricVectorType>::XprKind;
  enum {
    RowsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime,
    ColsAtCompileTime = SkewSymmetricVectorType::SizeAtCompileTime,
    MaxRowsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime,
    MaxColsAtCompileTime = SkewSymmetricVectorType::MaxSizeAtCompileTime,
    Flags = (traits<SkewSymmetricVectorType>::Flags & LvalueBit) | NoPreferredStorageOrderBit
  };
};
}  // namespace internal
 
template <typename SkewSymmetricVectorType_>
class SkewSymmetricWrapper : public SkewSymmetricBase<SkewSymmetricWrapper<SkewSymmetricVectorType_>>,
                             internal::no_assignment_operator {
 public:
#ifndef EIGEN_PARSED_BY_DOXYGEN
  using SkewSymmetricVectorType = SkewSymmetricVectorType_;
  using Nested = SkewSymmetricWrapper;
#endif

  EIGEN_DEVICE_FUNC explicit inline SkewSymmetricWrapper(SkewSymmetricVectorType& a_vector) : m_vector(a_vector) {}

  EIGEN_DEVICE_FUNC const SkewSymmetricVectorType& vector() const { return m_vector; }
 
 protected:
  typename SkewSymmetricVectorType::Nested m_vector;
};

template <typename Derived>
EIGEN_DEVICE_FUNC constexpr const SkewSymmetricWrapper<const Derived> MatrixBase<Derived>::asSkewSymmetric() const {
  return SkewSymmetricWrapper<const Derived>(derived());
}

template <typename Derived>
bool MatrixBase<Derived>::isSkewSymmetric(const RealScalar& prec) const {
  if (cols() != rows()) return false;
  return (this->transpose() + *this).isZero(prec);
}

template <typename Derived>
template <typename SkewDerived>
EIGEN_DEVICE_FUNC inline const Product<Derived, SkewDerived, LazyProduct> MatrixBase<Derived>::operator*(
    const SkewSymmetricBase<SkewDerived>& skew) const {
  return Product<Derived, SkewDerived, LazyProduct>(derived(), skew.derived());
}
 
namespace internal {
 
template <>
struct storage_kind_to_shape<SkewSymmetricShape> {
  using Shape = SkewSymmetricShape;
};
 
struct SkewSymmetric2Dense {};
 
template <>
struct AssignmentKind<DenseShape, SkewSymmetricShape> {
  using Kind = SkewSymmetric2Dense;
};
 
// SkewSymmetric matrix to Dense assignment
template <typename DstXprType, typename SrcXprType, typename Functor>
struct Assignment<DstXprType, SrcXprType, Functor, SkewSymmetric2Dense> {
  EIGEN_DEVICE_FUNC static void run(
      DstXprType& dst, const SrcXprType& src,
      const internal::assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>& /*func*/) {
    if ((dst.rows() != 3) || (dst.cols() != 3)) {
      dst.resize(3, 3);
    }
    dst.diagonal().setZero();
    const typename SrcXprType::SkewSymmetricVectorType v = src.vector();
    dst(0, 1) = -v(2);
    dst(1, 0) = v(2);
    dst(0, 2) = v(1);
    dst(2, 0) = -v(1);
    dst(1, 2) = -v(0);
    dst(2, 1) = v(0);
  }
  EIGEN_DEVICE_FUNC static void run(
      DstXprType& dst, const SrcXprType& src,
      const internal::add_assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>& /*func*/) {
    dst.vector() += src.vector();
  }
 
  EIGEN_DEVICE_FUNC static void run(
      DstXprType& dst, const SrcXprType& src,
      const internal::sub_assign_op<typename DstXprType::Scalar, typename SrcXprType::Scalar>& /*func*/) {
    dst.vector() -= src.vector();
  }
};
 
}  // namespace internal
 
}  // end namespace Eigen
 
#endif  // EIGEN_SKEWSYMMETRICMATRIX3_H