Fraxinus  18.10
An IGT application
cxDummyTool.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 <boost/cstdint.hpp>
13 #include "cxDummyTool.h"
14 
15 #include <vtkPolyData.h>
16 #include <vtkAppendPolyData.h>
17 #include <vtkConeSource.h>
18 #include <vtkCylinderSource.h>
19 #include <QTimer>
20 #include <QTime>
21 #include <vtkPlane.h>
22 #include <vtkClipPolyData.h>
23 
24 #include "cxTimeKeeper.h"
25 #include "cxMathUtils.h"
26 
27 namespace cx
28 {
29 
30 ProbeDefinition DummyToolTestUtilities::createProbeDefinition(ProbeDefinition::TYPE type, double depth, double width, Eigen::Array2i frameSize)
31 {
32  ProbeDefinition retval;
33  retval.setType(type);
34  Eigen::Array2i extent = frameSize - 1;
35  retval.setSector(0, depth, width, 0);
36 
37  Vector3D imageSpacing(width/extent[0], depth/extent[1], 1.0);
38  retval.setOrigin_p(Vector3D(frameSize[0]/2,0,0));
39  retval.setSpacing(imageSpacing);
40  retval.setClipRect_p(DoubleBoundingBox3D(0, extent[0], 0, extent[1], 0, 0));
41  retval.setSize(QSize(frameSize[0], frameSize[1]));
42 
43  return retval;
44 }
45 
46 ProbeDefinition DummyToolTestUtilities::createProbeDefinitionLinear(double depth, double width, Eigen::Array2i frameSize)
47 {
48  return createProbeDefinition(ProbeDefinition::tLINEAR, depth, width, frameSize);
49 }
50 
52 {
53  DummyToolPtr retval(new DummyTool());
54  retval->setProbeSector(probeDefinition);
55  retval->setVisible(true);
56  retval->startTracking(30);
57  return retval;
58 }
59 
63 
64 int DummyTool::mTransformCount = 0;
65 
66 
67 
68 
69 DummyTool::DummyTool(const QString& uid) :
70  ToolImpl(uid),
71  mVisible(false),
72  mTransformSaveFileName("DummyToolsAreToDumbToSaveThemselves"),
73  mTimer(new QTimer()),
74  mThread(NULL)
75 {
76  qRegisterMetaType<Transform3D>("Transform3D");
77  mUid = uid;
78  mName = uid;
79 
80  DoubleBoundingBox3D bb(Vector3D(0,0,0), Vector3D(512,512,256));
82  mPolyData = this->createPolyData(150, 15, 4, 2);
83 
84  connect(mTimer.get(), SIGNAL(timeout()),this, SLOT(sendTransform()));
85 }
86 
88 {
89  this->stopThread();
90 }
91 
92 std::set<Tool::Type> DummyTool::getTypes() const
93 {
94  return mTypes;
95 }
96 
98 {
99  setToolPositionMovement(createToolPositionMovement(bb));
100 }
101 
102 std::vector<Transform3D> DummyTool::getToolPositionMovement()
103 {
104  return mTransforms;
105 }
106 
111 void DummyTool::setToolPositionMovement(const std::vector<Transform3D>& positions)
112 {
113  mTransforms = positions;
114 }
115 
117 {
118  mTypes.clear();
119  mTypes.insert(type);
120 }
121 
122 void DummyTool::setTransformSaveFile(const QString& filename)
123 {
124  mTransformSaveFileName = filename;
125 }
126 
128 {
129  return mVisible;
130 }
131 QString DummyTool::getUid() const
132 {
133  return mUid;
134 }
135 QString DummyTool::getName() const
136 {
137  return mName;
138 }
139 void DummyTool::startTracking(int interval)
140 {
141  mThread = new DummyToolThread(interval);
142  connect(mThread, SIGNAL(ping()), this, SLOT(sendTransform()));
143  mThread->start();
144 
145 
146 // mTimer->start(interval);
147 //std::cout << "start tracking" << std::endl;
148  mVisible = true;
149 
150  emit toolVisible(mVisible);
151 }
153 {
154  return true;
155 }
156 
157 void DummyTool::stopThread()
158 {
159  if (!mThread)
160  {
161  return;
162  }
163  disconnect(mThread, SIGNAL(ping()), this, SLOT(sendTransform()));
164 
165  mThread->quit();
166  mThread->wait(2000); // forever or until dead thread
167 
168  if (mThread->isRunning())
169  {
170  mThread->terminate();
171  mThread->wait(); // forever or until dead thread
172  }
173  mThread = NULL;
174 }
175 
177 {
178  this->stopThread();
179 
180 // std::cout << "stop tracking" << std::endl;
181 
182  mVisible = false;
183  emit toolVisible(mVisible);
184 }
185 
186 void DummyTool::setVisible(bool val)
187 {
188  mVisible = val;
189  emit toolVisible(mVisible);
190 }
191 void DummyTool::sendTransform()
192 {
193  set_prMt(*getNextTransform());
194 // std::cout << "DummyTool::sendTransform(): " << this->get_prMt().coord(Vector3D(0,0,0)) << std::endl;
195 }
196 
202 vtkPolyDataPtr DummyTool::createPolyData(double h1, double h2, double r1, double r2)
203 {
204 
205 // double r1 = 10;
206 // double r2 = 3;
207 // double h1 = 140;
208 // double h2 = 10;
209 
210  vtkAppendPolyDataPtr assembly = vtkAppendPolyDataPtr::New();
211 
212  vtkPlanePtr plane = vtkPlanePtr::New();
213  plane->SetNormal(0,0,-1);
214  plane->SetOrigin(0,0,-h2);
215 
216  vtkConeSourcePtr cone1 = vtkConeSourcePtr::New();
217  double h1_extension = h1*r2 / (r1-r2);
218  double h1_mod = h1+h1_extension;
219  cone1->SetResolution(50);
220  cone1->SetRadius(r1);
221  cone1->SetHeight(h1_mod);
222  cone1->SetDirection(0,0,1);
223  double center1 = -h1/2-h2+h1_extension/2;
224  cone1->SetCenter(Vector3D(0,0,center1).begin());
225 
226  vtkClipPolyDataPtr clipper1 = vtkClipPolyDataPtr::New();
227  clipper1->SetInputConnection(cone1->GetOutputPort());
228  clipper1->SetClipFunction(plane);
229 
230  vtkConeSourcePtr cone2 = vtkConeSourcePtr::New();
231  cone2->SetResolution(25);
232  cone2->SetRadius(r2);
233  cone2->SetHeight(h2);
234  cone2->SetDirection(0,0,1);
235  double center2 = -h2/2;
236  cone2->SetCenter(Vector3D(0,0,center2).begin());
237 
238  assembly->AddInputConnection(clipper1->GetOutputPort());
239  assembly->AddInputConnection(cone2->GetOutputPort());
240 // mPolyData = assembly->GetOutput();
241  assembly->Update();
242  return assembly->GetOutput();
243 }
244 
245 void DummyTool::createLinearMovement(std::vector<Transform3D>* retval, Transform3D* T_in, const Transform3D& R, const Vector3D& a, const Vector3D& b, double step) const
246 {
247  Vector3D u = (b-a).normal();
248  unsigned N = roundAwayFromZero((b-a).length()/step);
249  Transform3D& T = *T_in;
250 
251  for (unsigned i=0; i<N; ++i)
252  {
253  Transform3D T_delta = createTransformTranslate(u*step);
254  T = T_delta * T;
255  retval->push_back(T * R);
256  }
257 }
258 
261 std::vector<Transform3D> DummyTool::createToolPositionMovement(const DoubleBoundingBox3D& bb) const
262 {
263 // std::cout<<"createToolPositionMovement:"<<bb<<std::endl;
264  std::vector<Transform3D> retval;
265 
266  Vector3D range = bb.range();
267  // define four points. Traverse them and then back to the starting point.
268  Vector3D a = bb.center() + Vector3D(range[0]/2, 0, 0);
269  Vector3D b = bb.center();
270  Vector3D c = b + Vector3D(0, -range[0]*0.1, 0);
271  Vector3D d = c + Vector3D(0, 0, range[2]/2);
272 
273 // Vector3D a = bb.corner(0,0,0);
274 // Vector3D b = bb.corner(1,0,0);
275 // Vector3D c = bb.corner(1,1,0);
276 // Vector3D d = bb.corner(1,1,1);
277 
278 // std::cout << "a" << a << std::endl;
279 // std::cout << "b" << b << std::endl;
280 // std::cout << "c" << c << std::endl;
281 // std::cout << "d" << d << std::endl;
282 
283  int steps = 200;
284  double step = *std::max_element(range.begin(), range.end()) / steps;
285 
289 
290  createLinearMovement(&retval, &T, R, a, b, step);
291 
292  for (unsigned i=0; i<50; ++i)
293  {
294  Transform3D r_delta = createTransformRotateZ(-M_PI*0.01);
295  R = r_delta * R;
296  retval.push_back(T * R);
297  }
298 
299  createLinearMovement(&retval, &T, R, b, c, step);
300 
301  for (unsigned i=0; i<50; ++i)
302  {
303  Transform3D r_delta = createTransformRotateZ(-M_PI*0.01);
304  R = r_delta * R;
305  retval.push_back(T * R);
306  }
307 
308  createLinearMovement(&retval, &T, R, c, d, step);
309  createLinearMovement(&retval, &T, R, d, a, step);
310 
311  for (unsigned i=0; i<20; ++i)
312  {
313  Transform3D r_delta = createTransformRotateZ(-M_PI/20);
314  R = r_delta * R;
315  retval.push_back(T * R);
316  }
317 
318  return retval;
319 }
320 
324 {
325  std::vector<Transform3D> retval;
326 
327  Vector3D range = bb.range();
328  // define four points. Traverse them and then back to the starting point.
329 // Vector3D a = bb.center() + Vector3D(range[0]/2, range[0]/10, range[0]/10);
330 // Vector3D b = bb.center();
331 // Vector3D c = b + Vector3D(-range[0]*0.1, -range[0]*0.1, -range[0]*0.1);
332 // Vector3D d = c + Vector3D(range[0]*0.1, range[0]*0.1, range[2]/3);
333  Vector3D a = bb.center() + Vector3D( range[0]*0.4, range[1]*0.4, range[2]*0.4);
334  Vector3D b = bb.center();
335  Vector3D c = bb.center() + Vector3D(-range[0]*0.4, -range[1]*0.1, -range[2]*0.1);
336  Vector3D d = bb.center() + Vector3D( range[0]*0.0, range[1]*0.1, range[2]*0.3);
337 
338  int steps = 200;
339  double step = *std::max_element(range.begin(), range.end()) / steps;
340 
344 
345  createLinearMovement(&retval, &T, R, a, b, step);
346  createLinearMovement(&retval, &T, R, b, c, step);
347  createLinearMovement(&retval, &T, R, c, d, step);
348  createLinearMovement(&retval, &T, R, d, a, step);
349 
350  return retval;
351 }
352 
353 
354 Transform3D* DummyTool::getNextTransform()
355 {
356  if(mTransformCount >= int(mTransforms.size()))
357  mTransformCount = 0;
358 
359  return &mTransforms.at(mTransformCount++);
360 }
361 
363 {
364  double timestamp = this->getTimestamp();
365  ToolImpl::set_prMt(prMt, timestamp);
366 }
367 
369 {
370  return createTransformTranslate(Vector3D(5,5,20));
371 }
372 
373 
374 }//namespace cx
void setToolPositionMovement(const std::vector< Transform3D > &positions)
void setVisible(bool val)
if available for this type, set visibility
PlainObject normal() const
Scalar * begin()
static DummyToolPtr createDummyTool(ProbeDefinition probeDefinition=ProbeDefinition())
Definition: cxDummyTool.cpp:51
void setSpacing(Vector3D spacing)
void setToolPositionMovementBB(const DoubleBoundingBox3D &bb)
Definition: cxDummyTool.cpp:97
std::vector< Transform3D > getToolPositionMovement()
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
void set_prMt(const Transform3D &ptMt)
Common functionality for Tool subclasses.
Definition: cxToolImpl.h:30
virtual Transform3D getCalibration_sMt() const
get the calibration transform from tool space to sensor space (where the spheres or similar live) ...
vtkPolyDataPtr mPolyData
the polydata used to represent the tool graphically
Definition: cxToolImpl.h:63
US beam is emitted straight forward.
vtkSmartPointer< class vtkAppendPolyData > vtkAppendPolyDataPtr
void setSector(double depthStart, double depthEnd, double width, double centerOffset=0)
virtual bool isCalibrated() const
a tool may not be calibrated, then no tracking is allowed
vtkSmartPointer< class vtkClipPolyData > vtkClipPolyDataPtr
QString mUid
Definition: cxTool.h:147
double roundAwayFromZero(double val)
Definition: cxMathUtils.cpp:21
boost::shared_ptr< class DummyTool > DummyToolPtr
void toolVisible(bool visible)
void setOrigin_p(Vector3D origin_p)
QString mName
Definition: cxTool.h:148
DummyTool(const QString &uid="dummytool")
Definition: cxDummyTool.cpp:69
virtual double getTimestamp() const
latest valid timestamp for the position matrix. 0 means indeterminate (for f.ex. manual tools) ...
Definition: cxDummyTool.h:181
void setClipRect_p(DoubleBoundingBox3D clipRect_p)
Transform3D createTransformTranslate(const Vector3D &translation)
virtual QString getName() const
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.
Implementation of a Tool used for testing.
Definition: cxDummyTool.h:149
vtkSmartPointer< vtkPolyData > vtkPolyDataPtr
vtkSmartPointer< class vtkConeSource > vtkConeSourcePtr
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
Definition of characteristics for an Ultrasound Probe Sector.
virtual void setTransformSaveFile(const QString &filename)
Vector3D center() const
virtual QString getUid() const
RealScalar length() const
Transform3D createTransformRotateZ(const double angle)
virtual void set_prMt(const Transform3D &prMt, double timestamp)
if available for this type, set pos, ts<0 means use current time
Definition: cxToolImpl.cpp:97
static ProbeDefinition createProbeDefinition(ProbeDefinition::TYPE, double depth=40, double width=50, Eigen::Array2i frameSize=Eigen::Array2i(80, 40))
Definition: cxDummyTool.cpp:30
virtual bool getVisible() const
static vtkPolyDataPtr createPolyData(double h1, double h2, double r1, double r2)
Transform3D createTransformRotateX(const double angle)
virtual void setType(Type)
vtkSmartPointer< class vtkPlane > vtkPlanePtr
void setSize(QSize size)
void startTracking(int interval=33)
static ProbeDefinition createProbeDefinitionLinear(double depth=40, double width=50, Eigen::Array2i frameSize=Eigen::Array2i(80, 40))
Definition: cxDummyTool.cpp:46
void stopTracking()
std::vector< Transform3D > createToolPositionMovementTranslationOnly(const DoubleBoundingBox3D &bb) const
#define M_PI
virtual std::set< Type > getTypes() const
Definition: cxDummyTool.cpp:92
Namespace for all CustusX production code.