1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
5 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
6 // Copyright (C) 2010 Hauke Heibel <hauke.heibel@gmail.com>
7 //
8 // This Source Code Form is subject to the terms of the Mozilla
9 // Public License v. 2.0. If a copy of the MPL was not distributed
10 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
11 
12 #ifndef EIGEN_TRANSFORM_H
13 #define EIGEN_TRANSFORM_H
14 
15 namespace Eigen {
16 
17 namespace internal {
18 
19 template<typename Transform>
20 struct transform_traits
21 {
22   enum
23   {
24     Dim = Transform::Dim,
25     HDim = Transform::HDim,
26     Mode = Transform::Mode,
27     IsProjective = (int(Mode)==int(Projective))
28   };
29 };
30 
31 template< typename TransformType,
32           typename MatrixType,
33           int Case = transform_traits<TransformType>::IsProjective ? 0
34                    : int(MatrixType::RowsAtCompileTime) == int(transform_traits<TransformType>::HDim) ? 1
35                    : 2>
36 struct transform_right_product_impl;
37 
38 template< typename Other,
39           int Mode,
40           int Options,
41           int Dim,
42           int HDim,
43           int OtherRows=Other::RowsAtCompileTime,
44           int OtherCols=Other::ColsAtCompileTime>
45 struct transform_left_product_impl;
46 
47 template< typename Lhs,
48           typename Rhs,
49           bool AnyProjective =
50             transform_traits<Lhs>::IsProjective ||
51             transform_traits<Rhs>::IsProjective>
52 struct transform_transform_product_impl;
53 
54 template< typename Other,
55           int Mode,
56           int Options,
57           int Dim,
58           int HDim,
59           int OtherRows=Other::RowsAtCompileTime,
60           int OtherCols=Other::ColsAtCompileTime>
61 struct transform_construct_from_matrix;
62 
63 template<typename TransformType> struct transform_take_affine_part;
64 
65 template<int Mode> struct transform_make_affine;
66 
67 } // end namespace internal
68 
69 /** \geometry_module \ingroup Geometry_Module
70   *
71   * \class Transform
72   *
73   * \brief Represents an homogeneous transformation in a N dimensional space
74   *
75   * \tparam _Scalar the scalar type, i.e., the type of the coefficients
76   * \tparam _Dim the dimension of the space
77   * \tparam _Mode the type of the transformation. Can be:
78   *              - #Affine: the transformation is stored as a (Dim+1)^2 matrix,
79   *                         where the last row is assumed to be [0 ... 0 1].
80   *              - #AffineCompact: the transformation is stored as a (Dim)x(Dim+1) matrix.
81   *              - #Projective: the transformation is stored as a (Dim+1)^2 matrix
82   *                             without any assumption.
83   * \tparam _Options has the same meaning as in class Matrix. It allows to specify DontAlign and/or RowMajor.
84   *                  These Options are passed directly to the underlying matrix type.
85   *
86   * The homography is internally represented and stored by a matrix which
87   * is available through the matrix() method. To understand the behavior of
88   * this class you have to think a Transform object as its internal
89   * matrix representation. The chosen convention is right multiply:
90   *
91   * \code v' = T * v \endcode
92   *
93   * Therefore, an affine transformation matrix M is shaped like this:
94   *
95   * \f$ \left( \begin{array}{cc}
96   * linear & translation\\
97   * 0 ... 0 & 1
98   * \end{array} \right) \f$
99   *
100   * Note that for a projective transformation the last row can be anything,
101   * and then the interpretation of different parts might be sightly different.
102   *
103   * However, unlike a plain matrix, the Transform class provides many features
104   * simplifying both its assembly and usage. In particular, it can be composed
105   * with any other transformations (Transform,Translation,RotationBase,Matrix)
106   * and can be directly used to transform implicit homogeneous vectors. All these
107   * operations are handled via the operator*. For the composition of transformations,
108   * its principle consists to first convert the right/left hand sides of the product
109   * to a compatible (Dim+1)^2 matrix and then perform a pure matrix product.
110   * Of course, internally, operator* tries to perform the minimal number of operations
111   * according to the nature of each terms. Likewise, when applying the transform
112   * to non homogeneous vectors, the latters are automatically promoted to homogeneous
113   * one before doing the matrix product. The convertions to homogeneous representations
114   * are performed as follow:
115   *
116   * \b Translation t (Dim)x(1):
117   * \f$ \left( \begin{array}{cc}
118   * I & t \\
119   * 0\,...\,0 & 1
120   * \end{array} \right) \f$
121   *
122   * \b Rotation R (Dim)x(Dim):
123   * \f$ \left( \begin{array}{cc}
124   * R & 0\\
125   * 0\,...\,0 & 1
126   * \end{array} \right) \f$
127   *
128   * \b Linear \b Matrix L (Dim)x(Dim):
129   * \f$ \left( \begin{array}{cc}
130   * L & 0\\
131   * 0\,...\,0 & 1
132   * \end{array} \right) \f$
133   *
134   * \b Affine \b Matrix A (Dim)x(Dim+1):
135   * \f$ \left( \begin{array}{c}
136   * A\\
137   * 0\,...\,0\,1
138   * \end{array} \right) \f$
139   *
140   * \b Column \b vector v (Dim)x(1):
141   * \f$ \left( \begin{array}{c}
142   * v\\
143   * 1
144   * \end{array} \right) \f$
145   *
146   * \b Set \b of \b column \b vectors V1...Vn (Dim)x(n):
147   * \f$ \left( \begin{array}{ccc}
148   * v_1 & ... & v_n\\
149   * 1 & ... & 1
150   * \end{array} \right) \f$
151   *
152   * The concatenation of a Transform object with any kind of other transformation
153   * always returns a Transform object.
154   *
155   * A little exception to the "as pure matrix product" rule is the case of the
156   * transformation of non homogeneous vectors by an affine transformation. In
157   * that case the last matrix row can be ignored, and the product returns non
158   * homogeneous vectors.
159   *
160   * Since, for instance, a Dim x Dim matrix is interpreted as a linear transformation,
161   * it is not possible to directly transform Dim vectors stored in a Dim x Dim matrix.
162   * The solution is either to use a Dim x Dynamic matrix or explicitly request a
163   * vector transformation by making the vector homogeneous:
164   * \code
165   * m' = T * m.colwise().homogeneous();
166   * \endcode
167   * Note that there is zero overhead.
168   *
169   * Conversion methods from/to Qt's QMatrix and QTransform are available if the
170   * preprocessor token EIGEN_QT_SUPPORT is defined.
171   *
172   * This class can be extended with the help of the plugin mechanism described on the page
173   * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_TRANSFORM_PLUGIN.
174   *
175   * \sa class Matrix, class Quaternion
176   */
177 template<typename _Scalar, int _Dim, int _Mode, int _Options>
178 class Transform
179 {
180 public:
181   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim==Dynamic ? Dynamic : (_Dim+1)*(_Dim+1))
182   enum {
183     Mode = _Mode,
184     Options = _Options,
185     Dim = _Dim,     ///< space dimension in which the transformation holds
186     HDim = _Dim+1,  ///< size of a respective homogeneous vector
187     Rows = int(Mode)==(AffineCompact) ? Dim : HDim
188   };
189   /** the scalar type of the coefficients */
190   typedef _Scalar Scalar;
191   typedef DenseIndex Index;
192   /** type of the matrix used to represent the transformation */
193   typedef typename internal::make_proper_matrix_type<Scalar,Rows,HDim,Options>::type MatrixType;
194   /** constified MatrixType */
195   typedef const MatrixType ConstMatrixType;
196   /** type of the matrix used to represent the linear part of the transformation */
197   typedef Matrix<Scalar,Dim,Dim,Options> LinearMatrixType;
198   /** type of read/write reference to the linear part of the transformation */
199   typedef Block<MatrixType,Dim,Dim,int(Mode)==(AffineCompact) && (Options&RowMajor)==0> LinearPart;
200   /** type of read reference to the linear part of the transformation */
201   typedef const Block<ConstMatrixType,Dim,Dim,int(Mode)==(AffineCompact) && (Options&RowMajor)==0> ConstLinearPart;
202   /** type of read/write reference to the affine part of the transformation */
203   typedef typename internal::conditional<int(Mode)==int(AffineCompact),
204                               MatrixType&,
205                               Block<MatrixType,Dim,HDim> >::type AffinePart;
206   /** type of read reference to the affine part of the transformation */
207   typedef typename internal::conditional<int(Mode)==int(AffineCompact),
208                               const MatrixType&,
209                               const Block<const MatrixType,Dim,HDim> >::type ConstAffinePart;
210   /** type of a vector */
211   typedef Matrix<Scalar,Dim,1> VectorType;
212   /** type of a read/write reference to the translation part of the rotation */
213   typedef Block<MatrixType,Dim,1,int(Mode)==(AffineCompact)> TranslationPart;
214   /** type of a read reference to the translation part of the rotation */
215   typedef const Block<ConstMatrixType,Dim,1,int(Mode)==(AffineCompact)> ConstTranslationPart;
216   /** corresponding translation type */
217   typedef Translation<Scalar,Dim> TranslationType;
218 
219   // this intermediate enum is needed to avoid an ICE with gcc 3.4 and 4.0
220   enum { TransformTimeDiagonalMode = ((Mode==int(Isometry))?Affine:int(Mode)) };
221   /** The return type of the product between a diagonal matrix and a transform */
222   typedef Transform<Scalar,Dim,TransformTimeDiagonalMode> TransformTimeDiagonalReturnType;
223 
224 protected:
225 
226   MatrixType m_matrix;
227 
228 public:
229 
230   /** Default constructor without initialization of the meaningful coefficients.
231     * If Mode==Affine, then the last row is set to [0 ... 0 1] */
Transform()232   inline Transform()
233   {
234     check_template_params();
235     internal::transform_make_affine<(int(Mode)==Affine) ? Affine : AffineCompact>::run(m_matrix);
236   }
237 
Transform(const Transform & other)238   inline Transform(const Transform& other)
239   {
240     check_template_params();
241     m_matrix = other.m_matrix;
242   }
243 
Transform(const TranslationType & t)244   inline explicit Transform(const TranslationType& t)
245   {
246     check_template_params();
247     *this = t;
248   }
Transform(const UniformScaling<Scalar> & s)249   inline explicit Transform(const UniformScaling<Scalar>& s)
250   {
251     check_template_params();
252     *this = s;
253   }
254   template<typename Derived>
Transform(const RotationBase<Derived,Dim> & r)255   inline explicit Transform(const RotationBase<Derived, Dim>& r)
256   {
257     check_template_params();
258     *this = r;
259   }
260 
261   inline Transform& operator=(const Transform& other)
262   { m_matrix = other.m_matrix; return *this; }
263 
264   typedef internal::transform_take_affine_part<Transform> take_affine_part;
265 
266   /** Constructs and initializes a transformation from a Dim^2 or a (Dim+1)^2 matrix. */
267   template<typename OtherDerived>
Transform(const EigenBase<OtherDerived> & other)268   inline explicit Transform(const EigenBase<OtherDerived>& other)
269   {
270     EIGEN_STATIC_ASSERT((internal::is_same<Scalar,typename OtherDerived::Scalar>::value),
271       YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY);
272 
273     check_template_params();
274     internal::transform_construct_from_matrix<OtherDerived,Mode,Options,Dim,HDim>::run(this, other.derived());
275   }
276 
277   /** Set \c *this from a Dim^2 or (Dim+1)^2 matrix. */
278   template<typename OtherDerived>
279   inline Transform& operator=(const EigenBase<OtherDerived>& other)
280   {
281     EIGEN_STATIC_ASSERT((internal::is_same<Scalar,typename OtherDerived::Scalar>::value),
282       YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY);
283 
284     internal::transform_construct_from_matrix<OtherDerived,Mode,Options,Dim,HDim>::run(this, other.derived());
285     return *this;
286   }
287 
288   template<int OtherOptions>
Transform(const Transform<Scalar,Dim,Mode,OtherOptions> & other)289   inline Transform(const Transform<Scalar,Dim,Mode,OtherOptions>& other)
290   {
291     check_template_params();
292     // only the options change, we can directly copy the matrices
293     m_matrix = other.matrix();
294   }
295 
296   template<int OtherMode,int OtherOptions>
Transform(const Transform<Scalar,Dim,OtherMode,OtherOptions> & other)297   inline Transform(const Transform<Scalar,Dim,OtherMode,OtherOptions>& other)
298   {
299     check_template_params();
300     // prevent conversions as:
301     // Affine | AffineCompact | Isometry = Projective
302     EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(OtherMode==int(Projective), Mode==int(Projective)),
303                         YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION)
304 
305     // prevent conversions as:
306     // Isometry = Affine | AffineCompact
307     EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(OtherMode==int(Affine)||OtherMode==int(AffineCompact), Mode!=int(Isometry)),
308                         YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION)
309 
310     enum { ModeIsAffineCompact = Mode == int(AffineCompact),
311            OtherModeIsAffineCompact = OtherMode == int(AffineCompact)
312     };
313 
314     if(ModeIsAffineCompact == OtherModeIsAffineCompact)
315     {
316       // We need the block expression because the code is compiled for all
317       // combinations of transformations and will trigger a compile time error
318       // if one tries to assign the matrices directly
319       m_matrix.template block<Dim,Dim+1>(0,0) = other.matrix().template block<Dim,Dim+1>(0,0);
320       makeAffine();
321     }
322     else if(OtherModeIsAffineCompact)
323     {
324       typedef typename Transform<Scalar,Dim,OtherMode,OtherOptions>::MatrixType OtherMatrixType;
325       internal::transform_construct_from_matrix<OtherMatrixType,Mode,Options,Dim,HDim>::run(this, other.matrix());
326     }
327     else
328     {
329       // here we know that Mode == AffineCompact and OtherMode != AffineCompact.
330       // if OtherMode were Projective, the static assert above would already have caught it.
331       // So the only possibility is that OtherMode == Affine
332       linear() = other.linear();
333       translation() = other.translation();
334     }
335   }
336 
337   template<typename OtherDerived>
Transform(const ReturnByValue<OtherDerived> & other)338   Transform(const ReturnByValue<OtherDerived>& other)
339   {
340     check_template_params();
341     other.evalTo(*this);
342   }
343 
344   template<typename OtherDerived>
345   Transform& operator=(const ReturnByValue<OtherDerived>& other)
346   {
347     other.evalTo(*this);
348     return *this;
349   }
350 
351   #ifdef EIGEN_QT_SUPPORT
352   inline Transform(const QMatrix& other);
353   inline Transform& operator=(const QMatrix& other);
354   inline QMatrix toQMatrix(void) const;
355   inline Transform(const QTransform& other);
356   inline Transform& operator=(const QTransform& other);
357   inline QTransform toQTransform(void) const;
358   #endif
359 
360   /** shortcut for m_matrix(row,col);
361     * \sa MatrixBase::operator(Index,Index) const */
operator()362   inline Scalar operator() (Index row, Index col) const { return m_matrix(row,col); }
363   /** shortcut for m_matrix(row,col);
364     * \sa MatrixBase::operator(Index,Index) */
operator()365   inline Scalar& operator() (Index row, Index col) { return m_matrix(row,col); }
366 
367   /** \returns a read-only expression of the transformation matrix */
matrix()368   inline const MatrixType& matrix() const { return m_matrix; }
369   /** \returns a writable expression of the transformation matrix */
matrix()370   inline MatrixType& matrix() { return m_matrix; }
371 
372   /** \returns a read-only expression of the linear part of the transformation */
linear()373   inline ConstLinearPart linear() const { return ConstLinearPart(m_matrix,0,0); }
374   /** \returns a writable expression of the linear part of the transformation */
linear()375   inline LinearPart linear() { return LinearPart(m_matrix,0,0); }
376 
377   /** \returns a read-only expression of the Dim x HDim affine part of the transformation */
affine()378   inline ConstAffinePart affine() const { return take_affine_part::run(m_matrix); }
379   /** \returns a writable expression of the Dim x HDim affine part of the transformation */
affine()380   inline AffinePart affine() { return take_affine_part::run(m_matrix); }
381 
382   /** \returns a read-only expression of the translation vector of the transformation */
translation()383   inline ConstTranslationPart translation() const { return ConstTranslationPart(m_matrix,0,Dim); }
384   /** \returns a writable expression of the translation vector of the transformation */
translation()385   inline TranslationPart translation() { return TranslationPart(m_matrix,0,Dim); }
386 
387   /** \returns an expression of the product between the transform \c *this and a matrix expression \a other
388     *
389     * The right hand side \a other might be either:
390     * \li a vector of size Dim,
391     * \li an homogeneous vector of size Dim+1,
392     * \li a set of vectors of size Dim x Dynamic,
393     * \li a set of homogeneous vectors of size Dim+1 x Dynamic,
394     * \li a linear transformation matrix of size Dim x Dim,
395     * \li an affine transformation matrix of size Dim x Dim+1,
396     * \li a transformation matrix of size Dim+1 x Dim+1.
397     */
398   // note: this function is defined here because some compilers cannot find the respective declaration
399   template<typename OtherDerived>
400   EIGEN_STRONG_INLINE const typename internal::transform_right_product_impl<Transform, OtherDerived>::ResultType
401   operator * (const EigenBase<OtherDerived> &other) const
402   { return internal::transform_right_product_impl<Transform, OtherDerived>::run(*this,other.derived()); }
403 
404   /** \returns the product expression of a transformation matrix \a a times a transform \a b
405     *
406     * The left hand side \a other might be either:
407     * \li a linear transformation matrix of size Dim x Dim,
408     * \li an affine transformation matrix of size Dim x Dim+1,
409     * \li a general transformation matrix of size Dim+1 x Dim+1.
410     */
411   template<typename OtherDerived> friend
412   inline const typename internal::transform_left_product_impl<OtherDerived,Mode,Options,_Dim,_Dim+1>::ResultType
413     operator * (const EigenBase<OtherDerived> &a, const Transform &b)
414   { return internal::transform_left_product_impl<OtherDerived,Mode,Options,Dim,HDim>::run(a.derived(),b); }
415 
416   /** \returns The product expression of a transform \a a times a diagonal matrix \a b
417     *
418     * The rhs diagonal matrix is interpreted as an affine scaling transformation. The
419     * product results in a Transform of the same type (mode) as the lhs only if the lhs
420     * mode is no isometry. In that case, the returned transform is an affinity.
421     */
422   template<typename DiagonalDerived>
423   inline const TransformTimeDiagonalReturnType
424     operator * (const DiagonalBase<DiagonalDerived> &b) const
425   {
426     TransformTimeDiagonalReturnType res(*this);
427     res.linear() *= b;
428     return res;
429   }
430 
431   /** \returns The product expression of a diagonal matrix \a a times a transform \a b
432     *
433     * The lhs diagonal matrix is interpreted as an affine scaling transformation. The
434     * product results in a Transform of the same type (mode) as the lhs only if the lhs
435     * mode is no isometry. In that case, the returned transform is an affinity.
436     */
437   template<typename DiagonalDerived>
438   friend inline TransformTimeDiagonalReturnType
439     operator * (const DiagonalBase<DiagonalDerived> &a, const Transform &b)
440   {
441     TransformTimeDiagonalReturnType res;
442     res.linear().noalias() = a*b.linear();
443     res.translation().noalias() = a*b.translation();
444     if (Mode!=int(AffineCompact))
445       res.matrix().row(Dim) = b.matrix().row(Dim);
446     return res;
447   }
448 
449   template<typename OtherDerived>
450   inline Transform& operator*=(const EigenBase<OtherDerived>& other) { return *this = *this * other; }
451 
452   /** Concatenates two transformations */
453   inline const Transform operator * (const Transform& other) const
454   {
455     return internal::transform_transform_product_impl<Transform,Transform>::run(*this,other);
456   }
457 
458   #ifdef __INTEL_COMPILER
459 private:
460   // this intermediate structure permits to workaround a bug in ICC 11:
461   //   error: template instantiation resulted in unexpected function type of "Eigen::Transform<double, 3, 32, 0>
462   //             (const Eigen::Transform<double, 3, 2, 0> &) const"
463   //  (the meaning of a name may have changed since the template declaration -- the type of the template is:
464   // "Eigen::internal::transform_transform_product_impl<Eigen::Transform<double, 3, 32, 0>,
465   //     Eigen::Transform<double, 3, Mode, Options>, <expression>>::ResultType (const Eigen::Transform<double, 3, Mode, Options> &) const")
466   //
467   template<int OtherMode,int OtherOptions> struct icc_11_workaround
468   {
469     typedef internal::transform_transform_product_impl<Transform,Transform<Scalar,Dim,OtherMode,OtherOptions> > ProductType;
470     typedef typename ProductType::ResultType ResultType;
471   };
472 
473 public:
474   /** Concatenates two different transformations */
475   template<int OtherMode,int OtherOptions>
476   inline typename icc_11_workaround<OtherMode,OtherOptions>::ResultType
477     operator * (const Transform<Scalar,Dim,OtherMode,OtherOptions>& other) const
478   {
479     typedef typename icc_11_workaround<OtherMode,OtherOptions>::ProductType ProductType;
480     return ProductType::run(*this,other);
481   }
482   #else
483   /** Concatenates two different transformations */
484   template<int OtherMode,int OtherOptions>
485   inline typename internal::transform_transform_product_impl<Transform,Transform<Scalar,Dim,OtherMode,OtherOptions> >::ResultType
486     operator * (const Transform<Scalar,Dim,OtherMode,OtherOptions>& other) const
487   {
488     return internal::transform_transform_product_impl<Transform,Transform<Scalar,Dim,OtherMode,OtherOptions> >::run(*this,other);
489   }
490   #endif
491 
492   /** \sa MatrixBase::setIdentity() */
setIdentity()493   void setIdentity() { m_matrix.setIdentity(); }
494 
495   /**
496    * \brief Returns an identity transformation.
497    * \todo In the future this function should be returning a Transform expression.
498    */
Identity()499   static const Transform Identity()
500   {
501     return Transform(MatrixType::Identity());
502   }
503 
504   template<typename OtherDerived>
505   inline Transform& scale(const MatrixBase<OtherDerived> &other);
506 
507   template<typename OtherDerived>
508   inline Transform& prescale(const MatrixBase<OtherDerived> &other);
509 
510   inline Transform& scale(const Scalar& s);
511   inline Transform& prescale(const Scalar& s);
512 
513   template<typename OtherDerived>
514   inline Transform& translate(const MatrixBase<OtherDerived> &other);
515 
516   template<typename OtherDerived>
517   inline Transform& pretranslate(const MatrixBase<OtherDerived> &other);
518 
519   template<typename RotationType>
520   inline Transform& rotate(const RotationType& rotation);
521 
522   template<typename RotationType>
523   inline Transform& prerotate(const RotationType& rotation);
524 
525   Transform& shear(const Scalar& sx, const Scalar& sy);
526   Transform& preshear(const Scalar& sx, const Scalar& sy);
527 
528   inline Transform& operator=(const TranslationType& t);
529   inline Transform& operator*=(const TranslationType& t) { return translate(t.vector()); }
530   inline Transform operator*(const TranslationType& t) const;
531 
532   inline Transform& operator=(const UniformScaling<Scalar>& t);
533   inline Transform& operator*=(const UniformScaling<Scalar>& s) { return scale(s.factor()); }
534   inline Transform<Scalar,Dim,(int(Mode)==int(Isometry)?int(Affine):int(Mode))> operator*(const UniformScaling<Scalar>& s) const
535   {
536     Transform<Scalar,Dim,(int(Mode)==int(Isometry)?int(Affine):int(Mode)),Options> res = *this;
537     res.scale(s.factor());
538     return res;
539   }
540 
541   inline Transform& operator*=(const DiagonalMatrix<Scalar,Dim>& s) { linear() *= s; return *this; }
542 
543   template<typename Derived>
544   inline Transform& operator=(const RotationBase<Derived,Dim>& r);
545   template<typename Derived>
546   inline Transform& operator*=(const RotationBase<Derived,Dim>& r) { return rotate(r.toRotationMatrix()); }
547   template<typename Derived>
548   inline Transform operator*(const RotationBase<Derived,Dim>& r) const;
549 
550   const LinearMatrixType rotation() const;
551   template<typename RotationMatrixType, typename ScalingMatrixType>
552   void computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const;
553   template<typename ScalingMatrixType, typename RotationMatrixType>
554   void computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const;
555 
556   template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
557   Transform& fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
558     const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale);
559 
560   inline Transform inverse(TransformTraits traits = (TransformTraits)Mode) const;
561 
562   /** \returns a const pointer to the column major internal matrix */
data()563   const Scalar* data() const { return m_matrix.data(); }
564   /** \returns a non-const pointer to the column major internal matrix */
data()565   Scalar* data() { return m_matrix.data(); }
566 
567   /** \returns \c *this with scalar type casted to \a NewScalarType
568     *
569     * Note that if \a NewScalarType is equal to the current scalar type of \c *this
570     * then this function smartly returns a const reference to \c *this.
571     */
572   template<typename NewScalarType>
cast()573   inline typename internal::cast_return_type<Transform,Transform<NewScalarType,Dim,Mode,Options> >::type cast() const
574   { return typename internal::cast_return_type<Transform,Transform<NewScalarType,Dim,Mode,Options> >::type(*this); }
575 
576   /** Copy constructor with scalar type conversion */
577   template<typename OtherScalarType>
Transform(const Transform<OtherScalarType,Dim,Mode,Options> & other)578   inline explicit Transform(const Transform<OtherScalarType,Dim,Mode,Options>& other)
579   {
580     check_template_params();
581     m_matrix = other.matrix().template cast<Scalar>();
582   }
583 
584   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
585     * determined by \a prec.
586     *
587     * \sa MatrixBase::isApprox() */
588   bool isApprox(const Transform& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
589   { return m_matrix.isApprox(other.m_matrix, prec); }
590 
591   /** Sets the last row to [0 ... 0 1]
592     */
makeAffine()593   void makeAffine()
594   {
595     internal::transform_make_affine<int(Mode)>::run(m_matrix);
596   }
597 
598   /** \internal
599     * \returns the Dim x Dim linear part if the transformation is affine,
600     *          and the HDim x Dim part for projective transformations.
601     */
linearExt()602   inline Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,Dim> linearExt()
603   { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,Dim>(0,0); }
604   /** \internal
605     * \returns the Dim x Dim linear part if the transformation is affine,
606     *          and the HDim x Dim part for projective transformations.
607     */
linearExt()608   inline const Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,Dim> linearExt() const
609   { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,Dim>(0,0); }
610 
611   /** \internal
612     * \returns the translation part if the transformation is affine,
613     *          and the last column for projective transformations.
614     */
translationExt()615   inline Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,1> translationExt()
616   { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,1>(0,Dim); }
617   /** \internal
618     * \returns the translation part if the transformation is affine,
619     *          and the last column for projective transformations.
620     */
translationExt()621   inline const Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,1> translationExt() const
622   { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,1>(0,Dim); }
623 
624 
625   #ifdef EIGEN_TRANSFORM_PLUGIN
626   #include EIGEN_TRANSFORM_PLUGIN
627   #endif
628 
629 protected:
630   #ifndef EIGEN_PARSED_BY_DOXYGEN
check_template_params()631     static EIGEN_STRONG_INLINE void check_template_params()
632     {
633       EIGEN_STATIC_ASSERT((Options & (DontAlign|RowMajor)) == Options, INVALID_MATRIX_TEMPLATE_PARAMETERS)
634     }
635   #endif
636 
637 };
638 
639 /** \ingroup Geometry_Module */
640 typedef Transform<float,2,Isometry> Isometry2f;
641 /** \ingroup Geometry_Module */
642 typedef Transform<float,3,Isometry> Isometry3f;
643 /** \ingroup Geometry_Module */
644 typedef Transform<double,2,Isometry> Isometry2d;
645 /** \ingroup Geometry_Module */
646 typedef Transform<double,3,Isometry> Isometry3d;
647 
648 /** \ingroup Geometry_Module */
649 typedef Transform<float,2,Affine> Affine2f;
650 /** \ingroup Geometry_Module */
651 typedef Transform<float,3,Affine> Affine3f;
652 /** \ingroup Geometry_Module */
653 typedef Transform<double,2,Affine> Affine2d;
654 /** \ingroup Geometry_Module */
655 typedef Transform<double,3,Affine> Affine3d;
656 
657 /** \ingroup Geometry_Module */
658 typedef Transform<float,2,AffineCompact> AffineCompact2f;
659 /** \ingroup Geometry_Module */
660 typedef Transform<float,3,AffineCompact> AffineCompact3f;
661 /** \ingroup Geometry_Module */
662 typedef Transform<double,2,AffineCompact> AffineCompact2d;
663 /** \ingroup Geometry_Module */
664 typedef Transform<double,3,AffineCompact> AffineCompact3d;
665 
666 /** \ingroup Geometry_Module */
667 typedef Transform<float,2,Projective> Projective2f;
668 /** \ingroup Geometry_Module */
669 typedef Transform<float,3,Projective> Projective3f;
670 /** \ingroup Geometry_Module */
671 typedef Transform<double,2,Projective> Projective2d;
672 /** \ingroup Geometry_Module */
673 typedef Transform<double,3,Projective> Projective3d;
674 
675 /**************************
676 *** Optional QT support ***
677 **************************/
678 
679 #ifdef EIGEN_QT_SUPPORT
680 /** Initializes \c *this from a QMatrix assuming the dimension is 2.
681   *
682   * This function is available only if the token EIGEN_QT_SUPPORT is defined.
683   */
684 template<typename Scalar, int Dim, int Mode,int Options>
Transform(const QMatrix & other)685 Transform<Scalar,Dim,Mode,Options>::Transform(const QMatrix& other)
686 {
687   check_template_params();
688   *this = other;
689 }
690 
691 /** Set \c *this from a QMatrix assuming the dimension is 2.
692   *
693   * This function is available only if the token EIGEN_QT_SUPPORT is defined.
694   */
695 template<typename Scalar, int Dim, int Mode,int Options>
696 Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const QMatrix& other)
697 {
698   EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
699   m_matrix << other.m11(), other.m21(), other.dx(),
700               other.m12(), other.m22(), other.dy(),
701               0, 0, 1;
702   return *this;
703 }
704 
705 /** \returns a QMatrix from \c *this assuming the dimension is 2.
706   *
707   * \warning this conversion might loss data if \c *this is not affine
708   *
709   * This function is available only if the token EIGEN_QT_SUPPORT is defined.
710   */
711 template<typename Scalar, int Dim, int Mode, int Options>
toQMatrix(void)712 QMatrix Transform<Scalar,Dim,Mode,Options>::toQMatrix(void) const
713 {
714   check_template_params();
715   EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
716   return QMatrix(m_matrix.coeff(0,0), m_matrix.coeff(1,0),
717                  m_matrix.coeff(0,1), m_matrix.coeff(1,1),
718                  m_matrix.coeff(0,2), m_matrix.coeff(1,2));
719 }
720 
721 /** Initializes \c *this from a QTransform assuming the dimension is 2.
722   *
723   * This function is available only if the token EIGEN_QT_SUPPORT is defined.
724   */
725 template<typename Scalar, int Dim, int Mode,int Options>
Transform(const QTransform & other)726 Transform<Scalar,Dim,Mode,Options>::Transform(const QTransform& other)
727 {
728   check_template_params();
729   *this = other;
730 }
731 
732 /** Set \c *this from a QTransform assuming the dimension is 2.
733   *
734   * This function is available only if the token EIGEN_QT_SUPPORT is defined.
735   */
736 template<typename Scalar, int Dim, int Mode, int Options>
737 Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const QTransform& other)
738 {
739   check_template_params();
740   EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
741   if (Mode == int(AffineCompact))
742     m_matrix << other.m11(), other.m21(), other.dx(),
743                 other.m12(), other.m22(), other.dy();
744   else
745     m_matrix << other.m11(), other.m21(), other.dx(),
746                 other.m12(), other.m22(), other.dy(),
747                 other.m13(), other.m23(), other.m33();
748   return *this;
749 }
750 
751 /** \returns a QTransform from \c *this assuming the dimension is 2.
752   *
753   * This function is available only if the token EIGEN_QT_SUPPORT is defined.
754   */
755 template<typename Scalar, int Dim, int Mode, int Options>
toQTransform(void)756 QTransform Transform<Scalar,Dim,Mode,Options>::toQTransform(void) const
757 {
758   EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
759   if (Mode == int(AffineCompact))
760     return QTransform(m_matrix.coeff(0,0), m_matrix.coeff(1,0),
761                       m_matrix.coeff(0,1), m_matrix.coeff(1,1),
762                       m_matrix.coeff(0,2), m_matrix.coeff(1,2));
763   else
764     return QTransform(m_matrix.coeff(0,0), m_matrix.coeff(1,0), m_matrix.coeff(2,0),
765                       m_matrix.coeff(0,1), m_matrix.coeff(1,1), m_matrix.coeff(2,1),
766                       m_matrix.coeff(0,2), m_matrix.coeff(1,2), m_matrix.coeff(2,2));
767 }
768 #endif
769 
770 /*********************
771 *** Procedural API ***
772 *********************/
773 
774 /** Applies on the right the non uniform scale transformation represented
775   * by the vector \a other to \c *this and returns a reference to \c *this.
776   * \sa prescale()
777   */
778 template<typename Scalar, int Dim, int Mode, int Options>
779 template<typename OtherDerived>
780 Transform<Scalar,Dim,Mode,Options>&
scale(const MatrixBase<OtherDerived> & other)781 Transform<Scalar,Dim,Mode,Options>::scale(const MatrixBase<OtherDerived> &other)
782 {
783   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
784   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
785   linearExt().noalias() = (linearExt() * other.asDiagonal());
786   return *this;
787 }
788 
789 /** Applies on the right a uniform scale of a factor \a c to \c *this
790   * and returns a reference to \c *this.
791   * \sa prescale(Scalar)
792   */
793 template<typename Scalar, int Dim, int Mode, int Options>
scale(const Scalar & s)794 inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::scale(const Scalar& s)
795 {
796   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
797   linearExt() *= s;
798   return *this;
799 }
800 
801 /** Applies on the left the non uniform scale transformation represented
802   * by the vector \a other to \c *this and returns a reference to \c *this.
803   * \sa scale()
804   */
805 template<typename Scalar, int Dim, int Mode, int Options>
806 template<typename OtherDerived>
807 Transform<Scalar,Dim,Mode,Options>&
prescale(const MatrixBase<OtherDerived> & other)808 Transform<Scalar,Dim,Mode,Options>::prescale(const MatrixBase<OtherDerived> &other)
809 {
810   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
811   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
812   m_matrix.template block<Dim,HDim>(0,0).noalias() = (other.asDiagonal() * m_matrix.template block<Dim,HDim>(0,0));
813   return *this;
814 }
815 
816 /** Applies on the left a uniform scale of a factor \a c to \c *this
817   * and returns a reference to \c *this.
818   * \sa scale(Scalar)
819   */
820 template<typename Scalar, int Dim, int Mode, int Options>
prescale(const Scalar & s)821 inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::prescale(const Scalar& s)
822 {
823   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
824   m_matrix.template topRows<Dim>() *= s;
825   return *this;
826 }
827 
828 /** Applies on the right the translation matrix represented by the vector \a other
829   * to \c *this and returns a reference to \c *this.
830   * \sa pretranslate()
831   */
832 template<typename Scalar, int Dim, int Mode, int Options>
833 template<typename OtherDerived>
834 Transform<Scalar,Dim,Mode,Options>&
translate(const MatrixBase<OtherDerived> & other)835 Transform<Scalar,Dim,Mode,Options>::translate(const MatrixBase<OtherDerived> &other)
836 {
837   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
838   translationExt() += linearExt() * other;
839   return *this;
840 }
841 
842 /** Applies on the left the translation matrix represented by the vector \a other
843   * to \c *this and returns a reference to \c *this.
844   * \sa translate()
845   */
846 template<typename Scalar, int Dim, int Mode, int Options>
847 template<typename OtherDerived>
848 Transform<Scalar,Dim,Mode,Options>&
pretranslate(const MatrixBase<OtherDerived> & other)849 Transform<Scalar,Dim,Mode,Options>::pretranslate(const MatrixBase<OtherDerived> &other)
850 {
851   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
852   if(int(Mode)==int(Projective))
853     affine() += other * m_matrix.row(Dim);
854   else
855     translation() += other;
856   return *this;
857 }
858 
859 /** Applies on the right the rotation represented by the rotation \a rotation
860   * to \c *this and returns a reference to \c *this.
861   *
862   * The template parameter \a RotationType is the type of the rotation which
863   * must be known by internal::toRotationMatrix<>.
864   *
865   * Natively supported types includes:
866   *   - any scalar (2D),
867   *   - a Dim x Dim matrix expression,
868   *   - a Quaternion (3D),
869   *   - a AngleAxis (3D)
870   *
871   * This mechanism is easily extendable to support user types such as Euler angles,
872   * or a pair of Quaternion for 4D rotations.
873   *
874   * \sa rotate(Scalar), class Quaternion, class AngleAxis, prerotate(RotationType)
875   */
876 template<typename Scalar, int Dim, int Mode, int Options>
877 template<typename RotationType>
878 Transform<Scalar,Dim,Mode,Options>&
rotate(const RotationType & rotation)879 Transform<Scalar,Dim,Mode,Options>::rotate(const RotationType& rotation)
880 {
881   linearExt() *= internal::toRotationMatrix<Scalar,Dim>(rotation);
882   return *this;
883 }
884 
885 /** Applies on the left the rotation represented by the rotation \a rotation
886   * to \c *this and returns a reference to \c *this.
887   *
888   * See rotate() for further details.
889   *
890   * \sa rotate()
891   */
892 template<typename Scalar, int Dim, int Mode, int Options>
893 template<typename RotationType>
894 Transform<Scalar,Dim,Mode,Options>&
prerotate(const RotationType & rotation)895 Transform<Scalar,Dim,Mode,Options>::prerotate(const RotationType& rotation)
896 {
897   m_matrix.template block<Dim,HDim>(0,0) = internal::toRotationMatrix<Scalar,Dim>(rotation)
898                                          * m_matrix.template block<Dim,HDim>(0,0);
899   return *this;
900 }
901 
902 /** Applies on the right the shear transformation represented
903   * by the vector \a other to \c *this and returns a reference to \c *this.
904   * \warning 2D only.
905   * \sa preshear()
906   */
907 template<typename Scalar, int Dim, int Mode, int Options>
908 Transform<Scalar,Dim,Mode,Options>&
shear(const Scalar & sx,const Scalar & sy)909 Transform<Scalar,Dim,Mode,Options>::shear(const Scalar& sx, const Scalar& sy)
910 {
911   EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
912   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
913   VectorType tmp = linear().col(0)*sy + linear().col(1);
914   linear() << linear().col(0) + linear().col(1)*sx, tmp;
915   return *this;
916 }
917 
918 /** Applies on the left the shear transformation represented
919   * by the vector \a other to \c *this and returns a reference to \c *this.
920   * \warning 2D only.
921   * \sa shear()
922   */
923 template<typename Scalar, int Dim, int Mode, int Options>
924 Transform<Scalar,Dim,Mode,Options>&
preshear(const Scalar & sx,const Scalar & sy)925 Transform<Scalar,Dim,Mode,Options>::preshear(const Scalar& sx, const Scalar& sy)
926 {
927   EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
928   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
929   m_matrix.template block<Dim,HDim>(0,0) = LinearMatrixType(1, sx, sy, 1) * m_matrix.template block<Dim,HDim>(0,0);
930   return *this;
931 }
932 
933 /******************************************************
934 *** Scaling, Translation and Rotation compatibility ***
935 ******************************************************/
936 
937 template<typename Scalar, int Dim, int Mode, int Options>
938 inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const TranslationType& t)
939 {
940   linear().setIdentity();
941   translation() = t.vector();
942   makeAffine();
943   return *this;
944 }
945 
946 template<typename Scalar, int Dim, int Mode, int Options>
947 inline Transform<Scalar,Dim,Mode,Options> Transform<Scalar,Dim,Mode,Options>::operator*(const TranslationType& t) const
948 {
949   Transform res = *this;
950   res.translate(t.vector());
951   return res;
952 }
953 
954 template<typename Scalar, int Dim, int Mode, int Options>
955 inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const UniformScaling<Scalar>& s)
956 {
957   m_matrix.setZero();
958   linear().diagonal().fill(s.factor());
959   makeAffine();
960   return *this;
961 }
962 
963 template<typename Scalar, int Dim, int Mode, int Options>
964 template<typename Derived>
965 inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const RotationBase<Derived,Dim>& r)
966 {
967   linear() = internal::toRotationMatrix<Scalar,Dim>(r);
968   translation().setZero();
969   makeAffine();
970   return *this;
971 }
972 
973 template<typename Scalar, int Dim, int Mode, int Options>
974 template<typename Derived>
975 inline Transform<Scalar,Dim,Mode,Options> Transform<Scalar,Dim,Mode,Options>::operator*(const RotationBase<Derived,Dim>& r) const
976 {
977   Transform res = *this;
978   res.rotate(r.derived());
979   return res;
980 }
981 
982 /************************
983 *** Special functions ***
984 ************************/
985 
986 /** \returns the rotation part of the transformation
987   *
988   *
989   * \svd_module
990   *
991   * \sa computeRotationScaling(), computeScalingRotation(), class SVD
992   */
993 template<typename Scalar, int Dim, int Mode, int Options>
994 const typename Transform<Scalar,Dim,Mode,Options>::LinearMatrixType
rotation()995 Transform<Scalar,Dim,Mode,Options>::rotation() const
996 {
997   LinearMatrixType result;
998   computeRotationScaling(&result, (LinearMatrixType*)0);
999   return result;
1000 }
1001 
1002 
1003 /** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being
1004   * not necessarily positive.
1005   *
1006   * If either pointer is zero, the corresponding computation is skipped.
1007   *
1008   *
1009   *
1010   * \svd_module
1011   *
1012   * \sa computeScalingRotation(), rotation(), class SVD
1013   */
1014 template<typename Scalar, int Dim, int Mode, int Options>
1015 template<typename RotationMatrixType, typename ScalingMatrixType>
computeRotationScaling(RotationMatrixType * rotation,ScalingMatrixType * scaling)1016 void Transform<Scalar,Dim,Mode,Options>::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const
1017 {
1018   JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU | ComputeFullV);
1019 
1020   Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1
1021   VectorType sv(svd.singularValues());
1022   sv.coeffRef(0) *= x;
1023   if(scaling) scaling->lazyAssign(svd.matrixV() * sv.asDiagonal() * svd.matrixV().adjoint());
1024   if(rotation)
1025   {
1026     LinearMatrixType m(svd.matrixU());
1027     m.col(0) /= x;
1028     rotation->lazyAssign(m * svd.matrixV().adjoint());
1029   }
1030 }
1031 
1032 /** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being
1033   * not necessarily positive.
1034   *
1035   * If either pointer is zero, the corresponding computation is skipped.
1036   *
1037   *
1038   *
1039   * \svd_module
1040   *
1041   * \sa computeRotationScaling(), rotation(), class SVD
1042   */
1043 template<typename Scalar, int Dim, int Mode, int Options>
1044 template<typename ScalingMatrixType, typename RotationMatrixType>
computeScalingRotation(ScalingMatrixType * scaling,RotationMatrixType * rotation)1045 void Transform<Scalar,Dim,Mode,Options>::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const
1046 {
1047   JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU | ComputeFullV);
1048 
1049   Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1
1050   VectorType sv(svd.singularValues());
1051   sv.coeffRef(0) *= x;
1052   if(scaling) scaling->lazyAssign(svd.matrixU() * sv.asDiagonal() * svd.matrixU().adjoint());
1053   if(rotation)
1054   {
1055     LinearMatrixType m(svd.matrixU());
1056     m.col(0) /= x;
1057     rotation->lazyAssign(m * svd.matrixV().adjoint());
1058   }
1059 }
1060 
1061 /** Convenient method to set \c *this from a position, orientation and scale
1062   * of a 3D object.
1063   */
1064 template<typename Scalar, int Dim, int Mode, int Options>
1065 template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
1066 Transform<Scalar,Dim,Mode,Options>&
fromPositionOrientationScale(const MatrixBase<PositionDerived> & position,const OrientationType & orientation,const MatrixBase<ScaleDerived> & scale)1067 Transform<Scalar,Dim,Mode,Options>::fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
1068   const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale)
1069 {
1070   linear() = internal::toRotationMatrix<Scalar,Dim>(orientation);
1071   linear() *= scale.asDiagonal();
1072   translation() = position;
1073   makeAffine();
1074   return *this;
1075 }
1076 
1077 namespace internal {
1078 
1079 template<int Mode>
1080 struct transform_make_affine
1081 {
1082   template<typename MatrixType>
runtransform_make_affine1083   static void run(MatrixType &mat)
1084   {
1085     static const int Dim = MatrixType::ColsAtCompileTime-1;
1086     mat.template block<1,Dim>(Dim,0).setZero();
1087     mat.coeffRef(Dim,Dim) = typename MatrixType::Scalar(1);
1088   }
1089 };
1090 
1091 template<>
1092 struct transform_make_affine<AffineCompact>
1093 {
1094   template<typename MatrixType> static void run(MatrixType &) { }
1095 };
1096 
1097 // selector needed to avoid taking the inverse of a 3x4 matrix
1098 template<typename TransformType, int Mode=TransformType::Mode>
1099 struct projective_transform_inverse
1100 {
1101   static inline void run(const TransformType&, TransformType&)
1102   {}
1103 };
1104 
1105 template<typename TransformType>
1106 struct projective_transform_inverse<TransformType, Projective>
1107 {
1108   static inline void run(const TransformType& m, TransformType& res)
1109   {
1110     res.matrix() = m.matrix().inverse();
1111   }
1112 };
1113 
1114 } // end namespace internal
1115 
1116 
1117 /**
1118   *
1119   * \returns the inverse transformation according to some given knowledge
1120   * on \c *this.
1121   *
1122   * \param hint allows to optimize the inversion process when the transformation
1123   * is known to be not a general transformation (optional). The possible values are:
1124   *  - #Projective if the transformation is not necessarily affine, i.e., if the
1125   *    last row is not guaranteed to be [0 ... 0 1]
1126   *  - #Affine if the last row can be assumed to be [0 ... 0 1]
1127   *  - #Isometry if the transformation is only a concatenations of translations
1128   *    and rotations.
1129   *  The default is the template class parameter \c Mode.
1130   *
1131   * \warning unless \a traits is always set to NoShear or NoScaling, this function
1132   * requires the generic inverse method of MatrixBase defined in the LU module. If
1133   * you forget to include this module, then you will get hard to debug linking errors.
1134   *
1135   * \sa MatrixBase::inverse()
1136   */
1137 template<typename Scalar, int Dim, int Mode, int Options>
1138 Transform<Scalar,Dim,Mode,Options>
1139 Transform<Scalar,Dim,Mode,Options>::inverse(TransformTraits hint) const
1140 {
1141   Transform res;
1142   if (hint == Projective)
1143   {
1144     internal::projective_transform_inverse<Transform>::run(*this, res);
1145   }
1146   else
1147   {
1148     if (hint == Isometry)
1149     {
1150       res.matrix().template topLeftCorner<Dim,Dim>() = linear().transpose();
1151     }
1152     else if(hint&Affine)
1153     {
1154       res.matrix().template topLeftCorner<Dim,Dim>() = linear().inverse();
1155     }
1156     else
1157     {
1158       eigen_assert(false && "Invalid transform traits in Transform::Inverse");
1159     }
1160     // translation and remaining parts
1161     res.matrix().template topRightCorner<Dim,1>()
1162       = - res.matrix().template topLeftCorner<Dim,Dim>() * translation();
1163     res.makeAffine(); // we do need this, because in the beginning res is uninitialized
1164   }
1165   return res;
1166 }
1167 
1168 namespace internal {
1169 
1170 /*****************************************************
1171 *** Specializations of take affine part            ***
1172 *****************************************************/
1173 
1174 template<typename TransformType> struct transform_take_affine_part {
1175   typedef typename TransformType::MatrixType MatrixType;
1176   typedef typename TransformType::AffinePart AffinePart;
1177   typedef typename TransformType::ConstAffinePart ConstAffinePart;
1178   static inline AffinePart run(MatrixType& m)
1179   { return m.template block<TransformType::Dim,TransformType::HDim>(0,0); }
1180   static inline ConstAffinePart run(const MatrixType& m)
1181   { return m.template block<TransformType::Dim,TransformType::HDim>(0,0); }
1182 };
1183 
1184 template<typename Scalar, int Dim, int Options>
1185 struct transform_take_affine_part<Transform<Scalar,Dim,AffineCompact, Options> > {
1186   typedef typename Transform<Scalar,Dim,AffineCompact,Options>::MatrixType MatrixType;
1187   static inline MatrixType& run(MatrixType& m) { return m; }
1188   static inline const MatrixType& run(const MatrixType& m) { return m; }
1189 };
1190 
1191 /*****************************************************
1192 *** Specializations of construct from matrix       ***
1193 *****************************************************/
1194 
1195 template<typename Other, int Mode, int Options, int Dim, int HDim>
1196 struct transform_construct_from_matrix<Other, Mode,Options,Dim,HDim, Dim,Dim>
1197 {
1198   static inline void run(Transform<typename Other::Scalar,Dim,Mode,Options> *transform, const Other& other)
1199   {
1200     transform->linear() = other;
1201     transform->translation().setZero();
1202     transform->makeAffine();
1203   }
1204 };
1205 
1206 template<typename Other, int Mode, int Options, int Dim, int HDim>
1207 struct transform_construct_from_matrix<Other, Mode,Options,Dim,HDim, Dim,HDim>
1208 {
1209   static inline void run(Transform<typename Other::Scalar,Dim,Mode,Options> *transform, const Other& other)
1210   {
1211     transform->affine() = other;
1212     transform->makeAffine();
1213   }
1214 };
1215 
1216 template<typename Other, int Mode, int Options, int Dim, int HDim>
1217 struct transform_construct_from_matrix<Other, Mode,Options,Dim,HDim, HDim,HDim>
1218 {
1219   static inline void run(Transform<typename Other::Scalar,Dim,Mode,Options> *transform, const Other& other)
1220   { transform->matrix() = other; }
1221 };
1222 
1223 template<typename Other, int Options, int Dim, int HDim>
1224 struct transform_construct_from_matrix<Other, AffineCompact,Options,Dim,HDim, HDim,HDim>
1225 {
1226   static inline void run(Transform<typename Other::Scalar,Dim,AffineCompact,Options> *transform, const Other& other)
1227   { transform->matrix() = other.template block<Dim,HDim>(0,0); }
1228 };
1229 
1230 /**********************************************************
1231 ***   Specializations of operator* with rhs EigenBase   ***
1232 **********************************************************/
1233 
1234 template<int LhsMode,int RhsMode>
1235 struct transform_product_result
1236 {
1237   enum
1238   {
1239     Mode =
1240       (LhsMode == (int)Projective    || RhsMode == (int)Projective    ) ? Projective :
1241       (LhsMode == (int)Affine        || RhsMode == (int)Affine        ) ? Affine :
1242       (LhsMode == (int)AffineCompact || RhsMode == (int)AffineCompact ) ? AffineCompact :
1243       (LhsMode == (int)Isometry      || RhsMode == (int)Isometry      ) ? Isometry : Projective
1244   };
1245 };
1246 
1247 template< typename TransformType, typename MatrixType >
1248 struct transform_right_product_impl< TransformType, MatrixType, 0 >
1249 {
1250   typedef typename MatrixType::PlainObject ResultType;
1251 
1252   static EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other)
1253   {
1254     return T.matrix() * other;
1255   }
1256 };
1257 
1258 template< typename TransformType, typename MatrixType >
1259 struct transform_right_product_impl< TransformType, MatrixType, 1 >
1260 {
1261   enum {
1262     Dim = TransformType::Dim,
1263     HDim = TransformType::HDim,
1264     OtherRows = MatrixType::RowsAtCompileTime,
1265     OtherCols = MatrixType::ColsAtCompileTime
1266   };
1267 
1268   typedef typename MatrixType::PlainObject ResultType;
1269 
1270   static EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other)
1271   {
1272     EIGEN_STATIC_ASSERT(OtherRows==HDim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
1273 
1274     typedef Block<ResultType, Dim, OtherCols, int(MatrixType::RowsAtCompileTime)==Dim> TopLeftLhs;
1275 
1276     ResultType res(other.rows(),other.cols());
1277     TopLeftLhs(res, 0, 0, Dim, other.cols()).noalias() = T.affine() * other;
1278     res.row(OtherRows-1) = other.row(OtherRows-1);
1279 
1280     return res;
1281   }
1282 };
1283 
1284 template< typename TransformType, typename MatrixType >
1285 struct transform_right_product_impl< TransformType, MatrixType, 2 >
1286 {
1287   enum {
1288     Dim = TransformType::Dim,
1289     HDim = TransformType::HDim,
1290     OtherRows = MatrixType::RowsAtCompileTime,
1291     OtherCols = MatrixType::ColsAtCompileTime
1292   };
1293 
1294   typedef typename MatrixType::PlainObject ResultType;
1295 
1296   static EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other)
1297   {
1298     EIGEN_STATIC_ASSERT(OtherRows==Dim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
1299 
1300     typedef Block<ResultType, Dim, OtherCols, true> TopLeftLhs;
1301     ResultType res(Replicate<typename TransformType::ConstTranslationPart, 1, OtherCols>(T.translation(),1,other.cols()));
1302     TopLeftLhs(res, 0, 0, Dim, other.cols()).noalias() += T.linear() * other;
1303 
1304     return res;
1305   }
1306 };
1307 
1308 /**********************************************************
1309 ***   Specializations of operator* with lhs EigenBase   ***
1310 **********************************************************/
1311 
1312 // generic HDim x HDim matrix * T => Projective
1313 template<typename Other,int Mode, int Options, int Dim, int HDim>
1314 struct transform_left_product_impl<Other,Mode,Options,Dim,HDim, HDim,HDim>
1315 {
1316   typedef Transform<typename Other::Scalar,Dim,Mode,Options> TransformType;
1317   typedef typename TransformType::MatrixType MatrixType;
1318   typedef Transform<typename Other::Scalar,Dim,Projective,Options> ResultType;
1319   static ResultType run(const Other& other,const TransformType& tr)
1320   { return ResultType(other * tr.matrix()); }
1321 };
1322 
1323 // generic HDim x HDim matrix * AffineCompact => Projective
1324 template<typename Other, int Options, int Dim, int HDim>
1325 struct transform_left_product_impl<Other,AffineCompact,Options,Dim,HDim, HDim,HDim>
1326 {
1327   typedef Transform<typename Other::Scalar,Dim,AffineCompact,Options> TransformType;
1328   typedef typename TransformType::MatrixType MatrixType;
1329   typedef Transform<typename Other::Scalar,Dim,Projective,Options> ResultType;
1330   static ResultType run(const Other& other,const TransformType& tr)
1331   {
1332     ResultType res;
1333     res.matrix().noalias() = other.template block<HDim,Dim>(0,0) * tr.matrix();
1334     res.matrix().col(Dim) += other.col(Dim);
1335     return res;
1336   }
1337 };
1338 
1339 // affine matrix * T
1340 template<typename Other,int Mode, int Options, int Dim, int HDim>
1341 struct transform_left_product_impl<Other,Mode,Options,Dim,HDim, Dim,HDim>
1342 {
1343   typedef Transform<typename Other::Scalar,Dim,Mode,Options> TransformType;
1344   typedef typename TransformType::MatrixType MatrixType;
1345   typedef TransformType ResultType;
1346   static ResultType run(const Other& other,const TransformType& tr)
1347   {
1348     ResultType res;
1349     res.affine().noalias() = other * tr.matrix();
1350     res.matrix().row(Dim) = tr.matrix().row(Dim);
1351     return res;
1352   }
1353 };
1354 
1355 // affine matrix * AffineCompact
1356 template<typename Other, int Options, int Dim, int HDim>
1357 struct transform_left_product_impl<Other,AffineCompact,Options,Dim,HDim, Dim,HDim>
1358 {
1359   typedef Transform<typename Other::Scalar,Dim,AffineCompact,Options> TransformType;
1360   typedef typename TransformType::MatrixType MatrixType;
1361   typedef TransformType ResultType;
1362   static ResultType run(const Other& other,const TransformType& tr)
1363   {
1364     ResultType res;
1365     res.matrix().noalias() = other.template block<Dim,Dim>(0,0) * tr.matrix();
1366     res.translation() += other.col(Dim);
1367     return res;
1368   }
1369 };
1370 
1371 // linear matrix * T
1372 template<typename Other,int Mode, int Options, int Dim, int HDim>
1373 struct transform_left_product_impl<Other,Mode,Options,Dim,HDim, Dim,Dim>
1374 {
1375   typedef Transform<typename Other::Scalar,Dim,Mode,Options> TransformType;
1376   typedef typename TransformType::MatrixType MatrixType;
1377   typedef TransformType ResultType;
1378   static ResultType run(const Other& other, const TransformType& tr)
1379   {
1380     TransformType res;
1381     if(Mode!=int(AffineCompact))
1382       res.matrix().row(Dim) = tr.matrix().row(Dim);
1383     res.matrix().template topRows<Dim>().noalias()
1384       = other * tr.matrix().template topRows<Dim>();
1385     return res;
1386   }
1387 };
1388 
1389 /**********************************************************
1390 *** Specializations of operator* with another Transform ***
1391 **********************************************************/
1392 
1393 template<typename Scalar, int Dim, int LhsMode, int LhsOptions, int RhsMode, int RhsOptions>
1394 struct transform_transform_product_impl<Transform<Scalar,Dim,LhsMode,LhsOptions>,Transform<Scalar,Dim,RhsMode,RhsOptions>,false >
1395 {
1396   enum { ResultMode = transform_product_result<LhsMode,RhsMode>::Mode };
1397   typedef Transform<Scalar,Dim,LhsMode,LhsOptions> Lhs;
1398   typedef Transform<Scalar,Dim,RhsMode,RhsOptions> Rhs;
1399   typedef Transform<Scalar,Dim,ResultMode,LhsOptions> ResultType;
1400   static ResultType run(const Lhs& lhs, const Rhs& rhs)
1401   {
1402     ResultType res;
1403     res.linear() = lhs.linear() * rhs.linear();
1404     res.translation() = lhs.linear() * rhs.translation() + lhs.translation();
1405     res.makeAffine();
1406     return res;
1407   }
1408 };
1409 
1410 template<typename Scalar, int Dim, int LhsMode, int LhsOptions, int RhsMode, int RhsOptions>
1411 struct transform_transform_product_impl<Transform<Scalar,Dim,LhsMode,LhsOptions>,Transform<Scalar,Dim,RhsMode,RhsOptions>,true >
1412 {
1413   typedef Transform<Scalar,Dim,LhsMode,LhsOptions> Lhs;
1414   typedef Transform<Scalar,Dim,RhsMode,RhsOptions> Rhs;
1415   typedef Transform<Scalar,Dim,Projective> ResultType;
1416   static ResultType run(const Lhs& lhs, const Rhs& rhs)
1417   {
1418     return ResultType( lhs.matrix() * rhs.matrix() );
1419   }
1420 };
1421 
1422 template<typename Scalar, int Dim, int LhsOptions, int RhsOptions>
1423 struct transform_transform_product_impl<Transform<Scalar,Dim,AffineCompact,LhsOptions>,Transform<Scalar,Dim,Projective,RhsOptions>,true >
1424 {
1425   typedef Transform<Scalar,Dim,AffineCompact,LhsOptions> Lhs;
1426   typedef Transform<Scalar,Dim,Projective,RhsOptions> Rhs;
1427   typedef Transform<Scalar,Dim,Projective> ResultType;
1428   static ResultType run(const Lhs& lhs, const Rhs& rhs)
1429   {
1430     ResultType res;
1431     res.matrix().template topRows<Dim>() = lhs.matrix() * rhs.matrix();
1432     res.matrix().row(Dim) = rhs.matrix().row(Dim);
1433     return res;
1434   }
1435 };
1436 
1437 template<typename Scalar, int Dim, int LhsOptions, int RhsOptions>
1438 struct transform_transform_product_impl<Transform<Scalar,Dim,Projective,LhsOptions>,Transform<Scalar,Dim,AffineCompact,RhsOptions>,true >
1439 {
1440   typedef Transform<Scalar,Dim,Projective,LhsOptions> Lhs;
1441   typedef Transform<Scalar,Dim,AffineCompact,RhsOptions> Rhs;
1442   typedef Transform<Scalar,Dim,Projective> ResultType;
1443   static ResultType run(const Lhs& lhs, const Rhs& rhs)
1444   {
1445     ResultType res(lhs.matrix().template leftCols<Dim>() * rhs.matrix());
1446     res.matrix().col(Dim) += lhs.matrix().col(Dim);
1447     return res;
1448   }
1449 };
1450 
1451 } // end namespace internal
1452 
1453 } // end namespace Eigen
1454 
1455 #endif // EIGEN_TRANSFORM_H
1456