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