CustusX  2021.09.21-dev+develop.e3cd0
An IGT application
cxTransform3D.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 "cxTransform3D.h"
13 
14 #include <sstream>
15 #include <vector>
16 #include <vtkMatrix4x4.h>
17 #include <vtkTransform.h>
18 #include "cxTypeConversions.h"
19 #include "cxBoundingBox3D.h"
20 #include "vtkForwardDeclarations.h"
21 
22 // --------------------------------------------------------
24 {
25 
26 boost::array<double, 16> flatten(const Eigen::Affine3d* self)
27 {
28  boost::array<double, 16> retval;
29  boost::array<double, 16>::iterator raw = retval.begin();
30 
31  for (int r = 0; r < 4; ++r)
32  for (int c = 0; c < 4; ++c)
33  *raw++ = (*self)(r, c);
34 
35  return retval;
36 }
37 
38 void fill(Eigen::Affine3d* self, vtkMatrix4x4Ptr m)
39 {
40  if (!m)
41  return;
42  for (int r = 0; r < 4; ++r)
43  for (int c = 0; c < 4; ++c)
44  (*self)(r, c) = m->GetElement(r, c);
45 }
46 
47 void fill(Eigen::Affine3d* self, float m[4][4])
48 {
49  if (!m)
50  return;
51  for (int r = 0; r < 4; ++r)
52  for (int c = 0; c < 4; ++c)
53  (*self)(r, c) = m[r][c];
54 }
55 
56 
60 void fill(Eigen::Affine3d* self, const double* raw)
61 {
62  for (int r = 0; r < 4; ++r)
63  for (int c = 0; c < 4; ++c)
64  (*self)(r, c) = *raw++;
65 }
66 
67 vtkMatrix4x4Ptr getVtkMatrix(const Eigen::Affine3d* self)
68 {
69  vtkMatrix4x4Ptr m = vtkMatrix4x4Ptr::New();
70 
71  for (int r = 0; r < 4; ++r)
72  for (int c = 0; c < 4; ++c)
73  m->SetElement(r, c, (*self)(r, c));
74  ;
75 
76  return m;
77 }
78 
79 vtkTransformPtr getVtkTransform(const Eigen::Affine3d* self)
80 {
81  vtkTransformPtr retval = vtkTransform::New();
82  retval->SetMatrix(self->getVtkMatrix());
83  retval->Update();
84  return retval;
85 }
86 
87 std::ostream& put(const Eigen::Affine3d* self, std::ostream& s, int indent, char newline)
88 {
89  QString ind(indent, ' ');
90 
91  std::ostringstream ss; // avoid changing state of input stream
92  ss << setprecision(3) << std::fixed;
93 
94  for (unsigned i = 0; i < 4; ++i)
95  {
96  ss << ind;
97  for (unsigned j = 0; j < 4; ++j)
98  {
99  ss << setw(10) << (*self)(i, j) << " ";
100  }
101  if (i != 3)
102  {
103  ss << newline;
104  }
105  }
106 
107  s << ss.str();
108 
109  return s;
110 }
111 
112 Eigen::Affine3d fromString(const QString& text, bool* _ok)
113 {
114  bool okval = false; // if input _ok is null, we still need a flag
115  bool* ok = &okval;
116  if (_ok)
117  ok = _ok;
118 
119  std::vector<double> raw = convertQString2DoubleVector(text, ok);
120  if (raw.size() != 16)
121  *ok = false;
122  if (!*ok)
123  return Eigen::Affine3d();
124 
125  Eigen::Affine3d retval;
126  fill(&retval, &*raw.begin());
127  return retval;
128 }
129 
130 } // namespace cx_transform3D_internal
131 
132 namespace cx
133 {
134 
135 bool similar(const Transform3D& a, const Transform3D& b, double tol)
136 {
137  boost::array<double, 16> m = a.flatten();
138  boost::array<double, 16> n = b.flatten();
139  for (int j = 0; j < 16; ++j)
140  {
141  if (!similar(n[j], m[j], tol))
142  {
143  return false;
144  }
145  }
146  return true;
147 }
148 // --------------------------------------------------------
149 
151 {
152  Vector3D a = m.coord(bb.bottomLeft());
153  Vector3D b = m.coord(bb.topRight());
154  return DoubleBoundingBox3D(a, b);
155 }
156 
158 {
159  Transform3D retval = Transform3D::Identity();
160  retval.scale(scale_);
161  return retval;
162 }
163 
165 {
166  Transform3D retval = Transform3D::Identity();
167  retval.translate(translation);
168  return retval;
169 }
170 
172 {
173  Transform3D retval = Transform3D::Identity();
174  retval.rotate(Eigen::AngleAxisd(angle, Vector3D::UnitX()));
175  return retval;
176 }
177 
179 {
180  Transform3D retval = Transform3D::Identity();
181  retval.rotate(Eigen::AngleAxisd(angle, Vector3D::UnitY()));
182  return retval;
183 }
184 
186 {
187  Transform3D retval = Transform3D::Identity();
188  retval.rotate(Eigen::AngleAxisd(angle, Vector3D::UnitZ()));
189  return retval;
190 }
191 
193 {
194  Vector3D k = cross(from, to);
195 
196 
197  // handle special cases
198  if (similar(k.length(), 0.0))
199  {
200  // dot==1 -> point in the same direction
201  if (similar(dot(from, to), 1.0))
202  {
203  return Transform3D::Identity();
204  }
205 
206  // dot==-1 ->point in opposite directions, cross product will fail.
207  // Find an arbitrary vector perpendicular to from, rotate 180 around that one.
208  if (similar(dot(from, to), -1.0))
209  {
210  Vector3D e_x = Vector3D::UnitX();
211  Vector3D kk = cross(from, e_x);
212  if (similar(kk.length(), 0.0))
213  {
214  Vector3D e_y = Vector3D::UnitY();
215  kk = cross(from, e_y);
216  }
217 
218  Transform3D retval = Transform3D::Identity();
219  retval.rotate(Eigen::AngleAxisd(M_PI, kk));
220  return retval;
221  }
222  }
223 
224  double dotnormal = dot(from, to)/from.length()/to.length();
225  double angle = acos(dotnormal);
226 
227  Transform3D retval = Transform3D::Identity();
228  retval.rotate(Eigen::AngleAxisd(angle, k.normal()));
229  return retval;
230 }
231 
232 
234 {
235  // translate input bottomleft to origin, scale, translate back to output bottomleft.
237  Vector3D inrange = in.range();
238  Vector3D outrange = out.range();
239  Vector3D scale;
240 
241  // check for zero input dimensions
242  for (unsigned i = 0; i < scale.size(); ++i)
243  {
244  if (fabs(inrange[i]) < 1.0E-5)
245  scale[i] = 0;
246  else
247  scale[i] = outrange[i] / inrange[i];
248  }
251  Transform3D M = T1 * S * T0;
252  return M;
253 }
254 
255 Transform3D createTransformIJC(const Vector3D& ivec, const Vector3D& jvec, const Vector3D& center)
256 {
257  Transform3D t = Transform3D::Identity();
258  t.matrix().col(0).head(3) = ivec;
259  t.matrix().col(1).head(3) = jvec;
260  t.matrix().col(2).head(3) = cross(ivec, jvec);
261  t.matrix().col(3).head(3) = center;
262  return t;
263 }
264 
265 
267 {
269 }
270 
271 cxResource_EXPORT Transform3D createTransformFromReferenceToExternal(PATIENT_COORDINATE_SYSTEM external)
272 {
273  if (external==pcsRAS)
274  return createTransformLPS2RAS();
275  else
276  return Transform3D::Identity();
277 }
278 
280 {
281  Eigen::IOFormat singleLineFormatting(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", " | ", "", "", "", "");
282  std::ostringstream stream;
283  stream << transform.matrix().format(singleLineFormatting);
284 
285  return stream.str();
286 }
287 
288 
289 } // namespace cx
290 // --------------------------------------------------------
291 
vtkSmartPointer< class vtkMatrix4x4 > vtkMatrix4x4Ptr
Definition: cxMathBase.h:37
pcsRAS
Right-Anterior-Superior, used by Slicer3D, ITK-Snap, nifti, MINC.
Transform3D createTransformRotateY(const double angle)
bool similar(const Transform3D &a, const Transform3D &b, double tol)
Vector3D topRight() const
std::ostream & put(const Eigen::Affine3d *self, std::ostream &s, int indent, char newline)
Transform3D createTransformScale(const Vector3D &scale_)
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
boost::array< double, 16 > flatten(const Eigen::Affine3d *self)
cxResource_EXPORT Transform3D createTransformFromReferenceToExternal(PATIENT_COORDINATE_SYSTEM external)
Transform3D createTransformRotationBetweenVectors(Vector3D from, Vector3D to)
cxResource_EXPORT Transform3D createTransformLPS2RAS()
std::string matrixAsSingleLineString(cx::Transform3D transform)
Vector3D bottomLeft() const
vtkSmartPointer< class vtkTransform > vtkTransformPtr
Definition: cxMathBase.h:41
Transform3D createTransformNormalize(const DoubleBoundingBox3D &in, const DoubleBoundingBox3D &out)
Vector3D cross(const Vector3D &a, const Vector3D &b)
compute cross product of a and b.
Definition: cxVector3D.cpp:41
vtkMatrix4x4Ptr getVtkMatrix(const Eigen::Affine3d *self)
Transform3D createTransformIJC(const Vector3D &ivec, const Vector3D &jvec, const Vector3D &center)
Eigen::Affine3d fromString(const QString &text, bool *_ok)
Transform3D createTransformTranslate(const Vector3D &translation)
double dot(const Vector3D &a, const Vector3D &b)
compute inner product (or dot product) of a and b.
Definition: cxVector3D.cpp:46
Representation of a floating-point bounding box in 3D. The data are stored as {xmin,xmax,ymin,ymax,zmin,zmax}, in order to simplify communication with vtk.
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
void fill(Eigen::Affine3d *self, vtkMatrix4x4Ptr m)
Transform3D createTransformRotateZ(const double angle)
vtkTransformPtr getVtkTransform(const Eigen::Affine3d *self)
Transform3D createTransformRotateX(const double angle)
std::vector< double > convertQString2DoubleVector(const QString &input, bool *ok)
float4 transform(float16 matrix, float4 voxel)
#define M_PI
Namespace for all CustusX production code.