vpPlot.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Plot curves.
 *
 * Authors:
 * Nicolas Melchior
 *
 *****************************************************************************/

#include <visp3/core/vpConfig.h>

#if defined(VISP_HAVE_DISPLAY)
#include <fstream>
#include <list>
#include <vector>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpPlot.h>

vpPlot::vpPlot() : I(), display(NULL), graphNbr(1), graphList(NULL), margei(30), margej(40), factori(1.f), factorj(1.)
{
}
vpPlot::vpPlot(unsigned int graph_nbr, unsigned int height, unsigned int width, int x,
               int y, const std::string &title)
  : I(), display(NULL), graphNbr(1), graphList(NULL), margei(30), margej(40), factori(1.f), factorj(1.)
{
  init(graph_nbr, height, width, x, y, title);
}

void vpPlot::init(unsigned int graph_nbr, unsigned int height, unsigned int width, int x,
                  int y, const std::string &title)
{
  I.init(height, width, 255);

#if defined VISP_HAVE_X11
  display = new vpDisplayX;
#elif defined VISP_HAVE_GDI
  display = new vpDisplayGDI;
#elif defined VISP_HAVE_OPENCV
  display = new vpDisplayOpenCV;
#elif defined VISP_HAVE_GTK
  display = new vpDisplayGTK;
#elif defined VISP_HAVE_D3D9
  display = new vpDisplayD3D;
#endif

  display->init(I, x, y, title.c_str());

  vpDisplay::display(I);

  factori = height / 700.0f;
  factorj = width / 700.0f;

  initNbGraph(graph_nbr);
}

vpPlot::~vpPlot()
{
  if (graphList != NULL) {
    delete[] graphList;
    graphList = NULL;
  }
  if (display != NULL) {
    delete display;
    display = NULL;
  }
}

void vpPlot::initNbGraph(unsigned int nbGraph)
{
  if (nbGraph > 4) {
    throw vpException(vpException::dimensionError, "Cannot create more than 4 graphs");
  }
  graphList = new vpPlotGraph[nbGraph];
  graphNbr = nbGraph;

  switch (nbGraph) {
  case 1:
    graphList[0].initSize(vpImagePoint(0, 0), (unsigned int)(700 * factorj), (unsigned int)(700 * factori), margei,
                          margej);
    break;
  case 2:
    graphList[0].initSize(vpImagePoint(0, 0), (unsigned int)(700 * factorj), (unsigned int)(350 * factori), margei,
                          margej);
    graphList[1].initSize(vpImagePoint((unsigned int)(350 * factori), 0), (unsigned int)(700 * factorj),
                          (unsigned int)(350 * factori), margei, margej);
    break;
  case 3:
    graphList[0].initSize(vpImagePoint(0, 0), (unsigned int)(350 * factorj), (unsigned int)(350 * factori), margei,
                          margej);
    graphList[1].initSize(vpImagePoint(0, (unsigned int)(350 * factorj)), (unsigned int)(350 * factorj),
                          (unsigned int)(350 * factori), margei, margej);
    graphList[2].initSize(vpImagePoint((unsigned int)(350 * factori), 0), (unsigned int)(700 * factorj),
                          (unsigned int)(350 * factori), margei, margej);
    break;
  case 4:
    graphList[0].initSize(vpImagePoint(0, 0), (unsigned int)(350 * factorj), (unsigned int)(350 * factori), margei,
                          margej);
    graphList[1].initSize(vpImagePoint(0, (unsigned int)(350 * factorj)), (unsigned int)(350 * factorj),
                          (unsigned int)(350 * factori), margei, margej);
    graphList[2].initSize(vpImagePoint((unsigned int)(350 * factori), 0), (unsigned int)(350 * factorj),
                          (unsigned int)(350 * factori), margei, margej);
    graphList[3].initSize(vpImagePoint((unsigned int)(350 * factori), (unsigned int)(350 * factorj)),
                          (unsigned int)(350 * factorj), (unsigned int)(350 * factori), margei, margej);
    break;
  }

  for (unsigned int i = 0; i < graphNbr; i++) {
    graphList[i].title.clear();
    graphList[i].unitx.clear();
    graphList[i].unity.clear();
    graphList[i].unitz.clear();
  }
}

void vpPlot::initGraph(unsigned int graphNum, unsigned int curveNbr) { (graphList + graphNum)->initGraph(curveNbr); }

// void
// vpPlot::initRange (const int graphNum,
//         double xmin, double xmax, double /*xdelt*/,
//         double ymin, double ymax, double /*ydelt*/,
//         bool gx, bool gy)
// {
//   (graphList+graphNum)->initScale(I,xmin,xmax,10,ymin,ymax,10,gx,gy);
// }

void vpPlot::initRange(unsigned int graphNum, double xmin, double xmax, double ymin, double ymax)
{
  (graphList + graphNum)->initScale(I, xmin, xmax, 10, ymin, ymax, 10, true, true);
}

void vpPlot::initRange(unsigned int graphNum, double xmin, double xmax, double ymin, double ymax, double zmin,
                       double zmax)
{
  (graphList + graphNum)->initScale(I, xmin, xmax, 10, ymin, ymax, 10, zmin, zmax, 10, true, true);
}

void vpPlot::setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
{
  (graphList + graphNum)->setCurveColor(curveNum, color);
}

void vpPlot::displayGrid()
{
  for (unsigned int i = 0; i < graphNbr; i++)
    graphList[i].displayGrid(I);
}

void vpPlot::plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
{
  (graphList + graphNum)->plot(I, curveNum, x, y);
}

void vpPlot::plot(unsigned int graphNum, double x, const vpColVector &v_y)
{
  if ((graphList + graphNum)->curveNbr == v_y.getRows()) {
    for (unsigned int i = 0; i < v_y.getRows(); ++i)
      this->plot(graphNum, i, x, v_y[i]);
  } else
    vpTRACE("error in plot vector : not the right dimension");
}
void vpPlot::plot(unsigned int graphNum, double x, const vpRowVector &v_y)
{
  if ((graphList + graphNum)->curveNbr == v_y.getRows()) {
    for (unsigned int i = 0; i < v_y.getRows(); ++i)
      this->plot(graphNum, i, x, v_y[i]);
  } else
    vpTRACE("error in plot vector : not the right dimension");
}

void vpPlot::plot(unsigned int graphNum, double x, const vpPoseVector &v_y)
{
  if ((graphList + graphNum)->curveNbr == v_y.getRows()) {
    for (unsigned int i = 0; i < v_y.getRows(); ++i)
      this->plot(graphNum, i, x, v_y[i]);
  } else
    vpTRACE("error in plot vector : not the right dimension");
}
void vpPlot::plot(unsigned int graphNum, double x, const vpTranslationVector &v_y)
{
  if ((graphList + graphNum)->curveNbr == v_y.getRows()) {
    for (unsigned int i = 0; i < v_y.getRows(); ++i)
      this->plot(graphNum, i, x, v_y[i]);
  } else
    vpTRACE("error in plot vector : not the right dimension");
}

void vpPlot::plot(unsigned int graphNum, double x, const vpRotationVector &v_y)
{
  if ((graphList + graphNum)->curveNbr == v_y.size()) {
    for (unsigned int i = 0; i < v_y.size(); ++i)
      this->plot(graphNum, i, x, v_y[i]);
  } else
    vpTRACE("error in plot vector : not the right dimension");
}

vpMouseButton::vpMouseButtonType vpPlot::plot(unsigned int graphNum, unsigned int curveNum, double x,
                                              double y, double z)
{
  return (graphList + graphNum)->plot(I, curveNum, x, y, z);
}

vpMouseButton::vpMouseButtonType vpPlot::plot(unsigned int graphNum, double x, const vpColVector &v_y,
                                              const vpColVector &v_z)
{
  vpMouseButton::vpMouseButtonType button = vpMouseButton::none;
  if ((graphList + graphNum)->curveNbr == v_y.getRows() && (graphList + graphNum)->curveNbr == v_z.getRows()) {
    for (unsigned int i = 0; i < v_y.getRows(); ++i)
      button = this->plot(graphNum, i, x, v_y[i], v_z[i]);
  } else
    vpTRACE("error in plot vector : not the right dimension");
  return button;
}

void vpPlot::navigate()
{
  vpMouseButton::vpMouseButtonType b = vpMouseButton::none;

  bool blocked = false;
  unsigned int iblocked = 0;
  vpImagePoint iP;

  while (b != vpMouseButton::button3) {
    if (!blocked) {
      vpDisplay::getPointerPosition(I, iP);
      for (unsigned int i = 0; i < graphNbr; i++) {
        if (iP.inRectangle((graphList + i)->graphZone)) {
          iblocked = i;
          break;
        }
      }
      if ((graphList + iblocked)->move(I, b)) {
        (graphList + iblocked)->replot3D(I);
      }
      blocked = (graphList + iblocked)->blocked;
    } else {
      if ((graphList + iblocked)->move(I, b)) {
        (graphList + iblocked)->replot3D(I);
      }
      blocked = (graphList + iblocked)->blocked;
    }
    vpTime::sleepMs(20);
  }
}

void vpPlot::getPixelValue(bool block)
{
  vpImagePoint iP;

  if (block)
    vpDisplay::getClick(I, iP);
  else
    vpDisplay::getPointerPosition(I, iP);

  for (unsigned int i = 0; i < graphNbr; i++) {
    if ((graphList + i)->getPixelValue(I, iP))
      break;
  }
}

void vpPlot::setTitle(unsigned int graphNum, const std::string &title)
{
  (graphList + graphNum)->setTitle(title);
}

void vpPlot::setUnitX(unsigned int graphNum, const std::string &unitx)
{
  (graphList + graphNum)->setUnitX(unitx);
}

void vpPlot::setUnitY(unsigned int graphNum, const std::string &unity)
{
  (graphList + graphNum)->setUnitY(unity);
}

void vpPlot::setUnitZ(unsigned int graphNum, const std::string &unitz)
{
  (graphList + graphNum)->setUnitZ(unitz);
}

void vpPlot::setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
{
  (graphList + graphNum)->setLegend(curveNum, legend);
}

void vpPlot::resetPointList(unsigned int graphNum)
{
  for (unsigned int i = 0; i < (graphList + graphNum)->curveNbr; i++)
    (graphList + graphNum)->resetPointList(i);
}

void vpPlot::setThickness(unsigned int graphNum, unsigned int curveNum, unsigned int thickness)
{
  (graphList + graphNum)->setCurveThickness(curveNum, thickness);
}

void vpPlot::setGraphThickness(unsigned int graphNum, unsigned int thickness)
{
  for (unsigned int curveNum = 0; curveNum < (graphList + graphNum)->curveNbr; curveNum++)
    (graphList + graphNum)->setCurveThickness(curveNum, thickness);
}

void vpPlot::setGridThickness(unsigned int graphNum, unsigned int thickness)
{
  (graphList + graphNum)->setGridThickness(thickness);
}

void vpPlot::resetPointList(unsigned int graphNum, unsigned int curveNum)
{
  (graphList + graphNum)->resetPointList(curveNum);
}

void vpPlot::saveData(unsigned int graphNum, const std::string &dataFile, const std::string &title_prefix)
{
  std::ofstream fichier;
  fichier.open(dataFile.c_str());

  unsigned int ind;
  double *p = new double[3];
  bool end = false;

  std::vector<std::list<double>::const_iterator> vec_iter_pointListx((graphList + graphNum)->curveNbr);
  std::vector<std::list<double>::const_iterator> vec_iter_pointListy((graphList + graphNum)->curveNbr);
  std::vector<std::list<double>::const_iterator> vec_iter_pointListz((graphList + graphNum)->curveNbr);

  fichier << title_prefix << (graphList + graphNum)->title << std::endl;

  for (ind = 0; ind < (graphList + graphNum)->curveNbr; ind++) {
    vec_iter_pointListx[ind] = (graphList + graphNum)->curveList[ind].pointListx.begin();
    vec_iter_pointListy[ind] = (graphList + graphNum)->curveList[ind].pointListy.begin();
    vec_iter_pointListz[ind] = (graphList + graphNum)->curveList[ind].pointListz.begin();
    //    (graphList+graphNum)->curveList[ind].pointListx.front();
    //    (graphList+graphNum)->curveList[ind].pointListy.front();
    //    (graphList+graphNum)->curveList[ind].pointListz.front();
  }

  while (end == false) {
    end = true;
    for (ind = 0; ind < (graphList + graphNum)->curveNbr; ind++) {
      //      if (!(graphList+graphNum)->curveList[ind].pointListx.outside()
      //          &&
      //          !(graphList+graphNum)->curveList[ind].pointListy.outside()
      //          &&
      //          !(graphList+graphNum)->curveList[ind].pointListz.outside())
      if ((vec_iter_pointListx[ind] != (graphList + graphNum)->curveList[ind].pointListx.end()) &&
          (vec_iter_pointListy[ind] != (graphList + graphNum)->curveList[ind].pointListy.end()) &&
          (vec_iter_pointListz[ind] != (graphList + graphNum)->curveList[ind].pointListz.end())) {
        p[0] = *vec_iter_pointListx[ind];
        p[1] = *vec_iter_pointListy[ind];
        p[2] = *vec_iter_pointListz[ind];
        //        p[0] =
        //        (graphList+graphNum)->curveList[ind].pointListx.value();
        //        p[1] =
        //        (graphList+graphNum)->curveList[ind].pointListy.value();
        //        p[2] =
        //        (graphList+graphNum)->curveList[ind].pointListz.value();

        fichier << p[0] << "\t" << p[1] << "\t" << p[2] << "\t";
        ++vec_iter_pointListx[ind];
        ++vec_iter_pointListy[ind];
        ++vec_iter_pointListz[ind];
        //        (graphList+graphNum)->curveList[ind].pointListx.next();
        //        (graphList+graphNum)->curveList[ind].pointListy.next();
        //        (graphList+graphNum)->curveList[ind].pointListz.next();
        //        if(!(graphList+graphNum)->curveList[ind].pointListx.nextOutside()
        //           &&
        //           !(graphList+graphNum)->curveList[ind].pointListy.nextOutside()
        //           &&
        //           !(graphList+graphNum)->curveList[ind].pointListz.nextOutside())
        if ((vec_iter_pointListx[ind] != (graphList + graphNum)->curveList[ind].pointListx.end()) &&
            (vec_iter_pointListy[ind] != (graphList + graphNum)->curveList[ind].pointListy.end()) &&
            (vec_iter_pointListz[ind] != (graphList + graphNum)->curveList[ind].pointListz.end()))
          end = false;
      } else {
        //        p[0] =
        //        (graphList+graphNum)->curveList[ind].pointListx.value();
        //        p[1] =
        //        (graphList+graphNum)->curveList[ind].pointListy.value();
        //        p[2] =
        //        (graphList+graphNum)->curveList[ind].pointListz.value();
        p[0] = (graphList + graphNum)->curveList[ind].pointListx.back();
        p[1] = (graphList + graphNum)->curveList[ind].pointListy.back();
        p[2] = (graphList + graphNum)->curveList[ind].pointListz.back();
        fichier << p[0] << "\t" << p[1] << "\t" << p[2] << "\t";
      }
    }
    fichier << std::endl;
  }

  delete[] p;
  fichier.close();
}

#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work arround to avoid warning: libvisp_core.a(vpPlot.cpp.o) has no symbols
void dummy_vpPlot(){};
#endif