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