NorMIT-nav  2023.01.05-dev+develop.0da12
An IGT application
cxReadFbgsMessage.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 "cxReadFbgsMessage.h"
13 #include <QStringRef>
14 #include <QStringList>
15 #include <vtkPointData.h>
16 #include <vtkPolyData.h>
17 #include <vtkPolyDataMapper.h>
18 #include <vtkActor.h>
19 #include <vtkCellArray.h>
20 #include <vtkProperty.h>
21 #include <vtkMatrix4x4.h>
22 #include "cxLogger.h"
23 #include "cxTransform3D.h"
24 #include "cxMesh.h"
25 #include "cxVisServices.h"
27 #include "cxFileManagerService.h"
29 #include "cxTime.h"
30 
31 namespace cx
32 {
34  mServices(services),
35  m_prMt(Transform3D::Identity())
36 {
37  //Using code from cxToolTracer as a basis (Used by ToolRep3D)
38  mPolyData = vtkPolyDataPtr::New();
39  mActor = vtkActorPtr::New();
40  mPolyDataMapper = vtkPolyDataMapperPtr::New();
41 
42  mPolyDataMapper->SetInputData(mPolyData);
43  mActor->SetMapper(mPolyDataMapper);
44 
45  mProperty = vtkPropertyPtr::New();
46  mActor->SetProperty( mProperty );
47  mProperty->SetPointSize(4);
48 
49  this->setColor(QColor("red"));
50 
51  mPoints = vtkPointsPtr::New();
52  mLines = vtkCellArrayPtr::New();
53 
54  mPolyData->SetPoints(mPoints);
55  mPolyData->SetLines(mLines);
56  mPolyData->SetVerts(mLines);
57 
58  mAxis.push_back(axisX);
59  mAxis.push_back(axisY);
60  mAxis.push_back(axisZ);
61 }
62 
64 {
65  return mPolyData;
66 }
67 
69 {
70  return mActor;
71 }
72 
74 {
75  m_prMt = prMt;
76  //mActor->SetUserMatrix(prMt.getVtkMatrix());
77 }
78 
80 {
81  mShapePointLockNumber = posNumber;
82 }
83 
85 {
86  return mRangeMax-1;
87 }
88 
89 void ReadFbgsMessage::setColor(QColor color)
90 {
91  mActor->GetProperty()->SetColor(color.redF(), color.greenF(), color.blueF());
92 }
93 
95 {
96  switch(axis)
97  {
98  case axisX:
99  return QString("Shape x [cm]");
100  break;
101  case axisY:
102  return QString("Shape y [cm]");
103  break;
104  case axisZ:
105  return QString("Shape z [cm]");
106  break;
107  case axisCOUNT:
108  default:
109  return QString();
110  break;
111  }
112 }
113 
114 std::vector<double>* ReadFbgsMessage::getAxisPosVector(AXIS axis)
115 {
116  switch(axis)
117  {
118  case axisX:
119  return &mXaxis;
120  break;
121  case axisY:
122  return &mYaxis;
123  break;
124  case axisZ:
125  return &mZaxis;
126  break;
127  case axisCOUNT:
128  default:
129  return nullptr;
130  break;
131  }
132 }
133 
134 void ReadFbgsMessage::readBuffer(QString buffer)
135 {
136  QStringList bufferList = buffer.split(" ");
137  int pos = 0;
138  for(int i = 0; i < mAxis.size(); ++i)
139  {
140  pos = this->readPosForOneAxis(mAxis[i], bufferList, pos);
141  if(pos == -1)
142  {
143  CX_LOG_WARNING() << "ReadFbgsMessage::readBuffer: Error reading " << getAxisString(mAxis[i]) << " values from TCP socket";
144 // CX_LOG_DEBUG() << "buffer: " << buffer;
145  return;
146  }
147  }
148  this->createPolyData();
149 }
150 
151 int ReadFbgsMessage::readPosForOneAxis(AXIS axis, QStringList &bufferList, int previousPos)
152 {
153  int pos = getAxisStringPosition(bufferList, axis, previousPos);
154  if(pos == -1)
155  return -1;
156  else
157  ++pos;
158 
159  int numValues;
160  if((bufferList.size() < 2) || !this->toInt(bufferList[pos++], numValues))
161  return -1;
162 
163  std::vector<double> *axisVextor = getAxisPosVector(axis);
164  int stopPos = pos + numValues;
165  if(stopPos > bufferList.size())
166  {
167  CX_LOG_WARNING() << "ReadFbgsMessage::readPosForOneAxis: Buffer don't have enough (" << numValues << ") values along axis: " << getAxisString(axis);
168  return -1;
169  }
170  for(; pos < stopPos; ++pos)
171  {
172  double value;
173  if(!this->toDouble(bufferList[pos], value))
174  return -1;
175  axisVextor->push_back(value*10);
176  }
177  return pos;
178 }
179 
180 bool ReadFbgsMessage::toInt(QString string, int &value)
181 {
182  bool ok;
183  value = string.toInt(&ok);
184  if(!ok)
185  CX_LOG_WARNING() << "ReadFbgsMessage::toInt: Cannot convert " << string << " to int";
186  return ok;
187 }
188 
189 bool ReadFbgsMessage::toDouble(QString string, double &value)
190 {
191  string = string.split('\n')[0];
192  bool ok;
193  value = string.toDouble(&ok);
194  if(!ok)
195  {
196  int intValue;
197  ok = this->toInt(string, intValue);
198  value = double(intValue);
199  if(!ok)
200  CX_LOG_WARNING() << "ReadFbgsMessage::toDouble: Cannot convert " << string << " to double";
201  }
202  return ok;
203 }
204 
205 int ReadFbgsMessage::getAxisStringPosition(QStringList &bufferList, AXIS axis, int startFrom)
206 {
207  QString axisString = getAxisString(axis);
208  for(int i = startFrom; i < bufferList.size(); ++i)
209  if (bufferList[i] == axisString)
210  return i;
211 
212  CX_LOG_WARNING() << "ReadFbgsMessage::getAxisStringPosition: Could't find separator string: " << axisString;
213  return -1;
214 }
215 
217 {
218  this->clearPolyData();
219  mRangeMax = mXaxis.size();
220  if((mRangeMax != mYaxis.size()) || (mRangeMax != mZaxis.size()))
221  {
222  CX_LOG_WARNING() << "ReadFbgsMessage::createPolyData: Not equal number of position data in all axes";
223  return false;
224  }
225 
226  Transform3D prMshape = this->lockShape(mShapePointLockNumber);
227  for(int i=0; i < mRangeMax; ++i)
228  {
229  Vector3D p(mXaxis[i], mYaxis[i], mZaxis[i]);
230  Vector3D p_pr = prMshape * p;
231  mPoints->InsertNextPoint(p_pr.begin());
232  }
233 
234  // fill cell points for the entire polydata.
235  mLines->Initialize();
236  std::vector<vtkIdType> ids(mPoints->GetNumberOfPoints());
237  for (unsigned i=0; i<ids.size(); ++i)
238  ids[i] = i;
239  mLines->InsertNextCell(ids.size(), &(*ids.begin()));
240  mLines->Modified();
241 
242  mPolyData->Modified();
243  this->clearAxisVectors();
244 
245  getMesh();
246  return true;
247 }
248 
250 {
251  Vector3D p(mXaxis[position], mYaxis[position], mZaxis[position]);
252 
253  Transform3D translateToP = createTransformTranslate(p);
254  Vector3D delta_p = getDeltaPosition(position);
255  Transform3D rotatePdirectionToZaxis = createTransformRotationBetweenVectors(delta_p, Vector3D::UnitZ());
256 
257  Transform3D prMshape = m_prMt * rotatePdirectionToZaxis * translateToP.inv();
258  return prMshape;
259 }
260 
262 {
263  int localRange = 5;
264  if(mRangeMax < 2)
265  return Vector3D(0,0,1);
266  else if(mRangeMax < localRange * 2)
267  localRange = mRangeMax/2;
268 
269  int pos1 = pos - localRange;
270  int pos2 = pos + localRange;
271  int shift = 0;
272  if(pos1 < 0)
273  shift = pos1;
274  if(pos2 >= mRangeMax)
275  shift = pos2 - mRangeMax + 1;
276  pos1 -= shift;
277  pos2 -= shift;
278  Vector3D p1 = Vector3D(mXaxis[pos1], mYaxis[pos1], mZaxis[pos1]);
279  Vector3D p2 = Vector3D(mXaxis[pos2], mYaxis[pos2], mZaxis[pos2]);
280  Vector3D delta_p = p2-p1;
281  return delta_p;
282 }
283 
285 {
286  mPoints->Reset();
287  mLines->Reset();
288  mPolyData->Modified();
289 }
290 
292 {
293  mXaxis.clear();
294  mYaxis.clear();
295  mZaxis.clear();
296 }
297 
299 {
300  //Using insertData adds the mesh as a CX mesh, while save just saves the file
301  //mServices->file()->save(mMesh, filename);
302 
303  //Reuse existing mesh
304  mMesh = boost::dynamic_pointer_cast<Mesh>(mServices->patient()->getData(getMeshUid()));
305 
306  if(!mMesh)
307  {
308  mMesh = Mesh::create(getMeshUid(),"FBGS fiber shape");
309  mMesh->getProperties().mLineWidth->setValue(5);
310  mMesh->setVtkPolyData(mPolyData);
311  mServices->patient()->insertData(mMesh, true);
312  }
313 
314  if(!mMeshAdded)
315  {
316  //Need to set this once, to connect correctly to Mesh::changed signal
317  mMesh->setVtkPolyData(mPolyData);
318  mMeshAdded = true;
319  }
320  return mMesh;
321 }
322 
324 {
325  if(!mMesh)
326  {
327  CX_LOG_WARNING() << "ReadFbgsMessage::saveMeshSnapshot: No mesh";
328  return false;
329  }
330  QString export_folder = mServices->session()->getSubFolder("Export");
331  QString filename = export_folder+"/"+mMesh->getUid()+ "_" + QDateTime::currentDateTime().toString(timestampSecondsFormat()) + ".vtk";
332  mServices->file()->save(mMesh, filename);
333  return true;
334 }
335 
336 }//cx
cx::ReadFbgsMessage::axisZ
@ axisZ
Definition: cxReadFbgsMessage.h:43
cx::ReadFbgsMessage::m_prMt
Transform3D m_prMt
Definition: cxReadFbgsMessage.h:68
cx::ReadFbgsMessage::saveMeshSnapshot
bool saveMeshSnapshot()
Definition: cxReadFbgsMessage.cpp:323
cxLogger.h
cx::ReadFbgsMessage::toInt
bool toInt(QString string, int &value)
Definition: cxReadFbgsMessage.cpp:180
cxFileManagerService.h
cx::ReadFbgsMessage::clearPolyData
void clearPolyData()
Definition: cxReadFbgsMessage.cpp:284
cx::ReadFbgsMessage::mActor
vtkActorPtr mActor
Definition: cxReadFbgsMessage.h:62
cx::ReadFbgsMessage::getMeshUid
static QString getMeshUid()
Definition: cxReadFbgsMessage.h:58
cx::ReadFbgsMessage::mZaxis
std::vector< double > mZaxis
Definition: cxReadFbgsMessage.h:76
cx
Namespace for all CustusX production code.
Definition: cx_dev_group_definitions.h:13
cxSessionStorageService.h
cx::ReadFbgsMessage::mMeshAdded
bool mMeshAdded
Definition: cxReadFbgsMessage.h:71
cx::ReadFbgsMessage::ReadFbgsMessage
ReadFbgsMessage(VisServicesPtr services)
Definition: cxReadFbgsMessage.cpp:33
cx::ReadFbgsMessage::readPosForOneAxis
int readPosForOneAxis(AXIS axis, QStringList &bufferList, int previousPos)
Definition: cxReadFbgsMessage.cpp:151
cx::ReadFbgsMessage::toDouble
bool toDouble(QString string, double &value)
Definition: cxReadFbgsMessage.cpp:189
cx::VisServicesPtr
boost::shared_ptr< class VisServices > VisServicesPtr
Definition: cxMainWindow.h:40
cx::ReadFbgsMessage::lockShape
virtual Transform3D lockShape(int position)
Definition: cxReadFbgsMessage.cpp:249
cx::ReadFbgsMessage::mShapePointLockNumber
int mShapePointLockNumber
Definition: cxReadFbgsMessage.h:67
cx::ReadFbgsMessage::getRangeMax
int getRangeMax()
Definition: cxReadFbgsMessage.cpp:84
cx::Mesh::create
static MeshPtr create(const QString &uid, const QString &name="", PatientModelServicePtr patientModelService=PatientModelService::getNullObject(), SpaceProviderPtr spaceProvider=SpaceProvider::getNullObject())
Definition: cxMesh.cpp:44
cx::ReadFbgsMessage::getActor
vtkActorPtr getActor()
Definition: cxReadFbgsMessage.cpp:68
cx::ReadFbgsMessage::clearAxisVectors
void clearAxisVectors()
Definition: cxReadFbgsMessage.cpp:291
cx::ReadFbgsMessage::mServices
VisServicesPtr mServices
Definition: cxReadFbgsMessage.h:60
cx::MeshPtr
boost::shared_ptr< class Mesh > MeshPtr
Definition: cxForwardDeclarations.h:48
cx::Transform3D
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
Definition: cxLandmarkPatientRegistrationWidget.h:33
cx::ReadFbgsMessage::set_prMt
void set_prMt(Transform3D prMt)
Definition: cxReadFbgsMessage.cpp:73
cx::ReadFbgsMessage::mLines
vtkCellArrayPtr mLines
Definition: cxReadFbgsMessage.h:66
cx::ReadFbgsMessage::axisCOUNT
@ axisCOUNT
Definition: cxReadFbgsMessage.h:44
cx::ReadFbgsMessage::getAxisPosVector
std::vector< double > * getAxisPosVector(AXIS axis)
Definition: cxReadFbgsMessage.cpp:114
cx::timestampSecondsFormat
QString timestampSecondsFormat()
Definition: cxTime.cpp:18
cx::ReadFbgsMessage::setColor
void setColor(QColor color)
Definition: cxReadFbgsMessage.cpp:89
cx::ReadFbgsMessage::getAxisStringPosition
int getAxisStringPosition(QStringList &bufferList, AXIS axis, int startFrom)
Definition: cxReadFbgsMessage.cpp:205
cx::ReadFbgsMessage::setShapePointLock
void setShapePointLock(int posNumber)
Definition: cxReadFbgsMessage.cpp:79
cxRegistrationTransform.h
cx::vtkPolyDataPtr
vtkSmartPointer< vtkPolyData > vtkPolyDataPtr
Definition: cxCenterlineRegistration.h:42
cx::ReadFbgsMessage::createPolyData
bool createPolyData()
Definition: cxReadFbgsMessage.cpp:216
cx::ReadFbgsMessage::AXIS
AXIS
Definition: cxReadFbgsMessage.h:39
cx::ReadFbgsMessage::mProperty
vtkPropertyPtr mProperty
Definition: cxReadFbgsMessage.h:64
vtkActorPtr
vtkSmartPointer< class vtkActor > vtkActorPtr
Definition: vtkForwardDeclarations.h:31
cx::ReadFbgsMessage::readBuffer
void readBuffer(QString buffer)
Definition: cxReadFbgsMessage.cpp:134
cxTime.h
cx::ReadFbgsMessage::mPolyData
vtkPolyDataPtr mPolyData
polydata representation of the probe, in space u
Definition: cxReadFbgsMessage.h:61
cx::ReadFbgsMessage::axisX
@ axisX
Definition: cxReadFbgsMessage.h:41
cx::ReadFbgsMessage::getAxisString
QString getAxisString(AXIS axis)
Definition: cxReadFbgsMessage.cpp:94
cx::ReadFbgsMessage::getDeltaPosition
Vector3D getDeltaPosition(int pos)
Definition: cxReadFbgsMessage.cpp:261
cxReadFbgsMessage.h
cx::createTransformRotationBetweenVectors
Transform3D createTransformRotationBetweenVectors(Vector3D from, Vector3D to)
Definition: cxTransform3D.cpp:192
cx::ReadFbgsMessage::mAxis
std::vector< AXIS > mAxis
Definition: cxReadFbgsMessage.h:73
cx::ReadFbgsMessage::getPolyData
vtkPolyDataPtr getPolyData()
Definition: cxReadFbgsMessage.cpp:63
cx::createTransformTranslate
Transform3D createTransformTranslate(const Vector3D &translation)
Definition: cxTransform3D.cpp:164
CX_LOG_WARNING
#define CX_LOG_WARNING
Definition: cxLogger.h:98
cx::ReadFbgsMessage::axisY
@ axisY
Definition: cxReadFbgsMessage.h:42
cx::ReadFbgsMessage::mYaxis
std::vector< double > mYaxis
Definition: cxReadFbgsMessage.h:75
cxTransform3D.h
cx::ReadFbgsMessage::mMesh
MeshPtr mMesh
Definition: cxReadFbgsMessage.h:70
cxMesh.h
cx::ReadFbgsMessage::mPolyDataMapper
vtkPolyDataMapperPtr mPolyDataMapper
Definition: cxReadFbgsMessage.h:63
cx::Vector3D
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
cx::ReadFbgsMessage::mPoints
vtkPointsPtr mPoints
Definition: cxReadFbgsMessage.h:65
cx::ReadFbgsMessage::getMesh
MeshPtr getMesh()
Definition: cxReadFbgsMessage.cpp:298
cx::ReadFbgsMessage::mXaxis
std::vector< double > mXaxis
Definition: cxReadFbgsMessage.h:74
cx::ReadFbgsMessage::mRangeMax
int mRangeMax
Definition: cxReadFbgsMessage.h:69
cxVisServices.h