43 #include <visp/vpConfig.h>
44 #ifndef DOXYGEN_SHOULD_SKIP_THIS
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>
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>
59 #include <visp/vpMath.h>
62 #if defined(VISP_HAVE_DISPLAY)
64 vpPlotGraph::vpPlotGraph()
68 gridColor.setColor(200,200,200);
85 scaleInitialized =
false;
103 vpPlotGraph::~vpPlotGraph()
105 if (curveList != NULL)
113 vpPlotGraph::initGraph (
unsigned int nbCurve)
115 curveList =
new vpPlotCurve[nbCurve];
120 for (
unsigned int i = 0; i < curveNbr; i++)
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,
"");
131 vpPlotGraph::initSize (
vpImagePoint topLeft,
unsigned int width,
unsigned int height,
unsigned int margei,
unsigned int margej)
133 this->topLeft = topLeft;
135 this->height = height;
136 graphZone.setTopLeft(topLeft);
137 graphZone.setWidth(width);
138 graphZone.setHeight(height);
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);
148 dGraphZone3D.setTopLeft(dTopLeft3D);
149 dGraphZone3D.setWidth(dWidth+1);
150 dGraphZone3D.setHeight(dHeight+1);
152 if (this->dWidth > this->dHeight)
155 w_xsize = this->dWidth/this->dHeight;
158 w_yval = w_ysize/2.0;
159 w_xval = w_xsize/2.0;
160 w_zval = w_zsize/2.0;
162 else if (this->dWidth == this->dHeight)
172 else if (this->dWidth < this->dHeight)
175 w_ysize = this->dHeight/this->dWidth;
178 w_yval = w_ysize/2.0;
179 w_xval = w_xsize/2.0;
180 w_zval = w_zsize/2.0;
187 cMf.buildFrom(0,0,cMo[2][3],0,0,0);
192 vpPlotGraph::findPose()
203 iP[2].
set_ij(dHeight-1,dWidth-1);
204 iP[3].
set_ij(dHeight-1,0);
210 for (
unsigned int i=0 ; i < 4 ; i++)
223 vpPlotGraph::computeGraphParameters()
225 zoomx = dWidth/(xmax-xmin);
226 zoomy = dHeight/(ymax-ymin);
227 xorg = dTopLeft.
get_j() - (xmin*zoomx);
228 yorg = dTopLeft.get_i() + (ymax*zoomy);
232 vpPlotGraph::setCurveColor(
const unsigned int curveNum,
const vpColor color)
234 (curveList+curveNum)->color = color;
238 vpPlotGraph::setTitle (
const char *title)
240 strcpy(this->title, title);
245 vpPlotGraph::setUnitX (
const char *unitx)
247 strcpy(this->unitx, unitx);
252 vpPlotGraph::setUnitY (
const char *unity)
254 strcpy(this->unity, unity);
259 vpPlotGraph::setUnitZ (
const char *unitz)
261 strcpy(this->unitz, unitz);
266 vpPlotGraph::setLegend (
const unsigned int curveNum,
const char *legend)
268 strcpy((curveList+curveNum)->legend,legend);
273 vpPlotGraph::setCurveThickness(
const unsigned int curveNum,
const unsigned int thickness)
275 (curveList+curveNum)->thickness = thickness;
279 laFonctionSansNom (
const double delta)
311 computeGraphParameters();
313 xdelt = (xmax-xmin)/nbDivisionx;
314 ydelt = (ymax-ymin)/nbDivisiony;
320 power = laFonctionSansNom(xdelt);
321 for(t=xmin;t<=xmax;t=t+xdelt)
323 double x = xorg+(zoomx*t);
329 if (t+xdelt <= xmax+1e-10)
333 ttemp = t*pow(10.0,power);
335 sprintf(valeur,
"%.2f", ttemp);
336 #if defined VISP_HAVE_X11
338 #elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
345 sprintf(valeur,
"x10e%d", -power);
346 #if defined VISP_HAVE_X11
348 #elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
353 power = laFonctionSansNom(ydelt);
354 for(t=ymin;t<=ymax;t=t+ydelt)
356 double y = yorg-(zoomy*t);
364 ttemp = t*pow(10.0,power);
367 sprintf(valeur,
"%.2f", ttemp);
368 #if defined VISP_HAVE_X11
370 #elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
376 sprintf(valeur,
"x10e%d", -power);
377 #if defined VISP_HAVE_X11
379 #elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
401 #
if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
406 unsigned int offsetx = vpMath::minimum<unsigned int>((
unsigned int)strlen(unitx), dWidth);
408 #if defined VISP_HAVE_X11
411 #elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
420 double size = (double)strlen(title);
424 dTopLeft.get_j()+dWidth/2.0-4*size),
432 unsigned int offsetj = 0;
433 for (
unsigned int i = 0; i < curveNbr; i++) {
434 unsigned int offset = epsj * strlen((curveList+i)->legend);
437 if (offsetj > dWidth) offsetj = dWidth;
439 for (
unsigned int i = 0; i < curveNbr; i++) {
442 dTopLeft.get_j()+dWidth-offsetj),
443 (curveList+i)->legend,
444 (curveList+i)->color);
449 vpPlotGraph::rescalex(
unsigned int side,
double extremity)
454 xmin = (3*extremity-xmax)/2;
457 xmax = (3*extremity-xmin)/2;
461 xdelt = (xmax-xmin)/(
double)nbDivisionx;
465 vpPlotGraph::rescaley(
unsigned int side,
double extremity)
470 ymin = (3*extremity-ymax)/2;
473 ymax = (3*extremity-ymin)/2;
477 ydelt = (ymax-ymin)/(
double)nbDivisiony;
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)
489 this->nbDivisionx = nbDivx;
490 this->nbDivisiony = nbDivy;
491 computeGraphParameters();
495 scaleInitialized =
true;
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)
510 this->nbDivisionx = nbDivx;
511 this->nbDivisiony = nbDivy;
512 this->nbDivisionz = nbDivz;
513 computeGraphParameters();
517 scaleInitialized =
true;
523 if (!scaleInitialized)
545 scaleInitialized =
true;
546 computeGraphParameters();
550 if (std::fabs(y) <= std::numeric_limits<double>::epsilon())
551 scaleInitialized =
false;
563 double i = yorg-(zoomy*y);
564 double j = xorg+(zoomx*x);
570 if (x > xmax) rescalex(1,x);
571 else if(x < xmin) rescalex(0,x);
573 if (y > ymax) rescaley(1,y);
574 else if(y < ymin) rescaley(0,y);
576 computeGraphParameters();
585 (curveList+curveNb)->plotPoint(I, iP, x, y);
586 #if (!defined VISP_HAVE_X11 && defined FLUSH_ON_PLOT)
597 for (
unsigned int i = 0; i < curveNbr; i++)
598 (curveList+i)->plotList(I,xorg,yorg,zoomx,zoomy);
613 double x = (iP.
get_j()-xorg)/zoomx;
614 double y = (yorg-iP.
get_i())/zoomy;
618 sprintf(valeur,
" x: %f", x);
620 sprintf(valeur,
" y: %f", y);
630 vpPlotGraph::resetPointList(
const unsigned int curveNum)
632 (curveList+curveNum)->pointListx.clear();
633 (curveList+curveNum)->pointListy.clear();
634 (curveList+curveNum)->pointListz.clear();
635 (curveList+curveNum)->nbPoint = 0;
648 if (!iP1In || !iP2In)
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;
660 if (!iP1In && !iP2In)
662 if (iP1.
get_i() < dTopLeft_i && iP2.
get_i() < dTopLeft_i)
664 if (iP1.
get_i() > dBottomRight_i && iP2.
get_i() > dBottomRight_i)
666 if (iP1.
get_j() < dTopLeft_j || iP1.
get_j() > dBottomRight_j)
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);
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);
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);
693 if (!iP1In && !iP2In)
695 if (iP1.
get_j() < dTopLeft_j && iP2.
get_j() < dTopLeft_j)
697 if (iP1.
get_j() > dBottomRight_j && iP2.
get_j() > dBottomRight_j)
699 if (iP1.
get_i() < dTopLeft_i || iP1.
get_i() > dBottomRight_i)
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);
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);
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);
728 double jtop = (dTopLeft_i-b)/a;
729 double jlow = (dBottomRight_i-b)/a;
731 double ileft = dTopLeft_j*a+b;
732 double iright = (dBottomRight_j)*a+b;
737 if(jtop >= dTopLeft_j && jtop <= dBottomRight_j)
739 iP[n].
set_ij(dTopLeft_i,jtop);
742 if(jlow >= dTopLeft_j && jlow <= dBottomRight_j)
744 iP[n].
set_ij(dBottomRight_i,jlow);
747 if(ileft >= dTopLeft_i && ileft <= dBottomRight_i && n <2)
749 iP[n].
set_ij(ileft,dTopLeft_j);
752 if(iright >= dTopLeft_i && iright <= dBottomRight_i && n <2)
754 iP[n].
set_ij(iright,dBottomRight_j);
761 if (!iP1In && !iP2In)
770 iP1 = iP[0]; iP2 = iP[1];
774 iP1 = iP[1]; iP2 = iP[0];
784 iP1 = iP[0];iP2 = iP[1];
788 iP1 = iP[1];iP2 = iP[0];
798 if (fabs(iP[0].get_j()-iP2.
get_j()) > 5)
812 if (fabs(iP[0].get_i()-iP2.
get_i()) > 5)
835 if (fabs(iP[0].get_j()-iP1.
get_j()) > 5)
849 if (fabs(iP[0].get_i()-iP1.
get_i()) > 5)
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);
889 vpPlotGraph::computeGraphParameters3D()
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;
918 computeGraphParameters3D();
920 xdelt = (xmax-xmin)/nbDivisionx;
921 ydelt = (ymax-ymin)/nbDivisiony;
922 zdelt = (zmax-zmin)/nbDivisionz;
933 for (
unsigned int i = 0; i < 6; i++)
939 iP[i] = iP[i] + dTopLeft3D;
953 power = laFonctionSansNom(xdelt);
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()));
961 unsigned int count = 1;
962 for(t=xmin;t<=xmax;t=t+xdelt)
964 double x = ptXorg+(zoomx_3D*t);
970 iPunit = iPunit + dTopLeft3D;
972 getGrid3DPoint(pente,iPunit,ip1,ip2,ip3);
974 if(check3Dline(ip1,ip2))
981 ttemp = t*pow(10.0,power);
983 sprintf(valeur,
"%.1f", ttemp);
992 sprintf(valeur,
"x10e%d", -power);
993 if(check3Dpoint(ip4))
997 power = laFonctionSansNom(ydelt);
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()));
1005 for(t=ymin;t<=ymax;t=t+ydelt)
1007 double y = ptYorg-(zoomy_3D*t);
1010 double u=0.0, v=0.0;
1013 iPunit = iPunit + dTopLeft3D;
1015 getGrid3DPoint(pente,iPunit,ip1,ip2,ip3);
1017 if(check3Dline(ip1,ip2))
1024 ttemp = t*pow(10.0,power);
1026 sprintf(valeur,
"%.1f", ttemp);
1035 sprintf(valeur,
"x10e%d", -power);
1036 if(check3Dpoint(ip4))
1040 power = laFonctionSansNom(zdelt);
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()));
1048 for(t=zmin;t<=zmax;t=t+zdelt)
1050 double z = ptZorg+(zoomz_3D*t);
1053 double u=0.0, v=0.0;
1056 iPunit = iPunit + dTopLeft3D;
1058 getGrid3DPoint(pente,iPunit,ip1,ip2,ip3);
1060 if(check3Dline(ip1,ip2))
1067 ttemp = t*pow(10.0,power);
1069 sprintf(valeur,
"%.1f", ttemp);
1078 sprintf(valeur,
"x10e%d", -power);
1079 if(check3Dpoint(ip4))
1085 if (check3Dline(iP[0],iP[1]))
1090 vpImagePoint iPunit(iP[1].get_i(),iP[1].get_j()-10*epsj);
1091 check3Dpoint (iPunit);
1095 if (check3Dline(iP[3],iP[2]))
1100 vpImagePoint iPunit(iP[2].get_i(),iP[2].get_j()-10*epsj);
1101 check3Dpoint (iPunit);
1105 if (check3Dline(iP[4],iP[5]))
1110 vpImagePoint iPunit(iP[5].get_i(),iP[5].get_j()-10*epsj);
1111 check3Dpoint (iPunit);
1123 vpPlotGraph::plot (
vpImage<unsigned char> &I,
const unsigned int curveNb,
const double x,
const double y,
const double z)
1125 if (!scaleInitialized)
1157 scaleInitialized =
true;
1158 computeGraphParameters3D();
1162 if (std::fabs(y) <= std::numeric_limits<double>::epsilon() || std::fabs(z) <= std::numeric_limits<double>::epsilon())
1163 scaleInitialized =
false;
1174 bool changed =
false;
1175 if (x > xmax) {rescalex(1,x); changed =
true;}
1176 else if(x < xmin) {rescalex(0,x);changed =
true;}
1178 if (y > ymax) {rescaley(1,y);changed =
true;}
1179 else if(y < ymin) {rescaley(0,y);changed =
true;}
1181 if (z > zmax) {rescalez(1,z);changed =
true;}
1182 else if(z < zmin) {rescalez(0,z);changed =
true;}
1184 if (changed || move(I))
1186 computeGraphParameters3D();
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;
1197 iP = iP + dTopLeft3D;
1199 (curveList+curveNb)->pointListx.end();
1200 (curveList+curveNb)->pointListy.end();
1201 (curveList+curveNb)->pointListz.end();
1202 if((curveList+curveNb)->nbPoint)
1204 if (check3Dline((curveList+curveNb)->lastPoint,iP))
1205 vpDisplay::displayLine(I,(curveList+curveNb)->lastPoint, iP, (curveList+curveNb)->color, (curveList+curveNb)->thickness);
1207 #if( defined VISP_HAVE_X11 || defined VISP_HAVE_GDI )
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;}
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++;
1227 #if( !defined VISP_HAVE_X11 && defined FLUSH_ON_PLOT)
1238 for (
unsigned int i = 0; i < curveNbr; i++)
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();
1248 while (k < (curveList+i)->nbPoint)
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;
1258 iP = iP + dTopLeft3D;
1263 if (check3Dline((curveList+i)->lastPoint,iP))
1268 (curveList+i)->lastPoint = iP;
1282 vpPlotGraph::rescalez(
unsigned int side,
double extremity)
1287 zmin = (3*extremity-zmax)/2;
1290 zmax = (3*extremity-zmin)/2;
1294 zdelt = (zmax-zmin)/(
double)nbDivisionz;
1300 bool changed =
false;
1304 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
1305 cMf = cMf*displacement;
1308 cMo = cMf* displacement * fMo;
1317 bool clicked =
false;
1318 bool clickedUp =
false;
1355 if (!(blockedr || blockedz))
1371 double diffi = iP.
get_i() - old_iPr.get_i();
1372 double diffj = iP.
get_j() - old_iPr.get_j();
1374 anglei = diffi*360/width;
1375 anglej = diffj*360/width;
1380 if (blockedr) old_iPr = iP;
1384 double diffi = iP.
get_i() - old_iPz.get_i();
1385 mov.buildFrom(0,0,diffi*0.01,0,0,0);
1389 if (blockedz) old_iPz = iP;