| 1 |
// This file is part of Eigen, a lightweight C++ template library |
| 2 |
// for linear algebra. |
| 3 |
// |
| 4 |
// Copyright (C) 2008 Gael Guennebaud <[email protected]> |
| 5 |
// Copyright (C) 2009 Benoit Jacob <[email protected]> |
| 6 |
// |
| 7 |
// This Source Code Form is subject to the terms of the Mozilla |
| 8 |
// Public License v. 2.0. If a copy of the MPL was not distributed |
| 9 |
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. |
| 10 |
|
| 11 |
// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway |
| 12 |
|
| 13 |
namespace Eigen { |
| 14 |
|
| 15 |
// Note that we have to pass Dim and HDim because it is not allowed to use a template |
| 16 |
// parameter to define a template specialization. To be more precise, in the following |
| 17 |
// specializations, it is not allowed to use Dim+1 instead of HDim. |
| 18 |
template< typename Other, |
| 19 |
int Dim, |
| 20 |
int HDim, |
| 21 |
int OtherRows=Other::RowsAtCompileTime, |
| 22 |
int OtherCols=Other::ColsAtCompileTime> |
| 23 |
struct ei_transform_product_impl; |
| 24 |
|
| 25 |
/** \geometry_module \ingroup Geometry_Module |
| 26 |
* |
| 27 |
* \class Transform |
| 28 |
* |
| 29 |
* \brief Represents an homogeneous transformation in a N dimensional space |
| 30 |
* |
| 31 |
* \param _Scalar the scalar type, i.e., the type of the coefficients |
| 32 |
* \param _Dim the dimension of the space |
| 33 |
* |
| 34 |
* The homography is internally represented and stored as a (Dim+1)^2 matrix which |
| 35 |
* is available through the matrix() method. |
| 36 |
* |
| 37 |
* Conversion methods from/to Qt's QMatrix and QTransform are available if the |
| 38 |
* preprocessor token EIGEN_QT_SUPPORT is defined. |
| 39 |
* |
| 40 |
* \sa class Matrix, class Quaternion |
| 41 |
*/ |
| 42 |
template<typename _Scalar, int _Dim> |
| 43 |
class Transform |
| 44 |
{ |
| 45 |
public: |
| 46 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim==Dynamic ? Dynamic : (_Dim+1)*(_Dim+1)) |
| 47 |
enum { |
| 48 |
Dim = _Dim, ///< space dimension in which the transformation holds |
| 49 |
HDim = _Dim+1 ///< size of a respective homogeneous vector |
| 50 |
}; |
| 51 |
/** the scalar type of the coefficients */ |
| 52 |
typedef _Scalar Scalar; |
| 53 |
/** type of the matrix used to represent the transformation */ |
| 54 |
typedef Matrix<Scalar,HDim,HDim> MatrixType; |
| 55 |
/** type of the matrix used to represent the linear part of the transformation */ |
| 56 |
typedef Matrix<Scalar,Dim,Dim> LinearMatrixType; |
| 57 |
/** type of read/write reference to the linear part of the transformation */ |
| 58 |
typedef Block<MatrixType,Dim,Dim> LinearPart; |
| 59 |
/** type of read/write reference to the linear part of the transformation */ |
| 60 |
typedef const Block<const MatrixType,Dim,Dim> ConstLinearPart; |
| 61 |
/** type of a vector */ |
| 62 |
typedef Matrix<Scalar,Dim,1> VectorType; |
| 63 |
/** type of a read/write reference to the translation part of the rotation */ |
| 64 |
typedef Block<MatrixType,Dim,1> TranslationPart; |
| 65 |
/** type of a read/write reference to the translation part of the rotation */ |
| 66 |
typedef const Block<const MatrixType,Dim,1> ConstTranslationPart; |
| 67 |
/** corresponding translation type */ |
| 68 |
typedef Translation<Scalar,Dim> TranslationType; |
| 69 |
/** corresponding scaling transformation type */ |
| 70 |
typedef Scaling<Scalar,Dim> ScalingType; |
| 71 |
|
| 72 |
protected: |
| 73 |
|
| 74 |
MatrixType m_matrix; |
| 75 |
|
| 76 |
public: |
| 77 |
|
| 78 |
/** Default constructor without initialization of the coefficients. */ |
| 79 |
inline Transform() { } |
| 80 |
|
| 81 |
inline Transform(const Transform& other) |
| 82 |
{ |
| 83 |
m_matrix = other.m_matrix; |
| 84 |
} |
| 85 |
|
| 86 |
inline explicit Transform(const TranslationType& t) { *this = t; } |
| 87 |
inline explicit Transform(const ScalingType& s) { *this = s; } |
| 88 |
template<typename Derived> |
| 89 |
inline explicit Transform(const RotationBase<Derived, Dim>& r) { *this = r; } |
| 90 |
|
| 91 |
inline Transform& operator=(const Transform& other) |
| 92 |
{ m_matrix = other.m_matrix; return *this; } |
| 93 |
|
| 94 |
template<typename OtherDerived, bool BigMatrix> // MSVC 2005 will commit suicide if BigMatrix has a default value |
| 95 |
struct construct_from_matrix |
| 96 |
{ |
| 97 |
static inline void run(Transform *transform, const MatrixBase<OtherDerived>& other) |
| 98 |
{ |
| 99 |
transform->matrix() = other; |
| 100 |
} |
| 101 |
}; |
| 102 |
|
| 103 |
template<typename OtherDerived> struct construct_from_matrix<OtherDerived, true> |
| 104 |
{ |
| 105 |
static inline void run(Transform *transform, const MatrixBase<OtherDerived>& other) |
| 106 |
{ |
| 107 |
transform->linear() = other; |
| 108 |
transform->translation().setZero(); |
| 109 |
transform->matrix()(Dim,Dim) = Scalar(1); |
| 110 |
transform->matrix().template block<1,Dim>(Dim,0).setZero(); |
| 111 |
} |
| 112 |
}; |
| 113 |
|
| 114 |
/** Constructs and initializes a transformation from a Dim^2 or a (Dim+1)^2 matrix. */ |
| 115 |
template<typename OtherDerived> |
| 116 |
inline explicit Transform(const MatrixBase<OtherDerived>& other) |
| 117 |
{ |
| 118 |
construct_from_matrix<OtherDerived, int(OtherDerived::RowsAtCompileTime) == Dim>::run(this, other); |
| 119 |
} |
| 120 |
|
| 121 |
/** Set \c *this from a (Dim+1)^2 matrix. */ |
| 122 |
template<typename OtherDerived> |
| 123 |
inline Transform& operator=(const MatrixBase<OtherDerived>& other) |
| 124 |
{ m_matrix = other; return *this; } |
| 125 |
|
| 126 |
#ifdef EIGEN_QT_SUPPORT |
| 127 |
inline Transform(const QMatrix& other); |
| 128 |
inline Transform& operator=(const QMatrix& other); |
| 129 |
inline QMatrix toQMatrix(void) const; |
| 130 |
inline Transform(const QTransform& other); |
| 131 |
inline Transform& operator=(const QTransform& other); |
| 132 |
inline QTransform toQTransform(void) const; |
| 133 |
#endif |
| 134 |
|
| 135 |
/** shortcut for m_matrix(row,col); |
| 136 |
* \sa MatrixBase::operaror(int,int) const */ |
| 137 |
inline Scalar operator() (int row, int col) const { return m_matrix(row,col); } |
| 138 |
/** shortcut for m_matrix(row,col); |
| 139 |
* \sa MatrixBase::operaror(int,int) */ |
| 140 |
inline Scalar& operator() (int row, int col) { return m_matrix(row,col); } |
| 141 |
|
| 142 |
/** \returns a read-only expression of the transformation matrix */ |
| 143 |
inline const MatrixType& matrix() const { return m_matrix; } |
| 144 |
/** \returns a writable expression of the transformation matrix */ |
| 145 |
inline MatrixType& matrix() { return m_matrix; } |
| 146 |
|
| 147 |
/** \returns a read-only expression of the linear (linear) part of the transformation */ |
| 148 |
inline ConstLinearPart linear() const { return m_matrix.template block<Dim,Dim>(0,0); } |
| 149 |
/** \returns a writable expression of the linear (linear) part of the transformation */ |
| 150 |
inline LinearPart linear() { return m_matrix.template block<Dim,Dim>(0,0); } |
| 151 |
|
| 152 |
/** \returns a read-only expression of the translation vector of the transformation */ |
| 153 |
inline ConstTranslationPart translation() const { return m_matrix.template block<Dim,1>(0,Dim); } |
| 154 |
/** \returns a writable expression of the translation vector of the transformation */ |
| 155 |
inline TranslationPart translation() { return m_matrix.template block<Dim,1>(0,Dim); } |
| 156 |
|
| 157 |
/** \returns an expression of the product between the transform \c *this and a matrix expression \a other |
| 158 |
* |
| 159 |
* The right hand side \a other might be either: |
| 160 |
* \li a vector of size Dim, |
| 161 |
* \li an homogeneous vector of size Dim+1, |
| 162 |
* \li a transformation matrix of size Dim+1 x Dim+1. |
| 163 |
*/ |
| 164 |
// note: this function is defined here because some compilers cannot find the respective declaration |
| 165 |
template<typename OtherDerived> |
| 166 |
inline const typename ei_transform_product_impl<OtherDerived,_Dim,_Dim+1>::ResultType |
| 167 |
operator * (const MatrixBase<OtherDerived> &other) const |
| 168 |
{ return ei_transform_product_impl<OtherDerived,Dim,HDim>::run(*this,other.derived()); } |
| 169 |
|
| 170 |
/** \returns the product expression of a transformation matrix \a a times a transform \a b |
| 171 |
* The transformation matrix \a a must have a Dim+1 x Dim+1 sizes. */ |
| 172 |
template<typename OtherDerived> |
| 173 |
friend inline const typename ProductReturnType<OtherDerived,MatrixType>::Type |
| 174 |
operator * (const MatrixBase<OtherDerived> &a, const Transform &b) |
| 175 |
{ return a.derived() * b.matrix(); } |
| 176 |
|
| 177 |
/** Contatenates two transformations */ |
| 178 |
inline const Transform |
| 179 |
operator * (const Transform& other) const |
| 180 |
{ return Transform(m_matrix * other.matrix()); } |
| 181 |
|
| 182 |
/** \sa MatrixBase::setIdentity() */ |
| 183 |
void setIdentity() { m_matrix.setIdentity(); } |
| 184 |
static const typename MatrixType::IdentityReturnType Identity() |
| 185 |
{ |
| 186 |
return MatrixType::Identity(); |
| 187 |
} |
| 188 |
|
| 189 |
template<typename OtherDerived> |
| 190 |
inline Transform& scale(const MatrixBase<OtherDerived> &other); |
| 191 |
|
| 192 |
template<typename OtherDerived> |
| 193 |
inline Transform& prescale(const MatrixBase<OtherDerived> &other); |
| 194 |
|
| 195 |
inline Transform& scale(Scalar s); |
| 196 |
inline Transform& prescale(Scalar s); |
| 197 |
|
| 198 |
template<typename OtherDerived> |
| 199 |
inline Transform& translate(const MatrixBase<OtherDerived> &other); |
| 200 |
|
| 201 |
template<typename OtherDerived> |
| 202 |
inline Transform& pretranslate(const MatrixBase<OtherDerived> &other); |
| 203 |
|
| 204 |
template<typename RotationType> |
| 205 |
inline Transform& rotate(const RotationType& rotation); |
| 206 |
|
| 207 |
template<typename RotationType> |
| 208 |
inline Transform& prerotate(const RotationType& rotation); |
| 209 |
|
| 210 |
Transform& shear(Scalar sx, Scalar sy); |
| 211 |
Transform& preshear(Scalar sx, Scalar sy); |
| 212 |
|
| 213 |
inline Transform& operator=(const TranslationType& t); |
| 214 |
inline Transform& operator*=(const TranslationType& t) { return translate(t.vector()); } |
| 215 |
inline Transform operator*(const TranslationType& t) const; |
| 216 |
|
| 217 |
inline Transform& operator=(const ScalingType& t); |
| 218 |
inline Transform& operator*=(const ScalingType& s) { return scale(s.coeffs()); } |
| 219 |
inline Transform operator*(const ScalingType& s) const; |
| 220 |
friend inline Transform operator*(const LinearMatrixType& mat, const Transform& t) |
| 221 |
{ |
| 222 |
Transform res = t; |
| 223 |
res.matrix().row(Dim) = t.matrix().row(Dim); |
| 224 |
res.matrix().template block<Dim,HDim>(0,0) = (mat * t.matrix().template block<Dim,HDim>(0,0)).lazy(); |
| 225 |
return res; |
| 226 |
} |
| 227 |
|
| 228 |
template<typename Derived> |
| 229 |
inline Transform& operator=(const RotationBase<Derived,Dim>& r); |
| 230 |
template<typename Derived> |
| 231 |
inline Transform& operator*=(const RotationBase<Derived,Dim>& r) { return rotate(r.toRotationMatrix()); } |
| 232 |
template<typename Derived> |
| 233 |
inline Transform operator*(const RotationBase<Derived,Dim>& r) const; |
| 234 |
|
| 235 |
LinearMatrixType rotation() const; |
| 236 |
template<typename RotationMatrixType, typename ScalingMatrixType> |
| 237 |
void computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const; |
| 238 |
template<typename ScalingMatrixType, typename RotationMatrixType> |
| 239 |
void computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const; |
| 240 |
|
| 241 |
template<typename PositionDerived, typename OrientationType, typename ScaleDerived> |
| 242 |
Transform& fromPositionOrientationScale(const MatrixBase<PositionDerived> &position, |
| 243 |
const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale); |
| 244 |
|
| 245 |
inline const MatrixType inverse(TransformTraits traits = Affine) const; |
| 246 |
|
| 247 |
/** \returns a const pointer to the column major internal matrix */ |
| 248 |
const Scalar* data() const { return m_matrix.data(); } |
| 249 |
/** \returns a non-const pointer to the column major internal matrix */ |
| 250 |
Scalar* data() { return m_matrix.data(); } |
| 251 |
|
| 252 |
/** \returns \c *this with scalar type casted to \a NewScalarType |
| 253 |
* |
| 254 |
* Note that if \a NewScalarType is equal to the current scalar type of \c *this |
| 255 |
* then this function smartly returns a const reference to \c *this. |
| 256 |
*/ |
| 257 |
template<typename NewScalarType> |
| 258 |
inline typename internal::cast_return_type<Transform,Transform<NewScalarType,Dim> >::type cast() const |
| 259 |
{ return typename internal::cast_return_type<Transform,Transform<NewScalarType,Dim> >::type(*this); } |
| 260 |
|
| 261 |
/** Copy constructor with scalar type conversion */ |
| 262 |
template<typename OtherScalarType> |
| 263 |
inline explicit Transform(const Transform<OtherScalarType,Dim>& other) |
| 264 |
{ m_matrix = other.matrix().template cast<Scalar>(); } |
| 265 |
|
| 266 |
/** \returns \c true if \c *this is approximately equal to \a other, within the precision |
| 267 |
* determined by \a prec. |
| 268 |
* |
| 269 |
* \sa MatrixBase::isApprox() */ |
| 270 |
bool isApprox(const Transform& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const |
| 271 |
{ return m_matrix.isApprox(other.m_matrix, prec); } |
| 272 |
|
| 273 |
#ifdef EIGEN_TRANSFORM_PLUGIN |
| 274 |
#include EIGEN_TRANSFORM_PLUGIN |
| 275 |
#endif |
| 276 |
|
| 277 |
protected: |
| 278 |
|
| 279 |
}; |
| 280 |
|
| 281 |
/** \ingroup Geometry_Module */ |
| 282 |
typedef Transform<float,2> Transform2f; |
| 283 |
/** \ingroup Geometry_Module */ |
| 284 |
typedef Transform<float,3> Transform3f; |
| 285 |
/** \ingroup Geometry_Module */ |
| 286 |
typedef Transform<double,2> Transform2d; |
| 287 |
/** \ingroup Geometry_Module */ |
| 288 |
typedef Transform<double,3> Transform3d; |
| 289 |
|
| 290 |
/************************** |
| 291 |
*** Optional QT support *** |
| 292 |
**************************/ |
| 293 |
|
| 294 |
#ifdef EIGEN_QT_SUPPORT |
| 295 |
/** Initialises \c *this from a QMatrix assuming the dimension is 2. |
| 296 |
* |
| 297 |
* This function is available only if the token EIGEN_QT_SUPPORT is defined. |
| 298 |
*/ |
| 299 |
template<typename Scalar, int Dim> |
| 300 |
Transform<Scalar,Dim>::Transform(const QMatrix& other) |
| 301 |
{ |
| 302 |
*this = other; |
| 303 |
} |
| 304 |
|
| 305 |
/** Set \c *this from a QMatrix assuming the dimension is 2. |
| 306 |
* |
| 307 |
* This function is available only if the token EIGEN_QT_SUPPORT is defined. |
| 308 |
*/ |
| 309 |
template<typename Scalar, int Dim> |
| 310 |
Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const QMatrix& other) |
| 311 |
{ |
| 312 |
EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) |
| 313 |
m_matrix << other.m11(), other.m21(), other.dx(), |
| 314 |
other.m12(), other.m22(), other.dy(), |
| 315 |
0, 0, 1; |
| 316 |
return *this; |
| 317 |
} |
| 318 |
|
| 319 |
/** \returns a QMatrix from \c *this assuming the dimension is 2. |
| 320 |
* |
| 321 |
* \warning this convertion might loss data if \c *this is not affine |
| 322 |
* |
| 323 |
* This function is available only if the token EIGEN_QT_SUPPORT is defined. |
| 324 |
*/ |
| 325 |
template<typename Scalar, int Dim> |
| 326 |
QMatrix Transform<Scalar,Dim>::toQMatrix(void) const |
| 327 |
{ |
| 328 |
EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) |
| 329 |
return QMatrix(m_matrix.coeff(0,0), m_matrix.coeff(1,0), |
| 330 |
m_matrix.coeff(0,1), m_matrix.coeff(1,1), |
| 331 |
m_matrix.coeff(0,2), m_matrix.coeff(1,2)); |
| 332 |
} |
| 333 |
|
| 334 |
/** Initialises \c *this from a QTransform assuming the dimension is 2. |
| 335 |
* |
| 336 |
* This function is available only if the token EIGEN_QT_SUPPORT is defined. |
| 337 |
*/ |
| 338 |
template<typename Scalar, int Dim> |
| 339 |
Transform<Scalar,Dim>::Transform(const QTransform& other) |
| 340 |
{ |
| 341 |
*this = other; |
| 342 |
} |
| 343 |
|
| 344 |
/** Set \c *this from a QTransform assuming the dimension is 2. |
| 345 |
* |
| 346 |
* This function is available only if the token EIGEN_QT_SUPPORT is defined. |
| 347 |
*/ |
| 348 |
template<typename Scalar, int Dim> |
| 349 |
Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const QTransform& other) |
| 350 |
{ |
| 351 |
EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) |
| 352 |
m_matrix << other.m11(), other.m21(), other.dx(), |
| 353 |
other.m12(), other.m22(), other.dy(), |
| 354 |
other.m13(), other.m23(), other.m33(); |
| 355 |
return *this; |
| 356 |
} |
| 357 |
|
| 358 |
/** \returns a QTransform from \c *this assuming the dimension is 2. |
| 359 |
* |
| 360 |
* This function is available only if the token EIGEN_QT_SUPPORT is defined. |
| 361 |
*/ |
| 362 |
template<typename Scalar, int Dim> |
| 363 |
QTransform Transform<Scalar,Dim>::toQTransform(void) const |
| 364 |
{ |
| 365 |
EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) |
| 366 |
return QTransform(m_matrix.coeff(0,0), m_matrix.coeff(1,0), m_matrix.coeff(2,0), |
| 367 |
m_matrix.coeff(0,1), m_matrix.coeff(1,1), m_matrix.coeff(2,1), |
| 368 |
m_matrix.coeff(0,2), m_matrix.coeff(1,2), m_matrix.coeff(2,2)); |
| 369 |
} |
| 370 |
#endif |
| 371 |
|
| 372 |
/********************* |
| 373 |
*** Procedural API *** |
| 374 |
*********************/ |
| 375 |
|
| 376 |
/** Applies on the right the non uniform scale transformation represented |
| 377 |
* by the vector \a other to \c *this and returns a reference to \c *this. |
| 378 |
* \sa prescale() |
| 379 |
*/ |
| 380 |
template<typename Scalar, int Dim> |
| 381 |
template<typename OtherDerived> |
| 382 |
Transform<Scalar,Dim>& |
| 383 |
Transform<Scalar,Dim>::scale(const MatrixBase<OtherDerived> &other) |
| 384 |
{ |
| 385 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) |
| 386 |
linear() = (linear() * other.asDiagonal()).lazy(); |
| 387 |
return *this; |
| 388 |
} |
| 389 |
|
| 390 |
/** Applies on the right a uniform scale of a factor \a c to \c *this |
| 391 |
* and returns a reference to \c *this. |
| 392 |
* \sa prescale(Scalar) |
| 393 |
*/ |
| 394 |
template<typename Scalar, int Dim> |
| 395 |
inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::scale(Scalar s) |
| 396 |
{ |
| 397 |
linear() *= s; |
| 398 |
return *this; |
| 399 |
} |
| 400 |
|
| 401 |
/** Applies on the left the non uniform scale transformation represented |
| 402 |
* by the vector \a other to \c *this and returns a reference to \c *this. |
| 403 |
* \sa scale() |
| 404 |
*/ |
| 405 |
template<typename Scalar, int Dim> |
| 406 |
template<typename OtherDerived> |
| 407 |
Transform<Scalar,Dim>& |
| 408 |
Transform<Scalar,Dim>::prescale(const MatrixBase<OtherDerived> &other) |
| 409 |
{ |
| 410 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) |
| 411 |
m_matrix.template block<Dim,HDim>(0,0) = (other.asDiagonal() * m_matrix.template block<Dim,HDim>(0,0)).lazy(); |
| 412 |
return *this; |
| 413 |
} |
| 414 |
|
| 415 |
/** Applies on the left a uniform scale of a factor \a c to \c *this |
| 416 |
* and returns a reference to \c *this. |
| 417 |
* \sa scale(Scalar) |
| 418 |
*/ |
| 419 |
template<typename Scalar, int Dim> |
| 420 |
inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::prescale(Scalar s) |
| 421 |
{ |
| 422 |
m_matrix.template corner<Dim,HDim>(TopLeft) *= s; |
| 423 |
return *this; |
| 424 |
} |
| 425 |
|
| 426 |
/** Applies on the right the translation matrix represented by the vector \a other |
| 427 |
* to \c *this and returns a reference to \c *this. |
| 428 |
* \sa pretranslate() |
| 429 |
*/ |
| 430 |
template<typename Scalar, int Dim> |
| 431 |
template<typename OtherDerived> |
| 432 |
Transform<Scalar,Dim>& |
| 433 |
Transform<Scalar,Dim>::translate(const MatrixBase<OtherDerived> &other) |
| 434 |
{ |
| 435 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) |
| 436 |
translation() += linear() * other; |
| 437 |
return *this; |
| 438 |
} |
| 439 |
|
| 440 |
/** Applies on the left the translation matrix represented by the vector \a other |
| 441 |
* to \c *this and returns a reference to \c *this. |
| 442 |
* \sa translate() |
| 443 |
*/ |
| 444 |
template<typename Scalar, int Dim> |
| 445 |
template<typename OtherDerived> |
| 446 |
Transform<Scalar,Dim>& |
| 447 |
Transform<Scalar,Dim>::pretranslate(const MatrixBase<OtherDerived> &other) |
| 448 |
{ |
| 449 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) |
| 450 |
translation() += other; |
| 451 |
return *this; |
| 452 |
} |
| 453 |
|
| 454 |
/** Applies on the right the rotation represented by the rotation \a rotation |
| 455 |
* to \c *this and returns a reference to \c *this. |
| 456 |
* |
| 457 |
* The template parameter \a RotationType is the type of the rotation which |
| 458 |
* must be known by ei_toRotationMatrix<>. |
| 459 |
* |
| 460 |
* Natively supported types includes: |
| 461 |
* - any scalar (2D), |
| 462 |
* - a Dim x Dim matrix expression, |
| 463 |
* - a Quaternion (3D), |
| 464 |
* - a AngleAxis (3D) |
| 465 |
* |
| 466 |
* This mechanism is easily extendable to support user types such as Euler angles, |
| 467 |
* or a pair of Quaternion for 4D rotations. |
| 468 |
* |
| 469 |
* \sa rotate(Scalar), class Quaternion, class AngleAxis, prerotate(RotationType) |
| 470 |
*/ |
| 471 |
template<typename Scalar, int Dim> |
| 472 |
template<typename RotationType> |
| 473 |
Transform<Scalar,Dim>& |
| 474 |
Transform<Scalar,Dim>::rotate(const RotationType& rotation) |
| 475 |
{ |
| 476 |
linear() *= ei_toRotationMatrix<Scalar,Dim>(rotation); |
| 477 |
return *this; |
| 478 |
} |
| 479 |
|
| 480 |
/** Applies on the left the rotation represented by the rotation \a rotation |
| 481 |
* to \c *this and returns a reference to \c *this. |
| 482 |
* |
| 483 |
* See rotate() for further details. |
| 484 |
* |
| 485 |
* \sa rotate() |
| 486 |
*/ |
| 487 |
template<typename Scalar, int Dim> |
| 488 |
template<typename RotationType> |
| 489 |
Transform<Scalar,Dim>& |
| 490 |
Transform<Scalar,Dim>::prerotate(const RotationType& rotation) |
| 491 |
{ |
| 492 |
m_matrix.template block<Dim,HDim>(0,0) = ei_toRotationMatrix<Scalar,Dim>(rotation) |
| 493 |
* m_matrix.template block<Dim,HDim>(0,0); |
| 494 |
return *this; |
| 495 |
} |
| 496 |
|
| 497 |
/** Applies on the right the shear transformation represented |
| 498 |
* by the vector \a other to \c *this and returns a reference to \c *this. |
| 499 |
* \warning 2D only. |
| 500 |
* \sa preshear() |
| 501 |
*/ |
| 502 |
template<typename Scalar, int Dim> |
| 503 |
Transform<Scalar,Dim>& |
| 504 |
Transform<Scalar,Dim>::shear(Scalar sx, Scalar sy) |
| 505 |
{ |
| 506 |
EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE) |
| 507 |
VectorType tmp = linear().col(0)*sy + linear().col(1); |
| 508 |
linear() << linear().col(0) + linear().col(1)*sx, tmp; |
| 509 |
return *this; |
| 510 |
} |
| 511 |
|
| 512 |
/** Applies on the left the shear transformation represented |
| 513 |
* by the vector \a other to \c *this and returns a reference to \c *this. |
| 514 |
* \warning 2D only. |
| 515 |
* \sa shear() |
| 516 |
*/ |
| 517 |
template<typename Scalar, int Dim> |
| 518 |
Transform<Scalar,Dim>& |
| 519 |
Transform<Scalar,Dim>::preshear(Scalar sx, Scalar sy) |
| 520 |
{ |
| 521 |
EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE) |
| 522 |
m_matrix.template block<Dim,HDim>(0,0) = LinearMatrixType(1, sx, sy, 1) * m_matrix.template block<Dim,HDim>(0,0); |
| 523 |
return *this; |
| 524 |
} |
| 525 |
|
| 526 |
/****************************************************** |
| 527 |
*** Scaling, Translation and Rotation compatibility *** |
| 528 |
******************************************************/ |
| 529 |
|
| 530 |
template<typename Scalar, int Dim> |
| 531 |
inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const TranslationType& t) |
| 532 |
{ |
| 533 |
linear().setIdentity(); |
| 534 |
translation() = t.vector(); |
| 535 |
m_matrix.template block<1,Dim>(Dim,0).setZero(); |
| 536 |
m_matrix(Dim,Dim) = Scalar(1); |
| 537 |
return *this; |
| 538 |
} |
| 539 |
|
| 540 |
template<typename Scalar, int Dim> |
| 541 |
inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const TranslationType& t) const |
| 542 |
{ |
| 543 |
Transform res = *this; |
| 544 |
res.translate(t.vector()); |
| 545 |
return res; |
| 546 |
} |
| 547 |
|
| 548 |
template<typename Scalar, int Dim> |
| 549 |
inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const ScalingType& s) |
| 550 |
{ |
| 551 |
m_matrix.setZero(); |
| 552 |
linear().diagonal() = s.coeffs(); |
| 553 |
m_matrix.coeffRef(Dim,Dim) = Scalar(1); |
| 554 |
return *this; |
| 555 |
} |
| 556 |
|
| 557 |
template<typename Scalar, int Dim> |
| 558 |
inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const ScalingType& s) const |
| 559 |
{ |
| 560 |
Transform res = *this; |
| 561 |
res.scale(s.coeffs()); |
| 562 |
return res; |
| 563 |
} |
| 564 |
|
| 565 |
template<typename Scalar, int Dim> |
| 566 |
template<typename Derived> |
| 567 |
inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const RotationBase<Derived,Dim>& r) |
| 568 |
{ |
| 569 |
linear() = ei_toRotationMatrix<Scalar,Dim>(r); |
| 570 |
translation().setZero(); |
| 571 |
m_matrix.template block<1,Dim>(Dim,0).setZero(); |
| 572 |
m_matrix.coeffRef(Dim,Dim) = Scalar(1); |
| 573 |
return *this; |
| 574 |
} |
| 575 |
|
| 576 |
template<typename Scalar, int Dim> |
| 577 |
template<typename Derived> |
| 578 |
inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const RotationBase<Derived,Dim>& r) const |
| 579 |
{ |
| 580 |
Transform res = *this; |
| 581 |
res.rotate(r.derived()); |
| 582 |
return res; |
| 583 |
} |
| 584 |
|
| 585 |
/************************ |
| 586 |
*** Special functions *** |
| 587 |
************************/ |
| 588 |
|
| 589 |
/** \returns the rotation part of the transformation |
| 590 |
* \nonstableyet |
| 591 |
* |
| 592 |
* \svd_module |
| 593 |
* |
| 594 |
* \sa computeRotationScaling(), computeScalingRotation(), class SVD |
| 595 |
*/ |
| 596 |
template<typename Scalar, int Dim> |
| 597 |
typename Transform<Scalar,Dim>::LinearMatrixType |
| 598 |
Transform<Scalar,Dim>::rotation() const |
| 599 |
{ |
| 600 |
LinearMatrixType result; |
| 601 |
computeRotationScaling(&result, (LinearMatrixType*)0); |
| 602 |
return result; |
| 603 |
} |
| 604 |
|
| 605 |
|
| 606 |
/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being |
| 607 |
* not necessarily positive. |
| 608 |
* |
| 609 |
* If either pointer is zero, the corresponding computation is skipped. |
| 610 |
* |
| 611 |
* \nonstableyet |
| 612 |
* |
| 613 |
* \svd_module |
| 614 |
* |
| 615 |
* \sa computeScalingRotation(), rotation(), class SVD |
| 616 |
*/ |
| 617 |
template<typename Scalar, int Dim> |
| 618 |
template<typename RotationMatrixType, typename ScalingMatrixType> |
| 619 |
void Transform<Scalar,Dim>::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const |
| 620 |
{ |
| 621 |
JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU|ComputeFullV); |
| 622 |
Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1 |
| 623 |
Matrix<Scalar, Dim, 1> sv(svd.singularValues()); |
| 624 |
sv.coeffRef(0) *= x; |
| 625 |
if(scaling) |
| 626 |
{ |
| 627 |
scaling->noalias() = svd.matrixV() * sv.asDiagonal() * svd.matrixV().adjoint(); |
| 628 |
} |
| 629 |
if(rotation) |
| 630 |
{ |
| 631 |
LinearMatrixType m(svd.matrixU()); |
| 632 |
m.col(0) /= x; |
| 633 |
rotation->noalias() = m * svd.matrixV().adjoint(); |
| 634 |
} |
| 635 |
} |
| 636 |
|
| 637 |
/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being |
| 638 |
* not necessarily positive. |
| 639 |
* |
| 640 |
* If either pointer is zero, the corresponding computation is skipped. |
| 641 |
* |
| 642 |
* \nonstableyet |
| 643 |
* |
| 644 |
* \svd_module |
| 645 |
* |
| 646 |
* \sa computeRotationScaling(), rotation(), class SVD |
| 647 |
*/ |
| 648 |
template<typename Scalar, int Dim> |
| 649 |
template<typename ScalingMatrixType, typename RotationMatrixType> |
| 650 |
void Transform<Scalar,Dim>::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const |
| 651 |
{ |
| 652 |
JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU|ComputeFullV); |
| 653 |
Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1 |
| 654 |
Matrix<Scalar, Dim, 1> sv(svd.singularValues()); |
| 655 |
sv.coeffRef(0) *= x; |
| 656 |
if(scaling) |
| 657 |
{ |
| 658 |
scaling->noalias() = svd.matrixU() * sv.asDiagonal() * svd.matrixU().adjoint(); |
| 659 |
} |
| 660 |
if(rotation) |
| 661 |
{ |
| 662 |
LinearMatrixType m(svd.matrixU()); |
| 663 |
m.col(0) /= x; |
| 664 |
rotation->noalias() = m * svd.matrixV().adjoint(); |
| 665 |
} |
| 666 |
} |
| 667 |
|
| 668 |
/** Convenient method to set \c *this from a position, orientation and scale |
| 669 |
* of a 3D object. |
| 670 |
*/ |
| 671 |
template<typename Scalar, int Dim> |
| 672 |
template<typename PositionDerived, typename OrientationType, typename ScaleDerived> |
| 673 |
Transform<Scalar,Dim>& |
| 674 |
Transform<Scalar,Dim>::fromPositionOrientationScale(const MatrixBase<PositionDerived> &position, |
| 675 |
const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale) |
| 676 |
{ |
| 677 |
linear() = ei_toRotationMatrix<Scalar,Dim>(orientation); |
| 678 |
linear() *= scale.asDiagonal(); |
| 679 |
translation() = position; |
| 680 |
m_matrix.template block<1,Dim>(Dim,0).setZero(); |
| 681 |
m_matrix(Dim,Dim) = Scalar(1); |
| 682 |
return *this; |
| 683 |
} |
| 684 |
|
| 685 |
/** \nonstableyet |
| 686 |
* |
| 687 |
* \returns the inverse transformation matrix according to some given knowledge |
| 688 |
* on \c *this. |
| 689 |
* |
| 690 |
* \param traits allows to optimize the inversion process when the transformion |
| 691 |
* is known to be not a general transformation. The possible values are: |
| 692 |
* - Projective if the transformation is not necessarily affine, i.e., if the |
| 693 |
* last row is not guaranteed to be [0 ... 0 1] |
| 694 |
* - Affine is the default, the last row is assumed to be [0 ... 0 1] |
| 695 |
* - Isometry if the transformation is only a concatenations of translations |
| 696 |
* and rotations. |
| 697 |
* |
| 698 |
* \warning unless \a traits is always set to NoShear or NoScaling, this function |
| 699 |
* requires the generic inverse method of MatrixBase defined in the LU module. If |
| 700 |
* you forget to include this module, then you will get hard to debug linking errors. |
| 701 |
* |
| 702 |
* \sa MatrixBase::inverse() |
| 703 |
*/ |
| 704 |
template<typename Scalar, int Dim> |
| 705 |
inline const typename Transform<Scalar,Dim>::MatrixType |
| 706 |
Transform<Scalar,Dim>::inverse(TransformTraits traits) const |
| 707 |
{ |
| 708 |
if (traits == Projective) |
| 709 |
{ |
| 710 |
return m_matrix.inverse(); |
| 711 |
} |
| 712 |
else |
| 713 |
{ |
| 714 |
MatrixType res; |
| 715 |
if (traits == Affine) |
| 716 |
{ |
| 717 |
res.template corner<Dim,Dim>(TopLeft) = linear().inverse(); |
| 718 |
} |
| 719 |
else if (traits == Isometry) |
| 720 |
{ |
| 721 |
res.template corner<Dim,Dim>(TopLeft) = linear().transpose(); |
| 722 |
} |
| 723 |
else |
| 724 |
{ |
| 725 |
ei_assert("invalid traits value in Transform::inverse()"); |
| 726 |
} |
| 727 |
// translation and remaining parts |
| 728 |
res.template corner<Dim,1>(TopRight) = - res.template corner<Dim,Dim>(TopLeft) * translation(); |
| 729 |
res.template corner<1,Dim>(BottomLeft).setZero(); |
| 730 |
res.coeffRef(Dim,Dim) = Scalar(1); |
| 731 |
return res; |
| 732 |
} |
| 733 |
} |
| 734 |
|
| 735 |
/***************************************************** |
| 736 |
*** Specializations of operator* with a MatrixBase *** |
| 737 |
*****************************************************/ |
| 738 |
|
| 739 |
template<typename Other, int Dim, int HDim> |
| 740 |
struct ei_transform_product_impl<Other,Dim,HDim, HDim,HDim> |
| 741 |
{ |
| 742 |
typedef Transform<typename Other::Scalar,Dim> TransformType; |
| 743 |
typedef typename TransformType::MatrixType MatrixType; |
| 744 |
typedef typename ProductReturnType<MatrixType,Other>::Type ResultType; |
| 745 |
static ResultType run(const TransformType& tr, const Other& other) |
| 746 |
{ return tr.matrix() * other; } |
| 747 |
}; |
| 748 |
|
| 749 |
template<typename Other, int Dim, int HDim> |
| 750 |
struct ei_transform_product_impl<Other,Dim,HDim, Dim,Dim> |
| 751 |
{ |
| 752 |
typedef Transform<typename Other::Scalar,Dim> TransformType; |
| 753 |
typedef typename TransformType::MatrixType MatrixType; |
| 754 |
typedef TransformType ResultType; |
| 755 |
static ResultType run(const TransformType& tr, const Other& other) |
| 756 |
{ |
| 757 |
TransformType res; |
| 758 |
res.translation() = tr.translation(); |
| 759 |
res.matrix().row(Dim) = tr.matrix().row(Dim); |
| 760 |
res.linear() = (tr.linear() * other).lazy(); |
| 761 |
return res; |
| 762 |
} |
| 763 |
}; |
| 764 |
|
| 765 |
template<typename Other, int Dim, int HDim> |
| 766 |
struct ei_transform_product_impl<Other,Dim,HDim, HDim,1> |
| 767 |
{ |
| 768 |
typedef Transform<typename Other::Scalar,Dim> TransformType; |
| 769 |
typedef typename TransformType::MatrixType MatrixType; |
| 770 |
typedef typename ProductReturnType<MatrixType,Other>::Type ResultType; |
| 771 |
static ResultType run(const TransformType& tr, const Other& other) |
| 772 |
{ return tr.matrix() * other; } |
| 773 |
}; |
| 774 |
|
| 775 |
template<typename Other, int Dim, int HDim> |
| 776 |
struct ei_transform_product_impl<Other,Dim,HDim, Dim,1> |
| 777 |
{ |
| 778 |
typedef typename Other::Scalar Scalar; |
| 779 |
typedef Transform<Scalar,Dim> TransformType; |
| 780 |
typedef Matrix<Scalar,Dim,1> ResultType; |
| 781 |
static ResultType run(const TransformType& tr, const Other& other) |
| 782 |
{ return ((tr.linear() * other) + tr.translation()) |
| 783 |
* (Scalar(1) / ( (tr.matrix().template block<1,Dim>(Dim,0) * other).coeff(0) + tr.matrix().coeff(Dim,Dim))); } |
| 784 |
}; |
| 785 |
|
| 786 |
} // end namespace Eigen |