D:/Programmierung/C/TRTK/include/TRTK/Transform3D.hpp
Go to the documentation of this file.
00001 /* 00002 Computes a 3D transform in homogeneous coordinates. 00003 00004 Copyright (C) 2010 - 2014 Christoph Haenisch 00005 00006 Chair of Medical Engineering (mediTEC) 00007 RWTH Aachen University 00008 Pauwelsstr. 20 00009 52074 Aachen 00010 Germany 00011 00012 See license.txt for more information. 00013 00014 Version 0.5.0 (2013-04-04) 00015 */ 00016 00021 /* Note: The concatenation of several affine matrices to one single 00022 * matrix is possible without any problems. However, if a 00023 * non-affine transform (e.g. a projective transform) is used, 00024 * it may only be used once and at the very end (i.e. multiplied 00025 * from left) due to the subsequent non-linear normalisation step. 00026 */ 00027 00028 00029 #ifndef TRANSFORM3D_HPP_0386836041 00030 #define TRANSFORM3D_HPP_0386836041 00031 00032 00033 #include <cassert> 00034 #include <cmath> 00035 #include <Eigen/Core> 00036 #include <Eigen/Dense> 00037 00038 #include "Coordinate.hpp" 00039 #include "ErrorObj.hpp" 00040 #include "Tools.hpp" 00041 00042 00043 namespace TRTK 00044 { 00045 00046 00207 template <class T> 00208 class Transform3D 00209 { 00210 public: 00211 00212 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00213 00214 enum Axis {X_AXIS, Y_AXIS, Z_AXIS}; 00215 enum Plane {XY_PLANE, XZ_PLANE, YZ_PLANE}; 00216 enum Unit {DEGREES, RADIANS}; 00217 00218 enum Error {DIVISION_BY_ZERO, 00219 INVALID_ARGUMENT, 00220 INVALID_AXIS, 00221 INVALID_UNIT, 00222 UNKNOWN_ERROR, 00223 WRONG_COORDINATE_SIZE}; 00224 00225 typedef T value_type; 00226 typedef Eigen::Matrix<T, 4, 4> Matrix4T; 00227 typedef Eigen::Matrix<T, 4, 1> Vector4T; 00228 typedef Eigen::Matrix<T, 3, 1> Vector3T; 00229 typedef Coordinate<T> coordinate_type; 00230 00231 Transform3D(); 00232 00233 Transform3D(const Matrix4T &); 00234 00235 Transform3D(T a11, T a12, T a13, T a14, 00236 T a21, T a22, T a23, T a24, 00237 T a31, T a32, T a33, T a34, 00238 T a41, T a42, T a43, T a44); 00239 00240 Transform3D(const Transform3D<T> & transform3D); 00241 00242 template <class U> 00243 Transform3D(const Transform3D<U> & transform3D); 00244 00245 virtual ~Transform3D(); 00246 00247 T & a11(); 00248 T & a12(); 00249 T & a13(); 00250 T & a14(); 00251 T & a21(); 00252 T & a22(); 00253 T & a23(); 00254 T & a24(); 00255 T & a31(); 00256 T & a32(); 00257 T & a33(); 00258 T & a34(); 00259 T & a41(); 00260 T & a42(); 00261 T & a43(); 00262 T & a44(); 00263 00264 const T & a11() const; 00265 const T & a12() const; 00266 const T & a13() const; 00267 const T & a14() const; 00268 const T & a21() const; 00269 const T & a22() const; 00270 const T & a23() const; 00271 const T & a24() const; 00272 const T & a31() const; 00273 const T & a32() const; 00274 const T & a33() const; 00275 const T & a34() const; 00276 const T & a41() const; 00277 const T & a42() const; 00278 const T & a43() const; 00279 const T & a44() const; 00280 00281 const Coordinate<T> operator* (const Coordinate<T> &) const; 00282 const Matrix4T operator* (const Matrix4T &) const; 00283 const Vector3T operator* (const Vector3T &) const; 00284 const Vector4T operator* (const Vector4T &) const; 00285 const Transform3D operator* (const Transform3D &) const; 00286 const Transform3D operator>> (const Transform3D &) const; 00287 Transform3D& operator= (const Transform3D &); 00288 00289 Matrix4T & getTransformationMatrix(); 00290 const Matrix4T & getTransformationMatrix() const; 00291 00292 const Transform3D inverse() const; 00293 00294 Transform3D orthographicProjection(const Plane = XY_PLANE) const; 00295 Transform3D perspectiveProjection(const T distance, const Plane = XY_PLANE) const; 00296 00297 Transform3D & reset(); 00298 00299 Transform3D & rotate(const double angle, const Axis = Z_AXIS, const Unit unit = RADIANS); 00300 Transform3D & rotateX(const double angle, const Unit unit = RADIANS); 00301 Transform3D & rotateY(const double angle, const Unit unit = RADIANS); 00302 Transform3D & rotateZ(const double angle, const Unit unit = RADIANS); 00303 Transform3D & rotateAxis(const double angle, const Coordinate<T> & axis, const Unit unit = RADIANS); 00304 Transform3D & rotateAxis(const double angle, const Vector3T & axis, const Unit unit = RADIANS); 00305 Transform3D & rotateAxis(const double angle, const Vector4T & axis, const Unit unit = RADIANS); 00306 00307 Transform3D & scale(const T sx, const T sy, const T sz); 00308 00309 // Transform3D & shear(const T sx, const T sy, const T sz); 00310 00311 Transform3D & translate(const T dx, const T dy, const T dz); 00312 Transform3D & translate(const Coordinate<T> & position); 00313 00314 bool is_affine() const; 00315 00316 static const double pi; 00317 00318 private: 00319 00320 void normalize(Vector4T &) const; 00321 00322 Matrix4T * m_matrix; 00323 bool m_is_affine; 00324 }; 00325 00326 00327 template <class T> 00328 const double Transform3D<T>::pi = 3.14159265358979323846264338327950288419716939937510; 00329 00330 00340 template <class T> 00341 Transform3D<T>::Transform3D() : m_matrix(new Matrix4T), m_is_affine(true) 00342 { 00343 m_matrix->setIdentity(); 00344 } 00345 00346 00358 template <class T> 00359 Transform3D<T>::Transform3D(const Matrix4T & matrix) 00360 : m_matrix(new Matrix4T()) 00361 { 00362 using Tools::isEqual; 00363 00364 *m_matrix = matrix; 00365 00366 if (isEqual<T>(matrix(3,0), 0) && 00367 isEqual<T>(matrix(3,1), 0) && 00368 isEqual<T>(matrix(3,2), 0) && 00369 isEqual<T>(matrix(3,3), 1)) 00370 { 00371 m_is_affine = true; 00372 } 00373 else 00374 { 00375 m_is_affine = false; 00376 } 00377 } 00378 00379 00389 template <class T> 00390 Transform3D<T>::Transform3D(T a11, T a12, T a13, T a14, 00391 T a21, T a22, T a23, T a24, 00392 T a31, T a32, T a33, T a34, 00393 T a41, T a42, T a43, T a44) 00394 : m_matrix(new Matrix4T()) 00395 { 00396 using Tools::isEqual; 00397 00398 *m_matrix << a11, a12, a13, a14, 00399 a21, a22, a23, a24, 00400 a31, a32, a33, a34, 00401 a41, a42, a43, a44; 00402 00403 if (isEqual<T>(a41, 0) && 00404 isEqual<T>(a42, 0) && 00405 isEqual<T>(a43, 0) && 00406 isEqual<T>(a44, 1)) 00407 { 00408 m_is_affine = true; 00409 } 00410 else 00411 { 00412 m_is_affine = false; 00413 } 00414 } 00415 00416 00425 template <class T> 00426 template <class U> 00427 Transform3D<T>::Transform3D(const Transform3D<U> & transform3D) 00428 : m_matrix(new Matrix4T()) 00429 { 00430 const int rows = transform3D.getTransformationMatrix().rows(); 00431 const int cols = transform3D.getTransformationMatrix().cols(); 00432 00433 m_matrix->resize(rows, cols); 00434 00435 for (int m = 0; m < rows; ++m) 00436 { 00437 for (int n = 0; n < cols; ++n) 00438 { 00439 (*m_matrix)(m, n) = T(transform3D.getTransformationMatrix()(m, n)); 00440 } 00441 } 00442 00443 m_is_affine = transform3D.is_affine(); 00444 } 00445 00446 00447 // Even though a templated copy constructor exist, we need to define 00448 // this one because otherwise the compiler would generate a default 00449 // copy constructor!? 00450 00458 template <class T> 00459 Transform3D<T>::Transform3D(const Transform3D<T> & transform3D) 00460 : m_matrix( new Matrix4T() ) 00461 { 00462 const int rows = transform3D.getTransformationMatrix().rows(); 00463 const int cols = transform3D.getTransformationMatrix().cols(); 00464 00465 m_matrix->resize(rows, cols); 00466 00467 for (int m = 0; m < rows; ++m) 00468 { 00469 for (int n = 0; n < cols; ++n) 00470 { 00471 (*m_matrix)(m, n) = T(transform3D.getTransformationMatrix()(m, n)); 00472 } 00473 } 00474 00475 m_is_affine = transform3D.is_affine(); 00476 } 00477 00478 00486 template <class T> 00487 Transform3D<T> & Transform3D<T>::operator= (const Transform3D & transform3D) 00488 { 00489 *m_matrix = *transform3D.m_matrix; 00490 m_is_affine = transform3D.m_is_affine; 00491 return *this; 00492 } 00493 00494 00500 template <class T> 00501 Transform3D<T>::~Transform3D() 00502 { 00503 delete m_matrix; 00504 } 00505 00506 00516 template <class T> 00517 T & Transform3D<T>::a11() 00518 { 00519 return (*m_matrix)(0, 0); 00520 } 00521 00522 00532 template <class T> 00533 T & Transform3D<T>::a12() 00534 { 00535 return (*m_matrix)(0, 1); 00536 } 00537 00538 00548 template <class T> 00549 T & Transform3D<T>::a13() 00550 { 00551 return (*m_matrix)(0, 2); 00552 } 00553 00554 00564 template <class T> 00565 T & Transform3D<T>::a14() 00566 { 00567 return (*m_matrix)(0, 3); 00568 } 00569 00570 00580 template <class T> 00581 T & Transform3D<T>::a21() 00582 { 00583 return (*m_matrix)(1, 0); 00584 } 00585 00586 00596 template <class T> 00597 T & Transform3D<T>::a22() 00598 { 00599 return (*m_matrix)(1, 1); 00600 } 00601 00602 00612 template <class T> 00613 T & Transform3D<T>::a23() 00614 { 00615 return (*m_matrix)(1, 2); 00616 } 00617 00618 00628 template <class T> 00629 T & Transform3D<T>::a24() 00630 { 00631 return (*m_matrix)(1, 3); 00632 } 00633 00634 00644 template <class T> 00645 T & Transform3D<T>::a31() 00646 { 00647 return (*m_matrix)(2, 0); 00648 } 00649 00650 00660 template <class T> 00661 T & Transform3D<T>::a32() 00662 { 00663 return (*m_matrix)(2, 1); 00664 } 00665 00666 00676 template <class T> 00677 T & Transform3D<T>::a33() 00678 { 00679 return (*m_matrix)(2, 2); 00680 } 00681 00682 00692 template <class T> 00693 T & Transform3D<T>::a34() 00694 { 00695 return (*m_matrix)(2, 3); 00696 } 00697 00698 00708 template <class T> 00709 T & Transform3D<T>::a41() 00710 { 00711 m_is_affine = false; 00712 return (*m_matrix)(3, 0); 00713 } 00714 00715 00725 template <class T> 00726 T & Transform3D<T>::a42() 00727 { 00728 m_is_affine = false; 00729 return (*m_matrix)(3, 1); 00730 } 00731 00732 00742 template <class T> 00743 T & Transform3D<T>::a43() 00744 { 00745 m_is_affine = false; 00746 return (*m_matrix)(3, 2); 00747 } 00748 00749 00759 template <class T> 00760 T & Transform3D<T>::a44() 00761 { 00762 m_is_affine = false; 00763 return (*m_matrix)(3, 3); 00764 } 00765 00766 00776 template <class T> 00777 const T & Transform3D<T>::a11() const 00778 { 00779 return (*m_matrix)(0, 0); 00780 } 00781 00782 00792 template <class T> 00793 const T & Transform3D<T>::a12() const 00794 { 00795 return (*m_matrix)(0, 1); 00796 } 00797 00798 00808 template <class T> 00809 const T & Transform3D<T>::a13() const 00810 { 00811 return (*m_matrix)(0, 2); 00812 } 00813 00814 00824 template <class T> 00825 const T & Transform3D<T>::a14() const 00826 { 00827 return (*m_matrix)(0, 3); 00828 } 00829 00830 00840 template <class T> 00841 const T & Transform3D<T>::a21() const 00842 { 00843 return (*m_matrix)(1, 0); 00844 } 00845 00846 00856 template <class T> 00857 const T & Transform3D<T>::a22() const 00858 { 00859 return (*m_matrix)(1, 1); 00860 } 00861 00862 00872 template <class T> 00873 const T & Transform3D<T>::a23() const 00874 { 00875 return (*m_matrix)(1, 2); 00876 } 00877 00878 00888 template <class T> 00889 const T & Transform3D<T>::a24() const 00890 { 00891 return (*m_matrix)(1, 3); 00892 } 00893 00894 00904 template <class T> 00905 const T & Transform3D<T>::a31() const 00906 { 00907 return (*m_matrix)(2, 0); 00908 } 00909 00910 00920 template <class T> 00921 const T & Transform3D<T>::a32() const 00922 { 00923 return (*m_matrix)(2, 1); 00924 } 00925 00926 00936 template <class T> 00937 const T & Transform3D<T>::a33() const 00938 { 00939 return (*m_matrix)(2, 2); 00940 } 00941 00942 00952 template <class T> 00953 const T & Transform3D<T>::a34() const 00954 { 00955 return (*m_matrix)(2, 3); 00956 } 00957 00958 00968 template <class T> 00969 const T & Transform3D<T>::a41() const 00970 { 00971 return (*m_matrix)(3, 0); 00972 } 00973 00974 00984 template <class T> 00985 const T & Transform3D<T>::a42() const 00986 { 00987 return (*m_matrix)(3, 1); 00988 } 00989 00990 01000 template <class T> 01001 const T & Transform3D<T>::a43() const 01002 { 01003 return (*m_matrix)(3, 2); 01004 } 01005 01006 01016 template <class T> 01017 const T & Transform3D<T>::a44() const 01018 { 01019 return (*m_matrix)(3, 3); 01020 } 01021 01022 01054 template <class T> 01055 const Coordinate<T> Transform3D<T>::operator* (const Coordinate<T> & coordinate) const 01056 { 01057 // 'coordinate' must be of size three or four. If 'coordinate' has a size 01058 // of four, it is assumed that the fourth component is equal to one. 01059 01060 const int size = coordinate.size(); 01061 01062 if (!(size == 3 || size == 4)) 01063 { 01064 ErrorObj error; 01065 error.setClassName("Transform3D<T>"); 01066 error.setFunctionName("operator*"); 01067 error.setErrorMessage("'coordinate' must be of size three or four."); 01068 error.setErrorCode(WRONG_COORDINATE_SIZE); 01069 throw error; 01070 } 01071 01072 assert(coordinate.size() == 3 || (coordinate.size() == 4 && coordinate.w() == 1)); 01073 01074 Vector4T vector(coordinate.x(), coordinate.y(), coordinate.z(), 1); 01075 Vector4T result = *m_matrix * vector; 01076 01077 if (!is_affine()) 01078 { 01079 normalize(result); 01080 } 01081 01082 if (size == 3) 01083 { 01084 return Coordinate<T>(result(0), result(1), result(2)); 01085 } 01086 else // size == 4 01087 { 01088 return Coordinate<T>(result(0), result(1), result(2), 1); 01089 } 01090 } 01091 01092 01122 template <class T> 01123 const typename Transform3D<T>::Vector3T Transform3D<T>::operator* (const Vector3T & vector) const 01124 { 01125 Vector4T vector4D(vector(0), vector(1), vector(2), 1); 01126 Vector4T result = *m_matrix * vector4D; 01127 01128 if (!is_affine()) 01129 { 01130 normalize(result); 01131 } 01132 01133 return result.head(3); 01134 } 01135 01136 01171 template <class T> 01172 const typename Transform3D<T>::Vector4T Transform3D<T>::operator* (const Vector4T & vector) const 01173 { 01174 assert(Tools::isEqual<T>(vector(3), 1)); 01175 01176 Vector4T result = *m_matrix * vector; 01177 01178 if (!is_affine()) 01179 { 01180 normalize(result); 01181 } 01182 01183 return result; 01184 } 01185 01186 01231 template <class T> 01232 const typename Transform3D<T>::Matrix4T Transform3D<T>::operator* (const Matrix4T & matrix) const 01233 { 01234 typename Transform3D<T>::Matrix4T result = *m_matrix * matrix; 01235 01236 return result; 01237 } 01238 01239 01285 template <class T> 01286 const Transform3D<T> Transform3D<T>::operator* (const Transform3D & transform) const 01287 { 01288 Transform3D<T> result = (*m_matrix * *transform.m_matrix).eval(); 01289 result.m_is_affine = m_is_affine && transform.m_is_affine; 01290 01291 return result; 01292 } 01293 01294 01340 template <class T> 01341 const Transform3D<T> Transform3D<T>::operator>> (const Transform3D & transform) const 01342 { 01343 Transform3D<T> result = (*transform.m_matrix * *m_matrix).eval(); 01344 result.m_is_affine = m_is_affine && transform.m_is_affine; 01345 01346 return result; 01347 } 01348 01349 01357 template <class T> 01358 typename Transform3D<T>::Matrix4T & Transform3D<T>::getTransformationMatrix() 01359 { 01360 return *m_matrix; 01361 } 01362 01363 01371 template <class T> 01372 const typename Transform3D<T>::Matrix4T & Transform3D<T>::getTransformationMatrix() const 01373 { 01374 return *m_matrix; 01375 } 01376 01377 01385 template <class T> 01386 const Transform3D<T> Transform3D<T>::inverse() const 01387 { 01388 return Transform3D(m_matrix->inverse()); 01389 } 01390 01391 01409 template <class T> 01410 Transform3D<T> Transform3D<T>::orthographicProjection(const Plane plane) const 01411 { 01412 Transform3D<T> transform3d; 01413 01414 switch(plane) 01415 { 01416 case XY_PLANE: 01417 (*transform3d.m_matrix)(2, 2) = 0; 01418 break; 01419 01420 case XZ_PLANE: 01421 (*transform3d.m_matrix)(1, 1) = 0; 01422 break; 01423 01424 case YZ_PLANE: 01425 (*transform3d.m_matrix)(0, 0) = 0; 01426 break; 01427 01428 default: 01429 ErrorObj error; 01430 error.setClassName("Transform3D<T>"); 01431 error.setFunctionName("orthographicProjection"); 01432 error.setErrorMessage("Unknown plane."); 01433 error.setErrorCode(INVALID_ARGUMENT); 01434 } 01435 01436 return transform3d; 01437 } 01438 01439 01458 template <class T> 01459 Transform3D<T> Transform3D<T>::perspectiveProjection(const T distance, const Plane plane) const 01460 { 01461 if (distance == 0) 01462 { 01463 ErrorObj error; 01464 error.setClassName("Transform3D<T>"); 01465 error.setFunctionName("perspectiveProjection"); 01466 error.setErrorMessage("Distance may not be zero."); 01467 error.setErrorCode(INVALID_ARGUMENT); 01468 } 01469 01470 Transform3D<T> transform3d; 01471 01472 switch(plane) 01473 { 01474 case XY_PLANE: 01475 (*transform3d.m_matrix)(2, 3) = distance; 01476 (*transform3d.m_matrix)(3, 2) = 1.0 / distance; 01477 break; 01478 01479 case XZ_PLANE: 01480 (*transform3d.m_matrix)(1, 3) = distance; 01481 (*transform3d.m_matrix)(3, 1) = 1.0 / distance; 01482 break; 01483 01484 case YZ_PLANE: 01485 (*transform3d.m_matrix)(0, 3) = distance; 01486 (*transform3d.m_matrix)(3, 0) = 1.0 / distance; 01487 break; 01488 01489 default: 01490 ErrorObj error; 01491 error.setClassName("Transform3D<T>"); 01492 error.setFunctionName("perspectiveProjection"); 01493 error.setErrorMessage("Unknown plane."); 01494 error.setErrorCode(INVALID_ARGUMENT); 01495 } 01496 01497 return transform3d; 01498 } 01499 01500 01508 template <class T> 01509 Transform3D<T> & Transform3D<T>::reset() 01510 { 01511 m_matrix->setIdentity(); 01512 m_is_affine = true; 01513 01514 return *this; 01515 } 01516 01517 01531 template <class T> 01532 Transform3D<T> & Transform3D<T>::rotate(const double angle, const Axis axis, const Unit unit) 01533 { 01534 switch (axis) 01535 { 01536 case X_AXIS: 01537 return rotateX(angle, unit); 01538 01539 case Y_AXIS: 01540 return rotateY(angle, unit); 01541 01542 case Z_AXIS: 01543 return rotateZ(angle, unit); 01544 01545 default: 01546 ErrorObj error; 01547 error.setClassName("Transform3D<T>"); 01548 error.setFunctionName("rotate"); 01549 error.setErrorMessage("Invalid axis."); 01550 error.setErrorCode(INVALID_AXIS); 01551 throw error; 01552 } 01553 } 01554 01555 01566 template <class T> 01567 Transform3D<T> & Transform3D<T>::rotateX(const double angle, const Unit unit) 01568 { 01569 double angle_in_rad; 01570 01571 switch (unit) 01572 { 01573 case RADIANS: 01574 angle_in_rad = angle; 01575 break; 01576 01577 case DEGREES: 01578 angle_in_rad = angle / 180.0 * pi; 01579 break; 01580 01581 default: 01582 ErrorObj error; 01583 error.setClassName("Transform3D<T>"); 01584 error.setFunctionName("rotateX"); 01585 error.setErrorMessage("Invalid unit."); 01586 error.setErrorCode(INVALID_UNIT); 01587 throw error; 01588 } 01589 01590 Matrix4T rotation; 01591 01592 const double c = std::cos(angle_in_rad); 01593 const double s = std::sin(angle_in_rad); 01594 01595 rotation << 1, 0, 0, 0, 01596 0, c, -s, 0, 01597 0, s, c, 0, 01598 0, 0, 0, 1; 01599 01600 *m_matrix = rotation * *m_matrix; 01601 01602 return *this; 01603 } 01604 01605 01616 template <class T> 01617 Transform3D<T> & Transform3D<T>::rotateY(const double angle, const Unit unit) 01618 { 01619 double angle_in_rad; 01620 01621 switch (unit) 01622 { 01623 case RADIANS: 01624 angle_in_rad = angle; 01625 break; 01626 01627 case DEGREES: 01628 angle_in_rad = angle / 180.0 * pi; 01629 break; 01630 01631 default: 01632 ErrorObj error; 01633 error.setClassName("Transform3D<T>"); 01634 error.setFunctionName("rotateX"); 01635 error.setErrorMessage("Invalid unit."); 01636 error.setErrorCode(INVALID_UNIT); 01637 throw error; 01638 } 01639 01640 Matrix4T rotation; 01641 01642 const double c = std::cos(angle_in_rad); 01643 const double s = std::sin(angle_in_rad); 01644 01645 rotation << c, 0, s, 0, 01646 0, 1, 0, 0, 01647 -s, 0, c, 0, 01648 0, 0, 0, 1; 01649 01650 *m_matrix = rotation * *m_matrix; 01651 01652 return *this; 01653 } 01654 01655 01666 template <class T> 01667 Transform3D<T> & Transform3D<T>::rotateZ(const double angle, const Unit unit) 01668 { 01669 double angle_in_rad; 01670 01671 switch (unit) 01672 { 01673 case RADIANS: 01674 angle_in_rad = angle; 01675 break; 01676 01677 case DEGREES: 01678 angle_in_rad = angle / 180.0 * pi; 01679 break; 01680 01681 default: 01682 ErrorObj error; 01683 error.setClassName("Transform3D<T>"); 01684 error.setFunctionName("rotateX"); 01685 error.setErrorMessage("Invalid unit."); 01686 error.setErrorCode(INVALID_UNIT); 01687 throw error; 01688 } 01689 01690 Matrix4T rotation; 01691 01692 const double c = std::cos(angle_in_rad); 01693 const double s = std::sin(angle_in_rad); 01694 01695 rotation << c, -s, 0, 0, 01696 s, c, 0, 0, 01697 0, 0, 1, 0, 01698 0, 0, 0, 1; 01699 01700 *m_matrix = rotation * *m_matrix; 01701 01702 return *this; 01703 } 01704 01705 01717 template <class T> 01718 Transform3D<T> & Transform3D<T>::rotateAxis(const double angle, const Coordinate<T> & axis, const Unit unit) 01719 { 01720 double angle_in_rad = 0.0; 01721 01722 switch (unit) 01723 { 01724 case RADIANS: 01725 angle_in_rad = angle; 01726 break; 01727 01728 case DEGREES: 01729 angle_in_rad = angle / 180.0 * pi; 01730 break; 01731 01732 default: 01733 ErrorObj error; 01734 error.setClassName("Transform3D<T>"); 01735 error.setFunctionName("rotateAxis"); 01736 error.setErrorMessage("Invalid unit."); 01737 error.setErrorCode(INVALID_UNIT); 01738 throw error; 01739 } 01740 01741 // The idea is to apply a change of basis such that the 01742 // axis of rotation becomes the z-axis (i.e. we must find 01743 // a mapping such that the given axis maps to [0, 0, 1]). 01744 // Then the rotation can be easily computed and eventually 01745 // a change back to the original basis can be applied. 01746 01747 // Create the change-of-basis matrix. 01748 01749 Coordinate<T> row1, row2, row3; 01750 01751 row3 = axis.normalized(); 01752 row2 = row3.orthogonal().normalize(); 01753 row1 = row2.cross(row3); 01754 01755 Matrix4T change_of_basis_matrix; 01756 01757 change_of_basis_matrix << row1.x(), row1.y(), row1.z(), 0, 01758 row2.x(), row2.y(), row2.z(), 0, 01759 row3.x(), row3.y(), row3.z(), 0, 01760 0, 0, 0, 1; 01761 01762 // Rotate around the z-axis in the changed basis. 01763 01764 *m_matrix = change_of_basis_matrix * *m_matrix; 01765 rotateZ(angle_in_rad); 01766 *m_matrix = change_of_basis_matrix.inverse() * *m_matrix; 01767 01768 return *this; 01769 } 01770 01771 01783 template <class T> 01784 Transform3D<T> & Transform3D<T>::rotateAxis(const double angle, const Vector3T & axis, const Unit unit) 01785 { 01786 coordinate_type coord(axis); 01787 return this->rotateAxis(angle, coord, unit); 01788 } 01789 01790 01802 template <class T> 01803 Transform3D<T> & Transform3D<T>::rotateAxis(const double angle, const Vector4T & axis, const Unit unit) 01804 { 01805 coordinate_type coord(axis); 01806 return this->rotateAxis(angle, coord, unit); 01807 } 01808 01809 01824 template <class T> 01825 Transform3D<T> & Transform3D<T>::scale(const T sx, const T sy, const T sz) 01826 { 01827 Matrix4T scale; 01828 01829 scale << sx, 0, 0, 0, 01830 0, sy, 0, 0, 01831 0, 0, sz, 0, 01832 0, 0, 0, 1; 01833 01834 *m_matrix = scale * *m_matrix; 01835 01836 return *this; 01837 } 01838 01839 01853 template <class T> 01854 Transform3D<T> & Transform3D<T>::translate(const T dx, const T dy, const T dz) 01855 { 01856 Matrix4T translation; 01857 01858 translation << 1, 0, 0, dx, 01859 0, 1, 0, dy, 01860 0, 0, 1, dz, 01861 0, 0, 0, 1; 01862 01863 *m_matrix = translation * *m_matrix; 01864 01865 return *this; 01866 } 01867 01868 01880 template <class T> 01881 Transform3D<T> & Transform3D<T>::translate(const Coordinate<T> & position) 01882 { 01883 assert(position.size() == 3); 01884 01885 Matrix4T translation; 01886 01887 translation << 1, 0, 0, position.x(), 01888 0, 1, 0, position.y(), 01889 0, 0, 1, position.z(), 01890 0, 0, 0, 1; 01891 01892 *m_matrix = translation * *m_matrix; 01893 01894 return *this; 01895 } 01896 01897 01905 template <class T> 01906 inline bool Transform3D<T>::is_affine() const 01907 { 01908 return m_is_affine; 01909 } 01910 01911 01927 template <class T> 01928 inline void Transform3D<T>::normalize(Vector4T & vec) const 01929 { 01930 if (Tools::isZero<T>(vec(3))) 01931 { 01932 ErrorObj error; 01933 error.setClassName("Transform3D"); 01934 error.setFunctionName("normalize"); 01935 error.setErrorMessage("Division by zero."); 01936 error.setErrorCode(DIVISION_BY_ZERO); 01937 throw error; 01938 } 01939 else 01940 { 01941 vec(0) /= vec(3); 01942 vec(1) /= vec(3); 01943 vec(2) /= vec(3); 01944 vec(3) = 1; 01945 } 01946 } 01947 01948 01949 } // namespace TRTK 01950 01951 01952 #endif // TRANSFORM3D_HPP_0386836041