CloudViewerCellPicker.cpp
Go to the documentation of this file.
1 /*
2  * CloudViewerCellPicker.cpp
3  *
4  * Created on: Aug 21, 2018
5  * Author: mathieu
6  */
7 
8 #include "rtabmap/gui/CloudViewerCellPicker.h"
9 
10 #include <vtkImageData.h>
11 #include <vtkRenderer.h>
12 #include <vtkAbstractPicker.h>
13 #include <vtkPicker.h>
14 #include <vtkAbstractCellLocator.h>
15 #include <vtkIdList.h>
16 #include <vtkCellPicker.h>
17 #include <vtkLODProp3D.h>
18 #include <vtkMapper.h>
19 #include <vtkGenericCell.h>
20 #include <vtkMath.h>
21 #include <vtkTexture.h>
22 #include <vtkObjectFactory.h>
23 #include <vtkSmartPointer.h>
24 #include <vtkPoints.h>
25 #include <vtkProperty.h>
26 
27 namespace rtabmap {
28 
29 // Standard VTK macro for *New ()
30 vtkStandardNewMacro (CloudViewerCellPicker);
31 
32 CloudViewerCellPicker::CloudViewerCellPicker()
33 {
34  cell_ = vtkGenericCell::New();
35  pointIds_ = vtkIdList::New();
36 }
37 
38 CloudViewerCellPicker::~CloudViewerCellPicker()
39 {
40  cell_->Delete();
41  pointIds_->Delete();
42 }
43 
44 double CloudViewerCellPicker::IntersectActorWithLine(const double p1[3],
45  const double p2[3],
46  double t1, double t2,
47  double tol,
48  vtkProp3D *prop,
49  vtkMapper *mapper)
50 {
51  // This code was taken from the original CellPicker with almost no
52  // modification except for the locator and texture additions.
53 
54  // Intersect each cell with ray. Keep track of one closest to
55  // the eye (within the tolerance tol) and within the clipping range).
56  // Note that we fudge the "closest to" (tMin+this->Tolerance) a little and
57  // keep track of the cell with the best pick based on parametric
58  // coordinate (pick the minimum, maximum parametric distance). This
59  // breaks ties in a reasonable way when cells are the same distance
60  // from the eye (like cells laying on a 2D plane).
61 
62  vtkDataSet *data = mapper->GetInput();
63  double tMin = VTK_DOUBLE_MAX;
64  double minPCoords[3];
65  double pDistMin = VTK_DOUBLE_MAX;
66  vtkIdType minCellId = -1;
67  int minSubId = -1;
68  double minXYZ[3];
69  minXYZ[0] = minXYZ[1] = minXYZ[2] = 0.0;
70  double ray[3] = {p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]};
71  vtkMath::Normalize(ray);
72  vtkActor * actor = vtkActor::SafeDownCast(prop);
73 
74  // Polydata has no 3D cells
75  int isPolyData = data->IsA("vtkPolyData");
76 
77  vtkCollectionSimpleIterator iter;
78  vtkAbstractCellLocator *locator = 0;
79  this->Locators->InitTraversal(iter);
80  while ( (locator = static_cast<vtkAbstractCellLocator *>(
81  this->Locators->GetNextItemAsObject(iter))) )
82  {
83  if (locator->GetDataSet() == data)
84  {
85  break;
86  }
87  }
88 
89  // Make a new p1 and p2 using the clipped t1 and t2
90  double q1[3], q2[3];
91  q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
92  q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
93  if (t1 != 0.0 || t2 != 1.0)
94  {
95  for (int j = 0; j < 3; j++)
96  {
97  q1[j] = p1[j]*(1.0 - t1) + p2[j]*t1;
98  q2[j] = p1[j]*(1.0 - t2) + p2[j]*t2;
99  }
100  }
101 
102  // Use the locator if one exists for this data
103  if (locator)
104  {
105  vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
106  vtkSmartPointer<vtkIdList> intersectCells = vtkSmartPointer<vtkIdList>::New();
107 
108  locator->IntersectWithLine(q1, q2, intersectPoints, intersectCells);
109  for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
110  {
111  double intersection[3];
112  intersectPoints->GetPoint(i, intersection);
113  }
114 
115  if (!locator->IntersectWithLine(q1, q2, tol, tMin, minXYZ,
116  minPCoords, minSubId, minCellId,
117  this->cell_))
118  {
119  return VTK_DOUBLE_MAX;
120  }
121 
122  // Stretch tMin out to the original range
123  if (t1 != 0.0 || t2 != 1.0)
124  {
125  tMin = t1*(1.0 - tMin) + t2*tMin;
126  }
127 
128  // If cell is a strip, then replace cell with a sub-cell
129  this->SubCellFromCell(this->cell_, minSubId);
130  }
131  else
132  {
133  vtkIdList *pointIds = this->pointIds_;
134  vtkIdType numCells = data->GetNumberOfCells();
135 
136  for (vtkIdType cellId = 0; cellId < numCells; cellId++)
137  {
138  double t;
139  double x[3];
140  double pcoords[3];
141  pcoords[0] = pcoords[1] = pcoords[2] = 0;
142  int newSubId = -1;
143  int numSubIds = 1;
144 
145  // If it is a strip, we need to iterate over the subIds
146  int cellType = data->GetCellType(cellId);
147  int useSubCells = this->HasSubCells(cellType);
148  if (useSubCells)
149  {
150  // Get the pointIds for the strip and the length of the strip
151  data->GetCellPoints(cellId, pointIds);
152  numSubIds = this->GetNumberOfSubCells(pointIds, cellType);
153  }
154 
155  // This will only loop once unless we need to deal with a strip
156  for (int subId = 0; subId < numSubIds; subId++)
157  {
158  if (useSubCells)
159  {
160  // Get a sub-cell from a the strip
161  this->GetSubCell(data, pointIds, subId, cellType, this->cell_);
162  }
163  else
164  {
165  data->GetCell(cellId, this->cell_);
166  }
167 
168  int cellPicked = 0;
169  if (isPolyData)
170  {
171  // Polydata can always be picked with original endpoints
172  cellPicked = this->cell_->IntersectWithLine(
173  const_cast<double *>(p1), const_cast<double *>(p2),
174  tol, t, x, pcoords, newSubId);
175  }
176  else
177  {
178  // Any 3D cells need to be intersected with a line segment that
179  // has been clipped with the clipping planes, in case one end is
180  // actually inside the cell.
181  cellPicked = this->cell_->IntersectWithLine(
182  q1, q2, tol, t, x, pcoords, newSubId);
183 
184  // Stretch t out to the original range
185  if (t1 != 0.0 || t2 != 1.0)
186  {
187  t = t1*(1.0 - t) + t2*t;
188  }
189  }
190 
191  if (cellPicked && t <= (tMin + this->Tolerance) && t >= t1 && t <= t2)
192  {
193  double pDist = this->cell_->GetParametricDistance(pcoords);
194  if (pDist < pDistMin || (pDist == pDistMin && t < tMin))
195  {
197 // BEGIN: Modifications from VTK 6.2
199  bool visible = true;
200  if(actor->GetProperty()->GetBackfaceCulling() ||
201  actor->GetProperty()->GetFrontfaceCulling())
202  {
203  // Get the cell weights
204  vtkIdType numPoints = this->cell_->GetNumberOfPoints();
205  double *weights = new double[numPoints];
206  for (vtkIdType i = 0; i < numPoints; i++)
207  {
208  weights[i] = 0;
209  }
210 
211  // Get the interpolation weights (point is thrown away)
212  double point[3] = {0.0,0.0,0.0};
213  this->cell_->EvaluateLocation(minSubId, minPCoords, point, weights);
214 
215  double normal[3] = {0.0,0.0,0.0};
216 
217  if (this->ComputeSurfaceNormal(data, this->cell_, weights, normal))
218  {
219  if(actor->GetProperty()->GetBackfaceCulling())
220  {
221  visible = ray[0]*normal[0] + ray[1]*normal[1] + ray[2]*normal[2] <= 0;
222  }
223  else
224  {
225  visible = ray[0]*normal[0] + ray[1]*normal[1] + ray[2]*normal[2] >= 0;
226  }
227  }
228  delete [] weights;
229  }
230  if(visible)
231  {
232  tMin = t;
233  pDistMin = pDist;
234  // save all of these
235  minCellId = cellId;
236  minSubId = newSubId;
237  if (useSubCells)
238  {
239  minSubId = subId;
240  }
241  for (int k = 0; k < 3; k++)
242  {
243  minXYZ[k] = x[k];
244  minPCoords[k] = pcoords[k];
245  }
246  }
248 // END: Modifications from VTK 6.2
250  } // for all subIds
251  } // if minimum, maximum
252  } // if a close cell
253  } // for all cells
254  }
255 
256  // Do this if a cell was intersected
257  if (minCellId >= 0 && tMin < this->GlobalTMin)
258  {
259  this->ResetPickInfo();
260 
261  // Get the cell, convert to triangle if it is a strip
262  vtkGenericCell *cell = this->cell_;
263 
264  // If we used a locator, we already have the picked cell
265  if (!locator)
266  {
267  int cellType = data->GetCellType(minCellId);
268 
269  if (this->HasSubCells(cellType))
270  {
271  data->GetCellPoints(minCellId, this->pointIds_);
272  this->GetSubCell(data, this->pointIds_, minSubId, cellType, cell);
273  }
274  else
275  {
276  data->GetCell(minCellId, cell);
277  }
278  }
279 
280  // Get the cell weights
281  vtkIdType numPoints = cell->GetNumberOfPoints();
282  double *weights = new double[numPoints];
283  for (vtkIdType i = 0; i < numPoints; i++)
284  {
285  weights[i] = 0;
286  }
287 
288  // Get the interpolation weights (point is thrown away)
289  double point[3];
290  cell->EvaluateLocation(minSubId, minPCoords, point, weights);
291 
292  this->Mapper = mapper;
293 
294  // Get the texture from the actor or the LOD
295  vtkActor *actor = 0;
296  vtkLODProp3D *lodActor = 0;
297  if ( (actor = vtkActor::SafeDownCast(prop)) )
298  {
299  this->Texture = actor->GetTexture();
300  }
301  else if ( (lodActor = vtkLODProp3D::SafeDownCast(prop)) )
302  {
303  int lodId = lodActor->GetPickLODID();
304  lodActor->GetLODTexture(lodId, &this->Texture);
305  }
306 
307  if (this->PickTextureData && this->Texture)
308  {
309  // Return the texture's image data to the user
310  vtkImageData *image = this->Texture->GetInput();
311  this->DataSet = image;
312 
313  // Get and check the image dimensions
314  int extent[6];
315  image->GetExtent(extent);
316  int dimensionsAreValid = 1;
317  int dimensions[3];
318  for (int i = 0; i < 3; i++)
319  {
320  dimensions[i] = extent[2*i + 1] - extent[2*i] + 1;
321  dimensionsAreValid = (dimensionsAreValid && dimensions[i] > 0);
322  }
323 
324  // Use the texture coord to set the information
325  double tcoord[3];
326  if (dimensionsAreValid &&
327  this->ComputeSurfaceTCoord(data, cell, weights, tcoord))
328  {
329  // Take the border into account when computing coordinates
330  double x[3];
331  x[0] = extent[0] + tcoord[0]*dimensions[0] - 0.5;
332  x[1] = extent[2] + tcoord[1]*dimensions[1] - 0.5;
333  x[2] = extent[4] + tcoord[2]*dimensions[2] - 0.5;
334 
335  this->SetImageDataPickInfo(x, extent);
336  }
337  }
338  else
339  {
340  // Return the polydata to the user
341  this->DataSet = data;
342  this->CellId = minCellId;
343  this->SubId = minSubId;
344  this->PCoords[0] = minPCoords[0];
345  this->PCoords[1] = minPCoords[1];
346  this->PCoords[2] = minPCoords[2];
347 
348  // Find the point with the maximum weight
349  double maxWeight = 0;
350  vtkIdType iMaxWeight = -1;
351  for (vtkIdType i = 0; i < numPoints; i++)
352  {
353  if (weights[i] > maxWeight)
354  {
355  iMaxWeight = i;
356  }
357  }
358 
359  // If maximum weight is found, use it to get the PointId
360  if (iMaxWeight != -1)
361  {
362  this->PointId = cell->PointIds->GetId(iMaxWeight);
363  }
364  }
365 
366  // Set the mapper position
367  this->MapperPosition[0] = minXYZ[0];
368  this->MapperPosition[1] = minXYZ[1];
369  this->MapperPosition[2] = minXYZ[2];
370 
371  // Compute the normal
372  if (!this->ComputeSurfaceNormal(data, cell, weights, this->MapperNormal))
373  {
374  // By default, the normal points back along view ray
375  this->MapperNormal[0] = p1[0] - p2[0];
376  this->MapperNormal[1] = p1[1] - p2[1];
377  this->MapperNormal[2] = p1[2] - p2[2];
378  vtkMath::Normalize(this->MapperNormal);
379  }
380 
381  delete [] weights;
382  }
383 
384  return tMin;
385 }
386 
387 } /* namespace rtabmap */
rtabmap::vtkStandardNewMacro
vtkStandardNewMacro(CloudViewerCellPicker)
rtabmap::CloudViewerCellPicker::IntersectActorWithLine
virtual double IntersectActorWithLine(const double p1[3], const double p2[3], double t1, double t2, double tol, vtkProp3D *prop, vtkMapper *mapper)
Definition: CloudViewerCellPicker.cpp:44

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58