CustusX  18.04
An IGT application
cxVBcameraPath.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 <iostream>
13 #include "vtkForwardDeclarations.h"
14 #include "vtkPolyData.h"
15 #include "vtkCardinalSpline.h"
16 #include "vtkPoints.h"
17 #include "vtkCellArray.h"
18 #include "vtkCamera.h"
19 #include "vtkParametricSpline.h"
20 #include "vtkSpline.h"
21 
22 #include "cxVBcameraPath.h"
23 #include "cxMesh.h"
24 #include "cxTrackingService.h"
25 #include "cxPatientModelService.h"
26 #include "cxViewServiceProxy.h"
27 #include "cxView.h"
28 
29 namespace cx {
30 
32  : mTrackingService(tracker)
33  , mPatientModelService(patientModel)
34  , mViewService(visualization)
35  , mLastCameraViewAngle(0)
36  , mLastCameraRotAngle(0)
37 {
38  mManualTool = mTrackingService->getManualTool();
39  mSpline = vtkParametricSplinePtr::New();
40  mLastStoredViewVector.Identity();
41 
42 }
43 
45 {
46 
47  if(!mesh)
48  {
49  std::cout << "cameraRawPointsSlot is empty !" << std::endl;
50  return;
51  }
52 
53  this->generateSplineCurve(mesh);
54 
55 }
56 
57 void CXVBcameraPath::generateSplineCurve(MeshPtr mesh)
58 {
59  vtkPolyDataPtr polyDataInput = mesh->getTransformedPolyDataCopy(mesh->get_rMd());
60  vtkPoints *vtkpoints = polyDataInput->GetPoints();
61 
62  mNumberOfInputPoints = polyDataInput->GetNumberOfPoints();
63 
64  mNumberOfControlPoints = mNumberOfInputPoints;
65 
66  // Setting the spline curve points
67  // First clean up previous stored data
68  mSpline->GetXSpline()->RemoveAllPoints();
69  mSpline->GetYSpline()->RemoveAllPoints();
70  mSpline->GetZSpline()->RemoveAllPoints();
71 
72  mSpline->SetPoints(vtkpoints);
73 }
74 
75 
76 
77 
79 {
80 
81  double splineParameter = pos / 100.0;
82 
83 // std::cout << "CXVBcameraPath::cameraPathPositionSlot , pos : " << pos
84 // << ", spline parameter : " << splineParameter << std::endl;
85 
86  double pos_r[3], focus_r[3], d_r[3];
87  double splineParameterArray[3];
88  splineParameterArray[0] = splineParameter;
89  splineParameterArray[1] = splineParameter;
90  splineParameterArray[2] = splineParameter;
91 
92  mSpline->Evaluate(splineParameterArray, pos_r, d_r);
93  splineParameterArray[0] = splineParameter+0.05;
94  splineParameterArray[1] = splineParameter+0.05;
95  splineParameterArray[2] = splineParameter+0.05;
96  mSpline->Evaluate(splineParameterArray, focus_r, d_r);
97 
98  mLastCameraPos_r = Vector3D(pos_r[0], pos_r[1], pos_r[2]);
99  mLastCameraFocus_r = Vector3D(focus_r[0], focus_r[1], focus_r[2]);
100  this->updateManualToolPosition();
101 
102 }
103 
104 void CXVBcameraPath::updateManualToolPosition()
105 {
106  Vector3D viewDirection_r;
107  // New View direction
108  if(similar(mLastCameraFocus_r, mLastCameraPos_r, 0.01)) {
109  viewDirection_r = mLastStoredViewVector;
110  } else {
111  viewDirection_r = (mLastCameraFocus_r - mLastCameraPos_r).normalized();
112  mLastStoredViewVector = viewDirection_r;
113  }
114 
115 
116  Vector3D xVector = Vector3D(0,1,0);
117  Vector3D yVector = cross(viewDirection_r, xVector).normalized();
118 
119  // Construct tool transform
120  Transform3D rMt = Transform3D::Identity();
121  rMt.matrix().col(0).head(3) = xVector;
122  rMt.matrix().col(1).head(3) = yVector;
123  rMt.matrix().col(2).head(3) = viewDirection_r;
124  rMt.matrix().col(3).head(3) = mLastCameraPos_r;
125 
126  Transform3D rotateX = createTransformRotateX(mLastCameraViewAngle);
127  Transform3D rotateZ = createTransformRotateZ(mLastCameraRotAngle);
128 
129  Transform3D rMpr = mPatientModelService->get_rMpr();
130  Transform3D prMt = rMpr.inv() * rMt * rotateZ * rotateX;
131 
132  mManualTool->set_prMt(prMt);
133 
134 }
135 
136 
138 {
139  mLastCameraViewAngle = static_cast<double>(angle) * (M_PI / 180.0);
140  this->updateManualToolPosition();
141 }
142 
144 {
145  mLastCameraRotAngle = static_cast<double>(angle) * (M_PI / 180.0);
146  this->updateManualToolPosition();
147 }
148 
149 } /* namespace cx */
void cameraRawPointsSlot(MeshPtr mesh)
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
boost::shared_ptr< class TrackingService > TrackingServicePtr
boost::shared_ptr< class ViewService > ViewServicePtr
void cameraRotateAngleSlot(int angle)
Vector3D cross(const Vector3D &a, const Vector3D &b)
compute cross product of a and b.
Definition: cxVector3D.cpp:41
boost::shared_ptr< class PatientModelService > PatientModelServicePtr
void cameraViewAngleSlot(int angle)
vtkSmartPointer< vtkPolyData > vtkPolyDataPtr
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
bool similar(const CameraInfo &lhs, const CameraInfo &rhs, double tol)
Transform3D createTransformRotateZ(const double angle)
void cameraPathPositionSlot(int pos)
Transform3D createTransformRotateX(const double angle)
boost::shared_ptr< class Mesh > MeshPtr
CXVBcameraPath(TrackingServicePtr tracker, PatientModelServicePtr patientModel, ViewServicePtr visualization)
#define M_PI
Namespace for all CustusX production code.