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