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