CustusX  2021.04.21-dev+develop.337f9
An IGT application
cxMathUtils.cpp
Go to the documentation of this file.
1 /*=========================================================================
2 This file is part of CustusX, an Image Guided Therapy Application.
3 
4 Copyright (c) SINTEF Department of Medical Technology.
5 All rights reserved.
6 
7 CustusX is released under a BSD 3-Clause license.
8 
9 See Lisence.txt (https://github.com/SINTEFMedtek/CustusX/blob/master/License.txt) for details.
10 =========================================================================*/
11 
12 #include "cxMathUtils.h"
13 
14 #include "cxVector3D.h"
15 
16 // For debugging:
17 #include "cxLogger.h"
18 
19 
20 namespace cx {
21 
22 //Deprecated. Don't use
23 double roundAwayFromZero(double val)
24 {
25  CX_LOG_WARNING() << "cx::roundAwayFromZero() is deprecated, and will be removed. Use std::round() or std::lround() instead.";
26  if(val >= 0)
27  return int(val+0.5);
28  else
29  return int(val-0.5);
30 }
31 
32 Eigen::ArrayXd matrixToQuaternion(Transform3D Tx)
33 // Converts a 4x4 transformation matrix to quaternion (7 elements)
34 {
35  Eigen::ArrayXd qArray = Eigen::ArrayXd::Zero(7);
36  Eigen::Quaterniond qA;
37 
38  qArray.segment<3>(4) = Tx.matrix().block<3, 1>(0,3); // Translation part
39  qA = Eigen::Quaterniond(Tx.matrix().block<3, 3>(0,0)); //Convert rot to quaternions
40  qArray.segment<4>(0) = qA.coeffs(); //Rotation parameters
41 
42  return qArray;
43 }
44 
45 Transform3D quaternionToMatrix(Eigen::ArrayXd qArray)
46 // Converts a quaternion (7 elements) to a 4x4 transformation matrix
47 {
48  Transform3D Tx;
49  Eigen::Quaterniond qA;
50 
51  qA.coeffs() = qArray.segment<4>(0);
52  Tx.matrix().block<3, 3>(0,0) = qA.toRotationMatrix();
53  Tx.matrix().block<3, 1>(0,3) = qArray.segment<3>(4);
54  Tx.matrix()(3,3) = 1;
55 
56  return Tx;
57 }
58 
59 }
Transform3D quaternionToMatrix(Eigen::ArrayXd qArray)
Definition: cxMathUtils.cpp:45
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
Eigen::ArrayXd matrixToQuaternion(Transform3D Tx)
Definition: cxMathUtils.cpp:32
double roundAwayFromZero(double val)
Definition: cxMathUtils.cpp:23
#define CX_LOG_WARNING
Definition: cxLogger.h:98
Namespace for all CustusX production code.