NorMIT-nav  16.5
An IGT application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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) 2008-2014, SINTEF Department of Medical Technology
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
9 
10 1. Redistributions of source code must retain the above copyright notice,
11  this list of conditions and the following disclaimer.
12 
13 2. Redistributions in binary form must reproduce the above copyright notice,
14  this list of conditions and the following disclaimer in the documentation
15  and/or other materials provided with the distribution.
16 
17 3. Neither the name of the copyright holder nor the names of its contributors
18  may be used to endorse or promote products derived from this software
19  without specific prior written permission.
20 
21 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
25 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 =========================================================================*/
32 
33 #include "cxFrame3D.h"
34 #include <math.h>
35 #include "cxUtilHelpers.h"
36 
37 namespace cx
38 {
39 
41 {
42  m_R = Transform3D::Identity();
43  mAngle = Vector3D(0, 0, 0);
44  mPos = Vector3D(0, 0, 0);
45 }
46 
48 {
49  Frame3D frame = Frame3D::create(m);
50  mAngle = frame.getEulerXYZ();
51  mPos = frame.mPos;
52  frame.mPos = Vector3D(0, 0, 0);
53  m_R = frame.transform();
54 }
55 
57 {
58  DecomposedTransform3D input(m);
59 
60  bool eqPos = similar(input.mPos, mPos);
61  if (!eqPos)
62  {
63  mPos = input.mPos;
64  }
65 
66  input.mPos = mPos;
67  bool eqRot = similar(input.getMatrix(), this->getMatrix());
68  // only reset angles if the input rotation matrix is different from the current.
69  if (!eqRot)
70  {
71  mAngle = input.mAngle;
72  m_R = input.m_R;
73  }
74 }
75 
77 {
78  // std::cout << "setAngles " << xyz << std::endl;
79 
80  if (!similar(xyz[0], mAngle[0]))
81  {
82  m_R = m_R * createTransformRotateX(xyz[0] - mAngle[0]);
83  mAngle[0] = xyz[0];
84  }
85  if (!similar(xyz[1], mAngle[1]))
86  {
87  m_R = m_R * createTransformRotateY(xyz[1] - mAngle[1]);
88  mAngle[1] = xyz[1];
89  }
90  if (!similar(xyz[2], mAngle[2]))
91  {
92  m_R = m_R * createTransformRotateZ(xyz[2] - mAngle[2]);
93  mAngle[2] = xyz[2];
94  }
95 }
96 
98 {
99  // std::cout << "setPosition " << pos << std::endl;
100  mPos = pos;
101 }
102 
104 {
105  return mAngle;
106 }
107 
109 {
110  return mPos;
111 }
112 
114 {
115  return createTransformTranslate(mPos) * m_R;
116 }
117 
118 //---------------------------------------------------------
119 //---------------------------------------------------------
120 //---------------------------------------------------------
121 
123 {
124  mAngleAxis = Eigen::AngleAxisd::Identity();
125  mPos = Vector3D(0, 0, 0);
126 }
127 
129 {
130 }
131 
133 {
134  Transform3D t;
135  t = mAngleAxis;
136  Vector3D ea = t.matrix().block<3, 3> (0, 0).eulerAngles(0, 1, 2);
137  return ea;
138 }
139 
141 {
142  mAngleAxis = Eigen::AngleAxisd(xyz[0], Vector3D::UnitX()) * Eigen::AngleAxisd(xyz[1], Vector3D::UnitY())
143  * Eigen::AngleAxisd(xyz[2], Vector3D::UnitZ());
144 }
145 
150 {
151  Frame3D retval;
152 
153  Eigen::Matrix3d R = T.matrix().block<3, 3> (0, 0); // extract rotational part
154  retval.mAngleAxis = Eigen::AngleAxisd(R); // construct angle axis from R
155  retval.mPos = T.matrix().block<3, 1> (0, 3); // extract translational part
156 
157  return retval;
158 }
159 //---------------------------------------------------------------------------
160 
162 {
164 }
165 
167 {
168  return mAngleAxis.axis();
169 }
170 
172 {
173  mAngleAxis = Eigen::AngleAxisd(mAngleAxis.angle(), k);
174  mAngleAxis = Eigen::AngleAxisd(mAngleAxis.angle(), k);
175 }
176 
177 std::ostream& operator<<(std::ostream& s, const Frame3D& t)
178 {
179  t.put(s);
180  return s;
181 }
182 
183 void Frame3D::put(std::ostream& s) const
184 {
185  s << "ThetaXY=" << getThetaXY(mAngleAxis.axis()) / M_PI * 180 << ", ThetaZ=" << getThetaZ(mAngleAxis.axis()) / M_PI
186  * 180 << ", Phi=" << mAngleAxis.angle() / M_PI * 180 << ", Pos=[" << mPos << "]";
187 }
188 
189 boost::array<double, 6> Frame3D::getCompactAxisAngleRep() const
190 {
191  boost::array<double, 6> retval;
192  retval[0] = getThetaXY(mAngleAxis.axis());
193  retval[1] = getThetaZ(mAngleAxis.axis());
194  retval[2] = mAngleAxis.angle();
195  retval[3] = mPos[0];
196  retval[4] = mPos[1];
197  retval[5] = mPos[2];
198  return retval;
199 }
200 
201 Frame3D Frame3D::fromCompactAxisAngleRep(const boost::array<double, 6>& rep)
202 {
203  Frame3D retval;
204 
205  retval.mAngleAxis = Eigen::AngleAxisd(rep[2], unitVector(rep[0], rep[1]));
206  retval.mPos = Vector3D(rep[3], rep[4], rep[5]);
207 
208  return retval;
209 }
210 
211 } // namespace cx
212 // --------------------------------------------------------
213 
void setRotationAxis(const Vector3D &k)
Definition: cxFrame3D.cpp:171
Transform3D createTransformRotateY(const double angle)
Helper class for visualizing rotational angles to a human user.
Definition: cxFrame3D.h:61
Vector3D rotationAxis() const
Definition: cxFrame3D.cpp:166
static Frame3D fromCompactAxisAngleRep(const boost::array< double, 6 > &rep)
Definition: cxFrame3D.cpp:201
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
Vector3D getPosition() const
Definition: cxFrame3D.cpp:108
Transform3D transform() const
Definition: cxFrame3D.cpp:161
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:77
Defines an axis-angle representation of a position+orientation in 3D space.
Definition: cxFrame3D.h:90
double getThetaXY(Vector3D k)
get thetaXY, meaning the angle of v projected onto the xy plane
Definition: cxVector3D.cpp:86
boost::array< double, 6 > getCompactAxisAngleRep() const
Definition: cxFrame3D.cpp:189
bool similar(const DoubleBoundingBox3D &a, const DoubleBoundingBox3D &b, double tol)
double getThetaZ(Vector3D k)
get thetaZ, z meaning the elevation from the xy plane
Definition: cxVector3D.cpp:91
Vector3D getAngles() const
Definition: cxFrame3D.cpp:103
void setEulerXYZ(const Vector3D &xyz)
Definition: cxFrame3D.cpp:140
static Frame3D create(const Transform3D &transform)
Definition: cxFrame3D.cpp:149
Vector3D mPos
position
Definition: cxFrame3D.h:110
Vector3D getEulerXYZ() const
Definition: cxFrame3D.cpp:132
void put(std::ostream &s) const
Definition: cxFrame3D.cpp:183
void reset(Transform3D m)
reinitialize with a fresh matrix.
Definition: cxFrame3D.cpp:56
Transform3D createTransformTranslate(const Vector3D &translation)
Transform3D getMatrix() const
Definition: cxFrame3D.cpp:113
std::ostream & operator<<(std::ostream &s, const IntBoundingBox3D &data)
void setPosition(Vector3D pos)
Definition: cxFrame3D.cpp:97
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:63
virtual ~Frame3D()
Definition: cxFrame3D.cpp:128
void setAngles(Vector3D xyz)
Definition: cxFrame3D.cpp:76
Transform3D createTransformRotateZ(const double angle)
Eigen::AngleAxisd mAngleAxis
angle-axis representation of rotation
Definition: cxFrame3D.h:109
Transform3D createTransformRotateX(const double angle)
#define M_PI