Main MRPT website > C++ reference
MRPT logo

Transform.h

Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
00006 // Copyright (C) 2010 Hauke Heibel <hauke.heibel@gmail.com>
00007 //
00008 // Eigen is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 3 of the License, or (at your option) any later version.
00012 //
00013 // Alternatively, you can redistribute it and/or
00014 // modify it under the terms of the GNU General Public License as
00015 // published by the Free Software Foundation; either version 2 of
00016 // the License, or (at your option) any later version.
00017 //
00018 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00019 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00020 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00021 // GNU General Public License for more details.
00022 //
00023 // You should have received a copy of the GNU Lesser General Public
00024 // License and a copy of the GNU General Public License along with
00025 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00026 
00027 #ifndef EIGEN_TRANSFORM_H
00028 #define EIGEN_TRANSFORM_H
00029 
00030 namespace internal {
00031 
00032 template<typename Transform>
00033 struct transform_traits
00034 {
00035   enum
00036   {
00037     Dim = Transform::Dim,
00038     HDim = Transform::HDim,
00039     Mode = Transform::Mode,
00040     IsProjective = (Mode==Projective)
00041   };
00042 };
00043 
00044 template< typename TransformType,
00045           typename MatrixType,
00046           bool IsProjective = transform_traits<TransformType>::IsProjective>
00047 struct transform_right_product_impl;
00048 
00049 template< typename Other,
00050           int Mode,
00051           int Dim,
00052           int HDim,
00053           int OtherRows=Other::RowsAtCompileTime,
00054           int OtherCols=Other::ColsAtCompileTime>
00055 struct transform_left_product_impl;
00056 
00057 template< typename Lhs,
00058           typename Rhs,
00059           bool AnyProjective = 
00060             transform_traits<Lhs>::IsProjective ||
00061             transform_traits<Lhs>::IsProjective>
00062 struct transform_transform_product_impl;
00063 
00064 template< typename Other,
00065           int Mode,
00066           int Dim,
00067           int HDim,
00068           int OtherRows=Other::RowsAtCompileTime,
00069           int OtherCols=Other::ColsAtCompileTime>
00070 struct transform_construct_from_matrix;
00071 
00072 template<typename TransformType> struct transform_take_affine_part;
00073 
00074 } // end namespace internal
00075 
00076 /** \geometry_module \ingroup Geometry_Module
00077   *
00078   * \class Transform
00079   *
00080   * \brief Represents an homogeneous transformation in a N dimensional space
00081   *
00082   * \param _Scalar the scalar type, i.e., the type of the coefficients
00083   * \param _Dim the dimension of the space
00084   * \param _Mode the type of the transformation. Can be:
00085   *              - Affine: the transformation is stored as a (Dim+1)^2 matrix,
00086   *                        where the last row is assumed to be [0 ... 0 1].
00087   *                        This is the default.
00088   *              - AffineCompact: the transformation is stored as a (Dim)x(Dim+1) matrix.
00089   *              - Projective: the transformation is stored as a (Dim+1)^2 matrix
00090   *                            without any assumption.
00091   *
00092   * The homography is internally represented and stored by a matrix which
00093   * is available through the matrix() method. To understand the behavior of
00094   * this class you have to think a Transform object as its internal
00095   * matrix representation. The chosen convention is right multiply:
00096   *
00097   * \code v' = T * v \endcode
00098   *
00099   * Therefore, an affine transformation matrix M is shaped like this:
00100   *
00101   * \f$ \left( \begin{array}{cc}
00102   * linear & translation\\
00103   * 0 ... 0 & 1
00104   * \end{array} \right) \f$
00105   *
00106   * Note that for a projective transformation the last row can be anything,
00107   * and then the interpretation of different parts might be sightly different.
00108   *
00109   * However, unlike a plain matrix, the Transform class provides many features
00110   * simplifying both its assembly and usage. In particular, it can be composed
00111   * with any other transformations (Transform,Translation,RotationBase,Matrix)
00112   * and can be directly used to transform implicit homogeneous vectors. All these
00113   * operations are handled via the operator*. For the composition of transformations,
00114   * its principle consists to first convert the right/left hand sides of the product
00115   * to a compatible (Dim+1)^2 matrix and then perform a pure matrix product.
00116   * Of course, internally, operator* tries to perform the minimal number of operations
00117   * according to the nature of each terms. Likewise, when applying the transform
00118   * to non homogeneous vectors, the latters are automatically promoted to homogeneous
00119   * one before doing the matrix product. The convertions to homogeneous representations
00120   * are performed as follow:
00121   *
00122   * \b Translation t (Dim)x(1):
00123   * \f$ \left( \begin{array}{cc}
00124   * I & t \\
00125   * 0\,...\,0 & 1
00126   * \end{array} \right) \f$
00127   *
00128   * \b Rotation R (Dim)x(Dim):
00129   * \f$ \left( \begin{array}{cc}
00130   * R & 0\\
00131   * 0\,...\,0 & 1
00132   * \end{array} \right) \f$
00133   *
00134   * \b Linear \b Matrix L (Dim)x(Dim):
00135   * \f$ \left( \begin{array}{cc}
00136   * L & 0\\
00137   * 0\,...\,0 & 1
00138   * \end{array} \right) \f$
00139   *
00140   * \b Affine \b Matrix A (Dim)x(Dim+1):
00141   * \f$ \left( \begin{array}{c}
00142   * A\\
00143   * 0\,...\,0\,1
00144   * \end{array} \right) \f$
00145   *
00146   * \b Column \b vector v (Dim)x(1):
00147   * \f$ \left( \begin{array}{c}
00148   * v\\
00149   * 1
00150   * \end{array} \right) \f$
00151   *
00152   * \b Set \b of \b column \b vectors V1...Vn (Dim)x(n):
00153   * \f$ \left( \begin{array}{ccc}
00154   * v_1 & ... & v_n\\
00155   * 1 & ... & 1
00156   * \end{array} \right) \f$
00157   *
00158   * The concatenation of a Transform object with any kind of other transformation
00159   * always returns a Transform object.
00160   *
00161   * A little exception to the "as pure matrix product" rule is the case of the
00162   * transformation of non homogeneous vectors by an affine transformation. In
00163   * that case the last matrix row can be ignored, and the product returns non
00164   * homogeneous vectors.
00165   *
00166   * Since, for instance, a Dim x Dim matrix is interpreted as a linear transformation,
00167   * it is not possible to directly transform Dim vectors stored in a Dim x Dim matrix.
00168   * The solution is either to use a Dim x Dynamic matrix or explicitly request a
00169   * vector transformation by making the vector homogeneous:
00170   * \code
00171   * m' = T * m.colwise().homogeneous();
00172   * \endcode
00173   * Note that there is zero overhead.
00174   *
00175   * Conversion methods from/to Qt's QMatrix and QTransform are available if the
00176   * preprocessor token EIGEN_QT_SUPPORT is defined.
00177   *
00178   * \sa class Matrix, class Quaternion
00179   */
00180 template<typename _Scalar, int _Dim, int _Mode>
00181 class Transform
00182 {
00183 public:
00184   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim==Dynamic ? Dynamic : (_Dim+1)*(_Dim+1))
00185   enum {
00186     Mode = _Mode,
00187     Dim = _Dim,     ///< space dimension in which the transformation holds
00188     HDim = _Dim+1,  ///< size of a respective homogeneous vector
00189     Rows = int(Mode)==(AffineCompact) ? Dim : HDim
00190   };
00191   /** the scalar type of the coefficients */
00192   typedef _Scalar Scalar;
00193   typedef DenseIndex Index;
00194   /** type of the matrix used to represent the transformation */
00195   typedef Matrix<Scalar,Rows,HDim> MatrixType;
00196   /** constified MatrixType */
00197   typedef const MatrixType ConstMatrixType;
00198   /** type of the matrix used to represent the linear part of the transformation */
00199   typedef Matrix<Scalar,Dim,Dim> LinearMatrixType;
00200   /** type of read/write reference to the linear part of the transformation */
00201   typedef Block<MatrixType,Dim,Dim> LinearPart;
00202   /** type of read reference to the linear part of the transformation */
00203   typedef const Block<ConstMatrixType,Dim,Dim> ConstLinearPart;
00204   /** type of read/write reference to the affine part of the transformation */
00205   typedef typename internal::conditional<int(Mode)==int(AffineCompact),
00206                               MatrixType&,
00207                               Block<MatrixType,Dim,HDim> >::type AffinePart;
00208   /** type of read reference to the affine part of the transformation */
00209   typedef typename internal::conditional<int(Mode)==int(AffineCompact),
00210                               const MatrixType&,
00211                               const Block<const MatrixType,Dim,HDim> >::type ConstAffinePart;
00212   /** type of a vector */
00213   typedef Matrix<Scalar,Dim,1> VectorType;
00214   /** type of a read/write reference to the translation part of the rotation */
00215   typedef Block<MatrixType,Dim,1> TranslationPart;
00216   /** type of a read reference to the translation part of the rotation */
00217   typedef const Block<ConstMatrixType,Dim,1> ConstTranslationPart;
00218   /** corresponding translation type */
00219   typedef Translation<Scalar,Dim> TranslationType;
00220   
00221   // this intermediate enum is needed to avoid an ICE with gcc 3.4 and 4.0
00222   enum { TransformTimeDiagonalMode = ((Mode==int(Isometry))?Affine:int(Mode)) };
00223   /** The return type of the product between a diagonal matrix and a transform */
00224   typedef Transform<Scalar,Dim,TransformTimeDiagonalMode> TransformTimeDiagonalReturnType;
00225 
00226 protected:
00227 
00228   MatrixType m_matrix;
00229 
00230 public:
00231 
00232   /** Default constructor without initialization of the meaningful coefficients.
00233     * If Mode==Affine, then the last row is set to [0 ... 0 1] */
00234   inline Transform()
00235   {
00236     if (int(Mode)==Affine)
00237       makeAffine();
00238   }
00239 
00240   inline Transform(const Transform& other)
00241   {
00242     m_matrix = other.m_matrix;
00243   }
00244 
00245   inline explicit Transform(const TranslationType& t) { *this = t; }
00246   inline explicit Transform(const UniformScaling<Scalar>& s) { *this = s; }
00247   template<typename Derived>
00248   inline explicit Transform(const RotationBase<Derived, Dim>& r) { *this = r; }
00249 
00250   inline Transform& operator=(const Transform& other)
00251   { m_matrix = other.m_matrix; return *this; }
00252 
00253   typedef internal::transform_take_affine_part<Transform> take_affine_part;
00254 
00255   /** Constructs and initializes a transformation from a Dim^2 or a (Dim+1)^2 matrix. */
00256   template<typename OtherDerived>
00257   inline explicit Transform(const EigenBase<OtherDerived>& other)
00258   {
00259     internal::transform_construct_from_matrix<OtherDerived,Mode,Dim,HDim>::run(this, other.derived());
00260   }
00261 
00262   /** Set \c *this from a Dim^2 or (Dim+1)^2 matrix. */
00263   template<typename OtherDerived>
00264   inline Transform& operator=(const EigenBase<OtherDerived>& other)
00265   {
00266     internal::transform_construct_from_matrix<OtherDerived,Mode,Dim,HDim>::run(this, other.derived());
00267     return *this;
00268   }
00269 
00270   template<int OtherMode>
00271   inline Transform(const Transform<Scalar,Dim,OtherMode>& other)
00272   {
00273     // prevent conversions as:
00274     // Affine | AffineCompact | Isometry = Projective
00275     EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(OtherMode==int(Projective), Mode==int(Projective)),
00276                         YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION)
00277 
00278     // prevent conversions as:
00279     // Isometry = Affine | AffineCompact
00280     EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(OtherMode==int(Affine)||OtherMode==int(AffineCompact), Mode!=int(Isometry)),
00281                         YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION)
00282 
00283     enum { ModeIsAffineCompact = Mode == int(AffineCompact),
00284            OtherModeIsAffineCompact = OtherMode == int(AffineCompact)
00285     };
00286 
00287     if(ModeIsAffineCompact == OtherModeIsAffineCompact)
00288     {
00289       // We need the block expression because the code is compiled for all
00290       // combinations of transformations and will trigger a compile time error
00291       // if one tries to assign the matrices directly
00292       m_matrix.template block<Dim,Dim+1>(0,0) = other.matrix().template block<Dim,Dim+1>(0,0);
00293       makeAffine();
00294     }
00295     else if(OtherModeIsAffineCompact)
00296     {
00297       typedef typename Transform<Scalar,Dim,OtherMode>::MatrixType OtherMatrixType;
00298       internal::transform_construct_from_matrix<OtherMatrixType,Mode,Dim,HDim>::run(this, other.matrix());
00299     }
00300     else
00301     {
00302       // here we know that Mode == AffineCompact and OtherMode != AffineCompact.
00303       // if OtherMode were Projective, the static assert above would already have caught it.
00304       // So the only possibility is that OtherMode == Affine
00305       linear() = other.linear();
00306       translation() = other.translation();
00307     }
00308   }
00309 
00310   template<typename OtherDerived>
00311   Transform(const ReturnByValue<OtherDerived>& other)
00312   {
00313     other.evalTo(*this);
00314   }
00315 
00316   template<typename OtherDerived>
00317   Transform& operator=(const ReturnByValue<OtherDerived>& other)
00318   {
00319     other.evalTo(*this);
00320     return *this;
00321   }
00322 
00323   #ifdef EIGEN_QT_SUPPORT
00324   inline Transform(const QMatrix& other);
00325   inline Transform& operator=(const QMatrix& other);
00326   inline QMatrix toQMatrix(void) const;
00327   inline Transform(const QTransform& other);
00328   inline Transform& operator=(const QTransform& other);
00329   inline QTransform toQTransform(void) const;
00330   #endif
00331 
00332   /** shortcut for m_matrix(row,col);
00333     * \sa MatrixBase::operator(Index,Index) const */
00334   inline Scalar operator() (Index row, Index col) const { return m_matrix(row,col); }
00335   /** shortcut for m_matrix(row,col);
00336     * \sa MatrixBase::operator(Index,Index) */
00337   inline Scalar& operator() (Index row, Index col) { return m_matrix(row,col); }
00338 
00339   /** \returns a read-only expression of the transformation matrix */
00340   inline const MatrixType& matrix() const { return m_matrix; }
00341   /** \returns a writable expression of the transformation matrix */
00342   inline MatrixType& matrix() { return m_matrix; }
00343 
00344   /** \returns a read-only expression of the linear part of the transformation */
00345   inline ConstLinearPart linear() const { return m_matrix.template block<Dim,Dim>(0,0); }
00346   /** \returns a writable expression of the linear part of the transformation */
00347   inline LinearPart linear() { return m_matrix.template block<Dim,Dim>(0,0); }
00348 
00349   /** \returns a read-only expression of the Dim x HDim affine part of the transformation */
00350   inline ConstAffinePart affine() const { return take_affine_part::run(m_matrix); }
00351   /** \returns a writable expression of the Dim x HDim affine part of the transformation */
00352   inline AffinePart affine() { return take_affine_part::run(m_matrix); }
00353 
00354   /** \returns a read-only expression of the translation vector of the transformation */
00355   inline ConstTranslationPart translation() const { return m_matrix.template block<Dim,1>(0,Dim); }
00356   /** \returns a writable expression of the translation vector of the transformation */
00357   inline TranslationPart translation() { return m_matrix.template block<Dim,1>(0,Dim); }
00358 
00359   /** \returns an expression of the product between the transform \c *this and a matrix expression \a other
00360     *
00361     * The right hand side \a other might be either:
00362     * \li a vector of size Dim,
00363     * \li an homogeneous vector of size Dim+1,
00364     * \li a set of vectors of size Dim x Dynamic,
00365     * \li a set of homogeneous vectors of size Dim+1 x Dynamic,
00366     * \li a linear transformation matrix of size Dim x Dim,
00367     * \li an affine transformation matrix of size Dim x Dim+1,
00368     * \li a transformation matrix of size Dim+1 x Dim+1.
00369     */
00370   // note: this function is defined here because some compilers cannot find the respective declaration
00371   template<typename OtherDerived>
00372   EIGEN_STRONG_INLINE const typename internal::transform_right_product_impl<Transform, OtherDerived>::ResultType
00373   operator * (const EigenBase<OtherDerived> &other) const
00374   { return internal::transform_right_product_impl<Transform, OtherDerived>::run(*this,other.derived()); }
00375 
00376   /** \returns the product expression of a transformation matrix \a a times a transform \a b
00377     *
00378     * The left hand side \a other might be either:
00379     * \li a linear transformation matrix of size Dim x Dim,
00380     * \li an affine transformation matrix of size Dim x Dim+1,
00381     * \li a general transformation matrix of size Dim+1 x Dim+1.
00382     */
00383   template<typename OtherDerived> friend
00384   inline const typename internal::transform_left_product_impl<OtherDerived,Mode,_Dim,_Dim+1>::ResultType
00385     operator * (const EigenBase<OtherDerived> &a, const Transform &b)
00386   { return internal::transform_left_product_impl<OtherDerived,Mode,Dim,HDim>::run(a.derived(),b); }
00387 
00388   /** \returns The product expression of a transform \a a times a diagonal matrix \a b
00389     *
00390     * The rhs diagonal matrix is interpreted as an affine scaling transformation. The
00391     * product results in a Transform of the same type (mode) as the lhs only if the lhs 
00392     * mode is no isometry. In that case, the returned transform is an affinity.
00393     */
00394   template<typename DiagonalDerived>
00395   inline const TransformTimeDiagonalReturnType
00396     operator * (const DiagonalBase<DiagonalDerived> &b) const
00397   {
00398     TransformTimeDiagonalReturnType res(*this);
00399     res.linear() *= b;
00400     return res;
00401   }
00402 
00403   /** \returns The product expression of a diagonal matrix \a a times a transform \a b
00404     *
00405     * The lhs diagonal matrix is interpreted as an affine scaling transformation. The
00406     * product results in a Transform of the same type (mode) as the lhs only if the lhs 
00407     * mode is no isometry. In that case, the returned transform is an affinity.
00408     */
00409   template<typename DiagonalDerived>
00410   friend inline TransformTimeDiagonalReturnType
00411     operator * (const DiagonalBase<DiagonalDerived> &a, const Transform &b)
00412   {
00413     TransformTimeDiagonalReturnType res;
00414     res.linear().noalias() = a*b.linear();
00415     res.translation().noalias() = a*b.translation();
00416     if (Mode!=int(AffineCompact))
00417       res.matrix().row(Dim) = b.matrix().row(Dim);
00418     return res;
00419   }
00420 
00421   template<typename OtherDerived>
00422   inline Transform& operator*=(const EigenBase<OtherDerived>& other) { return *this = *this * other; }
00423 
00424   /** Concatenates two transformations */
00425   inline const Transform operator * (const Transform& other) const
00426   {
00427     return internal::transform_transform_product_impl<Transform,Transform>::run(*this,other);
00428   }
00429 
00430   /** Concatenates two different transformations */
00431   template<int OtherMode>
00432   inline const typename internal::transform_transform_product_impl<
00433     Transform,Transform<Scalar,Dim,OtherMode> >::ResultType
00434     operator * (const Transform<Scalar,Dim,OtherMode>& other) const
00435   {
00436     return internal::transform_transform_product_impl<Transform,Transform<Scalar,Dim,OtherMode> >::run(*this,other);
00437   }
00438 
00439   /** \sa MatrixBase::setIdentity() */
00440   void setIdentity() { m_matrix.setIdentity(); }
00441 
00442   /**
00443    * \brief Returns an identity transformation.
00444    * \todo In the future this function should be returning a Transform expression.
00445    */
00446   static const Transform Identity()
00447   {
00448     return Transform(MatrixType::Identity());
00449   }
00450 
00451   template<typename OtherDerived>
00452   inline Transform& scale(const MatrixBase<OtherDerived> &other);
00453 
00454   template<typename OtherDerived>
00455   inline Transform& prescale(const MatrixBase<OtherDerived> &other);
00456 
00457   inline Transform& scale(Scalar s);
00458   inline Transform& prescale(Scalar s);
00459 
00460   template<typename OtherDerived>
00461   inline Transform& translate(const MatrixBase<OtherDerived> &other);
00462 
00463   template<typename OtherDerived>
00464   inline Transform& pretranslate(const MatrixBase<OtherDerived> &other);
00465 
00466   template<typename RotationType>
00467   inline Transform& rotate(const RotationType& rotation);
00468 
00469   template<typename RotationType>
00470   inline Transform& prerotate(const RotationType& rotation);
00471 
00472   Transform& shear(Scalar sx, Scalar sy);
00473   Transform& preshear(Scalar sx, Scalar sy);
00474 
00475   inline Transform& operator=(const TranslationType& t);
00476   inline Transform& operator*=(const TranslationType& t) { return translate(t.vector()); }
00477   inline Transform operator*(const TranslationType& t) const;
00478 
00479   inline Transform& operator=(const UniformScaling<Scalar>& t);
00480   inline Transform& operator*=(const UniformScaling<Scalar>& s) { return scale(s.factor()); }
00481   inline Transform operator*(const UniformScaling<Scalar>& s) const;
00482 
00483   inline Transform& operator*=(const DiagonalMatrix<Scalar,Dim>& s) { linear() *= s; return *this; }
00484 
00485   template<typename Derived>
00486   inline Transform& operator=(const RotationBase<Derived,Dim>& r);
00487   template<typename Derived>
00488   inline Transform& operator*=(const RotationBase<Derived,Dim>& r) { return rotate(r.toRotationMatrix()); }
00489   template<typename Derived>
00490   inline Transform operator*(const RotationBase<Derived,Dim>& r) const;
00491 
00492   LinearMatrixType rotation() const;
00493   template<typename RotationMatrixType, typename ScalingMatrixType>
00494   void computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const;
00495   template<typename ScalingMatrixType, typename RotationMatrixType>
00496   void computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const;
00497 
00498   template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
00499   Transform& fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
00500     const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale);
00501 
00502   inline Transform inverse(TransformTraits traits = (TransformTraits)Mode) const;
00503 
00504   /** \returns a const pointer to the column major internal matrix */
00505   const Scalar* data() const { return m_matrix.data(); }
00506   /** \returns a non-const pointer to the column major internal matrix */
00507   Scalar* data() { return m_matrix.data(); }
00508 
00509   /** \returns \c *this with scalar type casted to \a NewScalarType
00510     *
00511     * Note that if \a NewScalarType is equal to the current scalar type of \c *this
00512     * then this function smartly returns a const reference to \c *this.
00513     */
00514   template<typename NewScalarType>
00515   inline typename internal::cast_return_type<Transform,Transform<NewScalarType,Dim,Mode> >::type cast() const
00516   { return typename internal::cast_return_type<Transform,Transform<NewScalarType,Dim,Mode> >::type(*this); }
00517 
00518   /** Copy constructor with scalar type conversion */
00519   template<typename OtherScalarType>
00520   inline explicit Transform(const Transform<OtherScalarType,Dim,Mode>& other)
00521   { m_matrix = other.matrix().template cast<Scalar>(); }
00522 
00523   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
00524     * determined by \a prec.
00525     *
00526     * \sa MatrixBase::isApprox() */
00527   bool isApprox(const Transform& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const
00528   { return m_matrix.isApprox(other.m_matrix, prec); }
00529 
00530   /** Sets the last row to [0 ... 0 1]
00531     */
00532   void makeAffine()
00533   {
00534     if(int(Mode)!=int(AffineCompact))
00535     {
00536       matrix().template block<1,Dim>(Dim,0).setZero();
00537       matrix().coeffRef(Dim,Dim) = 1;
00538     }
00539   }
00540 
00541   /** \internal
00542     * \returns the Dim x Dim linear part if the transformation is affine,
00543     *          and the HDim x Dim part for projective transformations.
00544     */
00545   inline Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,Dim> linearExt()
00546   { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,Dim>(0,0); }
00547   /** \internal
00548     * \returns the Dim x Dim linear part if the transformation is affine,
00549     *          and the HDim x Dim part for projective transformations.
00550     */
00551   inline const Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,Dim> linearExt() const
00552   { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,Dim>(0,0); }
00553 
00554   /** \internal
00555     * \returns the translation part if the transformation is affine,
00556     *          and the last column for projective transformations.
00557     */
00558   inline Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,1> translationExt()
00559   { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,1>(0,Dim); }
00560   /** \internal
00561     * \returns the translation part if the transformation is affine,
00562     *          and the last column for projective transformations.
00563     */
00564   inline const Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,1> translationExt() const
00565   { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,1>(0,Dim); }
00566 
00567 
00568   #ifdef EIGEN_TRANSFORM_PLUGIN
00569   #include EIGEN_TRANSFORM_PLUGIN
00570   #endif
00571 
00572 };
00573 
00574 /** \ingroup Geometry_Module */
00575 typedef Transform<float,2,Isometry> Isometry2f;
00576 /** \ingroup Geometry_Module */
00577 typedef Transform<float,3,Isometry> Isometry3f;
00578 /** \ingroup Geometry_Module */
00579 typedef Transform<double,2,Isometry> Isometry2d;
00580 /** \ingroup Geometry_Module */
00581 typedef Transform<double,3,Isometry> Isometry3d;
00582 
00583 /** \ingroup Geometry_Module */
00584 typedef Transform<float,2,Affine> Affine2f;
00585 /** \ingroup Geometry_Module */
00586 typedef Transform<float,3,Affine> Affine3f;
00587 /** \ingroup Geometry_Module */
00588 typedef Transform<double,2,Affine> Affine2d;
00589 /** \ingroup Geometry_Module */
00590 typedef Transform<double,3,Affine> Affine3d;
00591 
00592 /** \ingroup Geometry_Module */
00593 typedef Transform<float,2,AffineCompact> AffineCompact2f;
00594 /** \ingroup Geometry_Module */
00595 typedef Transform<float,3,AffineCompact> AffineCompact3f;
00596 /** \ingroup Geometry_Module */
00597 typedef Transform<double,2,AffineCompact> AffineCompact2d;
00598 /** \ingroup Geometry_Module */
00599 typedef Transform<double,3,AffineCompact> AffineCompact3d;
00600 
00601 /** \ingroup Geometry_Module */
00602 typedef Transform<float,2,Projective> Projective2f;
00603 /** \ingroup Geometry_Module */
00604 typedef Transform<float,3,Projective> Projective3f;
00605 /** \ingroup Geometry_Module */
00606 typedef Transform<double,2,Projective> Projective2d;
00607 /** \ingroup Geometry_Module */
00608 typedef Transform<double,3,Projective> Projective3d;
00609 
00610 #ifdef EIGEN2_SUPPORT
00611 #include "src/Eigen2Support/Transform.h"
00612 #endif
00613 
00614 /**************************
00615 *** Optional QT support ***
00616 **************************/
00617 
00618 #ifdef EIGEN_QT_SUPPORT
00619 /** Initializes \c *this from a QMatrix assuming the dimension is 2.
00620   *
00621   * This function is available only if the token EIGEN_QT_SUPPORT is defined.
00622   */
00623 template<typename Scalar, int Dim, int Mode>
00624 Transform<Scalar,Dim,Mode>::Transform(const QMatrix& other)
00625 {
00626   *this = other;
00627 }
00628 
00629 /** Set \c *this from a QMatrix assuming the dimension is 2.
00630   *
00631   * This function is available only if the token EIGEN_QT_SUPPORT is defined.
00632   */
00633 template<typename Scalar, int Dim, int Mode>
00634 Transform<Scalar,Dim,Mode>& Transform<Scalar,Dim,Mode>::operator=(const QMatrix& other)
00635 {
00636   EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
00637   m_matrix << other.m11(), other.m21(), other.dx(),
00638               other.m12(), other.m22(), other.dy(),
00639               0, 0, 1;
00640   return *this;
00641 }
00642 
00643 /** \returns a QMatrix from \c *this assuming the dimension is 2.
00644   *
00645   * \warning this conversion might loss data if \c *this is not affine
00646   *
00647   * This function is available only if the token EIGEN_QT_SUPPORT is defined.
00648   */
00649 template<typename Scalar, int Dim, int Mode>
00650 QMatrix Transform<Scalar,Dim,Mode>::toQMatrix(void) const
00651 {
00652   EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
00653   return QMatrix(m_matrix.coeff(0,0), m_matrix.coeff(1,0),
00654                  m_matrix.coeff(0,1), m_matrix.coeff(1,1),
00655                  m_matrix.coeff(0,2), m_matrix.coeff(1,2));
00656 }
00657 
00658 /** Initializes \c *this from a QTransform assuming the dimension is 2.
00659   *
00660   * This function is available only if the token EIGEN_QT_SUPPORT is defined.
00661   */
00662 template<typename Scalar, int Dim, int Mode>
00663 Transform<Scalar,Dim,Mode>::Transform(const QTransform& other)
00664 {
00665   *this = other;
00666 }
00667 
00668 /** Set \c *this from a QTransform assuming the dimension is 2.
00669   *
00670   * This function is available only if the token EIGEN_QT_SUPPORT is defined.
00671   */
00672 template<typename Scalar, int Dim, int Mode>
00673 Transform<Scalar,Dim,Mode>& Transform<Scalar,Dim,Mode>::operator=(const QTransform& other)
00674 {
00675   EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
00676   m_matrix << other.m11(), other.m21(), other.dx(),
00677               other.m12(), other.m22(), other.dy(),
00678               other.m13(), other.m23(), other.m33();
00679   return *this;
00680 }
00681 
00682 /** \returns a QTransform from \c *this assuming the dimension is 2.
00683   *
00684   * This function is available only if the token EIGEN_QT_SUPPORT is defined.
00685   */
00686 template<typename Scalar, int Dim, int Mode>
00687 QTransform Transform<Scalar,Dim,Mode>::toQTransform(void) const
00688 {
00689   EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
00690   return QTransform(matrix.coeff(0,0), matrix.coeff(1,0), matrix.coeff(2,0)
00691                     matrix.coeff(0,1), matrix.coeff(1,1), matrix.coeff(2,1)
00692                     matrix.coeff(0,2), matrix.coeff(1,2), matrix.coeff(2,2));
00693 }
00694 #endif
00695 
00696 /*********************
00697 *** Procedural API ***
00698 *********************/
00699 
00700 /** Applies on the right the non uniform scale transformation represented
00701   * by the vector \a other to \c *this and returns a reference to \c *this.
00702   * \sa prescale()
00703   */
00704 template<typename Scalar, int Dim, int Mode>
00705 template<typename OtherDerived>
00706 Transform<Scalar,Dim,Mode>&
00707 Transform<Scalar,Dim,Mode>::scale(const MatrixBase<OtherDerived> &other)
00708 {
00709   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
00710   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
00711   linearExt().noalias() = (linearExt() * other.asDiagonal());
00712   return *this;
00713 }
00714 
00715 /** Applies on the right a uniform scale of a factor \a c to \c *this
00716   * and returns a reference to \c *this.
00717   * \sa prescale(Scalar)
00718   */
00719 template<typename Scalar, int Dim, int Mode>
00720 inline Transform<Scalar,Dim,Mode>& Transform<Scalar,Dim,Mode>::scale(Scalar s)
00721 {
00722   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
00723   linearExt() *= s;
00724   return *this;
00725 }
00726 
00727 /** Applies on the left the non uniform scale transformation represented
00728   * by the vector \a other to \c *this and returns a reference to \c *this.
00729   * \sa scale()
00730   */
00731 template<typename Scalar, int Dim, int Mode>
00732 template<typename OtherDerived>
00733 Transform<Scalar,Dim,Mode>&
00734 Transform<Scalar,Dim,Mode>::prescale(const MatrixBase<OtherDerived> &other)
00735 {
00736   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
00737   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
00738   m_matrix.template block<Dim,HDim>(0,0).noalias() = (other.asDiagonal() * m_matrix.template block<Dim,HDim>(0,0));
00739   return *this;
00740 }
00741 
00742 /** Applies on the left a uniform scale of a factor \a c to \c *this
00743   * and returns a reference to \c *this.
00744   * \sa scale(Scalar)
00745   */
00746 template<typename Scalar, int Dim, int Mode>
00747 inline Transform<Scalar,Dim,Mode>& Transform<Scalar,Dim,Mode>::prescale(Scalar s)
00748 {
00749   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
00750   m_matrix.template topRows<Dim>() *= s;
00751   return *this;
00752 }
00753 
00754 /** Applies on the right the translation matrix represented by the vector \a other
00755   * to \c *this and returns a reference to \c *this.
00756   * \sa pretranslate()
00757   */
00758 template<typename Scalar, int Dim, int Mode>
00759 template<typename OtherDerived>
00760 Transform<Scalar,Dim,Mode>&
00761 Transform<Scalar,Dim,Mode>::translate(const MatrixBase<OtherDerived> &other)
00762 {
00763   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
00764   translationExt() += linearExt() * other;
00765   return *this;
00766 }
00767 
00768 /** Applies on the left the translation matrix represented by the vector \a other
00769   * to \c *this and returns a reference to \c *this.
00770   * \sa translate()
00771   */
00772 template<typename Scalar, int Dim, int Mode>
00773 template<typename OtherDerived>
00774 Transform<Scalar,Dim,Mode>&
00775 Transform<Scalar,Dim,Mode>::pretranslate(const MatrixBase<OtherDerived> &other)
00776 {
00777   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
00778   if(int(Mode)==int(Projective))
00779     affine() += other * m_matrix.row(Dim);
00780   else
00781     translation() += other;
00782   return *this;
00783 }
00784 
00785 /** Applies on the right the rotation represented by the rotation \a rotation
00786   * to \c *this and returns a reference to \c *this.
00787   *
00788   * The template parameter \a RotationType is the type of the rotation which
00789   * must be known by internal::toRotationMatrix<>.
00790   *
00791   * Natively supported types includes:
00792   *   - any scalar (2D),
00793   *   - a Dim x Dim matrix expression,
00794   *   - a Quaternion (3D),
00795   *   - a AngleAxis (3D)
00796   *
00797   * This mechanism is easily extendable to support user types such as Euler angles,
00798   * or a pair of Quaternion for 4D rotations.
00799   *
00800   * \sa rotate(Scalar), class Quaternion, class AngleAxis, prerotate(RotationType)
00801   */
00802 template<typename Scalar, int Dim, int Mode>
00803 template<typename RotationType>
00804 Transform<Scalar,Dim,Mode>&
00805 Transform<Scalar,Dim,Mode>::rotate(const RotationType& rotation)
00806 {
00807   linearExt() *= internal::toRotationMatrix<Scalar,Dim>(rotation);
00808   return *this;
00809 }
00810 
00811 /** Applies on the left the rotation represented by the rotation \a rotation
00812   * to \c *this and returns a reference to \c *this.
00813   *
00814   * See rotate() for further details.
00815   *
00816   * \sa rotate()
00817   */
00818 template<typename Scalar, int Dim, int Mode>
00819 template<typename RotationType>
00820 Transform<Scalar,Dim,Mode>&
00821 Transform<Scalar,Dim,Mode>::prerotate(const RotationType& rotation)
00822 {
00823   m_matrix.template block<Dim,HDim>(0,0) = internal::toRotationMatrix<Scalar,Dim>(rotation)
00824                                          * m_matrix.template block<Dim,HDim>(0,0);
00825   return *this;
00826 }
00827 
00828 /** Applies on the right the shear transformation represented
00829   * by the vector \a other to \c *this and returns a reference to \c *this.
00830   * \warning 2D only.
00831   * \sa preshear()
00832   */
00833 template<typename Scalar, int Dim, int Mode>
00834 Transform<Scalar,Dim,Mode>&
00835 Transform<Scalar,Dim,Mode>::shear(Scalar sx, Scalar sy)
00836 {
00837   EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
00838   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
00839   VectorType tmp = linear().col(0)*sy + linear().col(1);
00840   linear() << linear().col(0) + linear().col(1)*sx, tmp;
00841   return *this;
00842 }
00843 
00844 /** Applies on the left the shear transformation represented
00845   * by the vector \a other to \c *this and returns a reference to \c *this.
00846   * \warning 2D only.
00847   * \sa shear()
00848   */
00849 template<typename Scalar, int Dim, int Mode>
00850 Transform<Scalar,Dim,Mode>&
00851 Transform<Scalar,Dim,Mode>::preshear(Scalar sx, Scalar sy)
00852 {
00853   EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
00854   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
00855   m_matrix.template block<Dim,HDim>(0,0) = LinearMatrixType(1, sx, sy, 1) * m_matrix.template block<Dim,HDim>(0,0);
00856   return *this;
00857 }
00858 
00859 /******************************************************
00860 *** Scaling, Translation and Rotation compatibility ***
00861 ******************************************************/
00862 
00863 template<typename Scalar, int Dim, int Mode>
00864 inline Transform<Scalar,Dim,Mode>& Transform<Scalar,Dim,Mode>::operator=(const TranslationType& t)
00865 {
00866   linear().setIdentity();
00867   translation() = t.vector();
00868   makeAffine();
00869   return *this;
00870 }
00871 
00872 template<typename Scalar, int Dim, int Mode>
00873 inline Transform<Scalar,Dim,Mode> Transform<Scalar,Dim,Mode>::operator*(const TranslationType& t) const
00874 {
00875   Transform res = *this;
00876   res.translate(t.vector());
00877   return res;
00878 }
00879 
00880 template<typename Scalar, int Dim, int Mode>
00881 inline Transform<Scalar,Dim,Mode>& Transform<Scalar,Dim,Mode>::operator=(const UniformScaling<Scalar>& s)
00882 {
00883   m_matrix.setZero();
00884   linear().diagonal().fill(s.factor());
00885   makeAffine();
00886   return *this;
00887 }
00888 
00889 template<typename Scalar, int Dim, int Mode>
00890 inline Transform<Scalar,Dim,Mode> Transform<Scalar,Dim,Mode>::operator*(const UniformScaling<Scalar>& s) const
00891 {
00892   Transform res = *this;
00893   res.scale(s.factor());
00894   return res;
00895 }
00896 
00897 template<typename Scalar, int Dim, int Mode>
00898 template<typename Derived>
00899 inline Transform<Scalar,Dim,Mode>& Transform<Scalar,Dim,Mode>::operator=(const RotationBase<Derived,Dim>& r)
00900 {
00901   linear() = internal::toRotationMatrix<Scalar,Dim>(r);
00902   translation().setZero();
00903   makeAffine();
00904   return *this;
00905 }
00906 
00907 template<typename Scalar, int Dim, int Mode>
00908 template<typename Derived>
00909 inline Transform<Scalar,Dim,Mode> Transform<Scalar,Dim,Mode>::operator*(const RotationBase<Derived,Dim>& r) const
00910 {
00911   Transform res = *this;
00912   res.rotate(r.derived());
00913   return res;
00914 }
00915 
00916 /************************
00917 *** Special functions ***
00918 ************************/
00919 
00920 /** \returns the rotation part of the transformation
00921   *
00922   *
00923   * \svd_module
00924   *
00925   * \sa computeRotationScaling(), computeScalingRotation(), class SVD
00926   */
00927 template<typename Scalar, int Dim, int Mode>
00928 typename Transform<Scalar,Dim,Mode>::LinearMatrixType
00929 Transform<Scalar,Dim,Mode>::rotation() const
00930 {
00931   LinearMatrixType result;
00932   computeRotationScaling(&result, (LinearMatrixType*)0);
00933   return result;
00934 }
00935 
00936 
00937 /** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being
00938   * not necessarily positive.
00939   *
00940   * If either pointer is zero, the corresponding computation is skipped.
00941   *
00942   *
00943   *
00944   * \svd_module
00945   *
00946   * \sa computeScalingRotation(), rotation(), class SVD
00947   */
00948 template<typename Scalar, int Dim, int Mode>
00949 template<typename RotationMatrixType, typename ScalingMatrixType>
00950 void Transform<Scalar,Dim,Mode>::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const
00951 {
00952   JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU | ComputeFullV);
00953 
00954   Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1
00955   VectorType sv(svd.singularValues());
00956   sv.coeffRef(0) *= x;
00957   if(scaling) scaling->lazyAssign(svd.matrixV() * sv.asDiagonal() * svd.matrixV().adjoint());
00958   if(rotation)
00959   {
00960     LinearMatrixType m(svd.matrixU());
00961     m.col(0) /= x;
00962     rotation->lazyAssign(m * svd.matrixV().adjoint());
00963   }
00964 }
00965 
00966 /** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being
00967   * not necessarily positive.
00968   *
00969   * If either pointer is zero, the corresponding computation is skipped.
00970   *
00971   *
00972   *
00973   * \svd_module
00974   *
00975   * \sa computeRotationScaling(), rotation(), class SVD
00976   */
00977 template<typename Scalar, int Dim, int Mode>
00978 template<typename ScalingMatrixType, typename RotationMatrixType>
00979 void Transform<Scalar,Dim,Mode>::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const
00980 {
00981   JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU | ComputeFullV);
00982 
00983   Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1
00984   VectorType sv(svd.singularValues());
00985   sv.coeffRef(0) *= x;
00986   if(scaling) scaling->lazyAssign(svd.matrixU() * sv.asDiagonal() * svd.matrixU().adjoint());
00987   if(rotation)
00988   {
00989     LinearMatrixType m(svd.matrixU());
00990     m.col(0) /= x;
00991     rotation->lazyAssign(m * svd.matrixV().adjoint());
00992   }
00993 }
00994 
00995 /** Convenient method to set \c *this from a position, orientation and scale
00996   * of a 3D object.
00997   */
00998 template<typename Scalar, int Dim, int Mode>
00999 template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
01000 Transform<Scalar,Dim,Mode>&
01001 Transform<Scalar,Dim,Mode>::fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
01002   const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale)
01003 {
01004   linear() = internal::toRotationMatrix<Scalar,Dim>(orientation);
01005   linear() *= scale.asDiagonal();
01006   translation() = position;
01007   makeAffine();
01008   return *this;
01009 }
01010 
01011 namespace internal {
01012 
01013 // selector needed to avoid taking the inverse of a 3x4 matrix
01014 template<typename TransformType, int Mode=TransformType::Mode>
01015 struct projective_transform_inverse
01016 {
01017   static inline void run(const TransformType&, TransformType&)
01018   {}
01019 };
01020 
01021 template<typename TransformType>
01022 struct projective_transform_inverse<TransformType, Projective>
01023 {
01024   static inline void run(const TransformType& m, TransformType& res)
01025   {
01026     res.matrix() = m.matrix().inverse();
01027   }
01028 };
01029 
01030 } // end namespace internal
01031 
01032 
01033 /**
01034   *
01035   * \returns the inverse transformation according to some given knowledge
01036   * on \c *this.
01037   *
01038   * \param hint allows to optimize the inversion process when the transformation
01039   * is known to be not a general transformation. The possible values are:
01040   *  - Projective if the transformation is not necessarily affine, i.e., if the
01041   *    last row is not guaranteed to be [0 ... 0 1]
01042   *  - Affine is the default, the last row is assumed to be [0 ... 0 1]
01043   *  - Isometry if the transformation is only a concatenations of translations
01044   *    and rotations.
01045   *
01046   * \warning unless \a traits is always set to NoShear or NoScaling, this function
01047   * requires the generic inverse method of MatrixBase defined in the LU module. If
01048   * you forget to include this module, then you will get hard to debug linking errors.
01049   *
01050   * \sa MatrixBase::inverse()
01051   */
01052 template<typename Scalar, int Dim, int Mode>
01053 Transform<Scalar,Dim,Mode>
01054 Transform<Scalar,Dim,Mode>::inverse(TransformTraits hint) const
01055 {
01056   Transform res;
01057   if (hint == Projective)
01058   {
01059     internal::projective_transform_inverse<Transform>::run(*this, res);
01060   }
01061   else
01062   {
01063     if (hint == Isometry)
01064     {
01065       res.matrix().template topLeftCorner<Dim,Dim>() = linear().transpose();
01066     }
01067     else if(hint&Affine)
01068     {
01069       res.matrix().template topLeftCorner<Dim,Dim>() = linear().inverse();
01070     }
01071     else
01072     {
01073       eigen_assert(false && "Invalid transform traits in Transform::Inverse");
01074     }
01075     // translation and remaining parts
01076     res.matrix().template topRightCorner<Dim,1>()
01077       = - res.matrix().template topLeftCorner<Dim,Dim>() * translation();
01078     res.makeAffine(); // we do need this, because in the beginning res is uninitialized
01079   }
01080   return res;
01081 }
01082 
01083 namespace internal {
01084 
01085 /*****************************************************
01086 *** Specializations of take affine part            ***
01087 *****************************************************/
01088 
01089 template<typename TransformType> struct transform_take_affine_part {
01090   typedef typename TransformType::MatrixType MatrixType;
01091   typedef typename TransformType::AffinePart AffinePart;
01092   typedef typename TransformType::ConstAffinePart ConstAffinePart;
01093   static inline AffinePart run(MatrixType& m)
01094   { return m.template block<TransformType::Dim,TransformType::HDim>(0,0); }
01095   static inline ConstAffinePart run(const MatrixType& m)
01096   { return m.template block<TransformType::Dim,TransformType::HDim>(0,0); }
01097 };
01098 
01099 template<typename Scalar, int Dim>
01100 struct transform_take_affine_part<Transform<Scalar,Dim,AffineCompact> > {
01101   typedef typename Transform<Scalar,Dim,AffineCompact>::MatrixType MatrixType;
01102   static inline MatrixType& run(MatrixType& m) { return m; }
01103   static inline const MatrixType& run(const MatrixType& m) { return m; }
01104 };
01105 
01106 /*****************************************************
01107 *** Specializations of construct from matrix       ***
01108 *****************************************************/
01109 
01110 template<typename Other, int Mode, int Dim, int HDim>
01111 struct transform_construct_from_matrix<Other, Mode,Dim,HDim, Dim,Dim>
01112 {
01113   static inline void run(Transform<typename Other::Scalar,Dim,Mode> *transform, const Other& other)
01114   {
01115     transform->linear() = other;
01116     transform->translation().setZero();
01117     transform->makeAffine();
01118   }
01119 };
01120 
01121 template<typename Other, int Mode, int Dim, int HDim>
01122 struct transform_construct_from_matrix<Other, Mode,Dim,HDim, Dim,HDim>
01123 {
01124   static inline void run(Transform<typename Other::Scalar,Dim,Mode> *transform, const Other& other)
01125   {
01126     transform->affine() = other;
01127     transform->makeAffine();
01128   }
01129 };
01130 
01131 template<typename Other, int Mode, int Dim, int HDim>
01132 struct transform_construct_from_matrix<Other, Mode,Dim,HDim, HDim,HDim>
01133 {
01134   static inline void run(Transform<typename Other::Scalar,Dim,Mode> *transform, const Other& other)
01135   { transform->matrix() = other; }
01136 };
01137 
01138 template<typename Other, int Dim, int HDim>
01139 struct transform_construct_from_matrix<Other, AffineCompact,Dim,HDim, HDim,HDim>
01140 {
01141   static inline void run(Transform<typename Other::Scalar,Dim,AffineCompact> *transform, const Other& other)
01142   { transform->matrix() = other.template block<Dim,HDim>(0,0); }
01143 };
01144 
01145 /**********************************************************
01146 ***   Specializations of operator* with rhs EigenBase   ***
01147 **********************************************************/
01148 
01149 template<int LhsMode,int RhsMode>
01150 struct transform_product_result
01151 {
01152   enum 
01153   { 
01154     Mode =
01155       (LhsMode == (int)Projective    || RhsMode == (int)Projective    ) ? Projective :
01156       (LhsMode == (int)Affine        || RhsMode == (int)Affine        ) ? Affine :
01157       (LhsMode == (int)AffineCompact || RhsMode == (int)AffineCompact ) ? AffineCompact :
01158       (LhsMode == (int)Isometry      || RhsMode == (int)Isometry      ) ? Isometry : Projective
01159   };
01160 };
01161 
01162 template< typename TransformType, typename MatrixType >
01163 struct transform_right_product_impl< TransformType, MatrixType, true >
01164 {
01165   typedef typename MatrixType::PlainObject ResultType;
01166 
01167   EIGEN_STRONG_INLINE static ResultType run(const TransformType& T, const MatrixType& other)
01168   {
01169     return T.matrix() * other;
01170   }
01171 };
01172 
01173 template< typename TransformType, typename MatrixType >
01174 struct transform_right_product_impl< TransformType, MatrixType, false >
01175 {
01176   enum { 
01177     Dim = TransformType::Dim, 
01178     HDim = TransformType::HDim,
01179     OtherRows = MatrixType::RowsAtCompileTime,
01180     OtherCols = MatrixType::ColsAtCompileTime
01181   };
01182 
01183   typedef typename MatrixType::PlainObject ResultType;
01184 
01185   EIGEN_STRONG_INLINE static ResultType run(const TransformType& T, const MatrixType& other)
01186   {
01187     EIGEN_STATIC_ASSERT(OtherRows==Dim || OtherRows==HDim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
01188 
01189     typedef Block<ResultType, Dim, OtherCols> TopLeftLhs;
01190     typedef Block<const MatrixType, Dim, OtherCols> TopLeftRhs;
01191 
01192     ResultType res(other.rows(),other.cols());
01193 
01194     TopLeftLhs(res, 0, 0, Dim, other.cols()) =
01195       ( T.linear() * TopLeftRhs(other, 0, 0, Dim, other.cols()) ).colwise() +
01196         T.translation();
01197 
01198     // we need to take .rows() because OtherRows might be Dim or HDim
01199     if (OtherRows==HDim)
01200       res.row(other.rows()) = other.row(other.rows());
01201 
01202     return res;
01203   }
01204 };
01205 
01206 /**********************************************************
01207 ***   Specializations of operator* with lhs EigenBase   ***
01208 **********************************************************/
01209 
01210 // generic HDim x HDim matrix * T => Projective
01211 template<typename Other,int Mode, int Dim, int HDim>
01212 struct transform_left_product_impl<Other,Mode,Dim,HDim, HDim,HDim>
01213 {
01214   typedef Transform<typename Other::Scalar,Dim,Mode> TransformType;
01215   typedef typename TransformType::MatrixType MatrixType;
01216   typedef Transform<typename Other::Scalar,Dim,Projective> ResultType;
01217   static ResultType run(const Other& other,const TransformType& tr)
01218   { return ResultType(other * tr.matrix()); }
01219 };
01220 
01221 // generic HDim x HDim matrix * AffineCompact => Projective
01222 template<typename Other, int Dim, int HDim>
01223 struct transform_left_product_impl<Other,AffineCompact,Dim,HDim, HDim,HDim>
01224 {
01225   typedef Transform<typename Other::Scalar,Dim,AffineCompact> TransformType;
01226   typedef typename TransformType::MatrixType MatrixType;
01227   typedef Transform<typename Other::Scalar,Dim,Projective> ResultType;
01228   static ResultType run(const Other& other,const TransformType& tr)
01229   {
01230     ResultType res;
01231     res.matrix().noalias() = other.template block<HDim,Dim>(0,0) * tr.matrix();
01232     res.matrix().col(Dim) += other.col(Dim);
01233     return res;
01234   }
01235 };
01236 
01237 // affine matrix * T
01238 template<typename Other,int Mode, int Dim, int HDim>
01239 struct transform_left_product_impl<Other,Mode,Dim,HDim, Dim,HDim>
01240 {
01241   typedef Transform<typename Other::Scalar,Dim,Mode> TransformType;
01242   typedef typename TransformType::MatrixType MatrixType;
01243   typedef TransformType ResultType;
01244   static ResultType run(const Other& other,const TransformType& tr)
01245   {
01246     ResultType res;
01247     res.affine().noalias() = other * tr.matrix();
01248     res.matrix().row(Dim) = tr.matrix().row(Dim);
01249     return res;
01250   }
01251 };
01252 
01253 // affine matrix * AffineCompact
01254 template<typename Other, int Dim, int HDim>
01255 struct transform_left_product_impl<Other,AffineCompact,Dim,HDim, Dim,HDim>
01256 {
01257   typedef Transform<typename Other::Scalar,Dim,AffineCompact> TransformType;
01258   typedef typename TransformType::MatrixType MatrixType;
01259   typedef TransformType ResultType;
01260   static ResultType run(const Other& other,const TransformType& tr)
01261   {
01262     ResultType res;
01263     res.matrix().noalias() = other.template block<Dim,Dim>(0,0) * tr.matrix();
01264     res.translation() += other.col(Dim);
01265     return res;
01266   }
01267 };
01268 
01269 // linear matrix * T
01270 template<typename Other,int Mode, int Dim, int HDim>
01271 struct transform_left_product_impl<Other,Mode,Dim,HDim, Dim,Dim>
01272 {
01273   typedef Transform<typename Other::Scalar,Dim,Mode> TransformType;
01274   typedef typename TransformType::MatrixType MatrixType;
01275   typedef TransformType ResultType;
01276   static ResultType run(const Other& other, const TransformType& tr)
01277   {
01278     TransformType res;
01279     if(Mode!=int(AffineCompact))
01280       res.matrix().row(Dim) = tr.matrix().row(Dim);
01281     res.matrix().template topRows<Dim>().noalias()
01282       = other * tr.matrix().template topRows<Dim>();
01283     return res;
01284   }
01285 };
01286 
01287 /**********************************************************
01288 *** Specializations of operator* with another Transform ***
01289 **********************************************************/
01290 
01291 template<typename Scalar, int Dim, int LhsMode, int RhsMode>
01292 struct transform_transform_product_impl<Transform<Scalar,Dim,LhsMode>,Transform<Scalar,Dim,RhsMode>,false >
01293 {
01294   enum { ResultMode = transform_product_result<LhsMode,RhsMode>::Mode };
01295   typedef Transform<Scalar,Dim,LhsMode> Lhs;
01296   typedef Transform<Scalar,Dim,RhsMode> Rhs;
01297   typedef Transform<Scalar,Dim,ResultMode> ResultType;
01298   static ResultType run(const Lhs& lhs, const Rhs& rhs)
01299   {
01300     ResultType res;
01301     res.linear() = lhs.linear() * rhs.linear();
01302     res.translation() = lhs.linear() * rhs.translation() + lhs.translation();
01303     res.makeAffine();
01304     return res;
01305   }
01306 };
01307 
01308 template<typename Scalar, int Dim, int LhsMode, int RhsMode>
01309 struct transform_transform_product_impl<Transform<Scalar,Dim,LhsMode>,Transform<Scalar,Dim,RhsMode>,true >
01310 {
01311   typedef Transform<Scalar,Dim,LhsMode> Lhs;
01312   typedef Transform<Scalar,Dim,RhsMode> Rhs;
01313   typedef Transform<Scalar,Dim,Projective> ResultType;
01314   static ResultType run(const Lhs& lhs, const Rhs& rhs)
01315   {
01316     return ResultType( lhs.matrix() * rhs.matrix() );
01317   }
01318 };
01319 
01320 } // end namespace internal
01321 
01322 #endif // EIGEN_TRANSFORM_H



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011