devtools/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2026-04-10 11:34:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Apply clang-format

Browse Source
This commit is contained in:
Tobias Wood
2023-11-29 11:12:48 +00:00
parent 9ea520fc45
commit f38e16c193
534 changed files with 103368 additions and 116934 deletions
Download Patch File Download Diff File Expand all files Collapse all files

442
Eigen/src/Geometry/AlignedBox.h
View File

@@ -52,76 +52,83 @@
namespace Eigen {
/** \geometry_module \ingroup Geometry_Module
*
*
* \class AlignedBox
*
* \brief An axis aligned box
*
* \tparam Scalar_ the type of the scalar coefficients
* \tparam AmbientDim_ the dimension of the ambient space, can be a compile time value or Dynamic.
*
* This class represents an axis aligned box as a pair of the minimal and maximal corners.
* \warning The result of most methods is undefined when applied to an empty box. You can check for empty boxes using isEmpty().
* \sa alignedboxtypedefs
*/
*
*
* \class AlignedBox
*
* \brief An axis aligned box
*
* \tparam Scalar_ the type of the scalar coefficients
* \tparam AmbientDim_ the dimension of the ambient space, can be a compile time value or Dynamic.
*
* This class represents an axis aligned box as a pair of the minimal and maximal corners.
* \warning The result of most methods is undefined when applied to an empty box. You can check for empty boxes using
* isEmpty(). \sa alignedboxtypedefs
*/
template <typename Scalar_, int AmbientDim_>
class AlignedBox
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,AmbientDim_)
class AlignedBox {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_, AmbientDim_)
enum { AmbientDimAtCompileTime = AmbientDim_ };
typedef Scalar_ Scalar;
typedef NumTraits<Scalar> ScalarTraits;
typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
typedef typename ScalarTraits::Real RealScalar;
typedef typename ScalarTraits::NonInteger NonInteger;
typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
typedef Scalar_ Scalar;
typedef NumTraits<Scalar> ScalarTraits;
typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
typedef typename ScalarTraits::Real RealScalar;
typedef typename ScalarTraits::NonInteger NonInteger;
typedef Matrix<Scalar, AmbientDimAtCompileTime, 1> VectorType;
typedef CwiseBinaryOp<internal::scalar_sum_op<Scalar>, const VectorType, const VectorType> VectorTypeSum;
/** Define constants to name the corners of a 1D, 2D or 3D axis aligned bounding box */
enum CornerType
{
enum CornerType {
/** 1D names @{ */
Min=0, Max=1,
Min = 0,
Max = 1,
/** @} */
/** Identifier for 2D corner @{ */
BottomLeft=0, BottomRight=1,
TopLeft=2, TopRight=3,
BottomLeft = 0,
BottomRight = 1,
TopLeft = 2,
TopRight = 3,
/** @} */
/** Identifier for 3D corner @{ */
BottomLeftFloor=0, BottomRightFloor=1,
TopLeftFloor=2, TopRightFloor=3,
BottomLeftCeil=4, BottomRightCeil=5,
TopLeftCeil=6, TopRightCeil=7
BottomLeftFloor = 0,
BottomRightFloor = 1,
TopLeftFloor = 2,
TopRightFloor = 3,
BottomLeftCeil = 4,
BottomRightCeil = 5,
TopLeftCeil = 6,
TopRightCeil = 7
/** @} */
};
/** Default constructor initializing a null box. */
EIGEN_DEVICE_FUNC inline AlignedBox()
{ if (EIGEN_CONST_CONDITIONAL(AmbientDimAtCompileTime!=Dynamic)) setEmpty(); }
EIGEN_DEVICE_FUNC inline AlignedBox() {
if (EIGEN_CONST_CONDITIONAL(AmbientDimAtCompileTime != Dynamic)) setEmpty();
}
/** Constructs a null box with \a _dim the dimension of the ambient space. */
EIGEN_DEVICE_FUNC inline explicit AlignedBox(Index _dim) : m_min(_dim), m_max(_dim)
{ setEmpty(); }
EIGEN_DEVICE_FUNC inline explicit AlignedBox(Index _dim) : m_min(_dim), m_max(_dim) { setEmpty(); }
/** Constructs a box with extremities \a _min and \a _max.
* \warning If either component of \a _min is larger than the same component of \a _max, the constructed box is empty. */
template<typename OtherVectorType1, typename OtherVectorType2>
EIGEN_DEVICE_FUNC inline AlignedBox(const OtherVectorType1& _min, const OtherVectorType2& _max) : m_min(_min), m_max(_max) {}
* \warning If either component of \a _min is larger than the same component of \a _max, the constructed box is empty.
*/
template <typename OtherVectorType1, typename OtherVectorType2>
EIGEN_DEVICE_FUNC inline AlignedBox(const OtherVectorType1& _min, const OtherVectorType2& _max)
: m_min(_min), m_max(_max) {}
/** Constructs a box containing a single point \a p. */
template<typename Derived>
EIGEN_DEVICE_FUNC inline explicit AlignedBox(const MatrixBase<Derived>& p) : m_min(p), m_max(m_min)
{ }
template <typename Derived>
EIGEN_DEVICE_FUNC inline explicit AlignedBox(const MatrixBase<Derived>& p) : m_min(p), m_max(m_min) {}
EIGEN_DEVICE_FUNC ~AlignedBox() {}
/** \returns the dimension in which the box holds */
EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); }
EIGEN_DEVICE_FUNC inline Index dim() const {
return AmbientDimAtCompileTime == Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime);
}
/** \deprecated use isEmpty() */
EIGEN_DEVICE_FUNC inline bool isNull() const { return isEmpty(); }
@@ -135,64 +142,69 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,AmbientDim_)
/** Makes \c *this an empty box.
* \sa isEmpty */
EIGEN_DEVICE_FUNC inline void setEmpty()
{
m_min.setConstant( ScalarTraits::highest() );
m_max.setConstant( ScalarTraits::lowest() );
EIGEN_DEVICE_FUNC inline void setEmpty() {
m_min.setConstant(ScalarTraits::highest());
m_max.setConstant(ScalarTraits::lowest());
}
/** \returns the minimal corner */
EIGEN_DEVICE_FUNC inline const VectorType& (min)() const { return m_min; }
EIGEN_DEVICE_FUNC inline const VectorType&(min)() const { return m_min; }
/** \returns a non const reference to the minimal corner */
EIGEN_DEVICE_FUNC inline VectorType& (min)() { return m_min; }
EIGEN_DEVICE_FUNC inline VectorType&(min)() { return m_min; }
/** \returns the maximal corner */
EIGEN_DEVICE_FUNC inline const VectorType& (max)() const { return m_max; }
EIGEN_DEVICE_FUNC inline const VectorType&(max)() const { return m_max; }
/** \returns a non const reference to the maximal corner */
EIGEN_DEVICE_FUNC inline VectorType& (max)() { return m_max; }
EIGEN_DEVICE_FUNC inline VectorType&(max)() { return m_max; }
/** \returns the center of the box */
EIGEN_DEVICE_FUNC inline const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(VectorTypeSum, RealScalar, quotient)
center() const
{ return (m_min+m_max)/RealScalar(2); }
center() const {
return (m_min + m_max) / RealScalar(2);
}
/** \returns the lengths of the sides of the bounding box.
* Note that this function does not get the same
* result for integral or floating scalar types: see
*/
EIGEN_DEVICE_FUNC inline const CwiseBinaryOp< internal::scalar_difference_op<Scalar,Scalar>, const VectorType, const VectorType> sizes() const
{ return m_max - m_min; }
* Note that this function does not get the same
* result for integral or floating scalar types: see
*/
EIGEN_DEVICE_FUNC inline const CwiseBinaryOp<internal::scalar_difference_op<Scalar, Scalar>, const VectorType,
const VectorType>
sizes() const {
return m_max - m_min;
}
/** \returns the volume of the bounding box */
EIGEN_DEVICE_FUNC inline Scalar volume() const
{ return sizes().prod(); }
EIGEN_DEVICE_FUNC inline Scalar volume() const { return sizes().prod(); }
/** \returns an expression for the bounding box diagonal vector
* if the length of the diagonal is needed: diagonal().norm()
* will provide it.
*/
EIGEN_DEVICE_FUNC inline CwiseBinaryOp< internal::scalar_difference_op<Scalar,Scalar>, const VectorType, const VectorType> diagonal() const
{ return sizes(); }
* if the length of the diagonal is needed: diagonal().norm()
* will provide it.
*/
EIGEN_DEVICE_FUNC inline CwiseBinaryOp<internal::scalar_difference_op<Scalar, Scalar>, const VectorType,
const VectorType>
diagonal() const {
return sizes();
}
/** \returns the vertex of the bounding box at the corner defined by
* the corner-id corner. It works only for a 1D, 2D or 3D bounding box.
* For 1D bounding boxes corners are named by 2 enum constants:
* BottomLeft and BottomRight.
* For 2D bounding boxes, corners are named by 4 enum constants:
* BottomLeft, BottomRight, TopLeft, TopRight.
* For 3D bounding boxes, the following names are added:
* BottomLeftCeil, BottomRightCeil, TopLeftCeil, TopRightCeil.
*/
EIGEN_DEVICE_FUNC inline VectorType corner(CornerType corner) const
{
* the corner-id corner. It works only for a 1D, 2D or 3D bounding box.
* For 1D bounding boxes corners are named by 2 enum constants:
* BottomLeft and BottomRight.
* For 2D bounding boxes, corners are named by 4 enum constants:
* BottomLeft, BottomRight, TopLeft, TopRight.
* For 3D bounding boxes, the following names are added:
* BottomLeftCeil, BottomRightCeil, TopLeftCeil, TopRightCeil.
*/
EIGEN_DEVICE_FUNC inline VectorType corner(CornerType corner) const {
EIGEN_STATIC_ASSERT(AmbientDim_ <= 3, THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE);
VectorType res;
Index mult = 1;
for(Index d=0; d<dim(); ++d)
{
if( mult & corner ) res[d] = m_max[d];
else res[d] = m_min[d];
for (Index d = 0; d < dim(); ++d) {
if (mult & corner)
res[d] = m_max[d];
else
res[d] = m_min[d];
mult *= 2;
}
return res;
@@ -200,45 +212,40 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,AmbientDim_)
/** \returns a random point inside the bounding box sampled with
* a uniform distribution */
EIGEN_DEVICE_FUNC inline VectorType sample() const
{
EIGEN_DEVICE_FUNC inline VectorType sample() const {
VectorType r(dim());
for(Index d=0; d<dim(); ++d)
{
if(!ScalarTraits::IsInteger)
{
r[d] = m_min[d] + (m_max[d]-m_min[d])
* internal::random<Scalar>(Scalar(0), Scalar(1));
}
else
for (Index d = 0; d < dim(); ++d) {
if (!ScalarTraits::IsInteger) {
r[d] = m_min[d] + (m_max[d] - m_min[d]) * internal::random<Scalar>(Scalar(0), Scalar(1));
} else
r[d] = internal::random(m_min[d], m_max[d]);
}
return r;
}
/** \returns true if the point \a p is inside the box \c *this. */
template<typename Derived>
EIGEN_DEVICE_FUNC inline bool contains(const MatrixBase<Derived>& p) const
{
typename internal::nested_eval<Derived,2>::type p_n(p.derived());
return (m_min.array()<=p_n.array()).all() && (p_n.array()<=m_max.array()).all();
template <typename Derived>
EIGEN_DEVICE_FUNC inline bool contains(const MatrixBase<Derived>& p) const {
typename internal::nested_eval<Derived, 2>::type p_n(p.derived());
return (m_min.array() <= p_n.array()).all() && (p_n.array() <= m_max.array()).all();
}
/** \returns true if the box \a b is entirely inside the box \c *this. */
EIGEN_DEVICE_FUNC inline bool contains(const AlignedBox& b) const
{ return (m_min.array()<=(b.min)().array()).all() && ((b.max)().array()<=m_max.array()).all(); }
EIGEN_DEVICE_FUNC inline bool contains(const AlignedBox& b) const {
return (m_min.array() <= (b.min)().array()).all() && ((b.max)().array() <= m_max.array()).all();
}
/** \returns true if the box \a b is intersecting the box \c *this.
* \sa intersection, clamp */
EIGEN_DEVICE_FUNC inline bool intersects(const AlignedBox& b) const
{ return (m_min.array()<=(b.max)().array()).all() && ((b.min)().array()<=m_max.array()).all(); }
EIGEN_DEVICE_FUNC inline bool intersects(const AlignedBox& b) const {
return (m_min.array() <= (b.max)().array()).all() && ((b.min)().array() <= m_max.array()).all();
}
/** Extends \c *this such that it contains the point \a p and returns a reference to \c *this.
* \sa extend(const AlignedBox&) */
template<typename Derived>
EIGEN_DEVICE_FUNC inline AlignedBox& extend(const MatrixBase<Derived>& p)
{
typename internal::nested_eval<Derived,2>::type p_n(p.derived());
template <typename Derived>
EIGEN_DEVICE_FUNC inline AlignedBox& extend(const MatrixBase<Derived>& p) {
typename internal::nested_eval<Derived, 2>::type p_n(p.derived());
m_min = m_min.cwiseMin(p_n);
m_max = m_max.cwiseMax(p_n);
return *this;
@@ -246,8 +253,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,AmbientDim_)
/** Extends \c *this such that it contains the box \a b and returns a reference to \c *this.
* \sa merged, extend(const MatrixBase&) */
EIGEN_DEVICE_FUNC inline AlignedBox& extend(const AlignedBox& b)
{
EIGEN_DEVICE_FUNC inline AlignedBox& extend(const AlignedBox& b) {
m_min = m_min.cwiseMin(b.m_min);
m_max = m_max.cwiseMax(b.m_max);
return *this;
@@ -256,8 +262,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,AmbientDim_)
/** Clamps \c *this by the box \a b and returns a reference to \c *this.
* \note If the boxes don't intersect, the resulting box is empty.
* \sa intersection(), intersects() */
EIGEN_DEVICE_FUNC inline AlignedBox& clamp(const AlignedBox& b)
{
EIGEN_DEVICE_FUNC inline AlignedBox& clamp(const AlignedBox& b) {
m_min = m_min.cwiseMax(b.m_min);
m_max = m_max.cwiseMin(b.m_max);
return *this;
@@ -266,69 +271,70 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,AmbientDim_)
/** Returns an AlignedBox that is the intersection of \a b and \c *this
* \note If the boxes don't intersect, the resulting box is empty.
* \sa intersects(), clamp, contains() */
EIGEN_DEVICE_FUNC inline AlignedBox intersection(const AlignedBox& b) const
{return AlignedBox(m_min.cwiseMax(b.m_min), m_max.cwiseMin(b.m_max)); }
EIGEN_DEVICE_FUNC inline AlignedBox intersection(const AlignedBox& b) const {
return AlignedBox(m_min.cwiseMax(b.m_min), m_max.cwiseMin(b.m_max));
}
/** Returns an AlignedBox that is the union of \a b and \c *this.
* \note Merging with an empty box may result in a box bigger than \c *this.
* \sa extend(const AlignedBox&) */
EIGEN_DEVICE_FUNC inline AlignedBox merged(const AlignedBox& b) const
{ return AlignedBox(m_min.cwiseMin(b.m_min), m_max.cwiseMax(b.m_max)); }
EIGEN_DEVICE_FUNC inline AlignedBox merged(const AlignedBox& b) const {
return AlignedBox(m_min.cwiseMin(b.m_min), m_max.cwiseMax(b.m_max));
}
/** Translate \c *this by the vector \a t and returns a reference to \c *this. */
template<typename Derived>
EIGEN_DEVICE_FUNC inline AlignedBox& translate(const MatrixBase<Derived>& a_t)
{
const typename internal::nested_eval<Derived,2>::type t(a_t.derived());
template <typename Derived>
EIGEN_DEVICE_FUNC inline AlignedBox& translate(const MatrixBase<Derived>& a_t) {
const typename internal::nested_eval<Derived, 2>::type t(a_t.derived());
m_min += t;
m_max += t;
return *this;
}
/** \returns a copy of \c *this translated by the vector \a t. */
template<typename Derived>
EIGEN_DEVICE_FUNC inline AlignedBox translated(const MatrixBase<Derived>& a_t) const
{
template <typename Derived>
EIGEN_DEVICE_FUNC inline AlignedBox translated(const MatrixBase<Derived>& a_t) const {
AlignedBox result(m_min, m_max);
result.translate(a_t);
return result;
}
/** \returns the squared distance between the point \a p and the box \c *this,
* and zero if \a p is inside the box.
* \sa exteriorDistance(const MatrixBase&), squaredExteriorDistance(const AlignedBox&)
*/
template<typename Derived>
* and zero if \a p is inside the box.
* \sa exteriorDistance(const MatrixBase&), squaredExteriorDistance(const AlignedBox&)
*/
template <typename Derived>
EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const MatrixBase<Derived>& p) const;
/** \returns the squared distance between the boxes \a b and \c *this,
* and zero if the boxes intersect.
* \sa exteriorDistance(const AlignedBox&), squaredExteriorDistance(const MatrixBase&)
*/
* and zero if the boxes intersect.
* \sa exteriorDistance(const AlignedBox&), squaredExteriorDistance(const MatrixBase&)
*/
EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const AlignedBox& b) const;
/** \returns the distance between the point \a p and the box \c *this,
* and zero if \a p is inside the box.
* \sa squaredExteriorDistance(const MatrixBase&), exteriorDistance(const AlignedBox&)
*/
template<typename Derived>
EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const MatrixBase<Derived>& p) const
{ EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(p))); }
* and zero if \a p is inside the box.
* \sa squaredExteriorDistance(const MatrixBase&), exteriorDistance(const AlignedBox&)
*/
template <typename Derived>
EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const MatrixBase<Derived>& p) const {
EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(p)));
}
/** \returns the distance between the boxes \a b and \c *this,
* and zero if the boxes intersect.
* \sa squaredExteriorDistance(const AlignedBox&), exteriorDistance(const MatrixBase&)
*/
EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const AlignedBox& b) const
{ EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(b))); }
* and zero if the boxes intersect.
* \sa squaredExteriorDistance(const AlignedBox&), exteriorDistance(const MatrixBase&)
*/
EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const AlignedBox& b) const {
EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(b)));
}
/**
* Specialization of transform for pure translation.
*/
template<int Mode, int Options>
template <int Mode, int Options>
EIGEN_DEVICE_FUNC inline void transform(
const typename Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>::TranslationType& translation)
{
const typename Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>::TranslationType& translation) {
this->translate(translation);
}
@@ -338,11 +344,11 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,AmbientDim_)
*
* \note This method is provided under BSD license (see the top of this file).
*/
template<int Mode, int Options>
EIGEN_DEVICE_FUNC inline void transform(const Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>& transform)
{
template <int Mode, int Options>
EIGEN_DEVICE_FUNC inline void transform(const Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>& transform) {
// Only Affine and Isometry transforms are currently supported.
EIGEN_STATIC_ASSERT(Mode == Affine || Mode == AffineCompact || Mode == Isometry, THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS);
EIGEN_STATIC_ASSERT(Mode == Affine || Mode == AffineCompact || Mode == Isometry,
THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS);
// Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV<AABB, Box>(...)
// https://github.com/flexible-collision-library/fcl/blob/fcl-0.4/src/shape/geometric_shapes_utility.cpp#L292
@@ -352,8 +358,8 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,AmbientDim_)
// two times rotated extent
const VectorType rotated_extent_2 = transform.linear().cwiseAbs() * sizes();
// two times new center
const VectorType rotated_center_2 = transform.linear() * (this->m_max + this->m_min) +
Scalar(2) * transform.translation();
const VectorType rotated_center_2 =
transform.linear() * (this->m_max + this->m_min) + Scalar(2) * transform.translation();
this->m_max = (rotated_center_2 + rotated_extent_2) / Scalar(2);
this->m_min = (rotated_center_2 - rotated_extent_2) / Scalar(2);
@@ -363,127 +369,117 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,AmbientDim_)
* \returns a copy of \c *this transformed by \a transform and recomputed to
* still be an axis-aligned box.
*/
template<int Mode, int Options>
EIGEN_DEVICE_FUNC AlignedBox transformed(const Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>& transform) const
{
template <int Mode, int Options>
EIGEN_DEVICE_FUNC AlignedBox
transformed(const Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>& transform) const {
AlignedBox result(m_min, m_max);
result.transform(transform);
return result;
}
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AlignedBox,
AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type cast() const
{
return typename internal::cast_return_type<AlignedBox,
AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type(*this);
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template <typename NewScalarType>
EIGEN_DEVICE_FUNC inline
typename internal::cast_return_type<AlignedBox, AlignedBox<NewScalarType, AmbientDimAtCompileTime> >::type
cast() const {
return typename internal::cast_return_type<AlignedBox, AlignedBox<NewScalarType, AmbientDimAtCompileTime> >::type(
*this);
}
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
EIGEN_DEVICE_FUNC inline explicit AlignedBox(const AlignedBox<OtherScalarType,AmbientDimAtCompileTime>& other)
{
template <typename OtherScalarType>
EIGEN_DEVICE_FUNC inline explicit AlignedBox(const AlignedBox<OtherScalarType, AmbientDimAtCompileTime>& other) {
m_min = (other.min)().template cast<Scalar>();
m_max = (other.max)().template cast<Scalar>();
}
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
EIGEN_DEVICE_FUNC bool isApprox(const AlignedBox& other, const RealScalar& prec = ScalarTraits::dummy_precision()) const
{ return m_min.isApprox(other.m_min, prec) && m_max.isApprox(other.m_max, prec); }
protected:
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
EIGEN_DEVICE_FUNC bool isApprox(const AlignedBox& other,
const RealScalar& prec = ScalarTraits::dummy_precision()) const {
return m_min.isApprox(other.m_min, prec) && m_max.isApprox(other.m_max, prec);
}
protected:
VectorType m_min, m_max;
};
template<typename Scalar,int AmbientDim>
template<typename Derived>
EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const MatrixBase<Derived>& a_p) const
{
typename internal::nested_eval<Derived,2*AmbientDim>::type p(a_p.derived());
template <typename Scalar, int AmbientDim>
template <typename Derived>
EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar, AmbientDim>::squaredExteriorDistance(
const MatrixBase<Derived>& a_p) const {
typename internal::nested_eval<Derived, 2 * AmbientDim>::type p(a_p.derived());
Scalar dist2(0);
Scalar aux;
for (Index k=0; k<dim(); ++k)
{
if( m_min[k] > p[k] )
{
for (Index k = 0; k < dim(); ++k) {
if (m_min[k] > p[k]) {
aux = m_min[k] - p[k];
dist2 += aux*aux;
}
else if( p[k] > m_max[k] )
{
dist2 += aux * aux;
} else if (p[k] > m_max[k]) {
aux = p[k] - m_max[k];
dist2 += aux*aux;
dist2 += aux * aux;
}
}
return dist2;
}
template<typename Scalar,int AmbientDim>
EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const AlignedBox& b) const
{
template <typename Scalar, int AmbientDim>
EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar, AmbientDim>::squaredExteriorDistance(const AlignedBox& b) const {
Scalar dist2(0);
Scalar aux;
for (Index k=0; k<dim(); ++k)
{
if( m_min[k] > b.m_max[k] )
{
for (Index k = 0; k < dim(); ++k) {
if (m_min[k] > b.m_max[k]) {
aux = m_min[k] - b.m_max[k];
dist2 += aux*aux;
}
else if( b.m_min[k] > m_max[k] )
{
dist2 += aux * aux;
} else if (b.m_min[k] > m_max[k]) {
aux = b.m_min[k] - m_max[k];
dist2 += aux*aux;
dist2 += aux * aux;
}
}
return dist2;
}
/** \defgroup alignedboxtypedefs Global aligned box typedefs
*
* \ingroup Geometry_Module
*
* Eigen defines several typedef shortcuts for most common aligned box types.
*
* The general patterns are the following:
*
* \c AlignedBoxSizeType where \c Size can be \c 1, \c 2,\c 3,\c 4 for fixed size boxes or \c X for dynamic size,
* and where \c Type can be \c i for integer, \c f for float, \c d for double.
*
* For example, \c AlignedBox3d is a fixed-size 3x3 aligned box type of doubles, and \c AlignedBoxXf is a dynamic-size aligned box of floats.
*
* \sa class AlignedBox
*/
*
* \ingroup Geometry_Module
*
* Eigen defines several typedef shortcuts for most common aligned box types.
*
* The general patterns are the following:
*
* \c AlignedBoxSizeType where \c Size can be \c 1, \c 2,\c 3,\c 4 for fixed size boxes or \c X for dynamic size,
* and where \c Type can be \c i for integer, \c f for float, \c d for double.
*
* For example, \c AlignedBox3d is a fixed-size 3x3 aligned box type of doubles, and \c AlignedBoxXf is a dynamic-size
* aligned box of floats.
*
* \sa class AlignedBox
*/
#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
/** \ingroup alignedboxtypedefs */ \
typedef AlignedBox<Type, Size> AlignedBox##SizeSuffix##TypeSuffix;
#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
/** \ingroup alignedboxtypedefs */ \
typedef AlignedBox<Type, Size> AlignedBox##SizeSuffix##TypeSuffix;
#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X)
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
#undef EIGEN_MAKE_TYPEDEFS
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_ALIGNEDBOX_H
#endif // EIGEN_ALIGNEDBOX_H

255
Eigen/src/Geometry/AngleAxis.h
View File

@@ -13,82 +13,82 @@
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
namespace Eigen {
namespace Eigen {
/** \geometry_module \ingroup Geometry_Module
*
* \class AngleAxis
*
* \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis
*
* \param Scalar_ the scalar type, i.e., the type of the coefficients.
*
* \warning When setting up an AngleAxis object, the axis vector \b must \b be \b normalized.
*
* The following two typedefs are provided for convenience:
* \li \c AngleAxisf for \c float
* \li \c AngleAxisd for \c double
*
* Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily
* mimic Euler-angles. Here is an example:
* \include AngleAxis_mimic_euler.cpp
* Output: \verbinclude AngleAxis_mimic_euler.out
*
* \note This class is not aimed to be used to store a rotation transformation,
* but rather to make easier the creation of other rotation (Quaternion, rotation Matrix)
* and transformation objects.
*
* \sa class Quaternion, class Transform, MatrixBase::UnitX()
*/
*
* \class AngleAxis
*
* \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis
*
* \param Scalar_ the scalar type, i.e., the type of the coefficients.
*
* \warning When setting up an AngleAxis object, the axis vector \b must \b be \b normalized.
*
* The following two typedefs are provided for convenience:
* \li \c AngleAxisf for \c float
* \li \c AngleAxisd for \c double
*
* Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily
* mimic Euler-angles. Here is an example:
* \include AngleAxis_mimic_euler.cpp
* Output: \verbinclude AngleAxis_mimic_euler.out
*
* \note This class is not aimed to be used to store a rotation transformation,
* but rather to make easier the creation of other rotation (Quaternion, rotation Matrix)
* and transformation objects.
*
* \sa class Quaternion, class Transform, MatrixBase::UnitX()
*/
namespace internal {
template<typename Scalar_> struct traits<AngleAxis<Scalar_> >
{
template <typename Scalar_>
struct traits<AngleAxis<Scalar_> > {
typedef Scalar_ Scalar;
};
}
} // namespace internal
template<typename Scalar_>
class AngleAxis : public RotationBase<AngleAxis<Scalar_>,3>
{
typedef RotationBase<AngleAxis<Scalar_>,3> Base;
public:
template <typename Scalar_>
class AngleAxis : public RotationBase<AngleAxis<Scalar_>, 3> {
typedef RotationBase<AngleAxis<Scalar_>, 3> Base;
public:
using Base::operator*;
enum { Dim = 3 };
/** the scalar type of the coefficients */
typedef Scalar_ Scalar;
typedef Matrix<Scalar,3,3> Matrix3;
typedef Matrix<Scalar,3,1> Vector3;
typedef Matrix<Scalar, 3, 3> Matrix3;
typedef Matrix<Scalar, 3, 1> Vector3;
typedef Quaternion<Scalar> QuaternionType;
protected:
protected:
Vector3 m_axis;
Scalar m_angle;
public:
public:
/** Default constructor without initialization. */
EIGEN_DEVICE_FUNC AngleAxis() {}
/** Constructs and initialize the angle-axis rotation from an \a angle in radian
* and an \a axis which \b must \b be \b normalized.
*
* \warning If the \a axis vector is not normalized, then the angle-axis object
* represents an invalid rotation. */
template<typename Derived>
EIGEN_DEVICE_FUNC
inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
* and an \a axis which \b must \b be \b normalized.
*
* \warning If the \a axis vector is not normalized, then the angle-axis object
* represents an invalid rotation. */
template <typename Derived>
EIGEN_DEVICE_FUNC inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis)
: m_axis(axis), m_angle(angle) {}
/** Constructs and initialize the angle-axis rotation from a quaternion \a q.
* This function implicitly normalizes the quaternion \a q.
*/
template<typename QuatDerived>
EIGEN_DEVICE_FUNC inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
* This function implicitly normalizes the quaternion \a q.
*/
template <typename QuatDerived>
EIGEN_DEVICE_FUNC inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) {
*this = q;
}
/** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
template<typename Derived>
EIGEN_DEVICE_FUNC inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
template <typename Derived>
EIGEN_DEVICE_FUNC inline explicit AngleAxis(const MatrixBase<Derived>& m) {
*this = m;
}
/** \returns the value of the rotation angle in radian */
EIGEN_DEVICE_FUNC Scalar angle() const { return m_angle; }
@@ -98,49 +98,52 @@ public:
/** \returns the rotation axis */
EIGEN_DEVICE_FUNC const Vector3& axis() const { return m_axis; }
/** \returns a read-write reference to the stored rotation axis.
*
* \warning The rotation axis must remain a \b unit vector.
*/
*
* \warning The rotation axis must remain a \b unit vector.
*/
EIGEN_DEVICE_FUNC Vector3& axis() { return m_axis; }
/** Concatenates two rotations */
EIGEN_DEVICE_FUNC inline QuaternionType operator* (const AngleAxis& other) const
{ return QuaternionType(*this) * QuaternionType(other); }
EIGEN_DEVICE_FUNC inline QuaternionType operator*(const AngleAxis& other) const {
return QuaternionType(*this) * QuaternionType(other);
}
/** Concatenates two rotations */
EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& other) const
{ return QuaternionType(*this) * other; }
EIGEN_DEVICE_FUNC inline QuaternionType operator*(const QuaternionType& other) const {
return QuaternionType(*this) * other;
}
/** Concatenates two rotations */
friend EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
{ return a * QuaternionType(b); }
friend EIGEN_DEVICE_FUNC inline QuaternionType operator*(const QuaternionType& a, const AngleAxis& b) {
return a * QuaternionType(b);
}
/** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */
EIGEN_DEVICE_FUNC AngleAxis inverse() const
{ return AngleAxis(-m_angle, m_axis); }
EIGEN_DEVICE_FUNC AngleAxis inverse() const { return AngleAxis(-m_angle, m_axis); }
template<class QuatDerived>
template <class QuatDerived>
EIGEN_DEVICE_FUNC AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
template<typename Derived>
template <typename Derived>
EIGEN_DEVICE_FUNC AngleAxis& operator=(const MatrixBase<Derived>& m);
template<typename Derived>
template <typename Derived>
EIGEN_DEVICE_FUNC AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix(void) const;
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
{ return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template <typename NewScalarType>
EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AngleAxis, AngleAxis<NewScalarType> >::type cast()
const {
return typename internal::cast_return_type<AngleAxis, AngleAxis<NewScalarType> >::type(*this);
}
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
EIGEN_DEVICE_FUNC inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
{
template <typename OtherScalarType>
EIGEN_DEVICE_FUNC inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) {
m_axis = other.axis().template cast<Scalar>();
m_angle = Scalar(other.angle());
}
@@ -148,45 +151,41 @@ public:
EIGEN_DEVICE_FUNC static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); }
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
EIGEN_DEVICE_FUNC bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
{ return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); }
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
EIGEN_DEVICE_FUNC bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec =
NumTraits<Scalar>::dummy_precision()) const {
return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle, other.m_angle, prec);
}
};
/** \ingroup Geometry_Module
* single precision angle-axis type */
* single precision angle-axis type */
typedef AngleAxis<float> AngleAxisf;
/** \ingroup Geometry_Module
* double precision angle-axis type */
* double precision angle-axis type */
typedef AngleAxis<double> AngleAxisd;
/** Set \c *this from a \b unit quaternion.
*
* The resulting axis is normalized, and the computed angle is in the [0,pi] range.
*
* This function implicitly normalizes the quaternion \a q.
*/
template<typename Scalar>
template<typename QuatDerived>
EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q)
{
*
* The resulting axis is normalized, and the computed angle is in the [0,pi] range.
*
* This function implicitly normalizes the quaternion \a q.
*/
template <typename Scalar>
template <typename QuatDerived>
EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q) {
EIGEN_USING_STD(atan2)
EIGEN_USING_STD(abs)
Scalar n = q.vec().norm();
if(n<NumTraits<Scalar>::epsilon())
n = q.vec().stableNorm();
if (n < NumTraits<Scalar>::epsilon()) n = q.vec().stableNorm();
if (n != Scalar(0))
{
m_angle = Scalar(2)*atan2(n, abs(q.w()));
if(q.w() < Scalar(0))
n = -n;
m_axis = q.vec() / n;
}
else
{
if (n != Scalar(0)) {
m_angle = Scalar(2) * atan2(n, abs(q.w()));
if (q.w() < Scalar(0)) n = -n;
m_axis = q.vec() / n;
} else {
m_angle = Scalar(0);
m_axis << Scalar(1), Scalar(0), Scalar(0);
}
@@ -194,57 +193,53 @@ EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const Quaterni
}
/** Set \c *this from a 3x3 rotation matrix \a mat.
*/
template<typename Scalar>
template<typename Derived>
EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
{
*/
template <typename Scalar>
template <typename Derived>
EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) {
// Since a direct conversion would not be really faster,
// let's use the robust Quaternion implementation:
return *this = QuaternionType(mat);
}
/**
* \brief Sets \c *this from a 3x3 rotation matrix.
**/
template<typename Scalar>
template<typename Derived>
EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
{
* \brief Sets \c *this from a 3x3 rotation matrix.
**/
template <typename Scalar>
template <typename Derived>
EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) {
return *this = QuaternionType(mat);
}
/** Constructs and \returns an equivalent 3x3 rotation matrix.
*/
template<typename Scalar>
typename AngleAxis<Scalar>::Matrix3
EIGEN_DEVICE_FUNC AngleAxis<Scalar>::toRotationMatrix(void) const
{
*/
template <typename Scalar>
typename AngleAxis<Scalar>::Matrix3 EIGEN_DEVICE_FUNC AngleAxis<Scalar>::toRotationMatrix(void) const {
EIGEN_USING_STD(sin)
EIGEN_USING_STD(cos)
Matrix3 res;
Vector3 sin_axis = sin(m_angle) * m_axis;
Vector3 sin_axis = sin(m_angle) * m_axis;
Scalar c = cos(m_angle);
Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
Vector3 cos1_axis = (Scalar(1) - c) * m_axis;
Scalar tmp;
tmp = cos1_axis.x() * m_axis.y();
res.coeffRef(0,1) = tmp - sin_axis.z();
res.coeffRef(1,0) = tmp + sin_axis.z();
res.coeffRef(0, 1) = tmp - sin_axis.z();
res.coeffRef(1, 0) = tmp + sin_axis.z();
tmp = cos1_axis.x() * m_axis.z();
res.coeffRef(0,2) = tmp + sin_axis.y();
res.coeffRef(2,0) = tmp - sin_axis.y();
res.coeffRef(0, 2) = tmp + sin_axis.y();
res.coeffRef(2, 0) = tmp - sin_axis.y();
tmp = cos1_axis.y() * m_axis.z();
res.coeffRef(1,2) = tmp - sin_axis.x();
res.coeffRef(2,1) = tmp + sin_axis.x();
res.coeffRef(1, 2) = tmp - sin_axis.x();
res.coeffRef(2, 1) = tmp + sin_axis.x();
res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c;
return res;
}
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_ANGLEAXIS_H
#endif // EIGEN_ANGLEAXIS_H

158
Eigen/src/Geometry/EulerAngles.h
View File

@@ -17,32 +17,33 @@
namespace Eigen {
/** \geometry_module \ingroup Geometry_Module
*
*
* \returns the canonical Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2)
*
* Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}.
* For instance, in:
* \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode
* "2" represents the z axis and "0" the x axis, etc. The returned angles are such that
* we have the following equality:
* \code
* mat == AngleAxisf(ea[0], Vector3f::UnitZ())
* * AngleAxisf(ea[1], Vector3f::UnitX())
* * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
* This corresponds to the right-multiply conventions (with right hand side frames).
*
* For Tait-Bryan angle configurations (a0 != a2), the returned angles are in the ranges [-pi:pi]x[-pi/2:pi/2]x[-pi:pi].
* For proper Euler angle configurations (a0 == a2), the returned angles are in the ranges [-pi:pi]x[0:pi]x[-pi:pi].
*
* The approach used is also described here: https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2012/07/euler-angles.pdf
*
* \sa class AngleAxis
*/
template<typename Derived>
EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
MatrixBase<Derived>::canonicalEulerAngles(Index a0, Index a1, Index a2) const
{
*
*
* \returns the canonical Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a
* a0,\a a1,\a a2)
*
* Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}.
* For instance, in:
* \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode
* "2" represents the z axis and "0" the x axis, etc. The returned angles are such that
* we have the following equality:
* \code
* mat == AngleAxisf(ea[0], Vector3f::UnitZ())
* * AngleAxisf(ea[1], Vector3f::UnitX())
* * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
* This corresponds to the right-multiply conventions (with right hand side frames).
*
* For Tait-Bryan angle configurations (a0 != a2), the returned angles are in the ranges [-pi:pi]x[-pi/2:pi/2]x[-pi:pi].
* For proper Euler angle configurations (a0 == a2), the returned angles are in the ranges [-pi:pi]x[0:pi]x[-pi:pi].
*
* The approach used is also described here:
* https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2012/07/euler-angles.pdf
*
* \sa class AngleAxis
*/
template <typename Derived>
EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar, 3, 1> MatrixBase<Derived>::canonicalEulerAngles(
Index a0, Index a1, Index a2) const {
/* Implemented from Graphics Gems IV */
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
@@ -53,8 +54,7 @@ MatrixBase<Derived>::canonicalEulerAngles(Index a0, Index a1, Index a2) const
const Index j = (a0 + 1 + odd) % 3;
const Index k = (a0 + 2 - odd) % 3;
if (a0 == a2)
{
if (a0 == a2) {
// Proper Euler angles (same first and last axis).
// The i, j, k indices enable addressing the input matrix as the XYX archetype matrix (see Graphics Gems IV),
// where e.g. coeff(k, i) means third column, first row in the XYX archetype matrix:
@@ -64,22 +64,19 @@ MatrixBase<Derived>::canonicalEulerAngles(Index a0, Index a1, Index a2) const
// Note: s2 is always positive.
Scalar s2 = numext::hypot(coeff(j, i), coeff(k, i));
if (odd)
{
if (odd) {
res[0] = numext::atan2(coeff(j, i), coeff(k, i));
// s2 is always positive, so res[1] will be within the canonical [0, pi] range
res[1] = numext::atan2(s2, coeff(i, i));
}
else
{
// In the !odd case, signs of all three angles are flipped at the very end. To keep the solution within the canonical range,
// we flip the solution and make res[1] always negative here (since s2 is always positive, -atan2(s2, c2) will always be negative).
// The final flip at the end due to !odd will thus make res[1] positive and canonical.
// NB: in the general case, there are two correct solutions, but only one is canonical. For proper Euler angles,
// flipping from one solution to the other involves flipping the sign of the second angle res[1] and adding/subtracting pi
// to the first and third angles. The addition/subtraction of pi to the first angle res[0] is handled here by flipping
// the signs of arguments to atan2, while the calculation of the third angle does not need special adjustment since
// it uses the adjusted res[0] as the input and produces a correct result.
} else {
// In the !odd case, signs of all three angles are flipped at the very end. To keep the solution within the
// canonical range, we flip the solution and make res[1] always negative here (since s2 is always positive,
// -atan2(s2, c2) will always be negative). The final flip at the end due to !odd will thus make res[1] positive
// and canonical. NB: in the general case, there are two correct solutions, but only one is canonical. For proper
// Euler angles, flipping from one solution to the other involves flipping the sign of the second angle res[1] and
// adding/subtracting pi to the first and third angles. The addition/subtraction of pi to the first angle res[0]
// is handled here by flipping the signs of arguments to atan2, while the calculation of the third angle does not
// need special adjustment since it uses the adjusted res[0] as the input and produces a correct result.
res[0] = numext::atan2(-coeff(j, i), -coeff(k, i));
res[1] = -numext::atan2(s2, coeff(i, i));
}
@@ -97,9 +94,7 @@ MatrixBase<Derived>::canonicalEulerAngles(Index a0, Index a1, Index a2) const
Scalar s1 = numext::sin(res[0]);
Scalar c1 = numext::cos(res[0]);
res[2] = numext::atan2(c1 * coeff(j, k) - s1 * coeff(k, k), c1 * coeff(j, j) - s1 * coeff(k, j));
}
else
{
} else {
// Tait-Bryan angles (all three axes are different; typically used for yaw-pitch-roll calculations).
// The i, j, k indices enable addressing the input matrix as the XYZ archetype matrix (see Graphics Gems IV),
// where e.g. coeff(k, i) means third column, first row in the XYZ archetype matrix:
@@ -110,15 +105,15 @@ MatrixBase<Derived>::canonicalEulerAngles(Index a0, Index a1, Index a2) const
res[0] = numext::atan2(coeff(j, k), coeff(k, k));
Scalar c2 = numext::hypot(coeff(i, i), coeff(i, j));
// c2 is always positive, so the following atan2 will always return a result in the correct canonical middle angle range [-pi/2, pi/2]
// c2 is always positive, so the following atan2 will always return a result in the correct canonical middle angle
// range [-pi/2, pi/2]
res[1] = numext::atan2(-coeff(i, k), c2);
Scalar s1 = numext::sin(res[0]);
Scalar c1 = numext::cos(res[0]);
res[2] = numext::atan2(s1 * coeff(k, i) - c1 * coeff(j, i), c1 * coeff(j, j) - s1 * coeff(k, j));
}
if (!odd)
{
if (!odd) {
res = -res;
}
@@ -126,19 +121,20 @@ MatrixBase<Derived>::canonicalEulerAngles(Index a0, Index a1, Index a2) const
}
/** \geometry_module \ingroup Geometry_Module
*
*
* \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2)
*
* NB: The returned angles are in non-canonical ranges [0:pi]x[-pi:pi]x[-pi:pi]. For canonical Tait-Bryan/proper Euler ranges, use canonicalEulerAngles.
*
* \sa MatrixBase::canonicalEulerAngles
* \sa class AngleAxis
*/
template<typename Derived>
EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
{
*
*
* \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a
* a2)
*
* NB: The returned angles are in non-canonical ranges [0:pi]x[-pi:pi]x[-pi:pi]. For canonical Tait-Bryan/proper Euler
* ranges, use canonicalEulerAngles.
*
* \sa MatrixBase::canonicalEulerAngles
* \sa class AngleAxis
*/
template <typename Derived>
EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>
MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const {
/* Implemented from Graphics Gems IV */
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
@@ -149,25 +145,18 @@ MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
const Index j = (a0 + 1 + odd) % 3;
const Index k = (a0 + 2 - odd) % 3;
if (a0 == a2)
{
if (a0 == a2) {
res[0] = numext::atan2(coeff(j, i), coeff(k, i));
if ((odd && res[0] < Scalar(0)) || ((!odd) && res[0] > Scalar(0)))
{
if (res[0] > Scalar(0))
{
if ((odd && res[0] < Scalar(0)) || ((!odd) && res[0] > Scalar(0))) {
if (res[0] > Scalar(0)) {
res[0] -= Scalar(EIGEN_PI);
}
else
{
} else {
res[0] += Scalar(EIGEN_PI);
}
Scalar s2 = numext::hypot(coeff(j, i), coeff(k, i));
res[1] = -numext::atan2(s2, coeff(i, i));
}
else
{
} else {
Scalar s2 = numext::hypot(coeff(j, i), coeff(k, i));
res[1] = numext::atan2(s2, coeff(i, i));
}
@@ -185,39 +174,30 @@ MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
Scalar s1 = numext::sin(res[0]);
Scalar c1 = numext::cos(res[0]);
res[2] = numext::atan2(c1 * coeff(j, k) - s1 * coeff(k, k), c1 * coeff(j, j) - s1 * coeff(k, j));
}
else
{
} else {
res[0] = numext::atan2(coeff(j, k), coeff(k, k));
Scalar c2 = numext::hypot(coeff(i, i), coeff(i, j));
if ((odd && res[0] < Scalar(0)) || ((!odd) && res[0] > Scalar(0)))
{
if (res[0] > Scalar(0))
{
if ((odd && res[0] < Scalar(0)) || ((!odd) && res[0] > Scalar(0))) {
if (res[0] > Scalar(0)) {
res[0] -= Scalar(EIGEN_PI);
}
else
{
} else {
res[0] += Scalar(EIGEN_PI);
}
res[1] = numext::atan2(-coeff(i, k), -c2);
}
else
{
} else {
res[1] = numext::atan2(-coeff(i, k), c2);
}
Scalar s1 = numext::sin(res[0]);
Scalar c1 = numext::cos(res[0]);
res[2] = numext::atan2(s1 * coeff(k, i) - c1 * coeff(j, i), c1 * coeff(j, j) - s1 * coeff(k, j));
}
if (!odd)
{
if (!odd) {
res = -res;
}
return res;
}
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_EULERANGLES_H
#endif // EIGEN_EULERANGLES_H

573
Eigen/src/Geometry/Homogeneous.h
View File

@@ -16,142 +16,132 @@
namespace Eigen {
/** \geometry_module \ingroup Geometry_Module
*
* \class Homogeneous
*
* \brief Expression of one (or a set of) homogeneous vector(s)
*
* \param MatrixType the type of the object in which we are making homogeneous
*
* This class represents an expression of one (or a set of) homogeneous vector(s).
* It is the return type of MatrixBase::homogeneous() and most of the time
* this is the only way it is used.
*
* \sa MatrixBase::homogeneous()
*/
*
* \class Homogeneous
*
* \brief Expression of one (or a set of) homogeneous vector(s)
*
* \param MatrixType the type of the object in which we are making homogeneous
*
* This class represents an expression of one (or a set of) homogeneous vector(s).
* It is the return type of MatrixBase::homogeneous() and most of the time
* this is the only way it is used.
*
* \sa MatrixBase::homogeneous()
*/
namespace internal {
template<typename MatrixType,int Direction>
struct traits<Homogeneous<MatrixType,Direction> >
: traits<MatrixType>
{
template <typename MatrixType, int Direction>
struct traits<Homogeneous<MatrixType, Direction> > : traits<MatrixType> {
typedef typename traits<MatrixType>::StorageKind StorageKind;
typedef typename ref_selector<MatrixType>::type MatrixTypeNested;
typedef std::remove_reference_t<MatrixTypeNested> 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,
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
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;
template <typename MatrixType, typename Lhs>
struct homogeneous_left_product_impl;
template <typename MatrixType, typename Rhs>
struct homogeneous_right_product_impl;
} // end namespace internal
} // end namespace internal
template<typename MatrixType,int Direction_> class Homogeneous
: public MatrixBase<Homogeneous<MatrixType,Direction_> >, internal::no_assignment_operator
{
public:
template <typename MatrixType, int Direction_>
class Homogeneous : public MatrixBase<Homogeneous<MatrixType, Direction_> >, internal::no_assignment_operator {
public:
typedef MatrixType NestedExpression;
enum { Direction = Direction_ };
typedef MatrixType NestedExpression;
enum { Direction = Direction_ };
typedef MatrixBase<Homogeneous> Base;
EIGEN_DENSE_PUBLIC_INTERFACE(Homogeneous)
typedef MatrixBase<Homogeneous> Base;
EIGEN_DENSE_PUBLIC_INTERFACE(Homogeneous)
EIGEN_DEVICE_FUNC explicit inline Homogeneous(const MatrixType& matrix) : m_matrix(matrix) {}
EIGEN_DEVICE_FUNC explicit inline Homogeneous(const MatrixType& matrix)
: m_matrix(matrix)
{}
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT {
return m_matrix.rows() + (int(Direction) == Vertical ? 1 : 0);
}
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT {
return m_matrix.cols() + (int(Direction) == Horizontal ? 1 : 0);
}
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows() + (int(Direction)==Vertical ? 1 : 0); }
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols() + (int(Direction)==Horizontal ? 1 : 0); }
EIGEN_DEVICE_FUNC const NestedExpression& nestedExpression() const { return m_matrix; }
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 {
eigen_assert(int(Direction) == Horizontal);
return Product<Homogeneous, Rhs>(*this, rhs.derived());
}
template<typename Rhs>
EIGEN_DEVICE_FUNC inline const Product<Homogeneous,Rhs>
operator* (const MatrixBase<Rhs>& rhs) const
{
eigen_assert(int(Direction)==Horizontal);
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) {
eigen_assert(int(Direction) == Vertical);
return Product<Lhs, Homogeneous>(lhs.derived(), rhs);
}
template<typename Lhs> friend
EIGEN_DEVICE_FUNC inline const Product<Lhs,Homogeneous>
operator* (const MatrixBase<Lhs>& lhs, const Homogeneous& rhs)
{
eigen_assert(int(Direction)==Vertical);
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 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));
}
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;
protected:
typename MatrixType::Nested m_matrix;
};
/** \geometry_module \ingroup Geometry_Module
*
* \returns a vector expression that is one longer than the vector argument, with the value 1 symbolically appended as the last coefficient.
*
* This can be used to convert affine coordinates to homogeneous coordinates.
*
* \only_for_vectors
*
* Example: \include MatrixBase_homogeneous.cpp
* Output: \verbinclude MatrixBase_homogeneous.out
*
* \sa VectorwiseOp::homogeneous(), class Homogeneous
*/
template<typename Derived>
EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::HomogeneousReturnType
MatrixBase<Derived>::homogeneous() const
{
*
* \returns a vector expression that is one longer than the vector argument, with the value 1 symbolically appended as
* the last coefficient.
*
* This can be used to convert affine coordinates to homogeneous coordinates.
*
* \only_for_vectors
*
* Example: \include MatrixBase_homogeneous.cpp
* Output: \verbinclude MatrixBase_homogeneous.out
*
* \sa VectorwiseOp::homogeneous(), class Homogeneous
*/
template <typename Derived>
EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::HomogeneousReturnType MatrixBase<Derived>::homogeneous() const {
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
return HomogeneousReturnType(derived());
}
/** \geometry_module \ingroup Geometry_Module
*
* \returns an expression where the value 1 is symbolically appended as the final coefficient to each column (or row) of the matrix.
*
* This can be used to convert affine coordinates to homogeneous coordinates.
*
* Example: \include VectorwiseOp_homogeneous.cpp
* Output: \verbinclude VectorwiseOp_homogeneous.out
*
* \sa MatrixBase::homogeneous(), class Homogeneous */
template<typename ExpressionType, int Direction>
EIGEN_DEVICE_FUNC inline Homogeneous<ExpressionType,Direction>
VectorwiseOp<ExpressionType,Direction>::homogeneous() const
{
*
* \returns an expression where the value 1 is symbolically appended as the final coefficient to each column (or row) of
* the matrix.
*
* This can be used to convert affine coordinates to homogeneous coordinates.
*
* Example: \include VectorwiseOp_homogeneous.cpp
* Output: \verbinclude VectorwiseOp_homogeneous.out
*
* \sa MatrixBase::homogeneous(), class Homogeneous */
template <typename ExpressionType, int Direction>
EIGEN_DEVICE_FUNC inline Homogeneous<ExpressionType, Direction> VectorwiseOp<ExpressionType, Direction>::homogeneous()
const {
return HomogeneousReturnType(_expression());
}
@@ -172,333 +162,294 @@ VectorwiseOp<ExpressionType,Direction>::homogeneous() const
* Output: \verbinclude MatrixBase_hnormalized.out
*
* \sa VectorwiseOp::hnormalized() */
template<typename Derived>
EIGEN_DEVICE_FUNC inline const typename MatrixBase<Derived>::HNormalizedReturnType
MatrixBase<Derived>::hnormalized() const
{
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);
return ConstStartMinusOne(derived(), 0, 0, ColsAtCompileTime == 1 ? size() - 1 : 1,
ColsAtCompileTime == 1 ? 1 : size() - 1) /
coeff(size() - 1);
}
/** \geometry_module \ingroup Geometry_Module
*
* \brief column or row-wise homogeneous normalization
*
* \returns an expression of the first N-1 coefficients of each column (or row) of \c *this divided by the last coefficient of each column (or row).
*
* This can be used to convert homogeneous coordinates to affine coordinates.
*
* It is conceptually equivalent to calling MatrixBase::hnormalized() to each column (or row) of \c *this.
*
* Example: \include DirectionWise_hnormalized.cpp
* Output: \verbinclude DirectionWise_hnormalized.out
*
* \sa MatrixBase::hnormalized() */
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));
*
* \brief column or row-wise homogeneous normalization
*
* \returns an expression of the first N-1 coefficients of each column (or row) of \c *this divided by the last
* coefficient of each column (or row).
*
* This can be used to convert homogeneous coordinates to affine coordinates.
*
* It is conceptually equivalent to calling MatrixBase::hnormalized() to each column (or row) of \c *this.
*
* Example: \include DirectionWise_hnormalized.cpp
* Output: \verbinclude DirectionWise_hnormalized.out
*
* \sa MatrixBase::hnormalized() */
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
{
template <typename MatrixOrTransformType>
struct take_matrix_for_product {
typedef MatrixOrTransformType type;
EIGEN_DEVICE_FUNC static const type& run(const type &x) { return x; }
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> >
{
template <typename Scalar, int Dim, int Mode, int Options>
struct take_matrix_for_product<Transform<Scalar, Dim, Mode, Options> > {
typedef Transform<Scalar, Dim, Mode, Options> TransformType;
typedef std::add_const_t<typename TransformType::ConstAffinePart> type;
EIGEN_DEVICE_FUNC static type run (const TransformType& x) { return x.affine(); }
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> >
{
template <typename Scalar, int Dim, int Options>
struct take_matrix_for_product<Transform<Scalar, Dim, Projective, Options> > {
typedef Transform<Scalar, Dim, Projective, Options> TransformType;
typedef typename TransformType::MatrixType type;
EIGEN_DEVICE_FUNC static const type& run (const TransformType& x) { return x.matrix(); }
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> >
{
template <typename MatrixType, typename Lhs>
struct traits<homogeneous_left_product_impl<Homogeneous<MatrixType, Vertical>, Lhs> > {
typedef typename take_matrix_for_product<Lhs>::type LhsMatrixType;
typedef remove_all_t<MatrixType> MatrixTypeCleaned;
typedef remove_all_t<LhsMatrixType> LhsMatrixTypeCleaned;
typedef typename make_proper_matrix_type<
typename traits<MatrixTypeCleaned>::Scalar,
LhsMatrixTypeCleaned::RowsAtCompileTime,
MatrixTypeCleaned::ColsAtCompileTime,
MatrixTypeCleaned::PlainObject::Options,
LhsMatrixTypeCleaned::MaxRowsAtCompileTime,
MatrixTypeCleaned::MaxColsAtCompileTime>::type ReturnType;
typename traits<MatrixTypeCleaned>::Scalar, LhsMatrixTypeCleaned::RowsAtCompileTime,
MatrixTypeCleaned::ColsAtCompileTime, MatrixTypeCleaned::PlainObject::Options,
LhsMatrixTypeCleaned::MaxRowsAtCompileTime, MatrixTypeCleaned::MaxColsAtCompileTime>::type ReturnType;
};
template<typename MatrixType,typename Lhs>
struct homogeneous_left_product_impl<Homogeneous<MatrixType,Vertical>,Lhs>
: public ReturnByValue<homogeneous_left_product_impl<Homogeneous<MatrixType,Vertical>,Lhs> >
{
template <typename MatrixType, typename Lhs>
struct homogeneous_left_product_impl<Homogeneous<MatrixType, Vertical>, Lhs>
: public ReturnByValue<homogeneous_left_product_impl<Homogeneous<MatrixType, Vertical>, Lhs> > {
typedef typename traits<homogeneous_left_product_impl>::LhsMatrixType LhsMatrixType;
typedef remove_all_t<LhsMatrixType> LhsMatrixTypeCleaned;
typedef remove_all_t<typename LhsMatrixTypeCleaned::Nested> LhsMatrixTypeNested;
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)
{}
: m_lhs(take_matrix_for_product<Lhs>::run(lhs)), m_rhs(rhs) {}
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
inline Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); }
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
inline Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); }
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
template<typename Dest> EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const
{
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());
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> >
{
typedef typename make_proper_matrix_type<typename traits<MatrixType>::Scalar,
MatrixType::RowsAtCompileTime,
Rhs::ColsAtCompileTime,
MatrixType::PlainObject::Options,
MatrixType::MaxRowsAtCompileTime,
Rhs::MaxColsAtCompileTime>::type ReturnType;
template <typename MatrixType, typename Rhs>
struct traits<homogeneous_right_product_impl<Homogeneous<MatrixType, Horizontal>, Rhs> > {
typedef
typename make_proper_matrix_type<typename traits<MatrixType>::Scalar, MatrixType::RowsAtCompileTime,
Rhs::ColsAtCompileTime, MatrixType::PlainObject::Options,
MatrixType::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime>::type ReturnType;
};
template<typename MatrixType,typename Rhs>
struct homogeneous_right_product_impl<Homogeneous<MatrixType,Horizontal>,Rhs>
: public ReturnByValue<homogeneous_right_product_impl<Homogeneous<MatrixType,Horizontal>,Rhs> >
{
template <typename MatrixType, typename Rhs>
struct homogeneous_right_product_impl<Homogeneous<MatrixType, Horizontal>, Rhs>
: public ReturnByValue<homogeneous_right_product_impl<Homogeneous<MatrixType, Horizontal>, Rhs> > {
typedef remove_all_t<typename Rhs::Nested> RhsNested;
EIGEN_DEVICE_FUNC homogeneous_right_product_impl(const MatrixType& lhs, const Rhs& rhs)
: m_lhs(lhs), m_rhs(rhs)
{}
EIGEN_DEVICE_FUNC homogeneous_right_product_impl(const MatrixType& lhs, const Rhs& rhs) : m_lhs(lhs), m_rhs(rhs) {}
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); }
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
template<typename Dest> EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const
{
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());
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> >
{
template <typename ArgType, int Direction>
struct evaluator_traits<Homogeneous<ArgType, Direction> > {
typedef typename storage_kind_to_evaluator_kind<typename ArgType::StorageKind>::Kind Kind;
typedef HomogeneousShape Shape;
};
template<> struct AssignmentKind<DenseShape,HomogeneousShape> { typedef Dense2Dense Kind; };
template <>
struct AssignmentKind<DenseShape, HomogeneousShape> {
typedef Dense2Dense Kind;
};
template<typename ArgType,int Direction>
struct unary_evaluator<Homogeneous<ArgType,Direction>, IndexBased>
: evaluator<typename Homogeneous<ArgType,Direction>::PlainObject >
{
typedef Homogeneous<ArgType,Direction> XprType;
template <typename ArgType, int Direction>
struct unary_evaluator<Homogeneous<ArgType, Direction>, IndexBased>
: evaluator<typename Homogeneous<ArgType, Direction>::PlainObject> {
typedef Homogeneous<ArgType, Direction> XprType;
typedef typename XprType::PlainObject PlainObject;
typedef evaluator<PlainObject> Base;
EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& op)
: Base(), m_temp(op)
{
EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& op) : Base(), m_temp(op) {
internal::construct_at<Base>(this, m_temp);
}
protected:
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>
{
typedef Homogeneous<ArgType,Vertical> SrcXprType;
EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,typename ArgType::Scalar> &)
{
template <typename DstXprType, typename ArgType, typename Scalar>
struct Assignment<DstXprType, Homogeneous<ArgType, Vertical>, internal::assign_op<Scalar, typename ArgType::Scalar>,
Dense2Dense> {
typedef Homogeneous<ArgType, Vertical> SrcXprType;
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);
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();
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>
{
typedef Homogeneous<ArgType,Horizontal> SrcXprType;
EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,typename ArgType::Scalar> &)
{
template <typename DstXprType, typename ArgType, typename Scalar>
struct Assignment<DstXprType, Homogeneous<ArgType, Horizontal>, internal::assign_op<Scalar, typename ArgType::Scalar>,
Dense2Dense> {
typedef Homogeneous<ArgType, Horizontal> SrcXprType;
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);
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();
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 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
};
typedef typename Rhs::template ConstNRowsBlockXpr<Dim>::Type LinearBlockConst;
typedef std::remove_const_t<LinearBlockConst> LinearBlock;
typedef typename Rhs::ConstRowXpr ConstantColumn;
typedef Replicate<const ConstantColumn,Rows,1> ConstantBlock;
typedef Product<Lhs,LinearBlock,LazyProduct> LinearProduct;
typedef CwiseBinaryOp<internal::scalar_sum_op<typename Lhs::Scalar,typename Rhs::Scalar>, const LinearProduct, const ConstantBlock> Xpr;
template <typename Lhs, typename Rhs>
struct homogeneous_right_product_refactoring_helper {
enum { Dim = Lhs::ColsAtCompileTime, Rows = Lhs::RowsAtCompileTime };
typedef typename Rhs::template ConstNRowsBlockXpr<Dim>::Type LinearBlockConst;
typedef std::remove_const_t<LinearBlockConst> LinearBlock;
typedef typename Rhs::ConstRowXpr ConstantColumn;
typedef Replicate<const ConstantColumn, Rows, 1> ConstantBlock;
typedef Product<Lhs, LinearBlock, LazyProduct> LinearProduct;
typedef CwiseBinaryOp<internal::scalar_sum_op<typename Lhs::Scalar, typename Rhs::Scalar>, const LinearProduct,
const ConstantBlock>
Xpr;
};
template<typename Lhs, typename Rhs, int ProductTag>
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>
{
: public evaluator<
typename homogeneous_right_product_refactoring_helper<typename Lhs::NestedExpression, Rhs>::Xpr> {
typedef Product<Lhs, Rhs, LazyProduct> XprType;
typedef homogeneous_right_product_refactoring_helper<typename Lhs::NestedExpression,Rhs> helper;
typedef homogeneous_right_product_refactoring_helper<typename Lhs::NestedExpression, Rhs> helper;
typedef typename helper::ConstantBlock ConstantBlock;
typedef typename helper::Xpr RefactoredXpr;
typedef evaluator<RefactoredXpr> Base;
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) )
{}
: 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>, 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);
}
};
// TODO: the following specialization is to address a regression from 3.2 to 3.3
// In the future, this path should be optimized.
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)
{
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
};
typedef typename Lhs::template ConstNColsBlockXpr<Dim>::Type LinearBlockConst;
typedef std::remove_const_t<LinearBlockConst> LinearBlock;
typedef typename Lhs::ConstColXpr ConstantColumn;
typedef Replicate<const ConstantColumn,1,Cols> ConstantBlock;
typedef Product<LinearBlock,Rhs,LazyProduct> LinearProduct;
typedef CwiseBinaryOp<internal::scalar_sum_op<typename Lhs::Scalar,typename Rhs::Scalar>, const LinearProduct, const ConstantBlock> Xpr;
template <typename Lhs, typename Rhs>
struct homogeneous_left_product_refactoring_helper {
enum { Dim = Rhs::RowsAtCompileTime, Cols = Rhs::ColsAtCompileTime };
typedef typename Lhs::template ConstNColsBlockXpr<Dim>::Type LinearBlockConst;
typedef std::remove_const_t<LinearBlockConst> LinearBlock;
typedef typename Lhs::ConstColXpr ConstantColumn;
typedef Replicate<const ConstantColumn, 1, Cols> ConstantBlock;
typedef Product<LinearBlock, Rhs, LazyProduct> LinearProduct;
typedef CwiseBinaryOp<internal::scalar_sum_op<typename Lhs::Scalar, typename Rhs::Scalar>, const LinearProduct,
const ConstantBlock>
Xpr;
};
template<typename Lhs, typename Rhs, int ProductTag>
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>
{
: public evaluator<typename homogeneous_left_product_refactoring_helper<Lhs, typename Rhs::NestedExpression>::Xpr> {
typedef Product<Lhs, Rhs, LazyProduct> XprType;
typedef homogeneous_left_product_refactoring_helper<Lhs,typename Rhs::NestedExpression> helper;
typedef homogeneous_left_product_refactoring_helper<Lhs, typename Rhs::NestedExpression> helper;
typedef typename helper::ConstantBlock ConstantBlock;
typedef typename helper::Xpr RefactoredXpr;
typedef evaluator<RefactoredXpr> Base;
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()) )
{}
: 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>
{
typedef Transform<Scalar,Dim,Mode,Options> TransformType;
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 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> {
typedef Transform<Scalar, Dim, Mode, Options> TransformType;
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>
template <typename ExpressionType, int Side, bool Transposed>
struct permutation_matrix_product<ExpressionType, Side, Transposed, HomogeneousShape>
: public permutation_matrix_product<ExpressionType, Side, Transposed, DenseShape>
{};
: public permutation_matrix_product<ExpressionType, Side, Transposed, DenseShape> {};
} // end namespace internal
} // end namespace internal
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_HOMOGENEOUS_H
#endif // EIGEN_HOMOGENEOUS_H

306
Eigen/src/Geometry/Hyperplane.h
View File

@@ -14,82 +14,73 @@
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
namespace Eigen {
namespace Eigen {
/** \geometry_module \ingroup Geometry_Module
*
* \class Hyperplane
*
* \brief A hyperplane
*
* A hyperplane is an affine subspace of dimension n-1 in a space of dimension n.
* For example, a hyperplane in a plane is a line; a hyperplane in 3-space is a plane.
*
* \tparam Scalar_ the scalar type, i.e., the type of the coefficients
* \tparam AmbientDim_ the dimension of the ambient space, can be a compile time value or Dynamic.
* Notice that the dimension of the hyperplane is AmbientDim_-1.
*
* This class represents an hyperplane as the zero set of the implicit equation
* \f$ n \cdot x + d = 0 \f$ where \f$ n \f$ is a unit normal vector of the plane (linear part)
* and \f$ d \f$ is the distance (offset) to the origin.
*/
*
* \class Hyperplane
*
* \brief A hyperplane
*
* A hyperplane is an affine subspace of dimension n-1 in a space of dimension n.
* For example, a hyperplane in a plane is a line; a hyperplane in 3-space is a plane.
*
* \tparam Scalar_ the scalar type, i.e., the type of the coefficients
* \tparam AmbientDim_ the dimension of the ambient space, can be a compile time value or Dynamic.
* Notice that the dimension of the hyperplane is AmbientDim_-1.
*
* This class represents an hyperplane as the zero set of the implicit equation
* \f$ n \cdot x + d = 0 \f$ where \f$ n \f$ is a unit normal vector of the plane (linear part)
* and \f$ d \f$ is the distance (offset) to the origin.
*/
template <typename Scalar_, int AmbientDim_, int Options_>
class Hyperplane
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,AmbientDim_==Dynamic ? Dynamic : AmbientDim_+1)
enum {
AmbientDimAtCompileTime = AmbientDim_,
Options = Options_
};
class Hyperplane {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,
AmbientDim_ == Dynamic ? Dynamic : AmbientDim_ + 1)
enum { AmbientDimAtCompileTime = AmbientDim_, Options = Options_ };
typedef Scalar_ Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
typedef Matrix<Scalar,Index(AmbientDimAtCompileTime)==Dynamic
? Dynamic
: Index(AmbientDimAtCompileTime)+1,1,Options> Coefficients;
typedef Block<Coefficients,AmbientDimAtCompileTime,1> NormalReturnType;
typedef const Block<const Coefficients,AmbientDimAtCompileTime,1> ConstNormalReturnType;
typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
typedef Matrix<Scalar, AmbientDimAtCompileTime, 1> VectorType;
typedef Matrix<Scalar, Index(AmbientDimAtCompileTime) == Dynamic ? Dynamic : Index(AmbientDimAtCompileTime) + 1, 1,
Options>
Coefficients;
typedef Block<Coefficients, AmbientDimAtCompileTime, 1> NormalReturnType;
typedef const Block<const Coefficients, AmbientDimAtCompileTime, 1> ConstNormalReturnType;
/** Default constructor without initialization */
EIGEN_DEVICE_FUNC inline Hyperplane() {}
template<int OtherOptions>
EIGEN_DEVICE_FUNC Hyperplane(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other)
: m_coeffs(other.coeffs())
{}
template <int OtherOptions>
EIGEN_DEVICE_FUNC Hyperplane(const Hyperplane<Scalar, AmbientDimAtCompileTime, OtherOptions>& other)
: m_coeffs(other.coeffs()) {}
/** Constructs a dynamic-size hyperplane with \a _dim the dimension
* of the ambient space */
EIGEN_DEVICE_FUNC inline explicit Hyperplane(Index _dim) : m_coeffs(_dim+1) {}
* of the ambient space */
EIGEN_DEVICE_FUNC inline explicit Hyperplane(Index _dim) : m_coeffs(_dim + 1) {}
/** Construct a plane from its normal \a n and a point \a e onto the plane.
* \warning the vector normal is assumed to be normalized.
*/
EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const VectorType& e)
: m_coeffs(n.size()+1)
{
* \warning the vector normal is assumed to be normalized.
*/
EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const VectorType& e) : m_coeffs(n.size() + 1) {
normal() = n;
offset() = -n.dot(e);
}
/** Constructs a plane from its normal \a n and distance to the origin \a d
* such that the algebraic equation of the plane is \f$ n \cdot x + d = 0 \f$.
* \warning the vector normal is assumed to be normalized.
*/
EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const Scalar& d)
: m_coeffs(n.size()+1)
{
* such that the algebraic equation of the plane is \f$ n \cdot x + d = 0 \f$.
* \warning the vector normal is assumed to be normalized.
*/
EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const Scalar& d) : m_coeffs(n.size() + 1) {
normal() = n;
offset() = d;
}
/** Constructs a hyperplane passing through the two points. If the dimension of the ambient space
* is greater than 2, then there isn't uniqueness, so an arbitrary choice is made.
*/
EIGEN_DEVICE_FUNC static inline Hyperplane Through(const VectorType& p0, const VectorType& p1)
{
* is greater than 2, then there isn't uniqueness, so an arbitrary choice is made.
*/
EIGEN_DEVICE_FUNC static inline Hyperplane Through(const VectorType& p0, const VectorType& p1) {
Hyperplane result(p0.size());
result.normal() = (p1 - p0).unitOrthogonal();
result.offset() = -p0.dot(result.normal());
@@ -97,34 +88,31 @@ public:
}
/** Constructs a hyperplane passing through the three points. The dimension of the ambient space
* is required to be exactly 3.
*/
EIGEN_DEVICE_FUNC static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2)
{
* is required to be exactly 3.
*/
EIGEN_DEVICE_FUNC static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3)
Hyperplane result(p0.size());
VectorType v0(p2 - p0), v1(p1 - p0);
result.normal() = v0.cross(v1);
RealScalar norm = result.normal().norm();
if(norm <= v0.norm() * v1.norm() * NumTraits<RealScalar>::epsilon())
{
Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
JacobiSVD<Matrix<Scalar,2,3>, ComputeFullV> svd(m);
if (norm <= v0.norm() * v1.norm() * NumTraits<RealScalar>::epsilon()) {
Matrix<Scalar, 2, 3> m;
m << v0.transpose(), v1.transpose();
JacobiSVD<Matrix<Scalar, 2, 3>, ComputeFullV> svd(m);
result.normal() = svd.matrixV().col(2);
}
else
} else
result.normal() /= norm;
result.offset() = -p0.dot(result.normal());
return result;
}
/** Constructs a hyperplane passing through the parametrized line \a parametrized.
* If the dimension of the ambient space is greater than 2, then there isn't uniqueness,
* so an arbitrary choice is made.
*/
* If the dimension of the ambient space is greater than 2, then there isn't uniqueness,
* so an arbitrary choice is made.
*/
// FIXME to be consistent with the rest this could be implemented as a static Through function ??
EIGEN_DEVICE_FUNC explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized)
{
EIGEN_DEVICE_FUNC explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized) {
normal() = parametrized.direction().unitOrthogonal();
offset() = -parametrized.origin().dot(normal());
}
@@ -132,154 +120,154 @@ public:
EIGEN_DEVICE_FUNC ~Hyperplane() {}
/** \returns the dimension in which the plane holds */
EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : Index(AmbientDimAtCompileTime); }
/** normalizes \c *this */
EIGEN_DEVICE_FUNC void normalize(void)
{
m_coeffs /= normal().norm();
EIGEN_DEVICE_FUNC inline Index dim() const {
return AmbientDimAtCompileTime == Dynamic ? m_coeffs.size() - 1 : Index(AmbientDimAtCompileTime);
}
/** normalizes \c *this */
EIGEN_DEVICE_FUNC void normalize(void) { m_coeffs /= normal().norm(); }
/** \returns the signed distance between the plane \c *this and a point \a p.
* \sa absDistance()
*/
* \sa absDistance()
*/
EIGEN_DEVICE_FUNC inline Scalar signedDistance(const VectorType& p) const { return normal().dot(p) + offset(); }
/** \returns the absolute distance between the plane \c *this and a point \a p.
* \sa signedDistance()
*/
* \sa signedDistance()
*/
EIGEN_DEVICE_FUNC inline Scalar absDistance(const VectorType& p) const { return numext::abs(signedDistance(p)); }
/** \returns the projection of a point \a p onto the plane \c *this.
*/
*/
EIGEN_DEVICE_FUNC inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); }
/** \returns a constant reference to the unit normal vector of the plane, which corresponds
* to the linear part of the implicit equation.
*/
EIGEN_DEVICE_FUNC inline ConstNormalReturnType normal() const { return ConstNormalReturnType(m_coeffs,0,0,dim(),1); }
* to the linear part of the implicit equation.
*/
EIGEN_DEVICE_FUNC inline ConstNormalReturnType normal() const {
return ConstNormalReturnType(m_coeffs, 0, 0, dim(), 1);
}
/** \returns a non-constant reference to the unit normal vector of the plane, which corresponds
* to the linear part of the implicit equation.
*/
EIGEN_DEVICE_FUNC inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); }
* to the linear part of the implicit equation.
*/
EIGEN_DEVICE_FUNC inline NormalReturnType normal() { return NormalReturnType(m_coeffs, 0, 0, dim(), 1); }
/** \returns the distance to the origin, which is also the "constant term" of the implicit equation
* \warning the vector normal is assumed to be normalized.
*/
* \warning the vector normal is assumed to be normalized.
*/
EIGEN_DEVICE_FUNC inline const Scalar& offset() const { return m_coeffs.coeff(dim()); }
/** \returns a non-constant reference to the distance to the origin, which is also the constant part
* of the implicit equation */
* of the implicit equation */
EIGEN_DEVICE_FUNC inline Scalar& offset() { return m_coeffs(dim()); }
/** \returns a constant reference to the coefficients c_i of the plane equation:
* \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$
*/
* \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$
*/
EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; }
/** \returns a non-constant reference to the coefficients c_i of the plane equation:
* \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$
*/
* \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$
*/
EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; }
/** \returns the intersection of *this with \a other.
*
* \warning The ambient space must be a plane, i.e. have dimension 2, so that \c *this and \a other are lines.
*
* \note If \a other is approximately parallel to *this, this method will return any point on *this.
*/
EIGEN_DEVICE_FUNC VectorType intersection(const Hyperplane& other) const
{
*
* \warning The ambient space must be a plane, i.e. have dimension 2, so that \c *this and \a other are lines.
*
* \note If \a other is approximately parallel to *this, this method will return any point on *this.
*/
EIGEN_DEVICE_FUNC VectorType intersection(const Hyperplane& other) const {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0);
// since the line equations ax+by=c are normalized with a^2+b^2=1, the following tests
// whether the two lines are approximately parallel.
if(internal::isMuchSmallerThan(det, Scalar(1)))
{ // special case where the two lines are approximately parallel. Pick any point on the first line.
if(numext::abs(coeffs().coeff(1))>numext::abs(coeffs().coeff(0)))
return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0));
else
return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0));
}
else
{ // general case
Scalar invdet = Scalar(1) / det;
return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)),
invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2)));
if (internal::isMuchSmallerThan(det, Scalar(1))) { // special case where the two lines are approximately parallel.
// Pick any point on the first line.
if (numext::abs(coeffs().coeff(1)) > numext::abs(coeffs().coeff(0)))
return VectorType(coeffs().coeff(1), -coeffs().coeff(2) / coeffs().coeff(1) - coeffs().coeff(0));
else
return VectorType(-coeffs().coeff(2) / coeffs().coeff(0) - coeffs().coeff(1), coeffs().coeff(0));
} else { // general case
Scalar invdet = Scalar(1) / det;
return VectorType(
invdet * (coeffs().coeff(1) * other.coeffs().coeff(2) - other.coeffs().coeff(1) * coeffs().coeff(2)),
invdet * (other.coeffs().coeff(0) * coeffs().coeff(2) - coeffs().coeff(0) * other.coeffs().coeff(2)));
}
}
/** Applies the transformation matrix \a mat to \c *this and returns a reference to \c *this.
*
* \param mat the Dim x Dim transformation matrix
* \param traits specifies whether the matrix \a mat represents an #Isometry
* or a more generic #Affine transformation. The default is #Affine.
*/
template<typename XprType>
EIGEN_DEVICE_FUNC inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine)
{
if (traits==Affine)
{
*
* \param mat the Dim x Dim transformation matrix
* \param traits specifies whether the matrix \a mat represents an #Isometry
* or a more generic #Affine transformation. The default is #Affine.
*/
template <typename XprType>
EIGEN_DEVICE_FUNC inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine) {
if (traits == Affine) {
normal() = mat.inverse().transpose() * normal();
m_coeffs /= normal().norm();
}
else if (traits==Isometry)
} else if (traits == Isometry)
normal() = mat * normal();
else
{
else {
eigen_assert(0 && "invalid traits value in Hyperplane::transform()");
}
return *this;
}
/** Applies the transformation \a t to \c *this and returns a reference to \c *this.
*
* \param t the transformation of dimension Dim
* \param traits specifies whether the transformation \a t represents an #Isometry
* or a more generic #Affine transformation. The default is #Affine.
* Other kind of transformations are not supported.
*/
template<int TrOptions>
EIGEN_DEVICE_FUNC inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime,Affine,TrOptions>& t,
TransformTraits traits = Affine)
{
*
* \param t the transformation of dimension Dim
* \param traits specifies whether the transformation \a t represents an #Isometry
* or a more generic #Affine transformation. The default is #Affine.
* Other kind of transformations are not supported.
*/
template <int TrOptions>
EIGEN_DEVICE_FUNC inline Hyperplane& transform(const Transform<Scalar, AmbientDimAtCompileTime, Affine, TrOptions>& t,
TransformTraits traits = Affine) {
transform(t.linear(), traits);
offset() -= normal().dot(t.translation());
return *this;
}
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Hyperplane,
Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type cast() const
{
return typename internal::cast_return_type<Hyperplane,
Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type(*this);
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template <typename NewScalarType>
EIGEN_DEVICE_FUNC inline
typename internal::cast_return_type<Hyperplane,
Hyperplane<NewScalarType, AmbientDimAtCompileTime, Options> >::type
cast() const {
return
typename internal::cast_return_type<Hyperplane,
Hyperplane<NewScalarType, AmbientDimAtCompileTime, Options> >::type(*this);
}
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType,int OtherOptions>
EIGEN_DEVICE_FUNC inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime,OtherOptions>& other)
{ m_coeffs = other.coeffs().template cast<Scalar>(); }
template <typename OtherScalarType, int OtherOptions>
EIGEN_DEVICE_FUNC inline explicit Hyperplane(
const Hyperplane<OtherScalarType, AmbientDimAtCompileTime, OtherOptions>& other) {
m_coeffs = other.coeffs().template cast<Scalar>();
}
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
template<int OtherOptions>
EIGEN_DEVICE_FUNC bool isApprox(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
{ return m_coeffs.isApprox(other.m_coeffs, prec); }
protected:
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
template <int OtherOptions>
EIGEN_DEVICE_FUNC bool isApprox(
const Hyperplane<Scalar, AmbientDimAtCompileTime, OtherOptions>& other,
const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const {
return m_coeffs.isApprox(other.m_coeffs, prec);
}
protected:
Coefficients m_coeffs;
};
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_HYPERPLANE_H
#endif // EIGEN_HYPERPLANE_H

307
Eigen/src/Geometry/OrthoMethods.h
View File

@@ -14,167 +14,157 @@
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
namespace Eigen {
namespace Eigen {
namespace internal {
// Vector3 version (default)
template<typename Derived, typename OtherDerived, int Size>
struct cross_impl
{
typedef typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType Scalar;
typedef Matrix<Scalar,MatrixBase<Derived>::RowsAtCompileTime,MatrixBase<Derived>::ColsAtCompileTime> return_type;
template <typename Derived, typename OtherDerived, int Size>
struct cross_impl {
typedef typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,
typename internal::traits<OtherDerived>::Scalar>::ReturnType Scalar;
typedef Matrix<Scalar, MatrixBase<Derived>::RowsAtCompileTime, MatrixBase<Derived>::ColsAtCompileTime> return_type;
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
return_type run(const MatrixBase<Derived>& first, const MatrixBase<OtherDerived>& second)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,3)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3)
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE return_type run(const MatrixBase<Derived>& first,
const MatrixBase<OtherDerived>& second) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, 3)
// Note that there is no need for an expression here since the compiler
// optimize such a small temporary very well (even within a complex expression)
typename internal::nested_eval<Derived,2>::type lhs(first.derived());
typename internal::nested_eval<OtherDerived,2>::type rhs(second.derived());
return return_type(
numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0))
);
typename internal::nested_eval<Derived, 2>::type lhs(first.derived());
typename internal::nested_eval<OtherDerived, 2>::type rhs(second.derived());
return return_type(numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)));
}
};
// Vector2 version
template<typename Derived, typename OtherDerived>
struct cross_impl<Derived, OtherDerived, 2>
{
typedef typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType Scalar;
template <typename Derived, typename OtherDerived>
struct cross_impl<Derived, OtherDerived, 2> {
typedef typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,
typename internal::traits<OtherDerived>::Scalar>::ReturnType Scalar;
typedef Scalar return_type;
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
return_type run(const MatrixBase<Derived>& first, const MatrixBase<OtherDerived>& second)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,2);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,2);
typename internal::nested_eval<Derived,2>::type lhs(first.derived());
typename internal::nested_eval<OtherDerived,2>::type rhs(second.derived());
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE return_type run(const MatrixBase<Derived>& first,
const MatrixBase<OtherDerived>& second) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 2);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, 2);
typename internal::nested_eval<Derived, 2>::type lhs(first.derived());
typename internal::nested_eval<OtherDerived, 2>::type rhs(second.derived());
return numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0));
}
};
} // end namespace internal
} // end namespace internal
/** \geometry_module \ingroup Geometry_Module
*
* \returns the cross product of \c *this and \a other. This is either a scalar for size-2 vectors or a size-3 vector for size-3 vectors.
*
* This method is implemented for two different cases: between vectors of fixed size 2 and between vectors of fixed size 3.
*
* For vectors of size 3, the output is simply the traditional cross product.
*
* For vectors of size 2, the output is a scalar.
* Given vectors \f$ v = \begin{bmatrix} v_1 & v_2 \end{bmatrix} \f$ and \f$ w = \begin{bmatrix} w_1 & w_2 \end{bmatrix} \f$,
* the result is simply \f$ v\times w = \overline{v_1 w_2 - v_2 w_1} = \text{conj}\left|\begin{smallmatrix} v_1 & w_1 \\ v_2 & w_2 \end{smallmatrix}\right| \f$;
* or, to put it differently, it is the third coordinate of the cross product of \f$ \begin{bmatrix} v_1 & v_2 & v_3 \end{bmatrix} \f$ and \f$ \begin{bmatrix} w_1 & w_2 & w_3 \end{bmatrix} \f$.
* For real-valued inputs, the result can be interpreted as the signed area of a parallelogram spanned by the two vectors.
*
* \note With complex numbers, the cross product is implemented as
* \f$ (\mathbf{a}+i\mathbf{b}) \times (\mathbf{c}+i\mathbf{d}) = (\mathbf{a} \times \mathbf{c} - \mathbf{b} \times \mathbf{d}) - i(\mathbf{a} \times \mathbf{d} + \mathbf{b} \times \mathbf{c})\f$
*
* \sa MatrixBase::cross3()
*/
template<typename Derived>
template<typename OtherDerived>
*
* \returns the cross product of \c *this and \a other. This is either a scalar for size-2 vectors or a size-3 vector
* for size-3 vectors.
*
* This method is implemented for two different cases: between vectors of fixed size 2 and between vectors of fixed
* size 3.
*
* For vectors of size 3, the output is simply the traditional cross product.
*
* For vectors of size 2, the output is a scalar.
* Given vectors \f$ v = \begin{bmatrix} v_1 & v_2 \end{bmatrix} \f$ and \f$ w = \begin{bmatrix} w_1 & w_2 \end{bmatrix}
* \f$, the result is simply \f$ v\times w = \overline{v_1 w_2 - v_2 w_1} = \text{conj}\left|\begin{smallmatrix} v_1 &
* w_1 \\ v_2 & w_2 \end{smallmatrix}\right| \f$; or, to put it differently, it is the third coordinate of the cross
* product of \f$ \begin{bmatrix} v_1 & v_2 & v_3 \end{bmatrix} \f$ and \f$ \begin{bmatrix} w_1 & w_2 & w_3
* \end{bmatrix} \f$. For real-valued inputs, the result can be interpreted as the signed area of a parallelogram
* spanned by the two vectors.
*
* \note With complex numbers, the cross product is implemented as
* \f$ (\mathbf{a}+i\mathbf{b}) \times (\mathbf{c}+i\mathbf{d}) = (\mathbf{a} \times \mathbf{c} - \mathbf{b} \times
* \mathbf{d}) - i(\mathbf{a} \times \mathbf{d} + \mathbf{b} \times \mathbf{c})\f$
*
* \sa MatrixBase::cross3()
*/
template <typename Derived>
template <typename OtherDerived>
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
#ifndef EIGEN_PARSED_BY_DOXYGEN
typename internal::cross_impl<Derived, OtherDerived>::return_type
typename internal::cross_impl<Derived, OtherDerived>::return_type
#else
inline std::conditional_t<SizeAtCompileTime==2, Scalar, PlainObject>
inline std::conditional_t<SizeAtCompileTime == 2, Scalar, PlainObject>
#endif
MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const
{
MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const {
return internal::cross_impl<Derived, OtherDerived>::run(*this, other);
}
namespace internal {
template< int Arch,typename VectorLhs,typename VectorRhs,
typename Scalar = typename VectorLhs::Scalar,
bool Vectorizable = bool((VectorLhs::Flags&VectorRhs::Flags)&PacketAccessBit)>
template <int Arch, typename VectorLhs, typename VectorRhs, typename Scalar = typename VectorLhs::Scalar,
bool Vectorizable = bool((VectorLhs::Flags & VectorRhs::Flags) & PacketAccessBit)>
struct cross3_impl {
EIGEN_DEVICE_FUNC static inline typename internal::plain_matrix_type<VectorLhs>::type
run(const VectorLhs& lhs, const VectorRhs& rhs)
{
EIGEN_DEVICE_FUNC static inline typename internal::plain_matrix_type<VectorLhs>::type run(const VectorLhs& lhs,
const VectorRhs& rhs) {
return typename internal::plain_matrix_type<VectorLhs>::type(
numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)),
0
);
numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)), 0);
}
};
}
} // namespace internal
/** \geometry_module \ingroup Geometry_Module
*
* \returns the cross product of \c *this and \a other using only the x, y, and z coefficients
*
* The size of \c *this and \a other must be four. This function is especially useful
* when using 4D vectors instead of 3D ones to get advantage of SSE/AltiVec vectorization.
*
* \sa MatrixBase::cross()
*/
template<typename Derived>
template<typename OtherDerived>
EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::PlainObject
MatrixBase<Derived>::cross3(const MatrixBase<OtherDerived>& other) const
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,4)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,4)
*
* \returns the cross product of \c *this and \a other using only the x, y, and z coefficients
*
* The size of \c *this and \a other must be four. This function is especially useful
* when using 4D vectors instead of 3D ones to get advantage of SSE/AltiVec vectorization.
*
* \sa MatrixBase::cross()
*/
template <typename Derived>
template <typename OtherDerived>
EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::PlainObject MatrixBase<Derived>::cross3(
const MatrixBase<OtherDerived>& other) const {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 4)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, 4)
typedef typename internal::nested_eval<Derived,2>::type DerivedNested;
typedef typename internal::nested_eval<OtherDerived,2>::type OtherDerivedNested;
typedef typename internal::nested_eval<Derived, 2>::type DerivedNested;
typedef typename internal::nested_eval<OtherDerived, 2>::type OtherDerivedNested;
DerivedNested lhs(derived());
OtherDerivedNested rhs(other.derived());
return internal::cross3_impl<Architecture::Target,
internal::remove_all_t<DerivedNested>,
internal::remove_all_t<OtherDerivedNested>>::run(lhs,rhs);
return internal::cross3_impl<Architecture::Target, internal::remove_all_t<DerivedNested>,
internal::remove_all_t<OtherDerivedNested>>::run(lhs, rhs);
}
/** \geometry_module \ingroup Geometry_Module
*
* \returns a matrix expression of the cross product of each column or row
* of the referenced expression with the \a other vector.
*
* The referenced matrix must have one dimension equal to 3.
* The result matrix has the same dimensions than the referenced one.
*
* \sa MatrixBase::cross() */
template<typename ExpressionType, int Direction>
template<typename OtherDerived>
EIGEN_DEVICE_FUNC
const typename VectorwiseOp<ExpressionType,Direction>::CrossReturnType
VectorwiseOp<ExpressionType,Direction>::cross(const MatrixBase<OtherDerived>& other) const
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3)
EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
typename internal::nested_eval<ExpressionType,2>::type mat(_expression());
typename internal::nested_eval<OtherDerived,2>::type vec(other.derived());
*
* \returns a matrix expression of the cross product of each column or row
* of the referenced expression with the \a other vector.
*
* The referenced matrix must have one dimension equal to 3.
* The result matrix has the same dimensions than the referenced one.
*
* \sa MatrixBase::cross() */
template <typename ExpressionType, int Direction>
template <typename OtherDerived>
EIGEN_DEVICE_FUNC const typename VectorwiseOp<ExpressionType, Direction>::CrossReturnType
VectorwiseOp<ExpressionType, Direction>::cross(const MatrixBase<OtherDerived>& other) const {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, 3)
EIGEN_STATIC_ASSERT(
(internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
CrossReturnType res(_expression().rows(),_expression().cols());
if(Direction==Vertical)
{
eigen_assert(CrossReturnType::RowsAtCompileTime==3 && "the matrix must have exactly 3 rows");
typename internal::nested_eval<ExpressionType, 2>::type mat(_expression());
typename internal::nested_eval<OtherDerived, 2>::type vec(other.derived());
CrossReturnType res(_expression().rows(), _expression().cols());
if (Direction == Vertical) {
eigen_assert(CrossReturnType::RowsAtCompileTime == 3 && "the matrix must have exactly 3 rows");
res.row(0) = (mat.row(1) * vec.coeff(2) - mat.row(2) * vec.coeff(1)).conjugate();
res.row(1) = (mat.row(2) * vec.coeff(0) - mat.row(0) * vec.coeff(2)).conjugate();
res.row(2) = (mat.row(0) * vec.coeff(1) - mat.row(1) * vec.coeff(0)).conjugate();
}
else
{
eigen_assert(CrossReturnType::ColsAtCompileTime==3 && "the matrix must have exactly 3 columns");
} else {
eigen_assert(CrossReturnType::ColsAtCompileTime == 3 && "the matrix must have exactly 3 columns");
res.col(0) = (mat.col(1) * vec.coeff(2) - mat.col(2) * vec.coeff(1)).conjugate();
res.col(1) = (mat.col(2) * vec.coeff(0) - mat.col(0) * vec.coeff(2)).conjugate();
res.col(2) = (mat.col(0) * vec.coeff(1) - mat.col(1) * vec.coeff(0)).conjugate();
@@ -184,39 +174,32 @@ VectorwiseOp<ExpressionType,Direction>::cross(const MatrixBase<OtherDerived>& ot
namespace internal {
template<typename Derived, int Size = Derived::SizeAtCompileTime>
struct unitOrthogonal_selector
{
template <typename Derived, int Size = Derived::SizeAtCompileTime>
struct unitOrthogonal_selector {
typedef typename plain_matrix_type<Derived>::type VectorType;
typedef typename traits<Derived>::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef Matrix<Scalar,2,1> Vector2;
EIGEN_DEVICE_FUNC
static inline VectorType run(const Derived& src)
{
typedef Matrix<Scalar, 2, 1> Vector2;
EIGEN_DEVICE_FUNC static inline VectorType run(const Derived& src) {
VectorType perp = VectorType::Zero(src.size());
Index maxi = 0;
Index sndi = 0;
src.cwiseAbs().maxCoeff(&maxi);
if (maxi==0)
sndi = 1;
RealScalar invnm = RealScalar(1)/(Vector2() << src.coeff(sndi),src.coeff(maxi)).finished().norm();
if (maxi == 0) sndi = 1;
RealScalar invnm = RealScalar(1) / (Vector2() << src.coeff(sndi), src.coeff(maxi)).finished().norm();
perp.coeffRef(maxi) = -numext::conj(src.coeff(sndi)) * invnm;
perp.coeffRef(sndi) = numext::conj(src.coeff(maxi)) * invnm;
perp.coeffRef(sndi) = numext::conj(src.coeff(maxi)) * invnm;
return perp;
}
}
};
template<typename Derived>
struct unitOrthogonal_selector<Derived,3>
{
template <typename Derived>
struct unitOrthogonal_selector<Derived, 3> {
typedef typename plain_matrix_type<Derived>::type VectorType;
typedef typename traits<Derived>::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
EIGEN_DEVICE_FUNC
static inline VectorType run(const Derived& src)
{
EIGEN_DEVICE_FUNC static inline VectorType run(const Derived& src) {
VectorType perp;
/* Let us compute the crossed product of *this with a vector
* that is not too close to being colinear to *this.
@@ -225,58 +208,52 @@ struct unitOrthogonal_selector<Derived,3>
/* unless the x and y coords are both close to zero, we can
* simply take ( -y, x, 0 ) and normalize it.
*/
if((!isMuchSmallerThan(src.x(), src.z()))
|| (!isMuchSmallerThan(src.y(), src.z())))
{
RealScalar invnm = RealScalar(1)/src.template head<2>().norm();
perp.coeffRef(0) = -numext::conj(src.y())*invnm;
perp.coeffRef(1) = numext::conj(src.x())*invnm;
if ((!isMuchSmallerThan(src.x(), src.z())) || (!isMuchSmallerThan(src.y(), src.z()))) {
RealScalar invnm = RealScalar(1) / src.template head<2>().norm();
perp.coeffRef(0) = -numext::conj(src.y()) * invnm;
perp.coeffRef(1) = numext::conj(src.x()) * invnm;
perp.coeffRef(2) = 0;
}
/* if both x and y are close to zero, then the vector is close
* to the z-axis, so it's far from colinear to the x-axis for instance.
* So we take the crossed product with (1,0,0) and normalize it.
*/
else
{
RealScalar invnm = RealScalar(1)/src.template tail<2>().norm();
else {
RealScalar invnm = RealScalar(1) / src.template tail<2>().norm();
perp.coeffRef(0) = 0;
perp.coeffRef(1) = -numext::conj(src.z())*invnm;
perp.coeffRef(2) = numext::conj(src.y())*invnm;
perp.coeffRef(1) = -numext::conj(src.z()) * invnm;
perp.coeffRef(2) = numext::conj(src.y()) * invnm;
}
return perp;
}
}
};
template<typename Derived>
struct unitOrthogonal_selector<Derived,2>
{
template <typename Derived>
struct unitOrthogonal_selector<Derived, 2> {
typedef typename plain_matrix_type<Derived>::type VectorType;
EIGEN_DEVICE_FUNC
static inline VectorType run(const Derived& src)
{ return VectorType(-numext::conj(src.y()), numext::conj(src.x())).normalized(); }
EIGEN_DEVICE_FUNC static inline VectorType run(const Derived& src) {
return VectorType(-numext::conj(src.y()), numext::conj(src.x())).normalized();
}
};
} // end namespace internal
} // end namespace internal
/** \geometry_module \ingroup Geometry_Module
*
* \returns a unit vector which is orthogonal to \c *this
*
* The size of \c *this must be at least 2. If the size is exactly 2,
* then the returned vector is a counter clock wise rotation of \c *this, i.e., (-y,x).normalized().
*
* \sa cross()
*/
template<typename Derived>
EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::PlainObject
MatrixBase<Derived>::unitOrthogonal() const
{
*
* \returns a unit vector which is orthogonal to \c *this
*
* The size of \c *this must be at least 2. If the size is exactly 2,
* then the returned vector is a counter clock wise rotation of \c *this, i.e., (-y,x).normalized().
*
* \sa cross()
*/
template <typename Derived>
EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::PlainObject MatrixBase<Derived>::unitOrthogonal() const {
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return internal::unitOrthogonal_selector<Derived>::run(derived());
}
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_ORTHOMETHODS_H
#endif // EIGEN_ORTHOMETHODS_H

229
Eigen/src/Geometry/ParametrizedLine.h
View File

@@ -14,59 +14,55 @@
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
namespace Eigen {
namespace Eigen {
/** \geometry_module \ingroup Geometry_Module
*
* \class ParametrizedLine
*
* \brief A parametrized line
*
* A parametrized line is defined by an origin point \f$ \mathbf{o} \f$ and a unit
* direction vector \f$ \mathbf{d} \f$ such that the line corresponds to
* the set \f$ l(t) = \mathbf{o} + t \mathbf{d} \f$, \f$ t \in \mathbf{R} \f$.
*
* \tparam Scalar_ the scalar type, i.e., the type of the coefficients
* \tparam AmbientDim_ the dimension of the ambient space, can be a compile time value or Dynamic.
*/
*
* \class ParametrizedLine
*
* \brief A parametrized line
*
* A parametrized line is defined by an origin point \f$ \mathbf{o} \f$ and a unit
* direction vector \f$ \mathbf{d} \f$ such that the line corresponds to
* the set \f$ l(t) = \mathbf{o} + t \mathbf{d} \f$, \f$ t \in \mathbf{R} \f$.
*
* \tparam Scalar_ the scalar type, i.e., the type of the coefficients
* \tparam AmbientDim_ the dimension of the ambient space, can be a compile time value or Dynamic.
*/
template <typename Scalar_, int AmbientDim_, int Options_>
class ParametrizedLine
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,AmbientDim_)
enum {
AmbientDimAtCompileTime = AmbientDim_,
Options = Options_
};
class ParametrizedLine {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_, AmbientDim_)
enum { AmbientDimAtCompileTime = AmbientDim_, Options = Options_ };
typedef Scalar_ Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
typedef Matrix<Scalar,AmbientDimAtCompileTime,1,Options> VectorType;
typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
typedef Matrix<Scalar, AmbientDimAtCompileTime, 1, Options> VectorType;
/** Default constructor without initialization */
EIGEN_DEVICE_FUNC inline ParametrizedLine() {}
template<int OtherOptions>
EIGEN_DEVICE_FUNC ParametrizedLine(const ParametrizedLine<Scalar,AmbientDimAtCompileTime,OtherOptions>& other)
: m_origin(other.origin()), m_direction(other.direction())
{}
template <int OtherOptions>
EIGEN_DEVICE_FUNC ParametrizedLine(const ParametrizedLine<Scalar, AmbientDimAtCompileTime, OtherOptions>& other)
: m_origin(other.origin()), m_direction(other.direction()) {}
/** Constructs a dynamic-size line with \a _dim the dimension
* of the ambient space */
* of the ambient space */
EIGEN_DEVICE_FUNC inline explicit ParametrizedLine(Index _dim) : m_origin(_dim), m_direction(_dim) {}
/** Initializes a parametrized line of direction \a direction and origin \a origin.
* \warning the vector direction is assumed to be normalized.
*/
* \warning the vector direction is assumed to be normalized.
*/
EIGEN_DEVICE_FUNC ParametrizedLine(const VectorType& origin, const VectorType& direction)
: m_origin(origin), m_direction(direction) {}
: m_origin(origin), m_direction(direction) {}
template <int OtherOptions>
EIGEN_DEVICE_FUNC explicit ParametrizedLine(const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane);
/** Constructs a parametrized line going from \a p0 to \a p1. */
EIGEN_DEVICE_FUNC static inline ParametrizedLine Through(const VectorType& p0, const VectorType& p1)
{ return ParametrizedLine(p0, (p1-p0).normalized()); }
EIGEN_DEVICE_FUNC static inline ParametrizedLine Through(const VectorType& p0, const VectorType& p1) {
return ParametrizedLine(p0, (p1 - p0).normalized());
}
EIGEN_DEVICE_FUNC ~ParametrizedLine() {}
@@ -80,48 +76,51 @@ public:
EIGEN_DEVICE_FUNC VectorType& direction() { return m_direction; }
/** \returns the squared distance of a point \a p to its projection onto the line \c *this.
* \sa distance()
*/
EIGEN_DEVICE_FUNC RealScalar squaredDistance(const VectorType& p) const
{
* \sa distance()
*/
EIGEN_DEVICE_FUNC RealScalar squaredDistance(const VectorType& p) const {
VectorType diff = p - origin();
return (diff - direction().dot(diff) * direction()).squaredNorm();
}
/** \returns the distance of a point \a p to its projection onto the line \c *this.
* \sa squaredDistance()
*/
EIGEN_DEVICE_FUNC RealScalar distance(const VectorType& p) const { EIGEN_USING_STD(sqrt) return sqrt(squaredDistance(p)); }
* \sa squaredDistance()
*/
EIGEN_DEVICE_FUNC RealScalar distance(const VectorType& p) const {
EIGEN_USING_STD(sqrt) return sqrt(squaredDistance(p));
}
/** \returns the projection of a point \a p onto the line \c *this. */
EIGEN_DEVICE_FUNC VectorType projection(const VectorType& p) const
{ return origin() + direction().dot(p-origin()) * direction(); }
EIGEN_DEVICE_FUNC VectorType projection(const VectorType& p) const {
return origin() + direction().dot(p - origin()) * direction();
}
EIGEN_DEVICE_FUNC VectorType pointAt(const Scalar& t) const;
template <int OtherOptions>
EIGEN_DEVICE_FUNC Scalar intersectionParameter(const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane) const;
EIGEN_DEVICE_FUNC Scalar
intersectionParameter(const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane) const;
template <int OtherOptions>
EIGEN_DEVICE_FUNC Scalar intersection(const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane) const;
template <int OtherOptions>
EIGEN_DEVICE_FUNC VectorType intersectionPoint(const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane) const;
EIGEN_DEVICE_FUNC VectorType
intersectionPoint(const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane) const;
/** Applies the transformation matrix \a mat to \c *this and returns a reference to \c *this.
*
* \param mat the Dim x Dim transformation matrix
* \param traits specifies whether the matrix \a mat represents an #Isometry
* or a more generic #Affine transformation. The default is #Affine.
*/
template<typename XprType>
EIGEN_DEVICE_FUNC inline ParametrizedLine& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine)
{
if (traits==Affine)
*
* \param mat the Dim x Dim transformation matrix
* \param traits specifies whether the matrix \a mat represents an #Isometry
* or a more generic #Affine transformation. The default is #Affine.
*/
template <typename XprType>
EIGEN_DEVICE_FUNC inline ParametrizedLine& transform(const MatrixBase<XprType>& mat,
TransformTraits traits = Affine) {
if (traits == Affine)
direction() = (mat * direction()).normalized();
else if (traits==Isometry)
else if (traits == Isometry)
direction() = mat * direction();
else
{
else {
eigen_assert(0 && "invalid traits value in ParametrizedLine::transform()");
}
origin() = mat * origin();
@@ -129,107 +128,105 @@ public:
}
/** Applies the transformation \a t to \c *this and returns a reference to \c *this.
*
* \param t the transformation of dimension Dim
* \param traits specifies whether the transformation \a t represents an #Isometry
* or a more generic #Affine transformation. The default is #Affine.
* Other kind of transformations are not supported.
*/
template<int TrOptions>
EIGEN_DEVICE_FUNC inline ParametrizedLine& transform(const Transform<Scalar,AmbientDimAtCompileTime,Affine,TrOptions>& t,
TransformTraits traits = Affine)
{
*
* \param t the transformation of dimension Dim
* \param traits specifies whether the transformation \a t represents an #Isometry
* or a more generic #Affine transformation. The default is #Affine.
* Other kind of transformations are not supported.
*/
template <int TrOptions>
EIGEN_DEVICE_FUNC inline ParametrizedLine& transform(
const Transform<Scalar, AmbientDimAtCompileTime, Affine, TrOptions>& t, TransformTraits traits = Affine) {
transform(t.linear(), traits);
origin() += t.translation();
return *this;
}
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<ParametrizedLine,
ParametrizedLine<NewScalarType,AmbientDimAtCompileTime,Options> >::type cast() const
{
return typename internal::cast_return_type<ParametrizedLine,
ParametrizedLine<NewScalarType,AmbientDimAtCompileTime,Options> >::type(*this);
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template <typename NewScalarType>
EIGEN_DEVICE_FUNC inline
typename internal::cast_return_type<ParametrizedLine,
ParametrizedLine<NewScalarType, AmbientDimAtCompileTime, Options> >::type
cast() const {
return typename internal::cast_return_type<
ParametrizedLine, ParametrizedLine<NewScalarType, AmbientDimAtCompileTime, Options> >::type(*this);
}
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType,int OtherOptions>
EIGEN_DEVICE_FUNC inline explicit ParametrizedLine(const ParametrizedLine<OtherScalarType,AmbientDimAtCompileTime,OtherOptions>& other)
{
template <typename OtherScalarType, int OtherOptions>
EIGEN_DEVICE_FUNC inline explicit ParametrizedLine(
const ParametrizedLine<OtherScalarType, AmbientDimAtCompileTime, OtherOptions>& other) {
m_origin = other.origin().template cast<Scalar>();
m_direction = other.direction().template cast<Scalar>();
}
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
EIGEN_DEVICE_FUNC bool isApprox(const ParametrizedLine& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
{ return m_origin.isApprox(other.m_origin, prec) && m_direction.isApprox(other.m_direction, prec); }
protected:
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
EIGEN_DEVICE_FUNC bool isApprox(const ParametrizedLine& other, const typename NumTraits<Scalar>::Real& prec =
NumTraits<Scalar>::dummy_precision()) const {
return m_origin.isApprox(other.m_origin, prec) && m_direction.isApprox(other.m_direction, prec);
}
protected:
VectorType m_origin, m_direction;
};
/** Constructs a parametrized line from a 2D hyperplane
*
* \warning the ambient space must have dimension 2 such that the hyperplane actually describes a line
*/
*
* \warning the ambient space must have dimension 2 such that the hyperplane actually describes a line
*/
template <typename Scalar_, int AmbientDim_, int Options_>
template <int OtherOptions>
EIGEN_DEVICE_FUNC inline ParametrizedLine<Scalar_, AmbientDim_,Options_>::ParametrizedLine(const Hyperplane<Scalar_, AmbientDim_,OtherOptions>& hyperplane)
{
EIGEN_DEVICE_FUNC inline ParametrizedLine<Scalar_, AmbientDim_, Options_>::ParametrizedLine(
const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
direction() = hyperplane.normal().unitOrthogonal();
origin() = -hyperplane.normal()*hyperplane.offset();
origin() = -hyperplane.normal() * hyperplane.offset();
}
/** \returns the point at \a t along this line
*/
*/
template <typename Scalar_, int AmbientDim_, int Options_>
EIGEN_DEVICE_FUNC inline typename ParametrizedLine<Scalar_, AmbientDim_,Options_>::VectorType
ParametrizedLine<Scalar_, AmbientDim_,Options_>::pointAt(const Scalar_& t) const
{
return origin() + (direction()*t);
EIGEN_DEVICE_FUNC inline typename ParametrizedLine<Scalar_, AmbientDim_, Options_>::VectorType
ParametrizedLine<Scalar_, AmbientDim_, Options_>::pointAt(const Scalar_& t) const {
return origin() + (direction() * t);
}
/** \returns the parameter value of the intersection between \c *this and the given \a hyperplane
*/
*/
template <typename Scalar_, int AmbientDim_, int Options_>
template <int OtherOptions>
EIGEN_DEVICE_FUNC inline Scalar_ ParametrizedLine<Scalar_, AmbientDim_,Options_>::intersectionParameter(const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane) const
{
return -(hyperplane.offset()+hyperplane.normal().dot(origin()))
/ hyperplane.normal().dot(direction());
EIGEN_DEVICE_FUNC inline Scalar_ ParametrizedLine<Scalar_, AmbientDim_, Options_>::intersectionParameter(
const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane) const {
return -(hyperplane.offset() + hyperplane.normal().dot(origin())) / hyperplane.normal().dot(direction());
}
/** \deprecated use intersectionParameter()
* \returns the parameter value of the intersection between \c *this and the given \a hyperplane
*/
* \returns the parameter value of the intersection between \c *this and the given \a hyperplane
*/
template <typename Scalar_, int AmbientDim_, int Options_>
template <int OtherOptions>
EIGEN_DEVICE_FUNC inline Scalar_ ParametrizedLine<Scalar_, AmbientDim_,Options_>::intersection(const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane) const
{
EIGEN_DEVICE_FUNC inline Scalar_ ParametrizedLine<Scalar_, AmbientDim_, Options_>::intersection(
const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane) const {
return intersectionParameter(hyperplane);
}
/** \returns the point of the intersection between \c *this and the given hyperplane
*/
*/
template <typename Scalar_, int AmbientDim_, int Options_>
template <int OtherOptions>
EIGEN_DEVICE_FUNC inline typename ParametrizedLine<Scalar_, AmbientDim_,Options_>::VectorType
ParametrizedLine<Scalar_, AmbientDim_,Options_>::intersectionPoint(const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane) const
{
EIGEN_DEVICE_FUNC inline typename ParametrizedLine<Scalar_, AmbientDim_, Options_>::VectorType
ParametrizedLine<Scalar_, AmbientDim_, Options_>::intersectionPoint(
const Hyperplane<Scalar_, AmbientDim_, OtherOptions>& hyperplane) const {
return pointAt(intersectionParameter(hyperplane));
}
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_PARAMETRIZEDLINE_H
#endif // EIGEN_PARAMETRIZEDLINE_H

934
Eigen/src/Geometry/Quaternion.h
View File

File diff suppressed because it is too large Load Diff

203
Eigen/src/Geometry/Rotation2D.h
View File

@@ -13,67 +13,62 @@
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
namespace Eigen {
namespace Eigen {
/** \geometry_module \ingroup Geometry_Module
*
* \class Rotation2D
*
* \brief Represents a rotation/orientation in a 2 dimensional space.
*
* \tparam Scalar_ the scalar type, i.e., the type of the coefficients
*
* This class is equivalent to a single scalar representing a counter clock wise rotation
* as a single angle in radian. It provides some additional features such as the automatic
* conversion from/to a 2x2 rotation matrix. Moreover this class aims to provide a similar
* interface to Quaternion in order to facilitate the writing of generic algorithms
* dealing with rotations.
*
* \sa class Quaternion, class Transform
*/
*
* \class Rotation2D
*
* \brief Represents a rotation/orientation in a 2 dimensional space.
*
* \tparam Scalar_ the scalar type, i.e., the type of the coefficients
*
* This class is equivalent to a single scalar representing a counter clock wise rotation
* as a single angle in radian. It provides some additional features such as the automatic
* conversion from/to a 2x2 rotation matrix. Moreover this class aims to provide a similar
* interface to Quaternion in order to facilitate the writing of generic algorithms
* dealing with rotations.
*
* \sa class Quaternion, class Transform
*/
namespace internal {
template<typename Scalar_> struct traits<Rotation2D<Scalar_> >
{
template <typename Scalar_>
struct traits<Rotation2D<Scalar_> > {
typedef Scalar_ Scalar;
};
} // end namespace internal
} // end namespace internal
template<typename Scalar_>
class Rotation2D : public RotationBase<Rotation2D<Scalar_>,2>
{
typedef RotationBase<Rotation2D<Scalar_>,2> Base;
public:
template <typename Scalar_>
class Rotation2D : public RotationBase<Rotation2D<Scalar_>, 2> {
typedef RotationBase<Rotation2D<Scalar_>, 2> Base;
public:
using Base::operator*;
enum { Dim = 2 };
/** the scalar type of the coefficients */
typedef Scalar_ Scalar;
typedef Matrix<Scalar,2,1> Vector2;
typedef Matrix<Scalar,2,2> Matrix2;
protected:
typedef Matrix<Scalar, 2, 1> Vector2;
typedef Matrix<Scalar, 2, 2> Matrix2;
protected:
Scalar m_angle;
public:
public:
/** Construct a 2D counter clock wise rotation from the angle \a a in radian. */
EIGEN_DEVICE_FUNC explicit inline Rotation2D(const Scalar& a) : m_angle(a) {}
/** Default constructor wihtout initialization. The represented rotation is undefined. */
EIGEN_DEVICE_FUNC Rotation2D() {}
/** Construct a 2D rotation from a 2x2 rotation matrix \a mat.
*
* \sa fromRotationMatrix()
*/
template<typename Derived>
EIGEN_DEVICE_FUNC explicit Rotation2D(const MatrixBase<Derived>& m)
{
*
* \sa fromRotationMatrix()
*/
template <typename Derived>
EIGEN_DEVICE_FUNC explicit Rotation2D(const MatrixBase<Derived>& m) {
fromRotationMatrix(m.derived());
}
@@ -82,18 +77,20 @@ public:
/** \returns a read-write reference to the rotation angle */
EIGEN_DEVICE_FUNC inline Scalar& angle() { return m_angle; }
/** \returns the rotation angle in [0,2pi] */
EIGEN_DEVICE_FUNC inline Scalar smallestPositiveAngle() const {
Scalar tmp = numext::fmod(m_angle,Scalar(2*EIGEN_PI));
return tmp<Scalar(0) ? tmp + Scalar(2*EIGEN_PI) : tmp;
Scalar tmp = numext::fmod(m_angle, Scalar(2 * EIGEN_PI));
return tmp < Scalar(0) ? tmp + Scalar(2 * EIGEN_PI) : tmp;
}
/** \returns the rotation angle in [-pi,pi] */
EIGEN_DEVICE_FUNC inline Scalar smallestAngle() const {
Scalar tmp = numext::fmod(m_angle,Scalar(2*EIGEN_PI));
if(tmp>Scalar(EIGEN_PI)) tmp -= Scalar(2*EIGEN_PI);
else if(tmp<-Scalar(EIGEN_PI)) tmp += Scalar(2*EIGEN_PI);
Scalar tmp = numext::fmod(m_angle, Scalar(2 * EIGEN_PI));
if (tmp > Scalar(EIGEN_PI))
tmp -= Scalar(2 * EIGEN_PI);
else if (tmp < -Scalar(EIGEN_PI))
tmp += Scalar(2 * EIGEN_PI);
return tmp;
}
@@ -101,95 +98,97 @@ public:
EIGEN_DEVICE_FUNC inline Rotation2D inverse() const { return Rotation2D(-m_angle); }
/** Concatenates two rotations */
EIGEN_DEVICE_FUNC inline Rotation2D operator*(const Rotation2D& other) const
{ return Rotation2D(m_angle + other.m_angle); }
EIGEN_DEVICE_FUNC inline Rotation2D operator*(const Rotation2D& other) const {
return Rotation2D(m_angle + other.m_angle);
}
/** Concatenates two rotations */
EIGEN_DEVICE_FUNC inline Rotation2D& operator*=(const Rotation2D& other)
{ m_angle += other.m_angle; return *this; }
EIGEN_DEVICE_FUNC inline Rotation2D& operator*=(const Rotation2D& other) {
m_angle += other.m_angle;
return *this;
}
/** Applies the rotation to a 2D vector */
EIGEN_DEVICE_FUNC Vector2 operator* (const Vector2& vec) const
{ return toRotationMatrix() * vec; }
template<typename Derived>
EIGEN_DEVICE_FUNC Vector2 operator*(const Vector2& vec) const { return toRotationMatrix() * vec; }
template <typename Derived>
EIGEN_DEVICE_FUNC Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m);
EIGEN_DEVICE_FUNC Matrix2 toRotationMatrix() const;
/** Set \c *this from a 2x2 rotation matrix \a mat.
* In other words, this function extract the rotation angle from the rotation matrix.
*
* This method is an alias for fromRotationMatrix()
*
* \sa fromRotationMatrix()
*/
template<typename Derived>
EIGEN_DEVICE_FUNC Rotation2D& operator=(const MatrixBase<Derived>& m)
{ return fromRotationMatrix(m.derived()); }
* In other words, this function extract the rotation angle from the rotation matrix.
*
* This method is an alias for fromRotationMatrix()
*
* \sa fromRotationMatrix()
*/
template <typename Derived>
EIGEN_DEVICE_FUNC Rotation2D& operator=(const MatrixBase<Derived>& m) {
return fromRotationMatrix(m.derived());
}
/** \returns the spherical interpolation between \c *this and \a other using
* parameter \a t. It is in fact equivalent to a linear interpolation.
*/
EIGEN_DEVICE_FUNC inline Rotation2D slerp(const Scalar& t, const Rotation2D& other) const
{
Scalar dist = Rotation2D(other.m_angle-m_angle).smallestAngle();
return Rotation2D(m_angle + dist*t);
* parameter \a t. It is in fact equivalent to a linear interpolation.
*/
EIGEN_DEVICE_FUNC inline Rotation2D slerp(const Scalar& t, const Rotation2D& other) const {
Scalar dist = Rotation2D(other.m_angle - m_angle).smallestAngle();
return Rotation2D(m_angle + dist * t);
}
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type cast() const
{ return typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type(*this); }
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template <typename NewScalarType>
EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Rotation2D, Rotation2D<NewScalarType> >::type cast()
const {
return typename internal::cast_return_type<Rotation2D, Rotation2D<NewScalarType> >::type(*this);
}
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
EIGEN_DEVICE_FUNC inline explicit Rotation2D(const Rotation2D<OtherScalarType>& other)
{
template <typename OtherScalarType>
EIGEN_DEVICE_FUNC inline explicit Rotation2D(const Rotation2D<OtherScalarType>& other) {
m_angle = Scalar(other.angle());
}
EIGEN_DEVICE_FUNC static inline Rotation2D Identity() { return Rotation2D(0); }
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
EIGEN_DEVICE_FUNC bool isApprox(const Rotation2D& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
{ return internal::isApprox(m_angle,other.m_angle, prec); }
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
EIGEN_DEVICE_FUNC bool isApprox(const Rotation2D& other, const typename NumTraits<Scalar>::Real& prec =
NumTraits<Scalar>::dummy_precision()) const {
return internal::isApprox(m_angle, other.m_angle, prec);
}
};
/** \ingroup Geometry_Module
* single precision 2D rotation type */
* single precision 2D rotation type */
typedef Rotation2D<float> Rotation2Df;
/** \ingroup Geometry_Module
* double precision 2D rotation type */
* double precision 2D rotation type */
typedef Rotation2D<double> Rotation2Dd;
/** Set \c *this from a 2x2 rotation matrix \a mat.
* In other words, this function extract the rotation angle
* from the rotation matrix.
*/
template<typename Scalar>
template<typename Derived>
EIGEN_DEVICE_FUNC Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
{
* In other words, this function extract the rotation angle
* from the rotation matrix.
*/
template <typename Scalar>
template <typename Derived>
EIGEN_DEVICE_FUNC Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) {
EIGEN_USING_STD(atan2)
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,YOU_MADE_A_PROGRAMMING_MISTAKE)
m_angle = atan2(mat.coeff(1,0), mat.coeff(0,0));
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 2 && Derived::ColsAtCompileTime == 2,
YOU_MADE_A_PROGRAMMING_MISTAKE)
m_angle = atan2(mat.coeff(1, 0), mat.coeff(0, 0));
return *this;
}
/** Constructs and \returns an equivalent 2x2 rotation matrix.
*/
template<typename Scalar>
typename Rotation2D<Scalar>::Matrix2
EIGEN_DEVICE_FUNC Rotation2D<Scalar>::toRotationMatrix(void) const
{
*/
template <typename Scalar>
typename Rotation2D<Scalar>::Matrix2 EIGEN_DEVICE_FUNC Rotation2D<Scalar>::toRotationMatrix(void) const {
EIGEN_USING_STD(sin)
EIGEN_USING_STD(cos)
Scalar sinA = sin(m_angle);
@@ -197,6 +196,6 @@ EIGEN_DEVICE_FUNC Rotation2D<Scalar>::toRotationMatrix(void) const
return (Matrix2() << cosA, -sinA, sinA, cosA).finished();
}
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_ROTATION2D_H
#endif // EIGEN_ROTATION2D_H

264
Eigen/src/Geometry/RotationBase.h
View File

@@ -13,197 +13,197 @@
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
namespace Eigen {
namespace Eigen {
// forward declaration
namespace internal {
template<typename RotationDerived, typename MatrixType, bool IsVector=MatrixType::IsVectorAtCompileTime>
template <typename RotationDerived, typename MatrixType, bool IsVector = MatrixType::IsVectorAtCompileTime>
struct rotation_base_generic_product_selector;
}
/** \class RotationBase
*
* \brief Common base class for compact rotation representations
*
* \tparam Derived is the derived type, i.e., a rotation type
* \tparam Dim_ the dimension of the space
*/
template<typename Derived, int Dim_>
class RotationBase
{
public:
enum { Dim = Dim_ };
/** the scalar type of the coefficients */
typedef typename internal::traits<Derived>::Scalar Scalar;
*
* \brief Common base class for compact rotation representations
*
* \tparam Derived is the derived type, i.e., a rotation type
* \tparam Dim_ the dimension of the space
*/
template <typename Derived, int Dim_>
class RotationBase {
public:
enum { Dim = Dim_ };
/** the scalar type of the coefficients */
typedef typename internal::traits<Derived>::Scalar Scalar;
/** corresponding linear transformation matrix type */
typedef Matrix<Scalar,Dim,Dim> RotationMatrixType;
typedef Matrix<Scalar,Dim,1> VectorType;
/** corresponding linear transformation matrix type */
typedef Matrix<Scalar, Dim, Dim> RotationMatrixType;
typedef Matrix<Scalar, Dim, 1> VectorType;
public:
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); }
public:
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); }
/** \returns an equivalent rotation matrix */
EIGEN_DEVICE_FUNC inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); }
/** \returns an equivalent rotation matrix */
EIGEN_DEVICE_FUNC inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); }
/** \returns an equivalent rotation matrix
* This function is added to be conform with the Transform class' naming scheme.
*/
EIGEN_DEVICE_FUNC inline RotationMatrixType matrix() const { return derived().toRotationMatrix(); }
/** \returns an equivalent rotation matrix
* This function is added to be conform with the Transform class' naming scheme.
*/
EIGEN_DEVICE_FUNC inline RotationMatrixType matrix() const { return derived().toRotationMatrix(); }
/** \returns the inverse rotation */
EIGEN_DEVICE_FUNC inline Derived inverse() const { return derived().inverse(); }
/** \returns the inverse rotation */
EIGEN_DEVICE_FUNC inline Derived inverse() const { return derived().inverse(); }
/** \returns the concatenation of the rotation \c *this with a translation \a t */
EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Isometry> operator*(const Translation<Scalar,Dim>& t) const
{ return Transform<Scalar,Dim,Isometry>(*this) * t; }
/** \returns the concatenation of the rotation \c *this with a translation \a t */
EIGEN_DEVICE_FUNC inline Transform<Scalar, Dim, Isometry> operator*(const Translation<Scalar, Dim>& t) const {
return Transform<Scalar, Dim, Isometry>(*this) * t;
}
/** \returns the concatenation of the rotation \c *this with a uniform scaling \a s */
EIGEN_DEVICE_FUNC inline RotationMatrixType operator*(const UniformScaling<Scalar>& s) const
{ return toRotationMatrix() * s.factor(); }
/** \returns the concatenation of the rotation \c *this with a uniform scaling \a s */
EIGEN_DEVICE_FUNC inline RotationMatrixType operator*(const UniformScaling<Scalar>& s) const {
return toRotationMatrix() * s.factor();
}
/** \returns the concatenation of the rotation \c *this with a generic expression \a e
* \a e can be:
* - a DimxDim linear transformation matrix
* - a DimxDim diagonal matrix (axis aligned scaling)
* - a vector of size Dim
*/
template<typename OtherDerived>
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::rotation_base_generic_product_selector<Derived,OtherDerived,OtherDerived::IsVectorAtCompileTime>::ReturnType
operator*(const EigenBase<OtherDerived>& e) const
{ return internal::rotation_base_generic_product_selector<Derived,OtherDerived>::run(derived(), e.derived()); }
/** \returns the concatenation of the rotation \c *this with a generic expression \a e
* \a e can be:
* - a DimxDim linear transformation matrix
* - a DimxDim diagonal matrix (axis aligned scaling)
* - a vector of size Dim
*/
template <typename OtherDerived>
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
typename internal::rotation_base_generic_product_selector<Derived, OtherDerived,
OtherDerived::IsVectorAtCompileTime>::ReturnType
operator*(const EigenBase<OtherDerived>& e) const {
return internal::rotation_base_generic_product_selector<Derived, OtherDerived>::run(derived(), e.derived());
}
/** \returns the concatenation of a linear transformation \a l with the rotation \a r */
template<typename OtherDerived> friend
EIGEN_DEVICE_FUNC inline RotationMatrixType operator*(const EigenBase<OtherDerived>& l, const Derived& r)
{ return l.derived() * r.toRotationMatrix(); }
/** \returns the concatenation of a linear transformation \a l with the rotation \a r */
template <typename OtherDerived>
friend EIGEN_DEVICE_FUNC inline RotationMatrixType operator*(const EigenBase<OtherDerived>& l, const Derived& r) {
return l.derived() * r.toRotationMatrix();
}
/** \returns the concatenation of a scaling \a l with the rotation \a r */
EIGEN_DEVICE_FUNC friend inline Transform<Scalar,Dim,Affine> operator*(const DiagonalMatrix<Scalar,Dim>& l, const Derived& r)
{
Transform<Scalar,Dim,Affine> res(r);
res.linear().applyOnTheLeft(l);
return res;
}
/** \returns the concatenation of a scaling \a l with the rotation \a r */
EIGEN_DEVICE_FUNC friend inline Transform<Scalar, Dim, Affine> operator*(const DiagonalMatrix<Scalar, Dim>& l,
const Derived& r) {
Transform<Scalar, Dim, Affine> res(r);
res.linear().applyOnTheLeft(l);
return res;
}
/** \returns the concatenation of the rotation \c *this with a transformation \a t */
template<int Mode, int Options>
EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Mode> operator*(const Transform<Scalar,Dim,Mode,Options>& t) const
{ return toRotationMatrix() * t; }
/** \returns the concatenation of the rotation \c *this with a transformation \a t */
template <int Mode, int Options>
EIGEN_DEVICE_FUNC inline Transform<Scalar, Dim, Mode> operator*(
const Transform<Scalar, Dim, Mode, Options>& t) const {
return toRotationMatrix() * t;
}
template<typename OtherVectorType>
EIGEN_DEVICE_FUNC inline VectorType _transformVector(const OtherVectorType& v) const
{ return toRotationMatrix() * v; }
template <typename OtherVectorType>
EIGEN_DEVICE_FUNC inline VectorType _transformVector(const OtherVectorType& v) const {
return toRotationMatrix() * v;
}
};
namespace internal {
// implementation of the generic product rotation * matrix
template<typename RotationDerived, typename MatrixType>
struct rotation_base_generic_product_selector<RotationDerived,MatrixType,false>
{
template <typename RotationDerived, typename MatrixType>
struct rotation_base_generic_product_selector<RotationDerived, MatrixType, false> {
enum { Dim = RotationDerived::Dim };
typedef Matrix<typename RotationDerived::Scalar,Dim,Dim> ReturnType;
EIGEN_DEVICE_FUNC static inline ReturnType run(const RotationDerived& r, const MatrixType& m)
{ return r.toRotationMatrix() * m; }
typedef Matrix<typename RotationDerived::Scalar, Dim, Dim> ReturnType;
EIGEN_DEVICE_FUNC static inline ReturnType run(const RotationDerived& r, const MatrixType& m) {
return r.toRotationMatrix() * m;
}
};
template<typename RotationDerived, typename Scalar, int Dim, int MaxDim>
struct rotation_base_generic_product_selector< RotationDerived, DiagonalMatrix<Scalar,Dim,MaxDim>, false >
{
typedef Transform<Scalar,Dim,Affine> ReturnType;
EIGEN_DEVICE_FUNC static inline ReturnType run(const RotationDerived& r, const DiagonalMatrix<Scalar,Dim,MaxDim>& m)
{
template <typename RotationDerived, typename Scalar, int Dim, int MaxDim>
struct rotation_base_generic_product_selector<RotationDerived, DiagonalMatrix<Scalar, Dim, MaxDim>, false> {
typedef Transform<Scalar, Dim, Affine> ReturnType;
EIGEN_DEVICE_FUNC static inline ReturnType run(const RotationDerived& r,
const DiagonalMatrix<Scalar, Dim, MaxDim>& m) {
ReturnType res(r);
res.linear() *= m;
return res;
}
};
template<typename RotationDerived,typename OtherVectorType>
struct rotation_base_generic_product_selector<RotationDerived,OtherVectorType,true>
{
template <typename RotationDerived, typename OtherVectorType>
struct rotation_base_generic_product_selector<RotationDerived, OtherVectorType, true> {
enum { Dim = RotationDerived::Dim };
typedef Matrix<typename RotationDerived::Scalar,Dim,1> ReturnType;
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE ReturnType run(const RotationDerived& r, const OtherVectorType& v)
{
typedef Matrix<typename RotationDerived::Scalar, Dim, 1> ReturnType;
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE ReturnType run(const RotationDerived& r, const OtherVectorType& v) {
return r._transformVector(v);
}
};
} // end namespace internal
} // end namespace internal
/** \geometry_module
*
* \brief Constructs a Dim x Dim rotation matrix from the rotation \a r
*/
template<typename Scalar_, int Rows_, int Cols_, int Storage_, int MaxRows_, int MaxCols_>
template<typename OtherDerived>
EIGEN_DEVICE_FUNC Matrix<Scalar_, Rows_, Cols_, Storage_, MaxRows_, MaxCols_>
::Matrix(const RotationBase<OtherDerived,ColsAtCompileTime>& r)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,int(OtherDerived::Dim),int(OtherDerived::Dim))
*
* \brief Constructs a Dim x Dim rotation matrix from the rotation \a r
*/
template <typename Scalar_, int Rows_, int Cols_, int Storage_, int MaxRows_, int MaxCols_>
template <typename OtherDerived>
EIGEN_DEVICE_FUNC Matrix<Scalar_, Rows_, Cols_, Storage_, MaxRows_, MaxCols_>::Matrix(
const RotationBase<OtherDerived, ColsAtCompileTime>& r) {
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix, int(OtherDerived::Dim), int(OtherDerived::Dim))
*this = r.toRotationMatrix();
}
/** \geometry_module
*
* \brief Set a Dim x Dim rotation matrix from the rotation \a r
*/
template<typename Scalar_, int Rows_, int Cols_, int Storage_, int MaxRows_, int MaxCols_>
template<typename OtherDerived>
*
* \brief Set a Dim x Dim rotation matrix from the rotation \a r
*/
template <typename Scalar_, int Rows_, int Cols_, int Storage_, int MaxRows_, int MaxCols_>
template <typename OtherDerived>
EIGEN_DEVICE_FUNC Matrix<Scalar_, Rows_, Cols_, Storage_, MaxRows_, MaxCols_>&
Matrix<Scalar_, Rows_, Cols_, Storage_, MaxRows_, MaxCols_>
::operator=(const RotationBase<OtherDerived,ColsAtCompileTime>& r)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,int(OtherDerived::Dim),int(OtherDerived::Dim))
Matrix<Scalar_, Rows_, Cols_, Storage_, MaxRows_, MaxCols_>::operator=(
const RotationBase<OtherDerived, ColsAtCompileTime>& r) {
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix, int(OtherDerived::Dim), int(OtherDerived::Dim))
return *this = r.toRotationMatrix();
}
namespace internal {
/** \internal
*
* Helper function to return an arbitrary rotation object to a rotation matrix.
*
* \tparam Scalar the numeric type of the matrix coefficients
* \tparam Dim the dimension of the current space
*
* It returns a Dim x Dim fixed size matrix.
*
* Default specializations are provided for:
* - any scalar type (2D),
* - any matrix expression,
* - any type based on RotationBase (e.g., Quaternion, AngleAxis, Rotation2D)
*
* Currently toRotationMatrix is only used by Transform.
*
* \sa class Transform, class Rotation2D, class Quaternion, class AngleAxis
*/
template<typename Scalar, int Dim>
EIGEN_DEVICE_FUNC static inline Matrix<Scalar,2,2> toRotationMatrix(const Scalar& s)
{
EIGEN_STATIC_ASSERT(Dim==2,YOU_MADE_A_PROGRAMMING_MISTAKE)
*
* Helper function to return an arbitrary rotation object to a rotation matrix.
*
* \tparam Scalar the numeric type of the matrix coefficients
* \tparam Dim the dimension of the current space
*
* It returns a Dim x Dim fixed size matrix.
*
* Default specializations are provided for:
* - any scalar type (2D),
* - any matrix expression,
* - any type based on RotationBase (e.g., Quaternion, AngleAxis, Rotation2D)
*
* Currently toRotationMatrix is only used by Transform.
*
* \sa class Transform, class Rotation2D, class Quaternion, class AngleAxis
*/
template <typename Scalar, int Dim>
EIGEN_DEVICE_FUNC static inline Matrix<Scalar, 2, 2> toRotationMatrix(const Scalar& s) {
EIGEN_STATIC_ASSERT(Dim == 2, YOU_MADE_A_PROGRAMMING_MISTAKE)
return Rotation2D<Scalar>(s).toRotationMatrix();
}
template<typename Scalar, int Dim, typename OtherDerived>
EIGEN_DEVICE_FUNC static inline Matrix<Scalar,Dim,Dim> toRotationMatrix(const RotationBase<OtherDerived,Dim>& r)
{
template <typename Scalar, int Dim, typename OtherDerived>
EIGEN_DEVICE_FUNC static inline Matrix<Scalar, Dim, Dim> toRotationMatrix(const RotationBase<OtherDerived, Dim>& r) {
return r.toRotationMatrix();
}
template<typename Scalar, int Dim, typename OtherDerived>
EIGEN_DEVICE_FUNC static inline const MatrixBase<OtherDerived>& toRotationMatrix(const MatrixBase<OtherDerived>& mat)
{
EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime==Dim && OtherDerived::ColsAtCompileTime==Dim,
YOU_MADE_A_PROGRAMMING_MISTAKE)
template <typename Scalar, int Dim, typename OtherDerived>
EIGEN_DEVICE_FUNC static inline const MatrixBase<OtherDerived>& toRotationMatrix(const MatrixBase<OtherDerived>& mat) {
EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime == Dim && OtherDerived::ColsAtCompileTime == Dim,
YOU_MADE_A_PROGRAMMING_MISTAKE)
return mat;
}
} // end namespace internal
} // end namespace internal
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_ROTATIONBASE_H
#endif // EIGEN_ROTATIONBASE_H

213
Eigen/src/Geometry/Scaling.h
View File

@@ -13,54 +13,46 @@
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
namespace Eigen {
namespace Eigen {
/** \geometry_module \ingroup Geometry_Module
*
* \class UniformScaling
*
* \brief Represents a generic uniform scaling transformation
*
* \tparam Scalar_ the scalar type, i.e., the type of the coefficients.
*
* This class represent a uniform scaling transformation. It is the return
* type of Scaling(Scalar), and most of the time this is the only way it
* is used. In particular, this class is not aimed to be used to store a scaling transformation,
* but rather to make easier the constructions and updates of Transform objects.
*
* To represent an axis aligned scaling, use the DiagonalMatrix class.
*
* \sa Scaling(), class DiagonalMatrix, MatrixBase::asDiagonal(), class Translation, class Transform
*/
*
* \class UniformScaling
*
* \brief Represents a generic uniform scaling transformation
*
* \tparam Scalar_ the scalar type, i.e., the type of the coefficients.
*
* This class represent a uniform scaling transformation. It is the return
* type of Scaling(Scalar), and most of the time this is the only way it
* is used. In particular, this class is not aimed to be used to store a scaling transformation,
* but rather to make easier the constructions and updates of Transform objects.
*
* To represent an axis aligned scaling, use the DiagonalMatrix class.
*
* \sa Scaling(), class DiagonalMatrix, MatrixBase::asDiagonal(), class Translation, class Transform
*/
namespace internal
{
// This helper helps nvcc+MSVC to properly parse this file.
// See bug 1412.
template <typename Scalar, int Dim, int Mode>
struct uniformscaling_times_affine_returntype
{
enum
{
NewMode = int(Mode) == int(Isometry) ? Affine : Mode
};
typedef Transform <Scalar, Dim, NewMode> type;
};
}
namespace internal {
// This helper helps nvcc+MSVC to properly parse this file.
// See bug 1412.
template <typename Scalar, int Dim, int Mode>
struct uniformscaling_times_affine_returntype {
enum { NewMode = int(Mode) == int(Isometry) ? Affine : Mode };
typedef Transform<Scalar, Dim, NewMode> type;
};
} // namespace internal
template<typename Scalar_>
class UniformScaling
{
public:
template <typename Scalar_>
class UniformScaling {
public:
/** the scalar type of the coefficients */
typedef Scalar_ Scalar;
protected:
protected:
Scalar m_factor;
public:
public:
/** Default constructor without initialization. */
UniformScaling() {}
/** Constructs and initialize a uniform scaling transformation */
@@ -70,127 +62,134 @@ public:
inline Scalar& factor() { return m_factor; }
/** Concatenates two uniform scaling */
inline UniformScaling operator* (const UniformScaling& other) const
{ return UniformScaling(m_factor * other.factor()); }
inline UniformScaling operator*(const UniformScaling& other) const {
return UniformScaling(m_factor * other.factor());
}
/** Concatenates a uniform scaling and a translation */
template<int Dim>
inline Transform<Scalar,Dim,Affine> operator* (const Translation<Scalar,Dim>& t) const;
template <int Dim>
inline Transform<Scalar, Dim, Affine> operator*(const Translation<Scalar, Dim>& t) const;
/** Concatenates a uniform scaling and an affine transformation */
template<int Dim, int Mode, int Options>
inline typename
internal::uniformscaling_times_affine_returntype<Scalar,Dim,Mode>::type
operator* (const Transform<Scalar, Dim, Mode, Options>& t) const
{
typename internal::uniformscaling_times_affine_returntype<Scalar,Dim,Mode>::type res = t;
template <int Dim, int Mode, int Options>
inline typename internal::uniformscaling_times_affine_returntype<Scalar, Dim, Mode>::type operator*(
const Transform<Scalar, Dim, Mode, Options>& t) const {
typename internal::uniformscaling_times_affine_returntype<Scalar, Dim, Mode>::type res = t;
res.prescale(factor());
return res;
}
/** Concatenates a uniform scaling and a linear transformation matrix */
// TODO returns an expression
template<typename Derived>
inline typename Eigen::internal::plain_matrix_type<Derived>::type operator* (const MatrixBase<Derived>& other) const
{ return other * m_factor; }
template <typename Derived>
inline typename Eigen::internal::plain_matrix_type<Derived>::type operator*(const MatrixBase<Derived>& other) const {
return other * m_factor;
}
template<typename Derived,int Dim>
inline Matrix<Scalar,Dim,Dim> operator*(const RotationBase<Derived,Dim>& r) const
{ return r.toRotationMatrix() * m_factor; }
template <typename Derived, int Dim>
inline Matrix<Scalar, Dim, Dim> operator*(const RotationBase<Derived, Dim>& r) const {
return r.toRotationMatrix() * m_factor;
}
/** \returns the inverse scaling */
inline UniformScaling inverse() const
{ return UniformScaling(Scalar(1)/m_factor); }
inline UniformScaling inverse() const { return UniformScaling(Scalar(1) / m_factor); }
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
inline UniformScaling<NewScalarType> cast() const
{ return UniformScaling<NewScalarType>(NewScalarType(m_factor)); }
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template <typename NewScalarType>
inline UniformScaling<NewScalarType> cast() const {
return UniformScaling<NewScalarType>(NewScalarType(m_factor));
}
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
inline explicit UniformScaling(const UniformScaling<OtherScalarType>& other)
{ m_factor = Scalar(other.factor()); }
template <typename OtherScalarType>
inline explicit UniformScaling(const UniformScaling<OtherScalarType>& other) {
m_factor = Scalar(other.factor());
}
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const UniformScaling& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
{ return internal::isApprox(m_factor, other.factor(), prec); }
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const UniformScaling& other,
const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const {
return internal::isApprox(m_factor, other.factor(), prec);
}
};
/** \addtogroup Geometry_Module */
//@{
/** Concatenates a linear transformation matrix and a uniform scaling
* \relates UniformScaling
*/
* \relates UniformScaling
*/
// NOTE this operator is defined in MatrixBase and not as a friend function
// of UniformScaling to fix an internal crash of Intel's ICC
template<typename Derived,typename Scalar>
EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,Scalar,product)
operator*(const MatrixBase<Derived>& matrix, const UniformScaling<Scalar>& s)
{ return matrix.derived() * s.factor(); }
template <typename Derived, typename Scalar>
EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived, Scalar, product)
operator*(const MatrixBase<Derived>& matrix, const UniformScaling<Scalar>& s) {
return matrix.derived() * s.factor();
}
/** Constructs a uniform scaling from scale factor \a s */
inline UniformScaling<float> Scaling(float s) { return UniformScaling<float>(s); }
/** Constructs a uniform scaling from scale factor \a s */
inline UniformScaling<double> Scaling(double s) { return UniformScaling<double>(s); }
/** Constructs a uniform scaling from scale factor \a s */
template<typename RealScalar>
inline UniformScaling<std::complex<RealScalar> > Scaling(const std::complex<RealScalar>& s)
{ return UniformScaling<std::complex<RealScalar> >(s); }
template <typename RealScalar>
inline UniformScaling<std::complex<RealScalar> > Scaling(const std::complex<RealScalar>& s) {
return UniformScaling<std::complex<RealScalar> >(s);
}
/** Constructs a 2D axis aligned scaling */
template<typename Scalar>
inline DiagonalMatrix<Scalar,2> Scaling(const Scalar& sx, const Scalar& sy)
{ return DiagonalMatrix<Scalar,2>(sx, sy); }
template <typename Scalar>
inline DiagonalMatrix<Scalar, 2> Scaling(const Scalar& sx, const Scalar& sy) {
return DiagonalMatrix<Scalar, 2>(sx, sy);
}
/** Constructs a 3D axis aligned scaling */
template<typename Scalar>
inline DiagonalMatrix<Scalar,3> Scaling(const Scalar& sx, const Scalar& sy, const Scalar& sz)
{ return DiagonalMatrix<Scalar,3>(sx, sy, sz); }
template <typename Scalar>
inline DiagonalMatrix<Scalar, 3> Scaling(const Scalar& sx, const Scalar& sy, const Scalar& sz) {
return DiagonalMatrix<Scalar, 3>(sx, sy, sz);
}
/** Constructs an axis aligned scaling expression from vector expression \a coeffs
* This is an alias for coeffs.asDiagonal()
*/
template<typename Derived>
inline const DiagonalWrapper<const Derived> Scaling(const MatrixBase<Derived>& coeffs)
{ return coeffs.asDiagonal(); }
* This is an alias for coeffs.asDiagonal()
*/
template <typename Derived>
inline const DiagonalWrapper<const Derived> Scaling(const MatrixBase<Derived>& coeffs) {
return coeffs.asDiagonal();
}
/** Constructs an axis aligned scaling expression from vector \a coeffs when passed as an rvalue reference */
template<typename Derived>
inline typename DiagonalWrapper<const Derived>::PlainObject Scaling(MatrixBase<Derived>&& coeffs)
{ return typename DiagonalWrapper<const Derived>::PlainObject(std::move(coeffs.derived())); }
template <typename Derived>
inline typename DiagonalWrapper<const Derived>::PlainObject Scaling(MatrixBase<Derived>&& coeffs) {
return typename DiagonalWrapper<const Derived>::PlainObject(std::move(coeffs.derived()));
}
/** \deprecated */
typedef DiagonalMatrix<float, 2> AlignedScaling2f;
/** \deprecated */
typedef DiagonalMatrix<double,2> AlignedScaling2d;
typedef DiagonalMatrix<double, 2> AlignedScaling2d;
/** \deprecated */
typedef DiagonalMatrix<float, 3> AlignedScaling3f;
/** \deprecated */
typedef DiagonalMatrix<double,3> AlignedScaling3d;
typedef DiagonalMatrix<double, 3> AlignedScaling3d;
//@}
template<typename Scalar>
template<int Dim>
inline Transform<Scalar,Dim,Affine>
UniformScaling<Scalar>::operator* (const Translation<Scalar,Dim>& t) const
{
Transform<Scalar,Dim,Affine> res;
template <typename Scalar>
template <int Dim>
inline Transform<Scalar, Dim, Affine> UniformScaling<Scalar>::operator*(const Translation<Scalar, Dim>& t) const {
Transform<Scalar, Dim, Affine> res;
res.matrix().setZero();
res.linear().diagonal().fill(factor());
res.translation() = factor() * t.vector();
res(Dim,Dim) = Scalar(1);
res(Dim, Dim) = Scalar(1);
return res;
}
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_SCALING_H
#endif // EIGEN_SCALING_H

1766
Eigen/src/Geometry/Transform.h
View File

File diff suppressed because it is too large Load Diff

167
Eigen/src/Geometry/Translation.h
View File

@@ -13,59 +13,54 @@
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
namespace Eigen {
namespace Eigen {
/** \geometry_module \ingroup Geometry_Module
*
* \class Translation
*
* \brief Represents a translation transformation
*
* \tparam Scalar_ the scalar type, i.e., the type of the coefficients.
* \tparam Dim_ the dimension of the space, can be a compile time value or Dynamic
*
* \note This class is not aimed to be used to store a translation transformation,
* but rather to make easier the constructions and updates of Transform objects.
*
* \sa class Scaling, class Transform
*/
template<typename Scalar_, int Dim_>
class Translation
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_,Dim_)
*
* \class Translation
*
* \brief Represents a translation transformation
*
* \tparam Scalar_ the scalar type, i.e., the type of the coefficients.
* \tparam Dim_ the dimension of the space, can be a compile time value or Dynamic
*
* \note This class is not aimed to be used to store a translation transformation,
* but rather to make easier the constructions and updates of Transform objects.
*
* \sa class Scaling, class Transform
*/
template <typename Scalar_, int Dim_>
class Translation {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar_, Dim_)
/** dimension of the space */
enum { Dim = Dim_ };
/** the scalar type of the coefficients */
typedef Scalar_ Scalar;
/** corresponding vector type */
typedef Matrix<Scalar,Dim,1> VectorType;
typedef Matrix<Scalar, Dim, 1> VectorType;
/** corresponding linear transformation matrix type */
typedef Matrix<Scalar,Dim,Dim> LinearMatrixType;
typedef Matrix<Scalar, Dim, Dim> LinearMatrixType;
/** corresponding affine transformation type */
typedef Transform<Scalar,Dim,Affine> AffineTransformType;
typedef Transform<Scalar, Dim, Affine> AffineTransformType;
/** corresponding isometric transformation type */
typedef Transform<Scalar,Dim,Isometry> IsometryTransformType;
protected:
typedef Transform<Scalar, Dim, Isometry> IsometryTransformType;
protected:
VectorType m_coeffs;
public:
public:
/** Default constructor without initialization. */
EIGEN_DEVICE_FUNC Translation() {}
/** */
EIGEN_DEVICE_FUNC inline Translation(const Scalar& sx, const Scalar& sy)
{
eigen_assert(Dim==2);
EIGEN_DEVICE_FUNC inline Translation(const Scalar& sx, const Scalar& sy) {
eigen_assert(Dim == 2);
m_coeffs.x() = sx;
m_coeffs.y() = sy;
}
/** */
EIGEN_DEVICE_FUNC inline Translation(const Scalar& sx, const Scalar& sy, const Scalar& sz)
{
eigen_assert(Dim==3);
EIGEN_DEVICE_FUNC inline Translation(const Scalar& sx, const Scalar& sy, const Scalar& sz) {
eigen_assert(Dim == 3);
m_coeffs.x() = sx;
m_coeffs.y() = sy;
m_coeffs.z() = sz;
@@ -94,49 +89,51 @@ public:
EIGEN_DEVICE_FUNC VectorType& translation() { return m_coeffs; }
/** Concatenates two translation */
EIGEN_DEVICE_FUNC inline Translation operator* (const Translation& other) const
{ return Translation(m_coeffs + other.m_coeffs); }
EIGEN_DEVICE_FUNC inline Translation operator*(const Translation& other) const {
return Translation(m_coeffs + other.m_coeffs);
}
/** Concatenates a translation and a uniform scaling */
EIGEN_DEVICE_FUNC inline AffineTransformType operator* (const UniformScaling<Scalar>& other) const;
EIGEN_DEVICE_FUNC inline AffineTransformType operator*(const UniformScaling<Scalar>& other) const;
/** Concatenates a translation and a linear transformation */
template<typename OtherDerived>
EIGEN_DEVICE_FUNC inline AffineTransformType operator* (const EigenBase<OtherDerived>& linear) const;
template <typename OtherDerived>
EIGEN_DEVICE_FUNC inline AffineTransformType operator*(const EigenBase<OtherDerived>& linear) const;
/** Concatenates a translation and a rotation */
template<typename Derived>
EIGEN_DEVICE_FUNC inline IsometryTransformType operator*(const RotationBase<Derived,Dim>& r) const
{ return *this * IsometryTransformType(r); }
template <typename Derived>
EIGEN_DEVICE_FUNC inline IsometryTransformType operator*(const RotationBase<Derived, Dim>& r) const {
return *this * IsometryTransformType(r);
}
/** \returns the concatenation of a linear transformation \a l with the translation \a t */
// its a nightmare to define a templated friend function outside its declaration
template<typename OtherDerived> friend
EIGEN_DEVICE_FUNC inline AffineTransformType operator*(const EigenBase<OtherDerived>& linear, const Translation& t)
{
template <typename OtherDerived>
friend EIGEN_DEVICE_FUNC inline AffineTransformType operator*(const EigenBase<OtherDerived>& linear,
const Translation& t) {
AffineTransformType res;
res.matrix().setZero();
res.linear() = linear.derived();
res.translation() = linear.derived() * t.m_coeffs;
res.matrix().row(Dim).setZero();
res(Dim,Dim) = Scalar(1);
res(Dim, Dim) = Scalar(1);
return res;
}
/** Concatenates a translation and a transformation */
template<int Mode, int Options>
EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Mode> operator* (const Transform<Scalar,Dim,Mode,Options>& t) const
{
Transform<Scalar,Dim,Mode> res = t;
template <int Mode, int Options>
EIGEN_DEVICE_FUNC inline Transform<Scalar, Dim, Mode> operator*(
const Transform<Scalar, Dim, Mode, Options>& t) const {
Transform<Scalar, Dim, Mode> res = t;
res.pretranslate(m_coeffs);
return res;
}
/** Applies translation to vector */
template<typename Derived>
inline std::enable_if_t<Derived::IsVectorAtCompileTime,VectorType>
operator* (const MatrixBase<Derived>& vec) const
{ return m_coeffs + vec.derived(); }
template <typename Derived>
inline std::enable_if_t<Derived::IsVectorAtCompileTime, VectorType> operator*(const MatrixBase<Derived>& vec) const {
return m_coeffs + vec.derived();
}
/** \returns the inverse translation (opposite) */
Translation inverse() const { return Translation(-m_coeffs); }
@@ -144,62 +141,64 @@ public:
static const Translation Identity() { return Translation(VectorType::Zero()); }
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Translation,Translation<NewScalarType,Dim> >::type cast() const
{ return typename internal::cast_return_type<Translation,Translation<NewScalarType,Dim> >::type(*this); }
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template <typename NewScalarType>
EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Translation, Translation<NewScalarType, Dim> >::type
cast() const {
return typename internal::cast_return_type<Translation, Translation<NewScalarType, Dim> >::type(*this);
}
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
EIGEN_DEVICE_FUNC inline explicit Translation(const Translation<OtherScalarType,Dim>& other)
{ m_coeffs = other.vector().template cast<Scalar>(); }
template <typename OtherScalarType>
EIGEN_DEVICE_FUNC inline explicit Translation(const Translation<OtherScalarType, Dim>& other) {
m_coeffs = other.vector().template cast<Scalar>();
}
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
EIGEN_DEVICE_FUNC bool isApprox(const Translation& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
{ return m_coeffs.isApprox(other.m_coeffs, prec); }
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
EIGEN_DEVICE_FUNC bool isApprox(const Translation& other, const typename NumTraits<Scalar>::Real& prec =
NumTraits<Scalar>::dummy_precision()) const {
return m_coeffs.isApprox(other.m_coeffs, prec);
}
};
/** \addtogroup Geometry_Module */
//@{
typedef Translation<float, 2> Translation2f;
typedef Translation<double,2> Translation2d;
typedef Translation<double, 2> Translation2d;
typedef Translation<float, 3> Translation3f;
typedef Translation<double,3> Translation3d;
typedef Translation<double, 3> Translation3d;
//@}
template<typename Scalar, int Dim>
EIGEN_DEVICE_FUNC inline typename Translation<Scalar,Dim>::AffineTransformType
Translation<Scalar,Dim>::operator* (const UniformScaling<Scalar>& other) const
{
template <typename Scalar, int Dim>
EIGEN_DEVICE_FUNC inline typename Translation<Scalar, Dim>::AffineTransformType Translation<Scalar, Dim>::operator*(
const UniformScaling<Scalar>& other) const {
AffineTransformType res;
res.matrix().setZero();
res.linear().diagonal().fill(other.factor());
res.translation() = m_coeffs;
res(Dim,Dim) = Scalar(1);
res(Dim, Dim) = Scalar(1);
return res;
}
template<typename Scalar, int Dim>
template<typename OtherDerived>
EIGEN_DEVICE_FUNC inline typename Translation<Scalar,Dim>::AffineTransformType
Translation<Scalar,Dim>::operator* (const EigenBase<OtherDerived>& linear) const
{
template <typename Scalar, int Dim>
template <typename OtherDerived>
EIGEN_DEVICE_FUNC inline typename Translation<Scalar, Dim>::AffineTransformType Translation<Scalar, Dim>::operator*(
const EigenBase<OtherDerived>& linear) const {
AffineTransformType res;
res.matrix().setZero();
res.linear() = linear.derived();
res.translation() = m_coeffs;
res.matrix().row(Dim).setZero();
res(Dim,Dim) = Scalar(1);
res(Dim, Dim) = Scalar(1);
return res;
}
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_TRANSLATION_H
#endif // EIGEN_TRANSLATION_H

144
Eigen/src/Geometry/Umeyama.h
View File

@@ -10,16 +10,16 @@
#ifndef EIGEN_UMEYAMA_H
#define EIGEN_UMEYAMA_H
// This file requires the user to include
// This file requires the user to include
// * Eigen/Core
// * Eigen/LU
// * Eigen/LU
// * Eigen/SVD
// * Eigen/Array
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
namespace Eigen {
namespace Eigen {
#ifndef EIGEN_PARSED_BY_DOXYGEN
@@ -31,79 +31,76 @@ namespace internal {
// Compile time return type deduction for different MatrixBase types.
// Different means here different alignment and parameters but the same underlying
// real scalar type.
template<typename MatrixType, typename OtherMatrixType>
struct umeyama_transform_matrix_type
{
template <typename MatrixType, typename OtherMatrixType>
struct umeyama_transform_matrix_type {
enum {
MinRowsAtCompileTime = internal::min_size_prefer_dynamic(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime),
MinRowsAtCompileTime =
internal::min_size_prefer_dynamic(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime),
// When possible we want to choose some small fixed size value since the result
// is likely to fit on the stack. So here, min_size_prefer_dynamic is not what we want.
HomogeneousDimension = int(MinRowsAtCompileTime) == Dynamic ? Dynamic : int(MinRowsAtCompileTime)+1
HomogeneousDimension = int(MinRowsAtCompileTime) == Dynamic ? Dynamic : int(MinRowsAtCompileTime) + 1
};
typedef Matrix<typename traits<MatrixType>::Scalar,
HomogeneousDimension,
HomogeneousDimension,
AutoAlign | (traits<MatrixType>::Flags & RowMajorBit ? RowMajor : ColMajor),
HomogeneousDimension,
HomogeneousDimension
> type;
typedef Matrix<typename traits<MatrixType>::Scalar, HomogeneousDimension, HomogeneousDimension,
AutoAlign | (traits<MatrixType>::Flags & RowMajorBit ? RowMajor : ColMajor), HomogeneousDimension,
HomogeneousDimension>
type;
};
}
} // namespace internal
#endif
/**
* \geometry_module \ingroup Geometry_Module
*
* \brief Returns the transformation between two point sets.
*
* The algorithm is based on:
* "Least-squares estimation of transformation parameters between two point patterns",
* Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573
*
* It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that
* \f{align*}
* \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2
* \f}
* is minimized.
*
* The algorithm is based on the analysis of the covariance matrix
* \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$
* of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where
* \f$d\f$ is corresponding to the dimension (which is typically small).
* The analysis is involving the SVD having a complexity of \f$O(d^3)\f$
* though the actual computational effort lies in the covariance
* matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when
* the input point sets have dimension \f$d \times m\f$.
*
* Currently the method is working only for floating point matrices.
*
* \todo Should the return type of umeyama() become a Transform?
*
* \param src Source points \f$ \mathbf{x} = \left( x_1, \hdots, x_n \right) \f$.
* \param dst Destination points \f$ \mathbf{y} = \left( y_1, \hdots, y_n \right) \f$.
* \param with_scaling Sets \f$ c=1 \f$ when <code>false</code> is passed.
* \return The homogeneous transformation
* \f{align*}
* T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}
* \f}
* minimizing the residual above. This transformation is always returned as an
* Eigen::Matrix.
*/
* \geometry_module \ingroup Geometry_Module
*
* \brief Returns the transformation between two point sets.
*
* The algorithm is based on:
* "Least-squares estimation of transformation parameters between two point patterns",
* Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573
*
* It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that
* \f{align*}
* \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2
* \f}
* is minimized.
*
* The algorithm is based on the analysis of the covariance matrix
* \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$
* of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where
* \f$d\f$ is corresponding to the dimension (which is typically small).
* The analysis is involving the SVD having a complexity of \f$O(d^3)\f$
* though the actual computational effort lies in the covariance
* matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when
* the input point sets have dimension \f$d \times m\f$.
*
* Currently the method is working only for floating point matrices.
*
* \todo Should the return type of umeyama() become a Transform?
*
* \param src Source points \f$ \mathbf{x} = \left( x_1, \hdots, x_n \right) \f$.
* \param dst Destination points \f$ \mathbf{y} = \left( y_1, \hdots, y_n \right) \f$.
* \param with_scaling Sets \f$ c=1 \f$ when <code>false</code> is passed.
* \return The homogeneous transformation
* \f{align*}
* T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}
* \f}
* minimizing the residual above. This transformation is always returned as an
* Eigen::Matrix.
*/
template <typename Derived, typename OtherDerived>
typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, bool with_scaling = true)
{
typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type umeyama(
const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, bool with_scaling = true) {
typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType;
typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL)
EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename internal::traits<OtherDerived>::Scalar>::value),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
EIGEN_STATIC_ASSERT(
(internal::is_same<Scalar, typename internal::traits<OtherDerived>::Scalar>::value),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
enum { Dimension = internal::min_size_prefer_dynamic(Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
@@ -111,8 +108,8 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo
typedef Matrix<Scalar, Dimension, Dimension> MatrixType;
typedef typename internal::plain_matrix_type_row_major<Derived>::type RowMajorMatrixType;
const Index m = src.rows(); // dimension
const Index n = src.cols(); // number of measurements
const Index m = src.rows(); // dimension
const Index n = src.cols(); // number of measurements
// required for demeaning ...
const RealScalar one_over_n = RealScalar(1) / static_cast<RealScalar>(n);
@@ -131,41 +128,38 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo
JacobiSVD<MatrixType, ComputeFullU | ComputeFullV> svd(sigma);
// Initialize the resulting transformation with an identity matrix...
TransformationMatrixType Rt = TransformationMatrixType::Identity(m+1,m+1);
TransformationMatrixType Rt = TransformationMatrixType::Identity(m + 1, m + 1);
// Eq. (39)
VectorType S = VectorType::Ones(m);
if ( svd.matrixU().determinant() * svd.matrixV().determinant() < 0 ) {
Index tmp = m - 1;
if (svd.matrixU().determinant() * svd.matrixV().determinant() < 0) {
Index tmp = m - 1;
S(tmp) = -1;
}
// Eq. (40) and (43)
Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose();
Rt.block(0, 0, m, m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose();
if (with_scaling)
{
if (with_scaling) {
// Eq. (36)-(37)
const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n;
// Eq. (42)
const Scalar c = Scalar(1)/src_var * svd.singularValues().dot(S);
const Scalar c = Scalar(1) / src_var * svd.singularValues().dot(S);
// Eq. (41)
Rt.col(m).head(m) = dst_mean;
Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean;
Rt.block(0,0,m,m) *= c;
}
else
{
Rt.col(m).head(m).noalias() -= c * Rt.topLeftCorner(m, m) * src_mean;
Rt.block(0, 0, m, m) *= c;
} else {
Rt.col(m).head(m) = dst_mean;
Rt.col(m).head(m).noalias() -= Rt.topLeftCorner(m,m)*src_mean;
Rt.col(m).head(m).noalias() -= Rt.topLeftCorner(m, m) * src_mean;
}
return Rt;
}
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_UMEYAMA_H
#endif // EIGEN_UMEYAMA_H

176
Eigen/src/Geometry/arch/Geometry_SIMD.h
View File

@@ -14,158 +14,136 @@
// IWYU pragma: private
#include "../InternalHeaderCheck.h"
namespace Eigen {
namespace Eigen {
namespace internal {
template<class Derived, class OtherDerived>
struct quat_product<Architecture::Target, Derived, OtherDerived, float>
{
template <class Derived, class OtherDerived>
struct quat_product<Architecture::Target, Derived, OtherDerived, float> {
enum {
AAlignment = traits<Derived>::Alignment,
BAlignment = traits<OtherDerived>::Alignment,
ResAlignment = traits<Quaternion<float> >::Alignment
};
static inline Quaternion<float> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b)
{
static inline Quaternion<float> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b) {
evaluator<typename Derived::Coefficients> ae(_a.coeffs());
evaluator<typename OtherDerived::Coefficients> be(_b.coeffs());
Quaternion<float> res;
const float neg_zero = numext::bit_cast<float>(0x80000000u);
const float arr[4] = {0.f, 0.f, 0.f, neg_zero};
const Packet4f mask = ploadu<Packet4f>(arr);
Packet4f a = ae.template packet<AAlignment,Packet4f>(0);
Packet4f b = be.template packet<BAlignment,Packet4f>(0);
Packet4f s1 = pmul(vec4f_swizzle1(a,1,2,0,2),vec4f_swizzle1(b,2,0,1,2));
Packet4f s2 = pmul(vec4f_swizzle1(a,3,3,3,1),vec4f_swizzle1(b,0,1,2,1));
pstoret<float,Packet4f,ResAlignment>(
&res.x(),
padd(psub(pmul(a,vec4f_swizzle1(b,3,3,3,3)),
pmul(vec4f_swizzle1(a,2,0,1,0),
vec4f_swizzle1(b,1,2,0,0))),
pxor(mask,padd(s1,s2))));
Packet4f a = ae.template packet<AAlignment, Packet4f>(0);
Packet4f b = be.template packet<BAlignment, Packet4f>(0);
Packet4f s1 = pmul(vec4f_swizzle1(a, 1, 2, 0, 2), vec4f_swizzle1(b, 2, 0, 1, 2));
Packet4f s2 = pmul(vec4f_swizzle1(a, 3, 3, 3, 1), vec4f_swizzle1(b, 0, 1, 2, 1));
pstoret<float, Packet4f, ResAlignment>(
&res.x(), padd(psub(pmul(a, vec4f_swizzle1(b, 3, 3, 3, 3)),
pmul(vec4f_swizzle1(a, 2, 0, 1, 0), vec4f_swizzle1(b, 1, 2, 0, 0))),
pxor(mask, padd(s1, s2))));
return res;
}
};
template<class Derived>
struct quat_conj<Architecture::Target, Derived, float>
{
enum {
ResAlignment = traits<Quaternion<float> >::Alignment
};
static inline Quaternion<float> run(const QuaternionBase<Derived>& q)
{
template <class Derived>
struct quat_conj<Architecture::Target, Derived, float> {
enum { ResAlignment = traits<Quaternion<float> >::Alignment };
static inline Quaternion<float> run(const QuaternionBase<Derived>& q) {
evaluator<typename Derived::Coefficients> qe(q.coeffs());
Quaternion<float> res;
const float neg_zero = numext::bit_cast<float>(0x80000000u);
const float arr[4] = {neg_zero, neg_zero, neg_zero,0.f};
const float arr[4] = {neg_zero, neg_zero, neg_zero, 0.f};
const Packet4f mask = ploadu<Packet4f>(arr);
pstoret<float,Packet4f,ResAlignment>(&res.x(), pxor(mask, qe.template packet<traits<Derived>::Alignment,Packet4f>(0)));
pstoret<float, Packet4f, ResAlignment>(&res.x(),
pxor(mask, qe.template packet<traits<Derived>::Alignment, Packet4f>(0)));
return res;
}
};
template<typename VectorLhs,typename VectorRhs>
struct cross3_impl<Architecture::Target,VectorLhs,VectorRhs,float,true>
{
enum {
ResAlignment = traits<typename plain_matrix_type<VectorLhs>::type>::Alignment
};
static inline typename plain_matrix_type<VectorLhs>::type
run(const VectorLhs& lhs, const VectorRhs& rhs)
{
template <typename VectorLhs, typename VectorRhs>
struct cross3_impl<Architecture::Target, VectorLhs, VectorRhs, float, true> {
enum { ResAlignment = traits<typename plain_matrix_type<VectorLhs>::type>::Alignment };
static inline typename plain_matrix_type<VectorLhs>::type run(const VectorLhs& lhs, const VectorRhs& rhs) {
evaluator<VectorLhs> lhs_eval(lhs);
evaluator<VectorRhs> rhs_eval(rhs);
Packet4f a = lhs_eval.template packet<traits<VectorLhs>::Alignment,Packet4f>(0);
Packet4f b = rhs_eval.template packet<traits<VectorRhs>::Alignment,Packet4f>(0);
Packet4f mul1 = pmul(vec4f_swizzle1(a,1,2,0,3),vec4f_swizzle1(b,2,0,1,3));
Packet4f mul2 = pmul(vec4f_swizzle1(a,2,0,1,3),vec4f_swizzle1(b,1,2,0,3));
Packet4f a = lhs_eval.template packet<traits<VectorLhs>::Alignment, Packet4f>(0);
Packet4f b = rhs_eval.template packet<traits<VectorRhs>::Alignment, Packet4f>(0);
Packet4f mul1 = pmul(vec4f_swizzle1(a, 1, 2, 0, 3), vec4f_swizzle1(b, 2, 0, 1, 3));
Packet4f mul2 = pmul(vec4f_swizzle1(a, 2, 0, 1, 3), vec4f_swizzle1(b, 1, 2, 0, 3));
typename plain_matrix_type<VectorLhs>::type res;
pstoret<float,Packet4f,ResAlignment>(&res.x(),psub(mul1,mul2));
pstoret<float, Packet4f, ResAlignment>(&res.x(), psub(mul1, mul2));
return res;
}
};
#if (defined EIGEN_VECTORIZE_SSE) || (EIGEN_ARCH_ARM64)
template<class Derived, class OtherDerived>
struct quat_product<Architecture::Target, Derived, OtherDerived, double>
{
enum {
BAlignment = traits<OtherDerived>::Alignment,
ResAlignment = traits<Quaternion<double> >::Alignment
};
template <class Derived, class OtherDerived>
struct quat_product<Architecture::Target, Derived, OtherDerived, double> {
enum { BAlignment = traits<OtherDerived>::Alignment, ResAlignment = traits<Quaternion<double> >::Alignment };
static inline Quaternion<double> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b)
{
Quaternion<double> res;
static inline Quaternion<double> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b) {
Quaternion<double> res;
evaluator<typename Derived::Coefficients> ae(_a.coeffs());
evaluator<typename OtherDerived::Coefficients> be(_b.coeffs());
evaluator<typename Derived::Coefficients> ae(_a.coeffs());
evaluator<typename OtherDerived::Coefficients> be(_b.coeffs());
const double* a = _a.coeffs().data();
Packet2d b_xy = be.template packet<BAlignment,Packet2d>(0);
Packet2d b_zw = be.template packet<BAlignment,Packet2d>(2);
Packet2d a_xx = pset1<Packet2d>(a[0]);
Packet2d a_yy = pset1<Packet2d>(a[1]);
Packet2d a_zz = pset1<Packet2d>(a[2]);
Packet2d a_ww = pset1<Packet2d>(a[3]);
const double* a = _a.coeffs().data();
Packet2d b_xy = be.template packet<BAlignment, Packet2d>(0);
Packet2d b_zw = be.template packet<BAlignment, Packet2d>(2);
Packet2d a_xx = pset1<Packet2d>(a[0]);
Packet2d a_yy = pset1<Packet2d>(a[1]);
Packet2d a_zz = pset1<Packet2d>(a[2]);
Packet2d a_ww = pset1<Packet2d>(a[3]);
// two temporaries:
Packet2d t1, t2;
// two temporaries:
Packet2d t1, t2;
/*
* t1 = ww*xy + yy*zw
* t2 = zz*xy - xx*zw
* res.xy = t1 +/- swap(t2)
*/
t1 = padd(pmul(a_ww, b_xy), pmul(a_yy, b_zw));
t2 = psub(pmul(a_zz, b_xy), pmul(a_xx, b_zw));
pstoret<double,Packet2d,ResAlignment>(&res.x(), paddsub(t1, preverse(t2)));
/*
* t1 = ww*zw - yy*xy
* t2 = zz*zw + xx*xy
* res.zw = t1 -/+ swap(t2) = swap( swap(t1) +/- t2)
*/
t1 = psub(pmul(a_ww, b_zw), pmul(a_yy, b_xy));
t2 = padd(pmul(a_zz, b_zw), pmul(a_xx, b_xy));
pstoret<double,Packet2d,ResAlignment>(&res.z(), preverse(paddsub(preverse(t1), t2)));
/*
* t1 = ww*xy + yy*zw
* t2 = zz*xy - xx*zw
* res.xy = t1 +/- swap(t2)
*/
t1 = padd(pmul(a_ww, b_xy), pmul(a_yy, b_zw));
t2 = psub(pmul(a_zz, b_xy), pmul(a_xx, b_zw));
pstoret<double, Packet2d, ResAlignment>(&res.x(), paddsub(t1, preverse(t2)));
return res;
}
/*
* t1 = ww*zw - yy*xy
* t2 = zz*zw + xx*xy
* res.zw = t1 -/+ swap(t2) = swap( swap(t1) +/- t2)
*/
t1 = psub(pmul(a_ww, b_zw), pmul(a_yy, b_xy));
t2 = padd(pmul(a_zz, b_zw), pmul(a_xx, b_xy));
pstoret<double, Packet2d, ResAlignment>(&res.z(), preverse(paddsub(preverse(t1), t2)));
return res;
}
};
template<class Derived>
struct quat_conj<Architecture::Target, Derived, double>
{
enum {
ResAlignment = traits<Quaternion<double> >::Alignment
};
static inline Quaternion<double> run(const QuaternionBase<Derived>& q)
{
template <class Derived>
struct quat_conj<Architecture::Target, Derived, double> {
enum { ResAlignment = traits<Quaternion<double> >::Alignment };
static inline Quaternion<double> run(const QuaternionBase<Derived>& q) {
evaluator<typename Derived::Coefficients> qe(q.coeffs());
Quaternion<double> res;
const double neg_zero = numext::bit_cast<double>(0x8000000000000000ull);
const double arr1[2] = {neg_zero, neg_zero};
const double arr2[2] = {neg_zero, 0.0};
const double arr2[2] = {neg_zero, 0.0};
const Packet2d mask0 = ploadu<Packet2d>(arr1);
const Packet2d mask2 = ploadu<Packet2d>(arr2);
pstoret<double,Packet2d,ResAlignment>(&res.x(), pxor(mask0, qe.template packet<traits<Derived>::Alignment,Packet2d>(0)));
pstoret<double,Packet2d,ResAlignment>(&res.z(), pxor(mask2, qe.template packet<traits<Derived>::Alignment,Packet2d>(2)));
pstoret<double, Packet2d, ResAlignment>(&res.x(),
pxor(mask0, qe.template packet<traits<Derived>::Alignment, Packet2d>(0)));
pstoret<double, Packet2d, ResAlignment>(&res.z(),
pxor(mask2, qe.template packet<traits<Derived>::Alignment, Packet2d>(2)));
return res;
}
};
#endif // end EIGEN_VECTORIZE_SSE_OR_EIGEN_ARCH_ARM64
#endif // end EIGEN_VECTORIZE_SSE_OR_EIGEN_ARCH_ARM64
} // end namespace internal
} // end namespace internal
} // end namespace Eigen
} // end namespace Eigen
#endif // EIGEN_GEOMETRY_SIMD_H
#endif // EIGEN_GEOMETRY_SIMD_H