NorMIT-nav  16.5
An IGT application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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) 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 <boost/cstdint.hpp>
34 #include "cxDummyTool.h"
35 
36 #include <vtkPolyData.h>
37 #include <vtkAppendPolyData.h>
38 #include <vtkConeSource.h>
39 #include <vtkCylinderSource.h>
40 #include <QTimer>
41 #include <QTime>
42 #include <vtkPlane.h>
43 #include <vtkClipPolyData.h>
44 
45 #include "cxTimeKeeper.h"
46 #include "cxMathUtils.h"
47 
48 namespace cx
49 {
50 
51 ProbeDefinition DummyToolTestUtilities::createProbeDefinition(ProbeDefinition::TYPE type, double depth, double width, Eigen::Array2i frameSize)
52 {
53  ProbeDefinition retval;
54  retval.setType(type);
55  Eigen::Array2i extent = frameSize - 1;
56  retval.setSector(0, depth, width, 0);
57 
58  Vector3D imageSpacing(width/extent[0], depth/extent[1], 1.0);
59  retval.setOrigin_p(Vector3D(frameSize[0]/2,0,0));
60  retval.setSpacing(imageSpacing);
61  retval.setClipRect_p(DoubleBoundingBox3D(0, extent[0], 0, extent[1], 0, 0));
62  retval.setSize(QSize(frameSize[0], frameSize[1]));
63 
64  return retval;
65 }
66 
67 ProbeDefinition DummyToolTestUtilities::createProbeDefinitionLinear(double depth, double width, Eigen::Array2i frameSize)
68 {
69  return createProbeDefinition(ProbeDefinition::tLINEAR, depth, width, frameSize);
70 }
71 
73 {
74  DummyToolPtr retval(new DummyTool());
75  retval->setProbeSector(probeDefinition);
76  retval->setVisible(true);
77  retval->startTracking(30);
78  return retval;
79 }
80 
84 
85 int DummyTool::mTransformCount = 0;
86 
87 
88 
89 
90 DummyTool::DummyTool(const QString& uid) :
91  ToolImpl(uid),
92  mVisible(false),
93  mTransformSaveFileName("DummyToolsAreToDumbToSaveThemselves"),
94  mTimer(new QTimer()),
95  mThread(NULL)
96 {
97  qRegisterMetaType<Transform3D>("Transform3D");
98  mUid = uid;
99  mName = uid;
100 
101  DoubleBoundingBox3D bb(Vector3D(0,0,0), Vector3D(512,512,256));
103  mPolyData = this->createPolyData(150, 15, 4, 2);
104 
105  connect(mTimer.get(), SIGNAL(timeout()),this, SLOT(sendTransform()));
106 }
107 
109 {
110  this->stopThread();
111 }
112 
113 std::set<Tool::Type> DummyTool::getTypes() const
114 {
115  return mTypes;
116 }
117 
119 {
120  setToolPositionMovement(createToolPositionMovement(bb));
121 }
122 
123 std::vector<Transform3D> DummyTool::getToolPositionMovement()
124 {
125  return mTransforms;
126 }
127 
132 void DummyTool::setToolPositionMovement(const std::vector<Transform3D>& positions)
133 {
134  mTransforms = positions;
135 }
136 
138 {
139  mTypes.clear();
140  mTypes.insert(type);
141 }
142 
144 {
145  return mPolyData;
146 }
147 void DummyTool::setTransformSaveFile(const QString& filename)
148 {
149  mTransformSaveFileName = filename;
150 }
151 
153 {
154  return mVisible;
155 }
156 QString DummyTool::getUid() const
157 {
158  return mUid;
159 }
160 QString DummyTool::getName() const
161 {
162  return mName;
163 }
164 void DummyTool::startTracking(int interval)
165 {
166  mThread = new DummyToolThread(interval);
167  connect(mThread, SIGNAL(ping()), this, SLOT(sendTransform()));
168  mThread->start();
169 
170 
171 // mTimer->start(interval);
172 //std::cout << "start tracking" << std::endl;
173  mVisible = true;
174 
175  emit toolVisible(mVisible);
176 }
178 {
179  return true;
180 }
181 
182 void DummyTool::stopThread()
183 {
184  if (!mThread)
185  {
186  return;
187  }
188  disconnect(mThread, SIGNAL(ping()), this, SLOT(sendTransform()));
189 
190  mThread->quit();
191  mThread->wait(2000); // forever or until dead thread
192 
193  if (mThread->isRunning())
194  {
195  mThread->terminate();
196  mThread->wait(); // forever or until dead thread
197  }
198  mThread = NULL;
199 }
200 
202 {
203  this->stopThread();
204 
205 // std::cout << "stop tracking" << std::endl;
206 
207  mVisible = false;
208  emit toolVisible(mVisible);
209 }
210 
211 void DummyTool::setVisible(bool val)
212 {
213  mVisible = val;
214  emit toolVisible(mVisible);
215 }
216 void DummyTool::sendTransform()
217 {
218  set_prMt(*getNextTransform());
219 // std::cout << "DummyTool::sendTransform(): " << this->get_prMt().coord(Vector3D(0,0,0)) << std::endl;
220 }
221 
227 vtkPolyDataPtr DummyTool::createPolyData(double h1, double h2, double r1, double r2)
228 {
229 
230 // double r1 = 10;
231 // double r2 = 3;
232 // double h1 = 140;
233 // double h2 = 10;
234 
235  vtkAppendPolyDataPtr assembly = vtkAppendPolyDataPtr::New();
236 
237  vtkPlanePtr plane = vtkPlanePtr::New();
238  plane->SetNormal(0,0,-1);
239  plane->SetOrigin(0,0,-h2);
240 
241  vtkConeSourcePtr cone1 = vtkConeSourcePtr::New();
242  double h1_extension = h1*r2 / (r1-r2);
243  double h1_mod = h1+h1_extension;
244  cone1->SetResolution(50);
245  cone1->SetRadius(r1);
246  cone1->SetHeight(h1_mod);
247  cone1->SetDirection(0,0,1);
248  double center1 = -h1/2-h2+h1_extension/2;
249  cone1->SetCenter(Vector3D(0,0,center1).begin());
250 
251  vtkClipPolyDataPtr clipper1 = vtkClipPolyDataPtr::New();
252  clipper1->SetInputConnection(cone1->GetOutputPort());
253  clipper1->SetClipFunction(plane);
254 
255  vtkConeSourcePtr cone2 = vtkConeSourcePtr::New();
256  cone2->SetResolution(25);
257  cone2->SetRadius(r2);
258  cone2->SetHeight(h2);
259  cone2->SetDirection(0,0,1);
260  double center2 = -h2/2;
261  cone2->SetCenter(Vector3D(0,0,center2).begin());
262 
263  assembly->AddInputConnection(clipper1->GetOutputPort());
264  assembly->AddInputConnection(cone2->GetOutputPort());
265 // mPolyData = assembly->GetOutput();
266  assembly->Update();
267  return assembly->GetOutput();
268 }
269 
270 void DummyTool::createLinearMovement(std::vector<Transform3D>* retval, Transform3D* T_in, const Transform3D& R, const Vector3D& a, const Vector3D& b, double step) const
271 {
272  Vector3D u = (b-a).normal();
273  unsigned N = roundAwayFromZero((b-a).length()/step);
274  Transform3D& T = *T_in;
275 
276  for (unsigned i=0; i<N; ++i)
277  {
278  Transform3D T_delta = createTransformTranslate(u*step);
279  T = T_delta * T;
280  retval->push_back(T * R);
281  }
282 }
283 
286 std::vector<Transform3D> DummyTool::createToolPositionMovement(const DoubleBoundingBox3D& bb) const
287 {
288 // std::cout<<"createToolPositionMovement:"<<bb<<std::endl;
289  std::vector<Transform3D> retval;
290 
291  Vector3D range = bb.range();
292  // define four points. Traverse them and then back to the starting point.
293  Vector3D a = bb.center() + Vector3D(range[0]/2, 0, 0);
294  Vector3D b = bb.center();
295  Vector3D c = b + Vector3D(0, -range[0]*0.1, 0);
296  Vector3D d = c + Vector3D(0, 0, range[2]/2);
297 
298 // Vector3D a = bb.corner(0,0,0);
299 // Vector3D b = bb.corner(1,0,0);
300 // Vector3D c = bb.corner(1,1,0);
301 // Vector3D d = bb.corner(1,1,1);
302 
303 // std::cout << "a" << a << std::endl;
304 // std::cout << "b" << b << std::endl;
305 // std::cout << "c" << c << std::endl;
306 // std::cout << "d" << d << std::endl;
307 
308  int steps = 200;
309  double step = *std::max_element(range.begin(), range.end()) / steps;
310 
314 
315  createLinearMovement(&retval, &T, R, a, b, step);
316 
317  for (unsigned i=0; i<50; ++i)
318  {
319  Transform3D r_delta = createTransformRotateZ(-M_PI*0.01);
320  R = r_delta * R;
321  retval.push_back(T * R);
322  }
323 
324  createLinearMovement(&retval, &T, R, b, c, step);
325 
326  for (unsigned i=0; i<50; ++i)
327  {
328  Transform3D r_delta = createTransformRotateZ(-M_PI*0.01);
329  R = r_delta * R;
330  retval.push_back(T * R);
331  }
332 
333  createLinearMovement(&retval, &T, R, c, d, step);
334  createLinearMovement(&retval, &T, R, d, a, step);
335 
336  for (unsigned i=0; i<20; ++i)
337  {
338  Transform3D r_delta = createTransformRotateZ(-M_PI/20);
339  R = r_delta * R;
340  retval.push_back(T * R);
341  }
342 
343  return retval;
344 }
345 
349 {
350  std::vector<Transform3D> retval;
351 
352  Vector3D range = bb.range();
353  // define four points. Traverse them and then back to the starting point.
354 // Vector3D a = bb.center() + Vector3D(range[0]/2, range[0]/10, range[0]/10);
355 // Vector3D b = bb.center();
356 // Vector3D c = b + Vector3D(-range[0]*0.1, -range[0]*0.1, -range[0]*0.1);
357 // Vector3D d = c + Vector3D(range[0]*0.1, range[0]*0.1, range[2]/3);
358  Vector3D a = bb.center() + Vector3D( range[0]*0.4, range[1]*0.4, range[2]*0.4);
359  Vector3D b = bb.center();
360  Vector3D c = bb.center() + Vector3D(-range[0]*0.4, -range[1]*0.1, -range[2]*0.1);
361  Vector3D d = bb.center() + Vector3D( range[0]*0.0, range[1]*0.1, range[2]*0.3);
362 
363  int steps = 200;
364  double step = *std::max_element(range.begin(), range.end()) / steps;
365 
369 
370  createLinearMovement(&retval, &T, R, a, b, step);
371  createLinearMovement(&retval, &T, R, b, c, step);
372  createLinearMovement(&retval, &T, R, c, d, step);
373  createLinearMovement(&retval, &T, R, d, a, step);
374 
375  return retval;
376 }
377 
378 
379 Transform3D* DummyTool::getNextTransform()
380 {
381  if(mTransformCount >= int(mTransforms.size()))
382  mTransformCount = 0;
383 
384  return &mTransforms.at(mTransformCount++);
385 }
386 
388 {
389  double timestamp = this->getTimestamp();
390  ToolImpl::set_prMt(prMt, timestamp);
391 }
392 
394 {
395  return createTransformTranslate(Vector3D(5,5,20));
396 }
397 
398 
399 }//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:72
void setSpacing(Vector3D spacing)
void setToolPositionMovementBB(const DoubleBoundingBox3D &bb)
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:50
virtual Transform3D getCalibration_sMt() const
get the calibration transform from tool space to sensor space (where the spheres or similar live) ...
US beam is emitted straight forward.
vtkSmartPointer< class vtkAppendPolyData > vtkAppendPolyDataPtr
virtual vtkPolyDataPtr getGraphicsPolyData() const
get geometric 3D description
void setSector(double depthStart, double depthEnd, double width, double centerOffset=0)
virtual bool isCalibrated() const
a tool may not be calibrated, then no tracking i allowed
vtkSmartPointer< class vtkClipPolyData > vtkClipPolyDataPtr
QString mUid
Definition: cxTool.h:168
double roundAwayFromZero(double val)
Definition: cxMathUtils.cpp:35
boost::shared_ptr< class DummyTool > DummyToolPtr
vtkSmartPointer< class vtkPolyData > vtkPolyDataPtr
void toolVisible(bool visible)
void setOrigin_p(Vector3D origin_p)
QString mName
Definition: cxTool.h:169
DummyTool(const QString &uid="dummytool")
Definition: cxDummyTool.cpp:90
virtual double getTimestamp() const
latest valid timestamp for the position matrix. 0 means indeterminate (for f.ex. manual tools) ...
Definition: cxDummyTool.h:203
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:170
vtkSmartPointer< class vtkConeSource > vtkConeSourcePtr
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:63
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:51
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:67
void stopTracking()
std::vector< Transform3D > createToolPositionMovementTranslationOnly(const DoubleBoundingBox3D &bb) const
#define M_PI
virtual std::set< Type > getTypes() const