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