vpPolygon.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2015 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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:
 * Defines a generic 2D polygon.
 *
 * Author:
 * Amaury Dame
 * Nicolas Melchior
 * Romain Tallonneau
 *
 *****************************************************************************/



#include <visp3/core/vpPolygon.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpUniRand.h>
#include <set>
#include <limits>
vpPolygon::vpPolygon()
  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
{
  std::vector<vpImagePoint> corners;
  corners.push_back(vpImagePoint(0,0));
  corners.push_back(vpImagePoint(1,0));
  corners.push_back(vpImagePoint(0,1));
  init(corners);
}

vpPolygon::vpPolygon(const std::vector<vpImagePoint>& corners)
  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
{
  if(corners.size() < 3){
    _goodPoly = false;
  }
  init(corners);
}

vpPolygon::vpPolygon(const vpPolygon &poly)
  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
{
  _corners = poly._corners;
  _center = poly._center;
  _area = poly._area;
  _goodPoly = poly._goodPoly;
  _bbox = poly._bbox;
}

vpPolygon::~vpPolygon()
{
}

vpPolygon &
vpPolygon::operator=(const vpPolygon& poly)
{
  _corners = poly._corners;
  _center = poly._center;
  _area = poly._area;
  _goodPoly = poly._goodPoly;
  return *this;
};

void
vpPolygon::buildFrom(const std::vector<vpImagePoint>& corners)
{
  init(corners);
}

void
vpPolygon::buildFrom(const std::vector<vpPoint>& corners, const vpCameraParameters& cam)
{
  std::vector<vpImagePoint> ipCorners(corners.size());
  for(unsigned int i=0; i<corners.size(); ++i){
    vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
  }
  buildFrom(ipCorners);
}

void
vpPolygon::initClick(const vpImage<unsigned char>& I)
{
  vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
  vpImagePoint ip;

  std::vector<vpImagePoint> cornersClick;

  while(button == vpMouseButton::button1){
    bool ret = vpDisplay::getClick(I, ip, button, true);
    if(ret && button == vpMouseButton::button1){
      vpDisplay::displayCross(I, ip, 5, vpColor::red);
      cornersClick.push_back(ip);
      vpDisplay::flush(I);
    }
  }

  buildFrom(cornersClick);
}


void
vpPolygon::init(const std::vector<vpImagePoint>& corners)
{
  _corners = corners;

  updateBoundingBox();
  updateArea();
  updateCenter();
}



bool 
vpPolygon::testIntersectionSegments(const vpImagePoint& ip1, const vpImagePoint& ip2, const vpImagePoint& ip3, const vpImagePoint& ip4)
{
  double di1 = ip2.get_i() - ip1.get_i();
  double dj1 = ip2.get_j() - ip1.get_j();
  
  double di2 = ip4.get_i() - ip3.get_i();
  double dj2 = ip4.get_j() - ip3.get_j();
  
  double denominator = di1 * dj2 - dj1 * di2;
  
  if(fabs(denominator) < std::numeric_limits<double>::epsilon()){
    throw vpException(vpException::divideByZeroError, "Denominator is null, lines are parallels");
  }
  
  double alpha = - ( ( ip1.get_i() - ip3.get_i() ) * dj2 + di2 * ( ip3.get_j() - ip1.get_j())) / denominator;  
  if(alpha < 0  || alpha >= 1){
    return false;
  }
  
  double beta = - (di1 * (ip3.get_j() - ip1.get_j() ) + dj1 * (ip1.get_i() - ip3.get_i()) ) / denominator;
  if(beta < 0  || beta >= 1){
    return false;
  }
  
  return true;
}

bool 
vpPolygon::isInside(const vpImagePoint& ip)
{
  vpImagePoint infPoint(100000, 100000); // take a point at 'inifinity'
  vpUniRand generator;
  infPoint.set_i( infPoint.get_i() + 1000 * generator());
  infPoint.set_j( infPoint.get_j() + 1000 * generator());// we add random since it appears that sometimes infPoint may cause a degenerated case (so realucnch and hope that result will be different).

  unsigned int nbinterscetion = 0;
  for(unsigned int i=0; i<_corners.size(); ++i){
    vpImagePoint ip1 = _corners[i];
    vpImagePoint ip2 = _corners[(i+1)%_corners.size()];
    bool intersection = false;    

    // If the points are the same we continue without trying to found
    // an intersection
    if (ip1 == ip2)
      continue;

    try{
      intersection = testIntersectionSegments(ip1, ip2, ip, infPoint );
    }catch(vpException e){
      return isInside(ip);
    }

    if(intersection){
      ++nbinterscetion;
    }
  }

  return ((nbinterscetion%2)==1);
}



void
vpPolygon::updateArea()
{
  if(_corners.size() == 0){
    _area = 0;
    _goodPoly = false;
    return;
  }
  _area = 0;

  for(unsigned int i=0; i<_corners.size(); ++i){
    unsigned int i_p_1 = ( i+1 ) % _corners.size();
    _area += _corners[i].get_j() * _corners[i_p_1].get_i()
      - _corners[i_p_1].get_j() * _corners[i].get_i();
  }

  _area /= 2;
  if(_area < 0){
    _area = - _area;
  }
}


void
vpPolygon::updateCenter()
{
  if(_corners.size() == 0){
    _center = vpImagePoint(0, 0);
    _goodPoly = false;
    return;
  }
  double i_tmp = 0;
  double j_tmp = 0;
#if 0
  for(unsigned int i=0; i<(_corners.size()-1); ++i){
    i_tmp += (_corners[i].get_i() + _corners[i+1].get_i()) *
             (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());

    j_tmp += (_corners[i].get_j() + _corners[i+1].get_j()) *
             (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
  }
#else
  for(unsigned int i=0; i<_corners.size(); ++i){
    unsigned int i_p_1 = ( i+1 ) % _corners.size();
    i_tmp += (_corners[i].get_i() + _corners[i_p_1].get_i()) *
             (_corners[i_p_1].get_i() * _corners[i].get_j() 
          - _corners[i_p_1].get_j() * _corners[i].get_i());

    j_tmp += (_corners[i].get_j() + _corners[i_p_1].get_j()) *
             (_corners[i_p_1].get_i() * _corners[i].get_j() 
          - _corners[i_p_1].get_j() * _corners[i].get_i());
  }
#endif

  if(_area > 0){
    _center.set_i(fabs(i_tmp / (6 * _area)));
    _center.set_j(fabs(j_tmp / (6 * _area)));
  }else{
    _center = _corners[0];
    _goodPoly = false;
  }
}

void
vpPolygon::updateBoundingBox()
{
  if(_corners.size() == 0){
    _bbox.setBottomRight(vpImagePoint(0, 0));
    _bbox.setTopLeft(vpImagePoint(0, 0));
    _goodPoly = false;
    return;
  }

  std::set<double> setI;
  std::set<double> setJ;
  for(unsigned int i=0; i<_corners.size(); ++i){
    setI.insert(_corners[i].get_i());
    setJ.insert(_corners[i].get_j());
  }
  vpImagePoint tl(*(setI.begin()), *(setJ.begin()));
  std::set<double>::const_iterator iterI = setI.end();
  std::set<double>::const_iterator iterJ = setJ.end();
  --iterI;
  --iterJ;
  vpImagePoint br(*iterI, *iterJ);

  if(tl == br){
    _goodPoly = false;
  }
  _bbox.setTopLeft(tl);
  _bbox.setBottomRight(br);
}

void
vpPolygon::display(const vpImage<unsigned char>& I, const vpColor& color, unsigned int thickness) const
{
  const unsigned int N = (unsigned int)_corners.size();
  for(unsigned int i=0; i<N; ++i){
    vpDisplay::displayLine(I, _corners[i], _corners[(i+1)%N], color, thickness);
  }
}

bool
vpPolygon::intersect(const vpImagePoint& p1, const vpImagePoint& p2, const double &i_test, const double &j_test, const double &i, const double &j)
{
  double dx = p2.get_j() - p1.get_j();
  double dy = p2.get_i() - p1.get_i();
  double ex = j - j_test;
  double ey = i - i_test;

  double den = dx * ey - dy * ex;
  double t = 0, u = 0;
  //if(den != 0){
  if(std::fabs(den) > std::fabs(den)*std::numeric_limits<double>::epsilon()){
    t = -( ey * ( p1.get_j() - j_test ) + ex * ( -p1.get_i() + i_test ) ) / den;
    u = -( dx * ( -p1.get_i() + i_test ) + dy * ( p1.get_j() - j_test ) ) / den;
  }
  else{
    throw vpException(vpException::divideByZeroError, "denominator null");
  }
  return ( t >= std::numeric_limits<double>::epsilon() && t < 1.0 && u >= std::numeric_limits<double>::epsilon() && u < 1.0);
}

bool
vpPolygon::isInside(const std::vector<vpImagePoint>& roi, const double &i, const double &j)
{
  double i_test = 100000.;
  double j_test = 100000.;
  unsigned int nbInter = 0;
  bool computeAgain = true;

  if(computeAgain){
    computeAgain = false;
    for(unsigned int k=0; k< roi.size(); k++){
      try{
        if(vpPolygon::intersect(roi[k], roi[(k+1)%roi.size()], i, j, i_test, j_test)){
          nbInter++;
        }
      }
      catch(...){
        computeAgain = true;
        break;
      }
    }

    if(computeAgain){
      i_test += 100;
      j_test -= 100;
      nbInter = 0;
    }
  }
  return ((nbInter%2) == 1);
}