NorMIT-nav  18.04-rc6
An IGT application
cxFrame3D.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 "cxFrame3D.h"
13 #include <math.h>
14 #include "cxUtilHelpers.h"
15 
16 namespace cx
17 {
18 
20 {
21  m_R = Transform3D::Identity();
22  mAngle = Vector3D(0, 0, 0);
23  mPos = Vector3D(0, 0, 0);
24 }
25 
27 {
28  Frame3D frame = Frame3D::create(m);
29  mAngle = frame.getEulerXYZ();
30  mPos = frame.mPos;
31  frame.mPos = Vector3D(0, 0, 0);
32  m_R = frame.transform();
33 }
34 
36 {
37  DecomposedTransform3D input(m);
38 
39  bool eqPos = similar(input.mPos, mPos);
40  if (!eqPos)
41  {
42  mPos = input.mPos;
43  }
44 
45  input.mPos = mPos;
46  bool eqRot = similar(input.getMatrix(), this->getMatrix());
47  // only reset angles if the input rotation matrix is different from the current.
48  if (!eqRot)
49  {
50  mAngle = input.mAngle;
51  m_R = input.m_R;
52  }
53 }
54 
56 {
57  // std::cout << "setAngles " << xyz << std::endl;
58 
59  if (!similar(xyz[0], mAngle[0]))
60  {
61  m_R = m_R * createTransformRotateX(xyz[0] - mAngle[0]);
62  mAngle[0] = xyz[0];
63  }
64  if (!similar(xyz[1], mAngle[1]))
65  {
66  m_R = m_R * createTransformRotateY(xyz[1] - mAngle[1]);
67  mAngle[1] = xyz[1];
68  }
69  if (!similar(xyz[2], mAngle[2]))
70  {
71  m_R = m_R * createTransformRotateZ(xyz[2] - mAngle[2]);
72  mAngle[2] = xyz[2];
73  }
74 }
75 
77 {
78  // std::cout << "setPosition " << pos << std::endl;
79  mPos = pos;
80 }
81 
83 {
84  return mAngle;
85 }
86 
88 {
89  return mPos;
90 }
91 
93 {
94  return createTransformTranslate(mPos) * m_R;
95 }
96 
97 //---------------------------------------------------------
98 //---------------------------------------------------------
99 //---------------------------------------------------------
100 
102 {
103  mAngleAxis = Eigen::AngleAxisd::Identity();
104  mPos = Vector3D(0, 0, 0);
105 }
106 
108 {
109 }
110 
112 {
113  Transform3D t;
114  t = mAngleAxis;
115  Vector3D ea = t.matrix().block<3, 3> (0, 0).eulerAngles(0, 1, 2);
116  return ea;
117 }
118 
120 {
121  mAngleAxis = Eigen::AngleAxisd(xyz[0], Vector3D::UnitX()) * Eigen::AngleAxisd(xyz[1], Vector3D::UnitY())
122  * Eigen::AngleAxisd(xyz[2], Vector3D::UnitZ());
123 }
124 
129 {
130  Frame3D retval;
131 
132  Eigen::Matrix3d R = T.matrix().block<3, 3> (0, 0); // extract rotational part
133  retval.mAngleAxis = Eigen::AngleAxisd(R); // construct angle axis from R
134  retval.mPos = T.matrix().block<3, 1> (0, 3); // extract translational part
135 
136  return retval;
137 }
138 //---------------------------------------------------------------------------
139 
141 {
142  return createTransformTranslate(mPos) * mAngleAxis;
143 }
144 
146 {
147  return mAngleAxis.axis();
148 }
149 
151 {
152  mAngleAxis = Eigen::AngleAxisd(mAngleAxis.angle(), k);
153  mAngleAxis = Eigen::AngleAxisd(mAngleAxis.angle(), k);
154 }
155 
156 std::ostream& operator<<(std::ostream& s, const Frame3D& t)
157 {
158  t.put(s);
159  return s;
160 }
161 
162 void Frame3D::put(std::ostream& s) const
163 {
164  s << "ThetaXY=" << getThetaXY(mAngleAxis.axis()) / M_PI * 180 << ", ThetaZ=" << getThetaZ(mAngleAxis.axis()) / M_PI
165  * 180 << ", Phi=" << mAngleAxis.angle() / M_PI * 180 << ", Pos=[" << mPos << "]";
166 }
167 
168 boost::array<double, 6> Frame3D::getCompactAxisAngleRep() const
169 {
170  boost::array<double, 6> retval;
171  retval[0] = getThetaXY(mAngleAxis.axis());
172  retval[1] = getThetaZ(mAngleAxis.axis());
173  retval[2] = mAngleAxis.angle();
174  retval[3] = mPos[0];
175  retval[4] = mPos[1];
176  retval[5] = mPos[2];
177  return retval;
178 }
179 
180 Frame3D Frame3D::fromCompactAxisAngleRep(const boost::array<double, 6>& rep)
181 {
182  Frame3D retval;
183 
184  retval.mAngleAxis = Eigen::AngleAxisd(rep[2], unitVector(rep[0], rep[1]));
185  retval.mPos = Vector3D(rep[3], rep[4], rep[5]);
186 
187  return retval;
188 }
189 
190 } // namespace cx
191 // --------------------------------------------------------
192 
void setRotationAxis(const Vector3D &k)
Definition: cxFrame3D.cpp:150
Transform3D createTransformRotateY(const double angle)
Helper class for visualizing rotational angles to a human user.
Definition: cxFrame3D.h:40
Vector3D rotationAxis() const
Definition: cxFrame3D.cpp:145
static Frame3D fromCompactAxisAngleRep(const boost::array< double, 6 > &rep)
Definition: cxFrame3D.cpp:180
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
Vector3D getPosition() const
Definition: cxFrame3D.cpp:87
Transform3D transform() const
Definition: cxFrame3D.cpp:140
Vector3D unitVector(double thetaXY, double thetaZ)
compute a unit vector given angles xy in the xy plane and z meaning the elevation from the xy plane...
Definition: cxVector3D.cpp:56
Defines an axis-angle representation of a position+orientation in 3D space.
Definition: cxFrame3D.h:69
double getThetaXY(Vector3D k)
get thetaXY, meaning the angle of v projected onto the xy plane
Definition: cxVector3D.cpp:65
boost::array< double, 6 > getCompactAxisAngleRep() const
Definition: cxFrame3D.cpp:168
double getThetaZ(Vector3D k)
get thetaZ, z meaning the elevation from the xy plane
Definition: cxVector3D.cpp:70
Vector3D getAngles() const
Definition: cxFrame3D.cpp:82
void setEulerXYZ(const Vector3D &xyz)
Definition: cxFrame3D.cpp:119
static Frame3D create(const Transform3D &transform)
Definition: cxFrame3D.cpp:128
Vector3D mPos
position
Definition: cxFrame3D.h:89
Vector3D getEulerXYZ() const
Definition: cxFrame3D.cpp:111
void put(std::ostream &s) const
Definition: cxFrame3D.cpp:162
void reset(Transform3D m)
reinitialize with a fresh matrix.
Definition: cxFrame3D.cpp:35
Transform3D createTransformTranslate(const Vector3D &translation)
Transform3D getMatrix() const
Definition: cxFrame3D.cpp:92
std::ostream & operator<<(std::ostream &s, const IntBoundingBox3D &data)
void setPosition(Vector3D pos)
Definition: cxFrame3D.cpp:76
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
virtual ~Frame3D()
Definition: cxFrame3D.cpp:107
void setAngles(Vector3D xyz)
Definition: cxFrame3D.cpp:55
bool similar(const CameraInfo &lhs, const CameraInfo &rhs, double tol)
Transform3D createTransformRotateZ(const double angle)
Eigen::AngleAxisd mAngleAxis
angle-axis representation of rotation
Definition: cxFrame3D.h:88
Transform3D createTransformRotateX(const double angle)
#define M_PI
Namespace for all CustusX production code.