Visual Servoing Platform  version 3.6.1 under development (2024-12-13)
vpPlotGraph.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
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 ViSP 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  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Define a graph for the vpPlot class.
32  *
33 *****************************************************************************/
34 #define FLUSH_ON_PLOT
35 
36 #include <visp3/core/vpConfig.h>
37 #ifndef DOXYGEN_SHOULD_SKIP_THIS
38 
39 #include <visp3/core/vpMath.h>
40 #include <visp3/core/vpMeterPixelConversion.h>
41 #include <visp3/core/vpPixelMeterConversion.h>
42 #include <visp3/gui/vpPlotGraph.h>
43 
44 #include <visp3/gui/vpDisplayD3D.h>
45 #include <visp3/gui/vpDisplayGDI.h>
46 #include <visp3/gui/vpDisplayGTK.h>
47 #include <visp3/gui/vpDisplayOpenCV.h>
48 #include <visp3/gui/vpDisplayX.h>
49 
50 #include <cmath> // std::fabs
51 #include <limits> // numeric_limits
52 #include <sstream>
53 
54 #if defined(VISP_HAVE_DISPLAY)
55 
56 BEGIN_VISP_NAMESPACE
57 
58 int laFonctionSansNom(double delta);
59 void getGrid3DPoint(double pente, vpImagePoint &iPunit, vpImagePoint &ip1, vpImagePoint &ip2, vpImagePoint &ip3);
60 
61 vpPlotGraph::vpPlotGraph()
62  : xorg(0.), yorg(0.), zoomx(1.), zoomy(1.), xmax(10), ymax(10), xmin(0), ymin(-10), xdelt(1), ydelt(1), gridx(true),
63  gridy(true), gridColor(), curveNbr(1), curveList(nullptr), scaleInitialized(false), firstPoint(true), nbDivisionx(10),
64  nbDivisiony(10), topLeft(), width(0), height(0), graphZone(), dTopLeft(), dWidth(0), dHeight(0), dGraphZone(),
65  dTopLeft3D(), dGraphZone3D(), cam(), cMo(), cMf(), w_xval(0), w_xsize(0), w_yval(0), w_ysize(0), w_zval(0),
66  w_zsize(0), ptXorg(0), ptYorg(0), ptZorg(0), zoomx_3D(1.), zoomy_3D(1.), zoomz_3D(1.), nbDivisionz(10), zorg(1.),
67  zoomz(1.), zmax(10), zmin(-10), zdelt(1), old_iPr(), old_iPz(), blockedr(false), blockedz(false), blocked(false),
68  epsi(5), epsj(6), dispUnit(false), dispTitle(false), dispLegend(false), gridThickness(1)
69 {
70  gridColor.setColor(200, 200, 200);
71 
72  old_iPr = vpImagePoint(-1, -1);
73  old_iPz = vpImagePoint(-1, -1);
74 
75  gridThickness = 1;
76 }
77 
78 vpPlotGraph::~vpPlotGraph()
79 {
80  if (curveList != nullptr) {
81  delete[] curveList;
82  curveList = nullptr;
83  }
84 }
85 
86 void vpPlotGraph::initGraph(unsigned int nbCurve)
87 {
88  curveList = new vpPlotCurve[nbCurve];
89  curveNbr = nbCurve;
90 
92 
93  for (unsigned int i = 0; i < curveNbr; ++i) {
94  (curveList + i)->color = colors[i % 6];
95  (curveList + i)->curveStyle = vpPlotCurve::line;
96  (curveList + i)->pointListx.clear();
97  (curveList + i)->pointListy.clear();
98  (curveList + i)->legend.clear();
99  }
100 }
101 
102 void vpPlotGraph::initSize(vpImagePoint top_left, unsigned int w, unsigned int h, unsigned int margei,
103  unsigned int margej)
104 {
105  this->topLeft = top_left;
106  this->width = w;
107  this->height = h;
108  graphZone.setTopLeft(topLeft);
109  graphZone.setWidth(width);
110  graphZone.setHeight(height);
111 
112  this->dTopLeft = vpImagePoint(topLeft.get_i() + margei, topLeft.get_j() + margej);
113  this->dWidth = width - margej - 10;
114  this->dHeight = height - 2 * margei;
115  dGraphZone.setTopLeft(dTopLeft);
116  dGraphZone.setWidth(dWidth + 1);
117  dGraphZone.setHeight(dHeight + 1);
118 
119  this->dTopLeft3D = vpImagePoint(topLeft.get_i() + margei, topLeft.get_j() + 10);
120  dGraphZone3D.setTopLeft(dTopLeft3D);
121  dGraphZone3D.setWidth(dWidth + 1);
122  dGraphZone3D.setHeight(dHeight + 1);
123 
124  if (this->dWidth > this->dHeight) {
125  w_ysize = 1.0;
126  w_xsize = this->dWidth / this->dHeight;
127  w_zsize = w_xsize;
128 
129  w_yval = w_ysize / 2.0;
130  w_xval = w_xsize / 2.0;
131  w_zval = w_zsize / 2.0;
132  }
133  else if (this->dWidth == this->dHeight) {
134  w_ysize = 1.0;
135  w_xsize = 1.0;
136  w_zsize = 1.0;
137 
138  w_yval = 0.5;
139  w_xval = 0.5;
140  w_zval = 0.5;
141  }
142  else if (this->dWidth < this->dHeight) {
143  w_xsize = 1.0;
144  w_ysize = this->dHeight / this->dWidth;
145  w_zsize = w_ysize;
146 
147  w_yval = w_ysize / 2.0;
148  w_xval = w_xsize / 2.0;
149  w_zval = w_zsize / 2.0;
150  }
151 
152  cam.initPersProjWithoutDistortion(1000, 1000, this->dWidth / 2.0, this->dHeight / 2.0);
153 
154  findPose();
155 
156  cMf.buildFrom(0, 0, cMo[2][3], 0, 0, 0);
157 }
158 
159 void vpPlotGraph::findPose()
160 {
161  vpPoint point_[4];
162  point_[0].setWorldCoordinates(-w_xval, -w_yval, -w_zval);
163  point_[1].setWorldCoordinates(w_xval, -w_yval, -w_zval);
164  point_[2].setWorldCoordinates(w_xval, w_yval, -w_zval);
165  point_[3].setWorldCoordinates(-w_xval, w_yval, -w_zval);
166 
167  vpImagePoint iP[4];
168  iP[0].set_ij(0, 0);
169  iP[1].set_ij(0, dWidth - 1);
170  iP[2].set_ij(dHeight - 1, dWidth - 1);
171  iP[3].set_ij(dHeight - 1, 0);
172 
173  double x = 0, y = 0;
174 
175  // Instead of pose computation we use an approximation
176  double Z = 0;
177  const unsigned int val_4 = 4;
178  for (unsigned int i = 0; i < val_4; ++i) {
179  vpPixelMeterConversion::convertPoint(cam, iP[i], x, y);
180  Z = vpMath::maximum(Z, point_[i].get_oX() / x);
181  Z = vpMath::maximum(Z, point_[i].get_oY() / y);
182  }
183  cMo[2][3] = Z;
184 }
185 
186 void vpPlotGraph::computeGraphParameters()
187 {
188  zoomx = dWidth / (xmax - xmin);
189  zoomy = dHeight / (ymax - ymin);
190  xorg = dTopLeft.get_j() - (xmin * zoomx);
191  yorg = dTopLeft.get_i() + (ymax * zoomy);
192 }
193 
194 void vpPlotGraph::setCurveColor(unsigned int curveNum, const vpColor &color) { (curveList + curveNum)->color = color; }
195 
196 void vpPlotGraph::setTitle(const std::string &title_)
197 {
198  title = title_;
199  dispTitle = true;
200 }
201 
202 void vpPlotGraph::setUnitX(const std::string &unit_x)
203 {
204  unitx = unit_x;
205  dispUnit = true;
206 }
207 
208 void vpPlotGraph::setUnitY(const std::string &unit_y)
209 {
210  unity = unit_y;
211  dispUnit = true;
212 }
213 
214 void vpPlotGraph::setUnitZ(const std::string &unit_z)
215 {
216  unitz = unit_z;
217  dispUnit = true;
218 }
219 
220 void vpPlotGraph::setLegend(unsigned int curveNum, const std::string &newlegend)
221 {
222  (curveList + curveNum)->legend = newlegend;
223  dispLegend = true;
224 }
225 
226 void vpPlotGraph::setCurveThickness(unsigned int curveNum, unsigned int thickness)
227 {
228  (curveList + curveNum)->thickness = thickness;
229 }
230 
231 int laFonctionSansNom(double delta)
232 {
233  double d = delta;
234  int power = 0;
235  if (d < 1) {
236  while (d < 1) {
237  d = d * 10;
238  ++power;
239  }
240  --power;
241  return power;
242  }
243 
244  if (d >= 10) {
245  while (d > 10) {
246  d = d / 10;
247  --power;
248  }
249  --power;
250  return power;
251  }
252 
253  return 0;
254 }
255 
256 void vpPlotGraph::displayGrid(vpImage<unsigned char> &I)
257 {
258  computeGraphParameters();
259 
260  xdelt = (xmax - xmin) / nbDivisionx;
261  ydelt = (ymax - ymin) / nbDivisiony;
262 
263  double t;
264  int power;
265 
266  power = laFonctionSansNom(xdelt);
267  for (t = xmin; t <= xmax; t = t + xdelt) {
268  double x = xorg + (zoomx * t);
269  if (gridy)
270  vpDisplay::displayDotLine(I, vpImagePoint(dTopLeft.get_i(), x), vpImagePoint(dTopLeft.get_i() + dHeight, x),
271  gridColor, gridThickness);
272  else
273  vpDisplay::displayDotLine(I, vpImagePoint(yorg, x), vpImagePoint(yorg - 3, x), vpColor::black, gridThickness);
274 
275  if (t + xdelt <= xmax + 1e-10) {
276  double ttemp;
277  if (power != 0)
278  ttemp = t * pow(10.0, power);
279  else
280  ttemp = t;
281  std::stringstream valeur;
282  valeur.precision(3);
283  valeur << ttemp;
284 #if defined(VISP_HAVE_X11)
285  vpDisplay::displayText(I, vpImagePoint(yorg + 3 * epsi, x), valeur.str(), vpColor::black);
286 #elif defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
287  vpDisplay::displayText(I, vpImagePoint(yorg + epsi, x), valeur.str(), vpColor::black);
288 #endif
289  }
290  }
291  if (power != 0) {
292  std::stringstream ss;
293  ss << "x10e";
294  ss << -power;
295 #if defined(VISP_HAVE_X11)
296  vpDisplay::displayText(I, vpImagePoint(yorg + 4 * epsi, dTopLeft.get_j() + dWidth - 6 * epsj), ss.str(),
298 #elif defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
299  vpDisplay::displayText(I, vpImagePoint(yorg + 4 * epsi, dTopLeft.get_j() + dWidth - 10 * epsj), ss.str(),
301 #endif
302  }
303 
304  power = laFonctionSansNom(ydelt);
305  for (t = ymin; t <= ymax; t = t + ydelt) {
306  double y = yorg - (zoomy * t);
307  if (gridx)
308  vpDisplay::displayDotLine(I, vpImagePoint(y, dTopLeft.get_j()), vpImagePoint(y, dTopLeft.get_j() + dWidth),
309  gridColor, gridThickness);
310  else
311  vpDisplay::displayDotLine(I, vpImagePoint(y, xorg), vpImagePoint(y, xorg + 3), vpColor::black, gridThickness);
312 
313  double ttemp;
314  if (power != 0)
315  ttemp = t * pow(10.0, power);
316  else
317  ttemp = t;
318 
319  std::stringstream valeur;
320  valeur.precision(3);
321  valeur << ttemp;
322 #if defined(VISP_HAVE_X11)
323  vpDisplay::displayText(I, vpImagePoint(y + epsi, topLeft.get_j() + epsj), valeur.str(), vpColor::black);
324 #elif defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
325  vpDisplay::displayText(I, vpImagePoint(y - epsi, topLeft.get_j() + epsj), valeur.str(), vpColor::black);
326 #endif
327  }
328  if (power != 0) {
329  std::stringstream ss;
330  ss << "x10e";
331  ss << -power;
332 #if defined(VISP_HAVE_X11)
333  vpDisplay::displayText(I, vpImagePoint(dTopLeft.get_i() - 3 * epsi, dTopLeft.get_j() - 6 * epsj), ss.str(),
335 #elif defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
336  vpDisplay::displayText(I, vpImagePoint(dTopLeft.get_i() - 3 * epsi, dTopLeft.get_j() - 6 * epsj), ss.str(),
338 #endif
339  }
340 
341  // Ligne horizontal
342  vpDisplay::displayArrow(I, vpImagePoint(yorg, dTopLeft.get_j()), vpImagePoint(yorg, dTopLeft.get_j() + dWidth),
343  vpColor::black, 4 * gridThickness, 2 * gridThickness, gridThickness);
344  // Ligne verticale
345  vpDisplay::displayArrow(I, vpImagePoint(dTopLeft.get_i() + dHeight, xorg), vpImagePoint(dTopLeft.get_i(), xorg),
346  vpColor::black, 4 * gridThickness, 2 * gridThickness, gridThickness);
347 
348  if (dispUnit)
349  displayUnit(I);
350  if (dispTitle)
351  displayTitle(I);
352  if (dispLegend)
353  displayLegend(I);
354 
355  // vpDisplay::flushROI(I,graphZone);
356 }
357 
358 void vpPlotGraph::displayUnit(vpImage<unsigned char> &
359 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || \
360  defined(VISP_HAVE_GTK)
361  I
362 #endif
363 )
364 {
365  unsigned int offsetx = vpMath::minimum<unsigned int>((unsigned int)unitx.size(), dWidth);
366 
367 #if defined(VISP_HAVE_X11)
368  vpDisplay::displayText(I, vpImagePoint(yorg - 2 * epsi, dTopLeft.get_j() + dWidth - offsetx * epsj), unitx.c_str(),
370  vpDisplay::displayText(I, vpImagePoint(dTopLeft.get_i(), dTopLeft.get_j() + epsj), unity.c_str(), vpColor::black);
371 #elif defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
372  vpDisplay::displayText(I, vpImagePoint(yorg - 5 * epsi, dTopLeft.get_j() + dWidth - offsetx * epsj), unitx.c_str(),
374  vpDisplay::displayText(I, vpImagePoint(dTopLeft.get_i(), dTopLeft.get_j() + epsj), unity.c_str(), vpColor::black);
375 #endif
376 }
377 
378 void vpPlotGraph::displayTitle(vpImage<unsigned char> &I)
379 {
380  double size = (double)title.size();
381  size = size / 2.0;
382  vpDisplay::displayText(I, vpImagePoint(dTopLeft.get_i() - 3 * epsi, dTopLeft.get_j() + dWidth / 2.0 - 4 * size),
383  title, vpColor::black);
384 }
385 
386 void vpPlotGraph::displayLegend(vpImage<unsigned char> &I)
387 {
388  size_t offsetj = 0;
389  for (unsigned int i = 0; i < curveNbr; ++i) {
390  size_t offset = epsj * (curveList + i)->legend.size();
391  offsetj = vpMath::maximum(offset, offsetj);
392  }
393  if (offsetj > dWidth)
394  offsetj = dWidth;
395 
396  for (unsigned int i = 0; i < curveNbr; ++i) {
397  vpDisplay::displayText(I, vpImagePoint(dTopLeft.get_i() + i * 5 * epsi, dTopLeft.get_j() + dWidth - offsetj),
398  (curveList + i)->legend.c_str(), (curveList + i)->color);
399  }
400 }
401 
402 void vpPlotGraph::rescalex(unsigned int side, double extremity)
403 {
404  switch (side) {
405  case 0:
406  xmin = (3 * extremity - xmax) / 2;
407  break;
408  case 1:
409  xmax = (3 * extremity - xmin) / 2;
410  break;
411  }
412 
413  xdelt = (xmax - xmin) / (double)nbDivisionx;
414 }
415 
416 void vpPlotGraph::rescaley(unsigned int side, double extremity)
417 {
418  switch (side) {
419  case 0:
420  ymin = (3 * extremity - ymax) / 2;
421  break;
422  case 1:
423  ymax = (3 * extremity - ymin) / 2;
424  break;
425  }
426 
427  ydelt = (ymax - ymin) / (double)nbDivisiony;
428 }
429 
430 void vpPlotGraph::initScale(vpImage<unsigned char> &I, double x_min, double x_max, int nbDivx, double y_min,
431  double y_max, int nbDivy, bool gx, bool gy)
432 {
433  this->xmin = x_min;
434  this->xmax = x_max;
435  this->ymin = y_min;
436  this->ymax = y_max;
437  this->gridx = gx;
438  this->gridy = gy;
439  this->nbDivisionx = nbDivx;
440  this->nbDivisiony = nbDivy;
441  computeGraphParameters();
442  clearGraphZone(I);
443  displayGrid(I);
444  vpDisplay::flushROI(I, graphZone);
445  scaleInitialized = true;
446 }
447 
448 void vpPlotGraph::initScale(vpImage<unsigned char> &I, double x_min, double x_max, int nbDivx, double y_min,
449  double y_max, int nbDivy, double z_min, double z_max, int nbDivz, bool gx, bool gy)
450 {
451  this->xmin = x_min;
452  this->xmax = x_max;
453  this->ymin = y_min;
454  this->ymax = y_max;
455  this->zmin = z_min;
456  this->zmax = z_max;
457  this->gridx = gx;
458  this->gridy = gy;
459  this->nbDivisionx = nbDivx;
460  this->nbDivisiony = nbDivy;
461  this->nbDivisionz = nbDivz;
462  computeGraphParameters();
463  clearGraphZone(I);
464  displayGrid(I);
465  vpDisplay::flushROI(I, graphZone);
466  scaleInitialized = true;
467 }
468 
469 void vpPlotGraph::plot(vpImage<unsigned char> &I, unsigned int curveNb, double x, double y)
470 {
471  if (!scaleInitialized) {
472  if (x < 0) {
473  xmax = 0;
474  rescalex(0, x);
475  }
476  if (x > 0) {
477  xmin = 0;
478  rescalex(1, x);
479  }
480  if (y < 0) {
481  ymax = 0;
482  rescaley(0, y);
483  }
484  if (y > 0) {
485  ymin = 0;
486  rescaley(1, y);
487  }
488  scaleInitialized = true;
489  computeGraphParameters();
490  clearGraphZone(I);
491  displayGrid(I);
492  // if (y == 0)
493  if (std::fabs(y) <= std::numeric_limits<double>::epsilon())
494  scaleInitialized = false;
495  }
496 
497  if (firstPoint) {
498  // clearGraphZone(I);
499  // displayGrid(I);
500  // vpDisplay::flushROI(I,graphZone);
501  replot(I);
502  firstPoint = false;
503  }
504 
505  double i = yorg - (zoomy * y);
506  double j = xorg + (zoomx * x);
507 
508  vpImagePoint iP(i, j);
509 
510  if (!iP.inRectangle(dGraphZone)) {
511  if (x > xmax)
512  rescalex(1, x);
513  else if (x < xmin)
514  rescalex(0, x);
515 
516  if (y > ymax)
517  rescaley(1, y);
518  else if (y < ymin)
519  rescaley(0, y);
520 
521  computeGraphParameters();
522 
523  replot(I);
524  i = yorg - (zoomy * y);
525  j = xorg + (zoomx * x);
526 
527  iP.set_ij(i, j);
528  }
529 
530  (curveList + curveNb)->plotPoint(I, iP, x, y);
531 #if (!defined VISP_HAVE_X11 && defined FLUSH_ON_PLOT)
532  vpDisplay::flushROI(I, graphZone);
533  // vpDisplay::flush(I);
534 #endif
535 }
536 
537 void vpPlotGraph::replot(vpImage<unsigned char> &I)
538 {
539  clearGraphZone(I);
540  displayGrid(I);
541  for (unsigned int i = 0; i < curveNbr; ++i)
542  (curveList + i)->plotList(I, xorg, yorg, zoomx, zoomy);
543  vpDisplay::flushROI(I, graphZone);
544 }
545 
546 void vpPlotGraph::clearGraphZone(vpImage<unsigned char> &I) { vpDisplay::displayROI(I, graphZone); }
547 
548 bool vpPlotGraph::getPixelValue(vpImage<unsigned char> &I, vpImagePoint &iP)
549 {
550  if (iP.inRectangle(dGraphZone)) {
551  double x = (iP.get_j() - xorg) / zoomx;
552  double y = (yorg - iP.get_i()) / zoomy;
553 
554  vpDisplay::displayROI(I, vpRect(vpImagePoint(topLeft.get_i() + height - 20, topLeft.get_j()), width - 1, 19));
555  std::stringstream ss_x;
556  ss_x << " x: ";
557  ss_x << x;
558  vpDisplay::displayText(I, vpImagePoint(topLeft.get_i() + height - 2, topLeft.get_j() + 5 * epsj), ss_x.str(),
560  std::stringstream ss_y;
561  ss_y << " y: ";
562  ss_y << y;
563  vpDisplay::displayText(I, vpImagePoint(topLeft.get_i() + height - 2, topLeft.get_j() + width / 2.0), ss_x.str(),
565  // vpDisplay::flush(I);
566  vpDisplay::flushROI(I, vpRect(vpImagePoint(topLeft.get_i() + height - 20, topLeft.get_j()), width - 1, 19));
567  return true;
568  }
569  return false;
570 }
571 
572 void vpPlotGraph::resetPointList(unsigned int curveNum)
573 {
574  (curveList + curveNum)->pointListx.clear();
575  (curveList + curveNum)->pointListy.clear();
576  (curveList + curveNum)->pointListz.clear();
577  (curveList + curveNum)->nbPoint = 0;
578  firstPoint = true;
579 }
580 
581 /************************************************************************************************/
582 
583 bool vpPlotGraph::check3Dline(vpImagePoint &iP1, vpImagePoint &iP2)
584 {
585  bool iP1In = iP1.inRectangle(dGraphZone3D);
586  bool iP2In = iP2.inRectangle(dGraphZone3D);
587 
588  if (!iP1In || !iP2In) {
589  double dTopLeft_i = dTopLeft3D.get_i();
590  double dTopLeft_j = dTopLeft3D.get_j();
591  double dBottomRight_i = dTopLeft_i + dHeight;
592  double dBottomRight_j = dTopLeft_j + dWidth;
593 
594  // Cas vertical
595  if (vpImagePoint::distance(iP1, iP2) < 9)
596  return false;
597  if (fabs(iP2.get_j() - iP1.get_j()) <= 2) {
598  if (!iP1In && !iP2In) {
599  if (iP1.get_i() < dTopLeft_i && iP2.get_i() < dTopLeft_i)
600  return false;
601  if (iP1.get_i() > dBottomRight_i && iP2.get_i() > dBottomRight_i)
602  return false;
603  if (iP1.get_j() < dTopLeft_j || iP1.get_j() > dBottomRight_j)
604  return false;
605  if (iP1.get_i() < dTopLeft_i)
606  iP1.set_i(dTopLeft_i);
607  else
608  iP1.set_i(dBottomRight_i);
609  if (iP2.get_i() < dTopLeft_i)
610  iP2.set_i(dTopLeft_i);
611  else
612  iP2.set_i(dBottomRight_i);
613  }
614  else if (!iP1In) {
615  if (iP1.get_j() < dTopLeft_j)
616  iP1.set_j(dTopLeft_j);
617  if (iP1.get_j() > dBottomRight_j)
618  iP1.set_j(dBottomRight_j);
619  if (iP1.get_i() < dTopLeft_i)
620  iP1.set_i(dTopLeft_i);
621  if (iP1.get_i() > dBottomRight_i)
622  iP1.set_i(dBottomRight_i);
623  return true;
624  }
625  else if (!iP2In) {
626  if (iP2.get_j() < dTopLeft_j)
627  iP2.set_j(dTopLeft_j);
628  if (iP2.get_j() > dBottomRight_j)
629  iP2.set_j(dBottomRight_j);
630  if (iP2.get_i() < dTopLeft_i)
631  iP2.set_i(dTopLeft_i);
632  if (iP2.get_i() > dBottomRight_i)
633  iP2.set_i(dBottomRight_i);
634  return true;
635  }
636  }
637  // cas horizontal
638  else if (fabs(iP2.get_i() - iP1.get_i()) <= 2) {
639  if (!iP1In && !iP2In) {
640  if (iP1.get_j() < dTopLeft_j && iP2.get_j() < dTopLeft_j)
641  return false;
642  if (iP1.get_j() > dBottomRight_j && iP2.get_j() > dBottomRight_j)
643  return false;
644  if (iP1.get_i() < dTopLeft_i || iP1.get_i() > dBottomRight_i)
645  return false;
646  if (iP1.get_j() < dTopLeft_j)
647  iP1.set_j(dTopLeft_j);
648  else
649  iP1.set_j(dBottomRight_j);
650  if (iP2.get_j() < dTopLeft_j)
651  iP2.set_j(dTopLeft_j);
652  else
653  iP2.set_j(dBottomRight_j);
654  }
655  else if (!iP1In) {
656  if (iP1.get_j() < dTopLeft_j)
657  iP1.set_j(dTopLeft_j);
658  if (iP1.get_j() > dBottomRight_j)
659  iP1.set_j(dBottomRight_j);
660  if (iP1.get_i() < dTopLeft_i)
661  iP1.set_i(dTopLeft_i);
662  if (iP1.get_i() > dBottomRight_i)
663  iP1.set_i(dBottomRight_i);
664  return true;
665  }
666  else if (!iP2In) {
667  if (iP2.get_j() < dTopLeft_j)
668  iP2.set_j(dTopLeft_j);
669  if (iP2.get_j() > dBottomRight_j)
670  iP2.set_j(dBottomRight_j);
671  if (iP2.get_i() < dTopLeft_i)
672  iP2.set_i(dTopLeft_i);
673  if (iP2.get_i() > dBottomRight_i)
674  iP2.set_i(dBottomRight_i);
675  return true;
676  }
677  }
678 
679  double a = (iP2.get_i() - iP1.get_i()) / (iP2.get_j() - iP1.get_j());
680  double b = iP1.get_i() - a * iP1.get_j();
681 
682  // test horizontal
683  double jtop = (dTopLeft_i - b) / a;
684  double jlow = (dBottomRight_i - b) / a;
685  // test vertical
686  double ileft = dTopLeft_j * a + b;
687  double iright = (dBottomRight_j)*a + b;
688 
689  vpImagePoint iP[2];
690  int n = 0;
691 
692  if (jtop >= dTopLeft_j && jtop <= dBottomRight_j) {
693  iP[n].set_ij(dTopLeft_i, jtop);
694  n++;
695  }
696  if (jlow >= dTopLeft_j && jlow <= dBottomRight_j) {
697  iP[n].set_ij(dBottomRight_i, jlow);
698  n++;
699  }
700  if (ileft >= dTopLeft_i && ileft <= dBottomRight_i && n < 2) {
701  iP[n].set_ij(ileft, dTopLeft_j);
702  n++;
703  }
704  if (iright >= dTopLeft_i && iright <= dBottomRight_i && n < 2) {
705  iP[n].set_ij(iright, dBottomRight_j);
706  n++;
707  }
708 
709  if (n < 2)
710  return false;
711 
712  if (!iP1In && !iP2In) {
713  if (fabs(a) < 1) {
714  if (vpMath::sign(iP1.get_j() - iP[0].get_j()) == vpMath::sign(iP2.get_j() - iP[0].get_j()))
715  return false;
716  int sign = vpMath::sign(iP1.get_j() - iP2.get_j());
717  if (sign == vpMath::sign(iP[0].get_j() - iP[1].get_j())) {
718  iP1 = iP[0];
719  iP2 = iP[1];
720  }
721  else {
722  iP1 = iP[1];
723  iP2 = iP[0];
724  }
725  }
726  else {
727  if (vpMath::sign(iP1.get_i() - iP[0].get_i()) == vpMath::sign(iP2.get_i() - iP[0].get_i()))
728  return false;
729  int sign = vpMath::sign(iP1.get_i() - iP2.get_i());
730  if (sign == vpMath::sign(iP[0].get_i() - iP[1].get_i())) {
731  iP1 = iP[0];
732  iP2 = iP[1];
733  }
734  else {
735  iP1 = iP[1];
736  iP2 = iP[0];
737  }
738  }
739  }
740  else if (!iP1In) {
741  vpImagePoint iPtemp = iP1;
742  if (fabs(a) < 1) {
743  int sign = vpMath::sign(iP1.get_j() - iP2.get_j());
744  if (fabs(iP[0].get_j() - iP2.get_j()) > 5) {
745  if (sign == vpMath::sign(iP[0].get_j() - iP2.get_j()))
746  iP1 = iP[0];
747  else
748  iP1 = iP[1];
749  }
750  else {
751  if (sign == vpMath::sign(iP[1].get_j() - iP2.get_j()))
752  iP1 = iP[1];
753  else
754  iP1 = iP[0];
755  }
756  }
757  else {
758  int sign = vpMath::sign(iP1.get_i() - iP2.get_i());
759  if (fabs(iP[0].get_i() - iP2.get_i()) > 5) {
760  if (sign == vpMath::sign(iP[0].get_i() - iP2.get_i()))
761  iP1 = iP[0];
762  else
763  iP1 = iP[1];
764  }
765  else {
766  if (sign == vpMath::sign(iP[1].get_i() - iP2.get_i()))
767  iP1 = iP[1];
768  else
769  iP1 = iP[0];
770  }
771  }
772  if (vpImagePoint::distance(iP1, iP2) < 9) {
773  iP1 = iPtemp;
774  return false;
775  }
776  }
777  else if (!iP2In) {
778  vpImagePoint iPtemp = iP2;
779  if (fabs(a) < 1) {
780  int sign = vpMath::sign(iP2.get_j() - iP1.get_j());
781  if (fabs(iP[0].get_j() - iP1.get_j()) > 5) {
782  if (sign == vpMath::sign(iP[0].get_j() - iP1.get_j()))
783  iP2 = iP[0];
784  else
785  iP2 = iP[1];
786  }
787  else {
788  if (sign == vpMath::sign(iP[1].get_j() - iP1.get_j()))
789  iP2 = iP[1];
790  else
791  iP2 = iP[0];
792  }
793  }
794  else {
795  int sign = vpMath::sign(iP2.get_i() - iP1.get_i());
796  if (fabs(iP[0].get_i() - iP1.get_i()) > 5) {
797  if (sign == vpMath::sign(iP[0].get_i() - iP1.get_i()))
798  iP2 = iP[0];
799  else
800  iP2 = iP[1];
801  }
802  else {
803  if (sign == vpMath::sign(iP[1].get_i() - iP1.get_i()))
804  iP2 = iP[1];
805  else
806  iP2 = iP[0];
807  }
808  }
809  if (vpImagePoint::distance(iP1, iP2) < 9) {
810  iP2 = iPtemp;
811  return false;
812  }
813  }
814  }
815  return true;
816 }
817 
818 bool vpPlotGraph::check3Dpoint(vpImagePoint &iP)
819 {
820  if (!iP.inRectangle(dGraphZone3D)) {
821  if (iP.get_i() < dTopLeft3D.get_i())
822  iP.set_i(dTopLeft3D.get_i());
823  else if (iP.get_i() > dTopLeft3D.get_i() + dHeight)
824  iP.set_i(dTopLeft3D.get_i() + dHeight - 1);
825  if (iP.get_j() < dTopLeft3D.get_j())
826  iP.set_j(dTopLeft3D.get_j());
827  else if (iP.get_j() > dTopLeft3D.get_j() + dWidth)
828  iP.set_j(dTopLeft3D.get_j() + dWidth - 1);
829  return false;
830  }
831  return true;
832 }
833 
834 void vpPlotGraph::computeGraphParameters3D()
835 {
836  zoomx_3D = w_xsize / (xmax - xmin);
837  zoomy_3D = w_ysize / (ymax - ymin);
838  zoomz_3D = w_zsize / (zmax - zmin);
839  ptXorg = w_xval - zoomx_3D * xmax;
840  ptYorg = w_yval + zoomy_3D * ymin;
841  ptZorg = w_zval - zoomz_3D * zmax;
842 }
843 
844 void getGrid3DPoint(double pente, vpImagePoint &iPunit, vpImagePoint &ip1, vpImagePoint &ip2, vpImagePoint &ip3)
845 {
846  if (pente <= 1) {
847  ip1 = iPunit - vpImagePoint(3, 0);
848  ip2 = iPunit + vpImagePoint(3, 0);
849  ip3 = iPunit - vpImagePoint(6, 6);
850  }
851  else {
852  ip1 = iPunit - vpImagePoint(0, 3);
853  ip2 = iPunit + vpImagePoint(0, 3);
854  ip3 = iPunit + vpImagePoint(6, 6);
855  }
856 }
857 
858 void vpPlotGraph::displayGrid3D(vpImage<unsigned char> &I)
859 {
860  const unsigned int nparam = 6;
861  computeGraphParameters3D();
862 
863  xdelt = (xmax - xmin) / nbDivisionx;
864  ydelt = (ymax - ymin) / nbDivisiony;
865  zdelt = (zmax - zmin) / nbDivisionz;
866 
867  vpPoint pt[nparam];
868  pt[0].setWorldCoordinates(-w_xval, ptYorg, ptZorg);
869  pt[1].setWorldCoordinates(w_xval, ptYorg, ptZorg);
870  pt[2].setWorldCoordinates(ptXorg, -w_yval, ptZorg);
871  pt[3].setWorldCoordinates(ptXorg, w_yval, ptZorg);
872  pt[4].setWorldCoordinates(ptXorg, ptYorg, -w_zval);
873  pt[5].setWorldCoordinates(ptXorg, ptYorg, w_zval);
874 
875  vpImagePoint iP[nparam];
876  for (unsigned int i = 0; i < nparam; ++i) {
877  pt[i].track(cMo);
878  double u = 0.0, v = 0.0;
879  vpMeterPixelConversion::convertPoint(cam, pt[i].get_x(), pt[i].get_y(), u, v);
880  iP[i].set_uv(u, v);
881  iP[i] = iP[i] + dTopLeft3D;
882  }
883 
884  int power;
885  double t;
886  vpPoint ptunit;
887  vpImagePoint iPunit;
888  double pente;
889  vpImagePoint ip1;
890  vpImagePoint ip2;
891  vpImagePoint ip3;
892  vpImagePoint ip4;
893 
894  power = laFonctionSansNom(xdelt);
895  ptunit.setWorldCoordinates(-w_xval, ptYorg, ptZorg);
896  // if (iP[0].get_j()-iP[1].get_j() != 0)
897  if (std::fabs(iP[0].get_j() - iP[1].get_j()) >
898  vpMath::maximum(std::fabs(iP[0].get_j()), std::fabs(iP[1].get_j())) * std::numeric_limits<double>::epsilon())
899  pente = fabs((iP[0].get_i() - iP[1].get_i()) / (iP[0].get_j() - iP[1].get_j()));
900  else
901  pente = 2;
902 
903  unsigned int count = 1;
904  for (t = xmin; t <= xmax; t = t + xdelt) {
905  double x = ptXorg + (zoomx_3D * t);
906  ptunit.set_oX(x);
907  ptunit.track(cMo);
908  double u = 0.0, v = 0.0;
909  vpMeterPixelConversion::convertPoint(cam, ptunit.get_x(), ptunit.get_y(), u, v);
910  iPunit.set_uv(u, v);
911  iPunit = iPunit + dTopLeft3D;
912 
913  getGrid3DPoint(pente, iPunit, ip1, ip2, ip3);
914 
915  if (check3Dline(ip1, ip2)) {
917  if (count % 2 == 1) {
918  double ttemp;
919  if (power != 0)
920  ttemp = t * pow(10.0, power);
921  else
922  ttemp = t;
923  std::stringstream ss;
924  ss.precision(2);
925  ss << ttemp;
926  vpDisplay::displayText(I, ip3, ss.str(), vpColor::black);
927  }
928  }
929  count++;
930  }
931  if (power != 0) {
932  ip4 = iP[1] - vpImagePoint(-15, 10);
933  std::stringstream ss;
934  ss << "x10e";
935  ss << -power;
936  if (check3Dpoint(ip4))
937  vpDisplay::displayText(I, ip4, ss.str(), vpColor::black);
938  }
939 
940  power = laFonctionSansNom(ydelt);
941  ptunit.setWorldCoordinates(ptXorg, -w_yval, ptZorg);
942  // if (iP[2].get_j()-iP[3].get_j() != 0)
943  if (std::fabs(iP[2].get_j() - iP[3].get_j()) >
944  vpMath::maximum(std::fabs(iP[2].get_j()), std::fabs(iP[3].get_j())) * std::numeric_limits<double>::epsilon())
945  pente = fabs((iP[2].get_i() - iP[3].get_i()) / (iP[2].get_j() - iP[3].get_j()));
946  else
947  pente = 2;
948  count = 0;
949  for (t = ymin; t <= ymax; t = t + ydelt) {
950  double y = ptYorg - (zoomy_3D * t);
951  ptunit.set_oY(y);
952  ptunit.track(cMo);
953  double u = 0.0, v = 0.0;
954  vpMeterPixelConversion::convertPoint(cam, ptunit.get_x(), ptunit.get_y(), u, v);
955  iPunit.set_uv(u, v);
956  iPunit = iPunit + dTopLeft3D;
957 
958  getGrid3DPoint(pente, iPunit, ip1, ip2, ip3);
959 
960  if (check3Dline(ip1, ip2)) {
962  if (count % 2 == 1) {
963  double ttemp;
964  if (power != 0)
965  ttemp = t * pow(10.0, power);
966  else
967  ttemp = t;
968  std::stringstream ss;
969  ss.precision(2);
970  ss << ttemp;
971  vpDisplay::displayText(I, ip3, ss.str(), vpColor::black);
972  }
973  }
974  count++;
975  }
976  if (power != 0) {
977  ip4 = iP[2] - vpImagePoint(-15, 10);
978  std::stringstream ss;
979  ss << "x10e";
980  ss << -power;
981  if (check3Dpoint(ip4))
982  vpDisplay::displayText(I, ip4, ss.str(), vpColor::black);
983  }
984 
985  power = laFonctionSansNom(zdelt);
986  ptunit.setWorldCoordinates(ptXorg, ptYorg, -w_zval);
987  // if (iP[4].get_j()-iP[5].get_j() != 0)
988  if (std::fabs(iP[4].get_j() - iP[5].get_j()) >
989  vpMath::maximum(std::fabs(iP[4].get_j()), std::fabs(iP[5].get_j())) * std::numeric_limits<double>::epsilon())
990  pente = fabs((iP[4].get_i() - iP[5].get_i()) / (iP[4].get_j() - iP[5].get_j()));
991  else
992  pente = 2;
993  count = 0;
994  for (t = zmin; t <= zmax; t = t + zdelt) {
995  double z = ptZorg + (zoomz_3D * t);
996  ptunit.set_oZ(z);
997  ptunit.track(cMo);
998  double u = 0.0, v = 0.0;
999  vpMeterPixelConversion::convertPoint(cam, ptunit.get_x(), ptunit.get_y(), u, v);
1000  iPunit.set_uv(u, v);
1001  iPunit = iPunit + dTopLeft3D;
1002 
1003  getGrid3DPoint(pente, iPunit, ip1, ip2, ip3);
1004 
1005  if (check3Dline(ip1, ip2)) {
1006  vpDisplay::displayLine(I, ip1, ip2, vpColor::black);
1007  if (count % 2 == 1) {
1008  double ttemp;
1009  if (power != 0)
1010  ttemp = t * pow(10.0, power);
1011  else
1012  ttemp = t;
1013  std::stringstream ss;
1014  ss.precision(2);
1015  ss << ttemp;
1016  vpDisplay::displayText(I, ip3, ss.str(), vpColor::black);
1017  }
1018  }
1019  count++;
1020  }
1021  if (power != 0) {
1022  ip4 = iP[5] - vpImagePoint(-15, 10);
1023  std::stringstream ss;
1024  ss << "x10e";
1025  ss << -power;
1026  if (check3Dpoint(ip4))
1027  vpDisplay::displayText(I, ip4, ss.str(), vpColor::black);
1028  }
1029 
1030  // Ligne horizontal
1031  if (check3Dline(iP[0], iP[1])) {
1032  vpDisplay::displayArrow(I, iP[0], iP[1], vpColor::black, gridThickness);
1033  if (dispUnit) {
1034  iPunit.set_ij(iP[1].get_i(), iP[1].get_j() - 10 * epsj);
1035  check3Dpoint(iPunit);
1036  vpDisplay::displayText(I, iPunit, unitx.c_str(), vpColor::black);
1037  }
1038  }
1039  if (check3Dline(iP[3], iP[2])) {
1040  vpDisplay::displayArrow(I, iP[3], iP[2], vpColor::black, gridThickness);
1041  if (dispUnit) {
1042  iPunit.set_ij(iP[2].get_i(), iP[2].get_j() - 10 * epsj);
1043  check3Dpoint(iPunit);
1044  vpDisplay::displayText(I, iPunit, unity.c_str(), vpColor::black);
1045  }
1046  }
1047  if (check3Dline(iP[4], iP[5])) {
1048  vpDisplay::displayArrow(I, iP[4], iP[5], vpColor::black, gridThickness);
1049  if (dispUnit) {
1050  iPunit.set_ij(iP[5].get_i(), iP[5].get_j() - 10 * epsj);
1051  check3Dpoint(iPunit);
1052  vpDisplay::displayText(I, iPunit, unitz.c_str(), vpColor::black);
1053  }
1054  }
1055 
1056  if (dispTitle)
1057  displayTitle(I);
1058  if (dispLegend)
1059  displayLegend(I);
1060 }
1061 
1062 vpMouseButton::vpMouseButtonType vpPlotGraph::plot(vpImage<unsigned char> &I, unsigned int curveNb, double x, double y,
1063  double z)
1064 {
1065  if (!scaleInitialized) {
1066  if (x < 0) {
1067  xmax = 0;
1068  rescalex(0, x);
1069  }
1070  if (x > 0) {
1071  xmin = 0;
1072  rescalex(1, x);
1073  }
1074  if (y < 0) {
1075  ymax = 0;
1076  rescaley(0, y);
1077  }
1078  if (y > 0) {
1079  ymin = 0;
1080  rescaley(1, y);
1081  }
1082  if (z < 0) {
1083  zmax = 0;
1084  rescalez(0, z);
1085  }
1086  if (z > 0) {
1087  zmin = 0;
1088  rescalez(1, z);
1089  }
1090  scaleInitialized = true;
1091  computeGraphParameters3D();
1092  clearGraphZone(I);
1093  displayGrid3D(I);
1094  // if (std::fabs(y) == 0 || z == 0)
1095  if (std::fabs(y) <= std::numeric_limits<double>::epsilon() ||
1096  std::fabs(z) <= std::numeric_limits<double>::epsilon())
1097  scaleInitialized = false;
1098  }
1099 
1100  if (firstPoint) {
1101  clearGraphZone(I);
1102  displayGrid3D(I);
1103  vpDisplay::flushROI(I, graphZone);
1104  firstPoint = false;
1105  }
1106 
1107  bool changed = false;
1108  if (x > xmax) {
1109  rescalex(1, x);
1110  changed = true;
1111  }
1112  else if (x < xmin) {
1113  rescalex(0, x);
1114  changed = true;
1115  }
1116 
1117  if (y > ymax) {
1118  rescaley(1, y);
1119  changed = true;
1120  }
1121  else if (y < ymin) {
1122  rescaley(0, y);
1123  changed = true;
1124  }
1125 
1126  if (z > zmax) {
1127  rescalez(1, z);
1128  changed = true;
1129  }
1130  else if (z < zmin) {
1131  rescalez(0, z);
1132  changed = true;
1133  }
1134 
1136 
1137  if (changed || move(I, button)) {
1138  computeGraphParameters3D();
1139  replot3D(I);
1140  }
1141 
1142  vpPoint pointPlot;
1143  pointPlot.setWorldCoordinates(ptXorg + (zoomx_3D * x), ptYorg - (zoomy_3D * y), ptZorg + (zoomz_3D * z));
1144  pointPlot.track(cMo);
1145  double u = 0.0, v = 0.0;
1146  vpMeterPixelConversion::convertPoint(cam, pointPlot.get_x(), pointPlot.get_y(), u, v);
1147  vpImagePoint iP;
1148  iP.set_uv(u, v);
1149  iP = iP + dTopLeft3D;
1150 
1151  if ((curveList + curveNb)->nbPoint) {
1152  if (check3Dline((curveList + curveNb)->lastPoint, iP))
1153  vpDisplay::displayLine(I, (curveList + curveNb)->lastPoint, iP, (curveList + curveNb)->color,
1154  (curveList + curveNb)->thickness);
1155  }
1156 #if (defined VISP_HAVE_X11 || defined VISP_HAVE_GDI)
1157  double top;
1158  double left;
1159  double width_;
1160  double height_;
1161 
1162  if (iP.get_i() <= (curveList + curveNb)->lastPoint.get_i()) {
1163  top = iP.get_i() - 5;
1164  height_ = (curveList + curveNb)->lastPoint.get_i() - top + 10;
1165  }
1166  else {
1167  top = (curveList + curveNb)->lastPoint.get_i() - 5;
1168  height_ = iP.get_i() - top + 10;
1169  }
1170  if (iP.get_j() <= (curveList + curveNb)->lastPoint.get_j()) {
1171  left = iP.get_j() - 5;
1172  width_ = (curveList + curveNb)->lastPoint.get_j() - left + 10;
1173  }
1174  else {
1175  left = (curveList + curveNb)->lastPoint.get_j() - 5;
1176  width_ = iP.get_j() - left + 10;
1177  }
1178  vpDisplay::flushROI(I, vpRect(left, top, width_, height_));
1179 #endif
1180 
1181  (curveList + curveNb)->lastPoint = iP;
1182  (curveList + curveNb)->pointListx.push_back(x);
1183  (curveList + curveNb)->pointListy.push_back(y);
1184  (curveList + curveNb)->pointListz.push_back(z);
1185  (curveList + curveNb)->nbPoint++;
1186 
1187 #if (!defined VISP_HAVE_X11 && defined FLUSH_ON_PLOT)
1188  vpDisplay::flushROI(I, graphZone);
1189 #endif
1190  return button;
1191 }
1192 
1193 void vpPlotGraph::replot3D(vpImage<unsigned char> &I)
1194 {
1195  clearGraphZone(I);
1196  displayGrid3D(I);
1197 
1198  for (unsigned int i = 0; i < curveNbr; ++i) {
1199  std::list<double>::const_iterator it_ptListx = (curveList + i)->pointListx.begin();
1200  std::list<double>::const_iterator it_ptListy = (curveList + i)->pointListy.begin();
1201  std::list<double>::const_iterator it_ptListz = (curveList + i)->pointListz.begin();
1202 
1203  unsigned int k = 0;
1204  vpImagePoint iP;
1205  vpPoint pointPlot;
1206  while (k < (curveList + i)->nbPoint) {
1207  double x = *it_ptListx;
1208  double y = *it_ptListy;
1209  double z = *it_ptListz;
1210  pointPlot.setWorldCoordinates(ptXorg + (zoomx_3D * x), ptYorg - (zoomy_3D * y), ptZorg + (zoomz_3D * z));
1211  pointPlot.track(cMo);
1212  double u = 0.0, v = 0.0;
1213  vpMeterPixelConversion::convertPoint(cam, pointPlot.get_x(), pointPlot.get_y(), u, v);
1214  iP.set_uv(u, v);
1215  iP = iP + dTopLeft3D;
1216 
1217  // vpDisplay::displayCross(I,iP,3,vpColor::cyan);
1218  if (k > 0) {
1219  if (check3Dline((curveList + i)->lastPoint, iP))
1220  vpDisplay::displayLine(I, (curveList + i)->lastPoint, iP, (curveList + i)->color);
1221  // vpDisplay::displayCross(I,iP,3,vpColor::orange);
1222  }
1223 
1224  (curveList + i)->lastPoint = iP;
1225 
1226  ++it_ptListx;
1227  ++it_ptListy;
1228  ++it_ptListz;
1229  k++;
1230  }
1231  }
1232  vpDisplay::flushROI(I, graphZone);
1233 }
1234 
1235 void vpPlotGraph::rescalez(unsigned int side, double extremity)
1236 {
1237  switch (side) {
1238  case 0:
1239  zmin = (3 * extremity - zmax) / 2;
1240  break;
1241  case 1:
1242  zmax = (3 * extremity - zmin) / 2;
1243  break;
1244  }
1245 
1246  zdelt = (zmax - zmin) / (double)nbDivisionz;
1247 }
1248 
1253 bool vpPlotGraph::move(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button)
1254 {
1255  bool changed = false;
1256  vpHomogeneousMatrix displacement = navigation(I, changed, button);
1257 
1258  // if (displacement[2][3] != 0)
1259  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
1260  cMf = cMf * displacement;
1261  vpHomogeneousMatrix fMo = cMf.inverse() * cMo;
1262 
1263  cMo = cMf * displacement * fMo;
1264  return changed;
1265 }
1266 
1267 vpHomogeneousMatrix vpPlotGraph::navigation(const vpImage<unsigned char> &I, bool &changed,
1269 {
1270  vpImagePoint iP;
1271  vpImagePoint trash;
1272  bool clicked = false;
1273  bool clickedUp = false;
1274 
1275  vpHomogeneousMatrix mov(0, 0, 0, 0, 0, 0);
1276  changed = false;
1277 
1278  // if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
1279 
1280  if (!blocked)
1281  clicked = vpDisplay::getClick(I, iP, b, false);
1282 
1283  if (blocked)
1284  clickedUp = vpDisplay::getClickUp(I, trash, b, false);
1285 
1286  if (clicked) {
1287  if (!iP.inRectangle(graphZone))
1288  return mov;
1289  }
1290 
1291  if (clicked) {
1292  if (b == vpMouseButton::button1)
1293  blockedr = true;
1294  if (b == vpMouseButton::button2)
1295  blockedz = true;
1296  blocked = true;
1297  }
1298 
1299  else if (clickedUp) {
1300  if (b == vpMouseButton::button1) {
1301  old_iPr = vpImagePoint(-1, -1);
1302  blockedr = false;
1303  }
1304  if (b == vpMouseButton::button2) {
1305  old_iPz = vpImagePoint(-1, -1);
1306  blockedz = false;
1307  }
1308  if (!(blockedr || blockedz)) {
1309  blocked = false;
1310  // while (vpDisplay::getClick(I,trash,b,false)) {};
1311  }
1312  }
1313 
1314  vpTime::sleepMs(5);
1316 
1317  if (old_iPr != vpImagePoint(-1, -1) && blockedr) {
1318  double width_ = vpMath::minimum(I.getWidth(), I.getHeight());
1319 
1320  double diffi = iP.get_i() - old_iPr.get_i();
1321  double diffj = iP.get_j() - old_iPr.get_j();
1322 
1323  double anglei = diffi * 360 / width_;
1324  double anglej = diffj * 360 / width_;
1325  mov.buildFrom(0, 0, 0, vpMath::rad(anglei), vpMath::rad(-anglej), 0);
1326  changed = true;
1327  }
1328 
1329  if (blockedr)
1330  old_iPr = iP;
1331 
1332  if (old_iPz != vpImagePoint(-1, -1) && blockedz) {
1333  double diffi = iP.get_i() - old_iPz.get_i();
1334  mov.buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
1335  changed = true;
1336  }
1337 
1338  if (blockedz)
1339  old_iPz = iP;
1340 
1341  return mov;
1342 }
1343 
1344 END_VISP_NAMESPACE
1345 
1346 #elif !defined(VISP_BUILD_SHARED_LIBS)
1347 // Work around to avoid warning: libvisp_gui.a(vpPlotGraph.cpp.o) has no symbols
1348 void dummy_vpPlotGraph() { };
1349 #endif
1350 #endif
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static const vpColor red
Definition: vpColor.h:217
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor orange
Definition: vpColor.h:227
static const vpColor blue
Definition: vpColor.h:223
static const vpColor purple
Definition: vpColor.h:228
static const vpColor black
Definition: vpColor.h:211
static const vpColor green
Definition: vpColor.h:220
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void displayROI(const vpImage< unsigned char > &I, const vpRect &roi)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void flushROI(const vpImage< unsigned char > &I, const vpRect &roi)
static bool getClickUp(const vpImage< unsigned char > &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking=true)
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
static void displayDotLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
static bool getPointerPosition(const vpImage< unsigned char > &I, vpImagePoint &ip)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
void set_j(double jj)
Definition: vpImagePoint.h:309
double get_j() const
Definition: vpImagePoint.h:125
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:320
bool inRectangle(const vpRect &rect) const
void set_i(double ii)
Definition: vpImagePoint.h:298
void set_uv(double u, double v)
Definition: vpImagePoint.h:357
double get_i() const
Definition: vpImagePoint.h:114
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
static double rad(double deg)
Definition: vpMath.h:129
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:254
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:262
static int sign(double x)
Definition: vpMath.h:429
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:426
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:461
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:424
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:463
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:459
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
Defines a rectangle in the plane.
Definition: vpRect.h:79
VISP_EXPORT void sleepMs(double t)