UsTK : Ultrasound ToolKit  version 2.0.1 under development (2023-12-07)
usNeedleModelingDisplayTools.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ustk software.
4  * Copyright (C) 2016 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ustk with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * This software was developed at:
17  * Inria Rennes - Bretagne Atlantique
18  * Campus Universitaire de Beaulieu
19  * 35042 Rennes Cedex
20  * France
21  *
22  * If you have questions regarding the use of this file, please contact
23  * Inria at ustk@inria.fr
24  *
25  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
26  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
27  *
28  * Author:
29  * Jason Chevrie
30  *
31  *****************************************************************************/
32 
33 #include <visp3/ustk_needle_modeling/usNeedleModelingDisplayTools.h>
34 
35 #include <visp3/core/vpDisplay.h>
36 #include <visp3/core/vpImage.h>
37 #include <visp3/core/vpRGBa.h>
38 
39 #include <visp3/ustk_core/usGeometryTools.h>
40 
41 namespace usNeedleModelingDisplayTools
42 {
43 
45 
46 template <class ImageDataType>
47 void displayBase(const usNeedleModelBaseTip &needleModel, const vpImage<ImageDataType> &I,
48  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale)
49 {
50  usGeometryDisplayTools::displayFrame(I, imageMworld * needleModel.getWorldMbase(), Xscale, Yscale);
51 }
52 template VISP_EXPORT void displayBase<unsigned char>(const usNeedleModelBaseTip &, const vpImage<unsigned char> &,
53  const vpHomogeneousMatrix &, double, double);
54 template VISP_EXPORT void displayBase<vpRGBa>(const usNeedleModelBaseTip &, const vpImage<vpRGBa> &,
55  const vpHomogeneousMatrix &, double, double);
56 
57 template <class ImageDataType>
58 void displayTip(const usNeedleModelBaseTip &needleModel, const vpImage<ImageDataType> &I,
59  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale)
60 {
61  usGeometryDisplayTools::displayFrame(I, imageMworld * needleModel.getWorldMtip(), Xscale, Yscale);
62 }
63 template VISP_EXPORT void displayTip<unsigned char>(const usNeedleModelBaseTip &, const vpImage<unsigned char> &,
64  const vpHomogeneousMatrix &, double, double);
65 template VISP_EXPORT void displayTip<vpRGBa>(const usNeedleModelBaseTip &, const vpImage<vpRGBa> &,
66  const vpHomogeneousMatrix &, double, double);
67 
68 template <class ImageDataType>
69 void display(const usNeedleModelBaseTip &needleModel, const vpImage<ImageDataType> &I,
70  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale)
71 {
72  displayBase(needleModel, I, imageMworld, Xscale, Yscale);
73  displayTip(needleModel, I, imageMworld, Xscale, Yscale);
74 }
75 template VISP_EXPORT void display(const usNeedleModelBaseTip &needleModel, const vpImage<unsigned char> &I,
76  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale);
77 template VISP_EXPORT void display(const usNeedleModelBaseTip &needleModel, const vpImage<vpRGBa> &I,
78  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale);
79 
81 
82 template <class ImageDataType>
83 void displayNeedle(const usNeedleModelPolynomial &needleModel, const vpImage<ImageDataType> &I,
84  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale, bool displayFullBody)
85 {
86  usGeometryDisplayTools::display((const usPolynomialCurve3D &)needleModel, I, imageMworld, Xscale, Yscale,
87  vpColor::red, 10);
88  if (displayFullBody) {
89  double r = 0.5 * needleModel.getOuterDiameter();
90  double l = needleModel.getParametricLength();
91  double step = l / 10;
92  vpColVector z(imageMworld.inverse().getCol(2), 0, 3);
93 
94  std::vector<double> params(11);
95  std::vector<vpColVector> p1(11);
96  std::vector<vpColVector> p2(11);
97  for (int i = 0; i < 11; i++) {
98  params.at(i) = i * step;
99  vpColVector p(needleModel.getPoint(params.at(i)));
100  vpColVector x(vpColVector::crossProd(needleModel.getTangent(params.at(i)), z).normalize());
101  p1.at(i) = p + r * x;
102  p2.at(i) = p - r * x;
103  }
104  vpColVector p(needleModel.getEndPoint());
105  vpColVector x(vpColVector::crossProd(needleModel.getEndTangent(), z).normalize());
106  p1.back() = p + r * x;
107  p2.back() = p - r * x;
108  usPolynomialCurve3D s;
109  s.defineFromPoints(p1, params, needleModel.getOrder());
110  usGeometryDisplayTools::display(s, I, imageMworld, Xscale, Yscale);
111  s.defineFromPoints(p2, params, needleModel.getOrder());
112  usGeometryDisplayTools::display(s, I, imageMworld, Xscale, Yscale);
113  }
114  display((const usNeedleModelBaseTip &)needleModel, I, imageMworld, Xscale, Yscale);
115 }
116 template VISP_EXPORT void displayNeedle<unsigned char>(const usNeedleModelPolynomial &, const vpImage<unsigned char> &,
117  const vpHomogeneousMatrix &, double, double, bool);
118 template VISP_EXPORT void displayNeedle<vpRGBa>(const usNeedleModelPolynomial &, const vpImage<vpRGBa> &,
119  const vpHomogeneousMatrix &, double, double, bool);
120 
121 template <class ImageDataType>
122 void displayBaseStaticTorsor(const usNeedleModelPolynomial &needleModel, const vpImage<ImageDataType> &I,
123  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale)
124 {
125  vpRotationMatrix R(imageMworld);
126  vpTranslationVector T(imageMworld);
127 
128  vpColVector worldTorsor = -needleModel.getBaseStaticTorsor();
129  vpColVector worldForce(worldTorsor, 0, 3);
130  vpColVector worldMoment(worldTorsor, 3, 3);
131 
132  vpColVector imageBase = R * needleModel.getStartPoint() + T;
133  vpColVector imageForce = R * worldForce;
134  vpColVector imageMoment = R * worldMoment;
135 
136  double x = Xscale * imageBase[0];
137  double y = Yscale * imageBase[1];
138 
139  double force_x = imageForce[0];
140  double force_y = imageForce[1];
141 
142  double moment_x = imageMoment[0];
143  double moment_y = imageMoment[1];
144 
145  vpDisplay::displayArrow(I, y, x, y + 100 * force_y, x + 100 * force_x, vpColor::red, 5, 5, 3);
146  vpDisplay::displayArrow(I, y, x, y + 3000 * moment_y, x + 3000 * moment_x, vpColor::green, 5, 5, 3);
147 }
148 template VISP_EXPORT void displayBaseStaticTorsor<unsigned char>(const usNeedleModelPolynomial &,
149  const vpImage<unsigned char> &,
150  const vpHomogeneousMatrix &, double, double);
151 template VISP_EXPORT void displayBaseStaticTorsor<vpRGBa>(const usNeedleModelPolynomial &, const vpImage<vpRGBa> &,
152  const vpHomogeneousMatrix &, double, double);
153 
154 template <class ImageDataType>
155 void displayCurvatureFromShape(const usNeedleModelPolynomial &needleModel, const vpImage<ImageDataType> &I,
156  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale)
157 {
158  usGeometryDisplayTools::displayCurvatureFromShape(needleModel, I, imageMworld, Xscale, Yscale);
159 }
160 template VISP_EXPORT void displayCurvatureFromShape<unsigned char>(const usNeedleModelPolynomial &,
161  const vpImage<unsigned char> &,
162  const vpHomogeneousMatrix &, double, double);
163 template VISP_EXPORT void displayCurvatureFromShape<vpRGBa>(const usNeedleModelPolynomial &, const vpImage<vpRGBa> &,
164  const vpHomogeneousMatrix &, double, double);
165 
166 template <class ImageDataType>
167 void display(const usNeedleModelPolynomial &needleModel, const vpImage<ImageDataType> &I,
168  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale, bool displayFullBody)
169 {
170  displayNeedle(needleModel, I, imageMworld, Xscale, Yscale, displayFullBody);
171  displayBaseStaticTorsor(needleModel, I, imageMworld, Xscale, Yscale);
172  displayCurvatureFromShape(needleModel, I, imageMworld, Xscale, Yscale);
173 }
174 template VISP_EXPORT void display<unsigned char>(const usNeedleModelPolynomial &, const vpImage<unsigned char> &,
175  const vpHomogeneousMatrix &, double, double, bool);
176 template VISP_EXPORT void display<vpRGBa>(const usNeedleModelPolynomial &, const vpImage<vpRGBa> &,
177  const vpHomogeneousMatrix &, double, double, bool);
178 
180 
181 template <class ImageDataType>
182 void displayNeedle(const usNeedleModelSpline &needleModel, const vpImage<ImageDataType> &I,
183  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale, bool displayFullBody)
184 {
185  usGeometryDisplayTools::displayLine((const usBSpline3D &)needleModel, I, imageMworld, Xscale, Yscale, vpColor::red);
186  if (displayFullBody) {
187  double r = 0.5 * needleModel.getOuterDiameter();
188  vpColVector z(imageMworld.inverse().getCol(2), 0, 3);
189 
190  std::vector<double> lengths(needleModel.getNbSegments());
191  std::vector<vpColVector> p1(needleModel.getNbSegments() + 1);
192  std::vector<vpColVector> p2(needleModel.getNbSegments() + 1);
193  for (int i = 0; i < needleModel.getNbSegments(); i++) {
194  lengths.at(i) = needleModel.accessSegment(i).getParametricLength();
195  vpColVector p(needleModel.accessSegment(i).getStartPoint());
196  vpColVector x(vpColVector::crossProd(needleModel.accessSegment(i).getStartTangent(), z).normalize());
197  p1.at(i) = p + r * x;
198  p2.at(i) = p - r * x;
199  }
200  vpColVector p(needleModel.accessLastSegment().getEndPoint());
201  vpColVector x(vpColVector::crossProd(needleModel.accessLastSegment().getEndTangent(), z).normalize());
202  p1.back() = p + r * x;
203  p2.back() = p - r * x;
204  usBSpline3D s;
205  s.defineFromPoints(p1, lengths, needleModel.accessSegment(0).getOrder());
206  usGeometryDisplayTools::display(s, I, imageMworld, Xscale, Yscale);
207  s.defineFromPoints(p2, lengths, needleModel.accessSegment(0).getOrder());
208  usGeometryDisplayTools::display(s, I, imageMworld, Xscale, Yscale);
209  }
210  usGeometryDisplayTools::displayExtremities((const usBSpline3D &)needleModel, I, imageMworld, Xscale, Yscale,
211  vpColor::red);
212  display((const usNeedleModelBaseTip &)needleModel, I, imageMworld, Xscale, Yscale);
213 }
214 template VISP_EXPORT void displayNeedle<unsigned char>(const usNeedleModelSpline &, const vpImage<unsigned char> &,
215  const vpHomogeneousMatrix &, double, double, bool);
216 template VISP_EXPORT void displayNeedle<vpRGBa>(const usNeedleModelSpline &, const vpImage<vpRGBa> &,
217  const vpHomogeneousMatrix &, double, double, bool);
218 
219 template <class ImageDataType>
220 void displayBaseStaticTorsor(const usNeedleModelSpline &needleModel, const vpImage<ImageDataType> &I,
221  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale)
222 {
223  vpRotationMatrix R(imageMworld);
224  vpTranslationVector T(imageMworld);
225 
226  vpColVector worldTorsor = -needleModel.getBaseStaticTorsor();
227  vpColVector worldForce(worldTorsor, 0, 3);
228  vpColVector worldMoment(worldTorsor, 3, 3);
229 
230  vpColVector imageBase = R * needleModel.accessSegment(0).getStartPoint() + T;
231  vpColVector imageForce = R * worldForce;
232  vpColVector imageMoment = R * worldMoment;
233 
234  double x = Xscale * imageBase[0];
235  double y = Yscale * imageBase[1];
236 
237  double force_x = imageForce[0];
238  double force_y = imageForce[1];
239 
240  double moment_x = imageMoment[0];
241  double moment_y = imageMoment[1];
242 
243  vpDisplay::displayArrow(I, y, x, y + 100 * force_y, x + 100 * force_x, vpColor::red, 5, 5, 3);
244  vpDisplay::displayArrow(I, y, x, y + 3000 * moment_y, x + 3000 * moment_x, vpColor::green, 5, 5, 3);
245 }
246 template VISP_EXPORT void displayBaseStaticTorsor<unsigned char>(const usNeedleModelSpline &needleModel,
247  const vpImage<unsigned char> &,
248  const vpHomogeneousMatrix &, double, double);
249 template VISP_EXPORT void displayBaseStaticTorsor<vpRGBa>(const usNeedleModelSpline &needleModel,
250  const vpImage<vpRGBa> &, const vpHomogeneousMatrix &, double,
251  double);
252 
253 template <class ImageDataType>
254 void displayCurvatureFromShape(const usNeedleModelSpline &needleModel, const vpImage<ImageDataType> &I,
255  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale)
256 {
257  usGeometryDisplayTools::displayCurvatureFromShape(needleModel, I, imageMworld, Xscale, Yscale);
258 }
259 template VISP_EXPORT void displayCurvatureFromShape<unsigned char>(const usNeedleModelSpline &,
260  const vpImage<unsigned char> &,
261  const vpHomogeneousMatrix &, double, double);
262 template VISP_EXPORT void displayCurvatureFromShape<vpRGBa>(const usNeedleModelSpline &, const vpImage<vpRGBa> &,
263  const vpHomogeneousMatrix &, double, double);
264 
265 template <class ImageDataType>
266 void display(const usNeedleModelSpline &needleModel, const vpImage<ImageDataType> &I,
267  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale, bool displayFullBody)
268 {
269  displayNeedle(needleModel, I, imageMworld, Xscale, Yscale, displayFullBody);
270  displayBaseStaticTorsor(needleModel, I, imageMworld, Xscale, Yscale);
271  displayCurvatureFromShape(needleModel, I, imageMworld, Xscale, Yscale);
272 }
273 template VISP_EXPORT void display<unsigned char>(const usNeedleModelSpline &, const vpImage<unsigned char> &,
274  const vpHomogeneousMatrix &, double, double, bool);
275 template VISP_EXPORT void display<vpRGBa>(const usNeedleModelSpline &, const vpImage<vpRGBa> &,
276  const vpHomogeneousMatrix &, double, double, bool);
277 
279 
280 template <class ImageType>
281 void display(const usVirtualSpring &spring, const vpImage<ImageType> &I, const vpHomogeneousMatrix &imageMworld,
282  double Xscale, double Yscale)
283 {
284  usGeometryDisplayTools::display((const usOrientedPlane3D &)spring, I, imageMworld, Xscale, Yscale, vpColor::green);
285 }
286 template VISP_EXPORT void display<unsigned char>(const usVirtualSpring &, const vpImage<unsigned char> &,
287  const vpHomogeneousMatrix &, double, double);
288 template VISP_EXPORT void display<vpRGBa>(const usVirtualSpring &, const vpImage<vpRGBa> &, const vpHomogeneousMatrix &,
289  double, double);
290 
292 
293 template <class ImageDataType>
294 void display(const usTissueModelSpline &tissue, const vpImage<ImageDataType> &I, const vpHomogeneousMatrix &imageMworld,
295  double Xscale, double Yscale)
296 {
297  usGeometryDisplayTools::display(tissue.accessSurface(), I, imageMworld, Xscale, Yscale, vpColor::black);
298  usGeometryDisplayTools::display(tissue.accessPath(), I, imageMworld, Xscale, Yscale, vpColor::blue);
299 }
300 template VISP_EXPORT void display<unsigned char>(const usTissueModelSpline &, const vpImage<unsigned char> &,
301  const vpHomogeneousMatrix &, double, double);
302 template VISP_EXPORT void display<vpRGBa>(const usTissueModelSpline &, const vpImage<vpRGBa> &,
303  const vpHomogeneousMatrix &, double, double);
304 
306 
307 template <class ImageDataType>
308 void display(const usNeedleTip &tip, const vpImage<ImageDataType> &I, const vpHomogeneousMatrix &imageMworld,
309  double Xscale, double Yscale)
310 {
311  usGeometryDisplayTools::displayFrame(I, imageMworld * tip.getWorldMbase().inverse(), Xscale, Yscale);
312 }
313 template VISP_EXPORT void display<unsigned char>(const usNeedleTip &, const vpImage<unsigned char> &,
314  const vpHomogeneousMatrix &, double, double);
315 template VISP_EXPORT void display<vpRGBa>(const usNeedleTip &, const vpImage<vpRGBa> &, const vpHomogeneousMatrix &,
316  double, double);
317 
319 
320 template <class ImageDataType>
321 void display(const usNeedleTipActuated &tip, const vpImage<ImageDataType> &I, const vpHomogeneousMatrix &imageMworld,
322  double Xscale, double Yscale)
323 {
324  vpRotationMatrix R(imageMworld);
325  vpTranslationVector T(imageMworld);
326 
327  vpColVector b = tip.getBasePosition();
328  vpColVector t = tip.getTipPosition();
329  vpColVector d = tip.getBaseAxisZ();
330 
331  vpColVector imageB(R * b + T);
332  vpColVector imageT(R * t + T);
333  vpColVector imageD(R * d);
334  vpColVector n(2, 0);
335  n[0] = imageD[1];
336  n[1] = -imageD[0];
337  n.normalize();
338 
339  double diameter = tip.getDiameter();
340 
341  double x[3];
342  double y[3];
343 
344  x[0] = Xscale * (imageT[0]);
345  y[0] = Yscale * (imageT[1]);
346 
347  x[1] = Xscale * (imageB[0] + diameter / 2 * n[0]);
348  y[1] = Yscale * (imageB[1] + diameter / 2 * n[1]);
349 
350  x[2] = Xscale * (imageB[0] - diameter / 2 * n[0]);
351  y[2] = Yscale * (imageB[1] - diameter / 2 * n[1]);
352 
353  for (int i = 0; i < 3; i++) {
354  vpDisplay::displayLine(I, y[i], x[i], y[(i + 1) % 3], x[(i + 1) % 3], vpColor::red);
355  }
356 }
357 template VISP_EXPORT void display<unsigned char>(const usNeedleTipActuated &, const vpImage<unsigned char> &,
358  const vpHomogeneousMatrix &, double, double);
359 template VISP_EXPORT void display<vpRGBa>(const usNeedleTipActuated &, const vpImage<vpRGBa> &,
360  const vpHomogeneousMatrix &, double, double);
361 
363 
364 template <class ImageDataType>
365 void display(const usNeedleTipBeveled &tip, const vpImage<ImageDataType> &I, const vpHomogeneousMatrix &imageMworld,
366  double Xscale, double Yscale)
367 {
368  vpRotationMatrix R(imageMworld);
369  vpTranslationVector T(imageMworld);
370 
371  vpColVector b = tip.getBasePosition();
372  vpColVector dy = tip.getBaseAxisY();
373  vpColVector dz = tip.getBaseAxisZ();
374 
375  vpColVector imageB(R * b + T);
376  vpColVector imageDy(R * dy);
377  vpColVector imageDz(R * dz);
378 
379  double diameter = tip.getDiameter();
380  double length = tip.getLength();
381 
382  double x[3];
383  double y[3];
384 
385  x[0] = Xscale * (imageB[0] + diameter / 2 * imageDy[0]);
386  y[0] = Yscale * (imageB[1] + diameter / 2 * imageDy[1]);
387 
388  x[1] = Xscale * (imageB[0] - diameter / 2 * imageDy[0]);
389  y[1] = Yscale * (imageB[1] - diameter / 2 * imageDy[1]);
390 
391  x[2] = Xscale * (imageB[0] + diameter / 2 * imageDy[0] + length * imageDz[0]);
392  y[2] = Yscale * (imageB[1] + diameter / 2 * imageDy[1] + length * imageDz[1]);
393 
394  for (int i = 0; i < 3; i++) {
395  vpDisplay::displayLine(I, y[i], x[i], y[(i + 1) % 3], x[(i + 1) % 3], vpColor::red);
396  }
397 }
398 template VISP_EXPORT void display<unsigned char>(const usNeedleTipBeveled &, const vpImage<unsigned char> &,
399  const vpHomogeneousMatrix &, double, double);
400 template VISP_EXPORT void display<vpRGBa>(const usNeedleTipBeveled &, const vpImage<vpRGBa> &,
401  const vpHomogeneousMatrix &, double, double);
402 
404 
405 template <class ImageDataType>
406 void display(const usNeedleTipPrebent &tip, const vpImage<ImageDataType> &I, const vpHomogeneousMatrix &imageMworld,
407  double Xscale, double Yscale)
408 {
409  vpRotationMatrix R(imageMworld);
410  vpTranslationVector T(imageMworld);
411 
412  vpColVector b = tip.getBasePosition();
413  vpColVector t = tip.getTipPosition();
414  vpColVector d = tip.getBaseAxisZ();
415 
416  vpColVector imageB(R * b + T);
417  vpColVector imageT(R * t + T);
418  vpColVector imageD(R * d);
419  vpColVector n(2, 0);
420  n[0] = imageD[1];
421  n[1] = -imageD[0];
422  n.normalize();
423 
424  double diameter = tip.getDiameter();
425 
426  double x[3];
427  double y[3];
428 
429  x[0] = Xscale * (imageT[0]);
430  y[0] = Yscale * (imageT[1]);
431 
432  x[1] = Xscale * (imageB[0] + diameter / 2 * n[0]);
433  y[1] = Yscale * (imageB[1] + diameter / 2 * n[1]);
434 
435  x[2] = Xscale * (imageB[0] - diameter / 2 * n[0]);
436  y[2] = Yscale * (imageB[1] - diameter / 2 * n[1]);
437 
438  for (int i = 0; i < 3; i++) {
439  vpDisplay::displayLine(I, y[i], x[i], y[(i + 1) % 3], x[(i + 1) % 3], vpColor::red);
440  }
441 }
442 template VISP_EXPORT void display<unsigned char>(const usNeedleTipPrebent &, const vpImage<unsigned char> &,
443  const vpHomogeneousMatrix &, double, double);
444 template VISP_EXPORT void display<vpRGBa>(const usNeedleTipPrebent &, const vpImage<vpRGBa> &,
445  const vpHomogeneousMatrix &, double, double);
446 
448 
449 template <class ImageDataType>
450 void display(const usNeedleTipSymmetric &tip, const vpImage<ImageDataType> &I, const vpHomogeneousMatrix &imageMworld,
451  double Xscale, double Yscale)
452 {
453  vpRotationMatrix R(imageMworld);
454  vpTranslationVector T(imageMworld);
455 
456  vpColVector b = tip.getBasePosition();
457  vpColVector d = tip.getBaseAxisZ();
458 
459  vpColVector imageB(R * b + T);
460  vpColVector imageD(R * d);
461  vpColVector n(2, 0);
462  n[0] = imageD[1];
463  n[1] = -imageD[0];
464  n.normalize();
465 
466  double diameter = tip.getDiameter();
467  double length = tip.getLength();
468 
469  double x[3];
470  double y[3];
471 
472  x[0] = Xscale * (imageB[0] + length * imageD[0]);
473  y[0] = Yscale * (imageB[1] + length * imageD[1]);
474 
475  x[1] = Xscale * (imageB[0] + diameter / 2 * n[0]);
476  y[1] = Yscale * (imageB[1] + diameter / 2 * n[1]);
477 
478  x[2] = Xscale * (imageB[0] - diameter / 2 * n[0]);
479  y[2] = Yscale * (imageB[1] - diameter / 2 * n[1]);
480 
481  for (int i = 0; i < 3; i++) {
482  vpDisplay::displayLine(I, y[i], x[i], y[(i + 1) % 3], x[(i + 1) % 3], vpColor::red);
483  }
484 }
485 template VISP_EXPORT void display<unsigned char>(const usNeedleTipSymmetric &, const vpImage<unsigned char> &,
486  const vpHomogeneousMatrix &, double, double);
487 template VISP_EXPORT void display<vpRGBa>(const usNeedleTipSymmetric &, const vpImage<vpRGBa> &,
488  const vpHomogeneousMatrix &, double, double);
489 
491 
492 template <class ImageDataType>
493 void display(const usNeedleInsertionModelKinematic &model, const vpImage<ImageDataType> &I,
494  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale)
495 {
496  display(model.accessNeedle(), I, imageMworld, Xscale, Yscale);
497 }
498 template VISP_EXPORT void display<unsigned char>(const usNeedleInsertionModelKinematic &,
499  const vpImage<unsigned char> &, const vpHomogeneousMatrix &, double,
500  double);
501 template VISP_EXPORT void display<vpRGBa>(const usNeedleInsertionModelKinematic &, const vpImage<vpRGBa> &,
502  const vpHomogeneousMatrix &, double, double);
503 
505 
506 template <class ImageDataType>
507 void displayTissueLayers(const usNeedleInsertionModelRayleighRitzSpline &model, const vpImage<ImageDataType> &I,
508  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale)
509 {
510  if (model.getNbLayers() < 2 || model.accessTissue().accessSurface().getDirection().frobeniusNorm() == 0)
511  return;
512 
513  if (model.accessTissue().accessPath().getNbSegments() > 0) {
514  double l = 0;
515  double restIndex = 0;
516  double nextLayerLength = model.getLayerLength(0);
517 
518  for (int i = 1; i < model.getNbLayers(); i++) {
519  while (l + model.accessTissue().accessPath().accessSegment(restIndex).getParametricLength() < nextLayerLength &&
520  restIndex < model.accessTissue().accessPath().getNbSegments() - 1) {
521  l += model.accessTissue().accessPath().accessSegment(restIndex).getParametricLength();
522  restIndex++;
523  }
524 
525  double restParam = nextLayerLength - l;
526  usOrientedPlane3D P(
527  usGeometryTools::getNormalPlane(model.accessTissue().accessPath().accessSegment(restIndex), restParam));
528 
529  usGeometryDisplayTools::display(P, I, imageMworld, Xscale, Yscale, vpColor::black);
530 
531  nextLayerLength += model.getLayerLength(i);
532  }
533  } else {
534  double nextLayerLength = model.getLayerLength(0);
535  vpColVector p = model.accessTissue().accessSurface().getPosition();
536  vpColVector d = model.accessTissue().accessSurface().getDirection();
537 
538  for (int i = 1; i < model.getNbLayers(); i++) {
539  usOrientedPlane3D P(p + nextLayerLength * d, d);
540 
541  usGeometryDisplayTools::display(P, I, imageMworld, Xscale, Yscale, vpColor::black);
542 
543  nextLayerLength += model.getLayerLength(i);
544  }
545  }
546 }
547 template VISP_EXPORT void displayTissueLayers<unsigned char>(const usNeedleInsertionModelRayleighRitzSpline &,
548  const vpImage<unsigned char> &,
549  const vpHomogeneousMatrix &, double, double);
550 template VISP_EXPORT void displayTissueLayers<vpRGBa>(const usNeedleInsertionModelRayleighRitzSpline &,
551  const vpImage<vpRGBa> &, const vpHomogeneousMatrix &, double,
552  double);
553 
554 template <class ImageDataType>
555 void displayInteraction(const usNeedleInsertionModelRayleighRitzSpline &model, const vpImage<ImageDataType> &I,
556  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale)
557 {
558  if (model.accessTissue().accessPath().getNbSegments() < 1)
559  return;
560 
561  double freeLength = model.getNeedleFreeLength();
562  double length = model.accessNeedle().getFullLength();
563  if (freeLength >= length)
564  return;
565 
566  vpRotationMatrix R(imageMworld);
567  vpTranslationVector T(imageMworld);
568 
569  vpColVector worldPoint(3);
570  vpColVector worldRestPoint(3);
571  vpColVector imagePoint(3);
572  vpColVector imageRestPoint(3);
573 
574  for (int i = 0; i < 10; i++) {
575  double l = freeLength + (length - freeLength) * (i / 9.0);
576 
577  int restIndex = 0;
578  double restParam = 0;
579  if (!model.getCorrespondingPathPoint(l, restIndex, restParam))
580  continue;
581 
582  worldRestPoint = model.accessTissue().accessPath().accessSegment(restIndex).getPoint(restParam);
583  worldPoint = model.accessNeedle().getPoint(l);
584 
585  imagePoint = R * worldPoint + T;
586  imageRestPoint = R * worldRestPoint + T;
587 
588  double x = Xscale * imagePoint[0];
589  double y = Yscale * imagePoint[1];
590 
591  double xr = Xscale * imageRestPoint[0];
592  double yr = Yscale * imageRestPoint[1];
593 
594  vpDisplay::displayArrow(I, y, x, yr, xr, vpColor::black);
595  }
596 }
597 template VISP_EXPORT void displayInteraction<unsigned char>(const usNeedleInsertionModelRayleighRitzSpline &,
598  const vpImage<unsigned char> &, const vpHomogeneousMatrix &,
599  double, double);
600 template VISP_EXPORT void displayInteraction<vpRGBa>(const usNeedleInsertionModelRayleighRitzSpline &,
601  const vpImage<vpRGBa> &, const vpHomogeneousMatrix &, double,
602  double);
603 
604 template <class ImageDataType>
605 void display(const usNeedleInsertionModelRayleighRitzSpline &model, const vpImage<ImageDataType> &I,
606  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale, bool displayFullBody)
607 {
608  display(model.accessNeedle(), I, imageMworld, Xscale, Yscale, displayFullBody);
609  switch (model.getNeedleTipType()) {
610  case usNeedleInsertionModelRayleighRitzSpline::NeedleTipType::ActuatedTip: {
611  display(dynamic_cast<const usNeedleTipActuated &>(model.accessNeedleTip()), I, imageMworld, Xscale, Yscale);
612  break;
613  }
614  case usNeedleInsertionModelRayleighRitzSpline::NeedleTipType::BeveledTip: {
615  display(dynamic_cast<const usNeedleTipBeveled &>(model.accessNeedleTip()), I, imageMworld, Xscale, Yscale);
616  break;
617  }
618  case usNeedleInsertionModelRayleighRitzSpline::NeedleTipType::PrebentTip: {
619  display(dynamic_cast<const usNeedleTipPrebent &>(model.accessNeedleTip()), I, imageMworld, Xscale, Yscale);
620  break;
621  }
622  case usNeedleInsertionModelRayleighRitzSpline::NeedleTipType::SymmetricTip: {
623  display(dynamic_cast<const usNeedleTipSymmetric &>(model.accessNeedleTip()), I, imageMworld, Xscale, Yscale);
624  break;
625  }
626  }
627  display(model.accessTissue(), I, imageMworld, Xscale, Yscale);
628  displayTissueLayers(model, I, imageMworld, Xscale, Yscale);
629  displayInteraction(model, I, imageMworld, Xscale, Yscale);
630 }
631 template VISP_EXPORT void display<unsigned char>(const usNeedleInsertionModelRayleighRitzSpline &,
632  const vpImage<unsigned char> &, const vpHomogeneousMatrix &, double,
633  double, bool);
634 template VISP_EXPORT void display<vpRGBa>(const usNeedleInsertionModelRayleighRitzSpline &, const vpImage<vpRGBa> &,
635  const vpHomogeneousMatrix &, double, double, bool);
636 
638 
639 template <class ImageDataType>
640 void display(const usNeedleInsertionModelVirtualSprings &model, const vpImage<ImageDataType> &I,
641  const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale, bool displayFullBody)
642 {
643  for (int i = 0; i < model.getNbSprings(); i++) {
644  if (model.accessSpring(i).IsPositionUpdateAllowed())
645  usGeometryDisplayTools::display((const usOrientedPlane3D &)model.accessSpring(i), I, imageMworld, Xscale, Yscale,
646  vpColor::green);
647  else
648  usGeometryDisplayTools::display((const usOrientedPlane3D &)model.accessSpring(i), I, imageMworld, Xscale, Yscale,
649  vpColor::red);
650  }
651  display(model.accessNeedle(), I, imageMworld, Xscale, Yscale, displayFullBody);
652  usGeometryDisplayTools::display(model.accessSurface(), I, imageMworld, Xscale, Yscale, vpColor::black);
653  // displayBaseStaticTorsor(model.accessNeedle(), I, imageMworld, Xscale, Yscale);
654  // displayCurvatureFromShape(model.accessNeedle(), I, imageMworld, Xscale, Yscale);
655 }
656 template VISP_EXPORT void display<unsigned char>(const usNeedleInsertionModelVirtualSprings &,
657  const vpImage<unsigned char> &, const vpHomogeneousMatrix &, double,
658  double, bool);
659 template VISP_EXPORT void display<vpRGBa>(const usNeedleInsertionModelVirtualSprings &, const vpImage<vpRGBa> &,
660  const vpHomogeneousMatrix &, double, double, bool);
661 
662 } // namespace usNeedleModelingDisplayTools
usBSpline3D::accessSegment
const usPolynomialCurve3D & accessSegment(int i) const
Definition: usBSpline3D.cpp:396
usBSpline3D::getNbSegments
int getNbSegments() const
Parameters setters and getters.
Definition: usBSpline3D.cpp:59
usBSpline3D::accessLastSegment
const usPolynomialCurve3D & accessLastSegment() const
Definition: usBSpline3D.cpp:398
usBSpline3D::getPoint
vpColVector getPoint(double param) const
Measure curve information.
Definition: usBSpline3D.cpp:459
usBSpline3D::defineFromPoints
void defineFromPoints(const std::vector< vpColVector > &points, const std::vector< double > &lengths, int order=3)
Definition: usBSpline3D.cpp:100
usNeedleInsertionModelKinematic::accessNeedle
const usNeedleModelBaseTip & accessNeedle() const
Needle.
Definition: usNeedleInsertionModelKinematic.cpp:70
usNeedleInsertionModelRayleighRitzSpline::getNeedleFreeLength
double getNeedleFreeLength(int *seg=nullptr, double *param=nullptr) const
Definition: usNeedleInsertionModelRayleighRitzSpline.cpp:363
usNeedleInsertionModelRayleighRitzSpline::accessTissue
const usTissueModelSpline & accessTissue() const
Tissue parameters.
Definition: usNeedleInsertionModelRayleighRitzSpline.cpp:244
usNeedleInsertionModelRayleighRitzSpline::accessNeedle
const usNeedleModelSpline & accessNeedle() const
Parameters setters and getters.
Definition: usNeedleInsertionModelRayleighRitzSpline.cpp:236
usNeedleInsertionModelRayleighRitzSpline::getCorrespondingPathPoint
bool getCorrespondingPathPoint(double l, int &correspondingRestIndex, double &correspondingRestParam) const
Definition: usNeedleInsertionModelRayleighRitzSpline.cpp:438
usNeedleInsertionModelVirtualSprings::accessSurface
const usOrientedPlane3D & accessSurface() const
Tissue.
Definition: usNeedleInsertionModelVirtualSprings.cpp:331
usNeedleInsertionModelVirtualSprings::accessNeedle
const usNeedleModelSpline & accessNeedle() const
Model parameters.
Definition: usNeedleInsertionModelVirtualSprings.cpp:293
usNeedleModelBaseTip::getWorldMtip
vpHomogeneousMatrix getWorldMtip() const
Definition: usNeedleModelBaseTip.cpp:78
usNeedleModelBaseTip::getWorldMbase
vpHomogeneousMatrix getWorldMbase() const
Definition: usNeedleModelBaseTip.cpp:62
usNeedleModelPolynomial::getBaseStaticTorsor
vpColVector getBaseStaticTorsor() const
Force at base.
Definition: usNeedleModelPolynomial.cpp:238
usNeedleModelSpline::getBaseStaticTorsor
vpColVector getBaseStaticTorsor() const
Force at base.
Definition: usNeedleModelSpline.cpp:324
usNeedleModelSpline::getOuterDiameter
double getOuterDiameter() const
Definition: usNeedleModelSpline.cpp:170
usNeedleTipBeveled::getLength
double getLength() const
Definition: usNeedleTipBeveled.cpp:73
usNeedleTipBeveled::getDiameter
double getDiameter() const
Definition: usNeedleTipBeveled.cpp:65
usNeedleTipPrebent::getDiameter
double getDiameter() const
Definition: usNeedleTipPrebent.cpp:67
usNeedleTip::getWorldMbase
vpHomogeneousMatrix getWorldMbase() const
Definition: usNeedleTip.cpp:76
usNeedleTip::getTipPosition
vpColVector getTipPosition() const
Definition: usNeedleTip.cpp:101
usNeedleTip::getBasePosition
vpColVector getBasePosition() const
Definition: usNeedleTip.cpp:87
usNeedleTip::getBaseAxisZ
vpColVector getBaseAxisZ() const
Definition: usNeedleTip.cpp:109
usNeedleTip::getBaseAxisY
vpColVector getBaseAxisY() const
Definition: usNeedleTip.cpp:107
usOrientedPlane3D::getDirection
vpColVector getDirection() const
Definition: usOrientedPlane3D.cpp:116
usOrientedPlane3D::getPosition
vpColVector getPosition() const
Definition: usOrientedPlane3D.cpp:80
usPolynomialCurve3D::defineFromPoints
void defineFromPoints(const std::vector< vpColVector > &points, const std::vector< double > &param, unsigned int order=0)
Definition: usPolynomialCurve3D.cpp:278
usPolynomialCurve3D::getStartTangent
vpColVector getStartTangent() const
Definition: usPolynomialCurve3D.cpp:253
usPolynomialCurve3D::getTangent
vpColVector getTangent(double parameter) const
Definition: usPolynomialCurve3D.cpp:241
usPolynomialCurve3D::getOrder
unsigned int getOrder() const
Definition: usPolynomialCurve3D.cpp:136
usPolynomialCurve3D::getPoint
vpColVector getPoint(double parameter) const
Definition: usPolynomialCurve3D.cpp:217
usPolynomialCurve3D::getEndPoint
vpColVector getEndPoint() const
Definition: usPolynomialCurve3D.cpp:239
usPolynomialCurve3D::getEndTangent
vpColVector getEndTangent() const
Definition: usPolynomialCurve3D.cpp:255
usPolynomialCurve3D::getParametricLength
double getParametricLength() const
Definition: usPolynomialCurve3D.cpp:160
usPolynomialCurve3D::getStartPoint
vpColVector getStartPoint() const
Definition: usPolynomialCurve3D.cpp:237
usTissueModelSpline::accessPath
const usBSpline3D & accessPath() const
Definition: usTissueModelSpline.cpp:58
usTissueModelSpline::accessSurface
const usOrientedPlane3D & accessSurface() const
Parameters setters and getters.
Definition: usTissueModelSpline.cpp:54
usVirtualSpring::IsPositionUpdateAllowed
bool IsPositionUpdateAllowed() const
Definition: usVirtualSpring.cpp:105
usGeometryDisplayTools::displayExtremities
VISP_EXPORT void displayExtremities(const usBSpline3D &spline, const vpImage< ImageDataType > &I, const vpHomogeneousMatrix &imageMworld, double Xscale=3000, double Yscale=3000, const vpColor &color=vpColor::red, double visibilityDistance=std::numeric_limits< double >::infinity())
Definition: usGeometryDisplayTools.cpp:254
usGeometryDisplayTools::display
VISP_EXPORT void display(const usOrientedPlane3D &plane, const vpImage< ImageDataType > &I, const vpHomogeneousMatrix &imageMworld, double Xscale=3000, double Yscale=3000, const vpColor &color=vpColor::green)
Display usOrientedPlane3D.
Definition: usGeometryDisplayTools.cpp:85
usGeometryDisplayTools::displayLine
VISP_EXPORT void displayLine(const usBSpline3D &spline, const vpImage< ImageDataType > &I, const vpHomogeneousMatrix &imageMworld, double Xscale=3000, double Yscale=3000, const vpColor &color=vpColor::red, int nbRenderingLinesPerSegment=10, double visibilityDistance=std::numeric_limits< double >::infinity())
Display usBSpline3D.
Definition: usGeometryDisplayTools.cpp:239
usGeometryDisplayTools::displayFrame
VISP_EXPORT void displayFrame(const vpImage< ImageDataType > &I, const vpHomogeneousMatrix &imageMFrame, double Xscale=3000, double Yscale=3000)
Definition: usGeometryDisplayTools.cpp:44
usGeometryDisplayTools::displayCurvatureFromShape
VISP_EXPORT void displayCurvatureFromShape(const usPolynomialCurve3D &spline, const vpImage< ImageDataType > &I, const vpHomogeneousMatrix &imageMworld, double Xscale=3000, double Yscale=3000, const vpColor &color=vpColor::black)
Definition: usGeometryDisplayTools.cpp:192
usGeometryTools::getNormalPlane
VISP_EXPORT usOrientedPlane3D getNormalPlane(const usPolynomialCurve3D &p, double l)
Definition: usGeometryTools.cpp:291
usNeedleModelingDisplayTools::displayTip< vpRGBa >
template VISP_EXPORT void displayTip< vpRGBa >(const usNeedleModelBaseTip &, const vpImage< vpRGBa > &, const vpHomogeneousMatrix &, double, double)
usNeedleModelingDisplayTools::display< unsigned char >
template VISP_EXPORT void display< unsigned char >(const usNeedleModelPolynomial &, const vpImage< unsigned char > &, const vpHomogeneousMatrix &, double, double, bool)
usNeedleModelingDisplayTools::displayBaseStaticTorsor< vpRGBa >
template VISP_EXPORT void displayBaseStaticTorsor< vpRGBa >(const usNeedleModelPolynomial &, const vpImage< vpRGBa > &, const vpHomogeneousMatrix &, double, double)
usNeedleModelingDisplayTools::displayBase< vpRGBa >
template VISP_EXPORT void displayBase< vpRGBa >(const usNeedleModelBaseTip &, const vpImage< vpRGBa > &, const vpHomogeneousMatrix &, double, double)
usNeedleModelingDisplayTools::displayTip
VISP_EXPORT void displayTip(const usNeedleModelBaseTip &needleModel, const vpImage< ImageDataType > &I, const vpHomogeneousMatrix &imageMworld, double Xscale=3000, double Yscale=3000)
Definition: usNeedleModelingDisplayTools.cpp:58
usNeedleModelingDisplayTools::displayBase
VISP_EXPORT void displayBase(const usNeedleModelBaseTip &needleModel, const vpImage< ImageDataType > &I, const vpHomogeneousMatrix &imageMworld, double Xscale=3000, double Yscale=3000)
Display usNeedleModelBaseTip.
Definition: usNeedleModelingDisplayTools.cpp:47
usNeedleModelingDisplayTools::displayBaseStaticTorsor< unsigned char >
template VISP_EXPORT void displayBaseStaticTorsor< unsigned char >(const usNeedleModelPolynomial &, const vpImage< unsigned char > &, const vpHomogeneousMatrix &, double, double)
usNeedleModelingDisplayTools::displayInteraction< vpRGBa >
template VISP_EXPORT void displayInteraction< vpRGBa >(const usNeedleInsertionModelRayleighRitzSpline &, const vpImage< vpRGBa > &, const vpHomogeneousMatrix &, double, double)
usNeedleModelingDisplayTools::displayCurvatureFromShape
VISP_EXPORT void displayCurvatureFromShape(const usNeedleModelPolynomial &needleModel, const vpImage< ImageDataType > &I, const vpHomogeneousMatrix &imageMworld, double Xscale=3000, double Yscale=3000)
Definition: usNeedleModelingDisplayTools.cpp:155
usNeedleModelingDisplayTools::displayTip< unsigned char >
template VISP_EXPORT void displayTip< unsigned char >(const usNeedleModelBaseTip &, const vpImage< unsigned char > &, const vpHomogeneousMatrix &, double, double)
usNeedleModelingDisplayTools::displayTissueLayers< unsigned char >
template VISP_EXPORT void displayTissueLayers< unsigned char >(const usNeedleInsertionModelRayleighRitzSpline &, const vpImage< unsigned char > &, const vpHomogeneousMatrix &, double, double)
usNeedleModelingDisplayTools::displayInteraction< unsigned char >
template VISP_EXPORT void displayInteraction< unsigned char >(const usNeedleInsertionModelRayleighRitzSpline &, const vpImage< unsigned char > &, const vpHomogeneousMatrix &, double, double)
usNeedleModelingDisplayTools::displayCurvatureFromShape< unsigned char >
template VISP_EXPORT void displayCurvatureFromShape< unsigned char >(const usNeedleModelPolynomial &, const vpImage< unsigned char > &, const vpHomogeneousMatrix &, double, double)
usNeedleModelingDisplayTools::displayCurvatureFromShape< vpRGBa >
template VISP_EXPORT void displayCurvatureFromShape< vpRGBa >(const usNeedleModelPolynomial &, const vpImage< vpRGBa > &, const vpHomogeneousMatrix &, double, double)
usNeedleModelingDisplayTools::displayInteraction
VISP_EXPORT void displayInteraction(const usNeedleInsertionModelRayleighRitzSpline &model, const vpImage< ImageDataType > &I, const vpHomogeneousMatrix &imageMworld, double Xscale=3000, double Yscale=3000)
Definition: usNeedleModelingDisplayTools.cpp:555
usNeedleModelingDisplayTools::display< vpRGBa >
template VISP_EXPORT void display< vpRGBa >(const usNeedleModelPolynomial &, const vpImage< vpRGBa > &, const vpHomogeneousMatrix &, double, double, bool)
usNeedleModelingDisplayTools::displayNeedle< vpRGBa >
template VISP_EXPORT void displayNeedle< vpRGBa >(const usNeedleModelPolynomial &, const vpImage< vpRGBa > &, const vpHomogeneousMatrix &, double, double, bool)
usNeedleModelingDisplayTools::displayBaseStaticTorsor
VISP_EXPORT void displayBaseStaticTorsor(const usNeedleModelPolynomial &needleModel, const vpImage< ImageDataType > &I, const vpHomogeneousMatrix &imageMworld, double Xscale=3000, double Yscale=3000)
Definition: usNeedleModelingDisplayTools.cpp:122
usNeedleModelingDisplayTools::displayNeedle
VISP_EXPORT void displayNeedle(const usNeedleModelPolynomial &needleModel, const vpImage< ImageDataType > &I, const vpHomogeneousMatrix &imageMworld, double Xscale=3000, double Yscale=3000, bool displayFullBody=false)
Display usNeedleModelPolynomial.
Definition: usNeedleModelingDisplayTools.cpp:83
usNeedleModelingDisplayTools::displayTissueLayers< vpRGBa >
template VISP_EXPORT void displayTissueLayers< vpRGBa >(const usNeedleInsertionModelRayleighRitzSpline &, const vpImage< vpRGBa > &, const vpHomogeneousMatrix &, double, double)
usNeedleModelingDisplayTools::displayNeedle< unsigned char >
template VISP_EXPORT void displayNeedle< unsigned char >(const usNeedleModelPolynomial &, const vpImage< unsigned char > &, const vpHomogeneousMatrix &, double, double, bool)
usNeedleModelingDisplayTools::displayBase< unsigned char >
template VISP_EXPORT void displayBase< unsigned char >(const usNeedleModelBaseTip &, const vpImage< unsigned char > &, const vpHomogeneousMatrix &, double, double)
usNeedleModelingDisplayTools::displayTissueLayers
void displayTissueLayers(const usNeedleInsertionModelRayleighRitzSpline &model, const vpImage< ImageDataType > &I, const vpHomogeneousMatrix &imageMworld, double Xscale, double Yscale)
Display usNeedleInsertionModelRayleighRitzSpline.
Definition: usNeedleModelingDisplayTools.cpp:507
usNeedleModelingDisplayTools::display
VISP_EXPORT void display(const usNeedleModelBaseTip &needleModel, vpImage< unsigned char > &I, const vpHomogeneousMatrix &imageMworld, double Xscale=3000, double Yscale=3000)