15 #include "vtkPolyData.h"
16 #include "vtkCardinalSpline.h"
17 #include "vtkPoints.h"
18 #include "vtkCellArray.h"
19 #include "vtkCamera.h"
20 #include "vtkParametricSpline.h"
21 #include "vtkSpline.h"
34 mTrackingService(tracker),
35 mPatientModelService(patientModel),
36 mViewService(visualization),
37 mLastCameraViewAngle(0),
38 mLastCameraRotAngle(0),
39 mAutomaticRotation(true),
40 mWritePositionsToFile(false)
42 mManualTool = mTrackingService->getManualTool();
43 mSpline = vtkParametricSplinePtr::New();
44 mLastStoredViewVector.Identity();
50 if(mRoutePositions.size() > 0)
51 if(mRoutePositions.size() == mCameraRotations.size())
53 this->generateSplineCurve(mRoutePositions);
59 std::cout <<
"cameraRawPointsSlot is empty !" << std::endl;
63 this->generateSplineCurve(mesh);
66 void CXVBcameraPath::generateSplineCurve(
MeshPtr mesh)
68 vtkPolyDataPtr polyDataInput = mesh->getTransformedPolyDataCopy(mesh->get_rMd());
69 vtkPoints *vtkpoints = polyDataInput->GetPoints();
71 mNumberOfInputPoints = polyDataInput->GetNumberOfPoints();
73 mNumberOfControlPoints = mNumberOfInputPoints;
77 mSpline->GetXSpline()->RemoveAllPoints();
78 mSpline->GetYSpline()->RemoveAllPoints();
79 mSpline->GetZSpline()->RemoveAllPoints();
81 mSpline->SetPoints(vtkpoints);
84 void CXVBcameraPath::generateSplineCurve(std::vector< Eigen::Vector3d > routePositions)
87 for (
int i = 0; i < routePositions.size(); i++)
88 vtkPoints->InsertNextPoint(routePositions[i](0),routePositions[i](1),routePositions[i](2));
92 mSpline->GetXSpline()->RemoveAllPoints();
93 mSpline->GetYSpline()->RemoveAllPoints();
94 mSpline->GetZSpline()->RemoveAllPoints();
96 mSpline->SetPoints(vtkPoints);
106 double splineFocusDistance = 0.05;
107 if (mPositionPercentage > 0.8)
108 splineFocusDistance = 0.02;
110 double pos_r[3], focus_r[3], d_r[3];
111 double splineParameterArray[3];
112 splineParameterArray[0] = mPositionPercentage;
113 splineParameterArray[1] = mPositionPercentage;
114 splineParameterArray[2] = mPositionPercentage;
116 mSpline->Evaluate(splineParameterArray, pos_r, d_r);
117 splineParameterArray[0] = mPositionPercentage + splineFocusDistance;
118 splineParameterArray[1] = mPositionPercentage + splineFocusDistance;
119 splineParameterArray[2] = mPositionPercentage + splineFocusDistance;
120 mSpline->Evaluate(splineParameterArray, focus_r, d_r);
122 mLastCameraPos_r =
Vector3D(pos_r[0], pos_r[1], pos_r[2]);
123 mLastCameraFocus_r =
Vector3D(focus_r[0], focus_r[1], focus_r[2]);
125 if(mAutomaticRotation)
126 if(mRoutePositions.size() > 0 && mRoutePositions.size() == mCameraRotationsSmoothed.size())
128 int index = (int) (mPositionPercentage * (mRoutePositions.size() - 1));
129 mLastCameraRotAngle = mCameraRotationsSmoothed[index];
133 this->updateManualToolPosition();
137 void CXVBcameraPath::updateManualToolPosition()
141 if(
similar(mLastCameraFocus_r, mLastCameraPos_r, 0.01)) {
142 viewDirection_r = mLastStoredViewVector;
144 viewDirection_r = (mLastCameraFocus_r - mLastCameraPos_r).normalized();
145 mLastStoredViewVector = viewDirection_r;
150 Vector3D yVector =
cross(viewDirection_r, xVector).normalized();
154 rMt.matrix().col(0).head(3) = xVector;
155 rMt.matrix().col(1).head(3) = yVector;
156 rMt.matrix().col(2).head(3) = viewDirection_r;
157 rMt.matrix().col(3).head(3) = mLastCameraPos_r;
162 Transform3D rMpr = mPatientModelService->get_rMpr();
163 Transform3D prMt = rMpr.inv() * rMt * rotateZ * rotateX;
165 mManualTool->set_prMt(prMt);
167 if(mWritePositionsToFile)
168 this->writePositionToFile(prMt);
176 mWritePositionsToFile = write;
177 if(mWritePositionsToFile)
178 mTimeSinceStartRecording.start();
183 mPositionsFilePath = path;
186 void CXVBcameraPath::writePositionToFile(
Transform3D prMt)
188 QFile positionFile(mPositionsFilePath +
"_positions.txt");
189 if (positionFile.open(QIODevice::Append))
191 QTextStream stream(&positionFile);
192 stream << prMt(0,0) <<
" " << prMt(0,1) <<
" " << prMt(0,2) <<
" " << prMt(0,3) << endl;
193 stream << prMt(1,0) <<
" " << prMt(1,1) <<
" " << prMt(1,2) <<
" " << prMt(1,3) << endl;
194 stream << prMt(2,0) <<
" " << prMt(2,1) <<
" " << prMt(2,2) <<
" " << prMt(2,3) << endl;
197 QFile timestampFile(mPositionsFilePath +
"_timestamps.txt");
198 if (timestampFile.open(QIODevice::Append))
200 QTextStream stream(×tampFile);
201 stream << mTimeSinceStartRecording.elapsed() << endl;
204 QFile branchingPositionFile(mPositionsFilePath +
"_branching.txt");
205 if (branchingPositionFile.open(QIODevice::Append))
207 bool branchingPoint = 0;
208 int originalRouteIndex = (int) (mRoutePositions.size()-1) * (1-mPositionPercentage);
209 if (std::find(mBranchingIndex.begin(), mBranchingIndex.end(), originalRouteIndex) != mBranchingIndex.end())
211 QTextStream stream(&branchingPositionFile);
212 stream << branchingPoint << endl;
218 std::vector< double > CXVBcameraPath::smoothCameraRotations(std::vector< double > cameraRotations)
221 int numberOfElements = cameraRotations.size();
222 std::vector< double > cameraRotationsSmoothed = cameraRotations;
225 int maxPositionsToSmooth = (int) (10 * numberOfElements/100);
226 int positionsToSmooth = maxPositionsToSmooth;
227 for(
int i=0; i<numberOfElements; i++)
229 positionsToSmooth = std::min((
int) (positionsToSmooth+.5*numberOfElements/100), maxPositionsToSmooth);
230 bool firstTurnPassed =
false;
231 for(
int j=i+1; j<std::min(i+positionsToSmooth, numberOfElements); j++)
232 if (cameraRotations[j] != cameraRotations[j-1])
236 positionsToSmooth = j-i;
240 firstTurnPassed =
true;
243 std::vector< double > averageElements(cameraRotations.begin()+i, cameraRotations.begin()+std::min(i+positionsToSmooth,numberOfElements-1));
244 if(averageElements.size() > 0)
245 cameraRotationsSmoothed[i] = std::accumulate(averageElements.begin(), averageElements.end(), 0.0) / averageElements.size();
248 return cameraRotationsSmoothed;
254 mLastCameraViewAngle =
static_cast<double>(angle) * (
M_PI / 180.0);
255 this->updateManualToolPosition();
260 mLastCameraRotAngle =
static_cast<double>(angle) * (
M_PI / 180.0);
261 this->updateManualToolPosition();
266 mRoutePositions = routePositions;
271 mCameraRotations = cameraRotations;
272 mCameraRotationsSmoothed = smoothCameraRotations(mCameraRotations);
277 mBranchingIndex = branchingIndex;
282 mAutomaticRotation = automaticRotation;
288 return 2*positionPercentage / (1 + positionPercentage/100.0);