vpMbHiddenFaces.h

/****************************************************************************
 *
 * 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:
 * Generic model based tracker. This class declares the methods to implement in 
 * order to have a model based tracker. 
 *
 * Authors:
 * Romain Tallonneau
 * Aurelien Yol
 *
 *****************************************************************************/
#pragma once

#ifndef vpMbHiddenFaces_HH
#define vpMbHiddenFaces_HH

#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/mbt/vpMbtPolygon.h>
#include <visp3/mbt/vpMbScanLine.h>

#ifdef VISP_HAVE_OGRE
  #include <visp3/ar/vpAROgre.h>
#endif

#include <vector>
#include <limits>

template<class PolygonType = vpMbtPolygon>
class vpMbHiddenFaces
{
  private:
  std::vector<PolygonType *> Lpol ;
  unsigned int nbVisiblePolygon;
  vpMbScanLine scanlineRender;
  
#ifdef VISP_HAVE_OGRE
  vpImage<unsigned char> ogreBackground;
  bool ogreInitialised;
  unsigned int nbRayAttempts;
  double ratioVisibleRay;
  vpAROgre *ogre;
  std::vector< Ogre::ManualObject* > lOgrePolygons;
  bool ogreShowConfigDialog;
#endif
  
  unsigned int  setVisiblePrivate(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
                           bool &changed, 
                           bool useOgre = false, bool testRoi = false,
                           const vpImage<unsigned char> &I = vpImage<unsigned char>(),
                           const vpCameraParameters &cam = vpCameraParameters()) ;

  public :
                    vpMbHiddenFaces() ;
                  ~vpMbHiddenFaces() ;
                
    void          addPolygon(PolygonType *p)  ;

    bool computeVisibility(const vpHomogeneousMatrix &cMo,
                           const double &angleAppears, const double &angleDisappears,
                           bool &changed, bool useOgre, bool testRoi,
                           const vpImage<unsigned char> &I,
                           const vpCameraParameters &cam,
                           const vpTranslationVector &cameraPos,
                           unsigned int index);

    void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);

    void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h);

    void computeScanLineQuery(const vpPoint &a, const vpPoint &b,
                              std::vector<std::pair<vpPoint, vpPoint> > &lines, const bool &displayResults = false);

    vpMbScanLine& getMbScanLineRenderer() { return scanlineRender; }

#ifdef VISP_HAVE_OGRE
    void          displayOgre(const vpHomogeneousMatrix &cMo);
#endif   
 
    std::vector<PolygonType*>& getPolygon() {return Lpol;}

#ifdef VISP_HAVE_OGRE
  void            initOgre(const vpCameraParameters &cam = vpCameraParameters());
#endif
    
    unsigned int getNbVisiblePolygon() const {return nbVisiblePolygon;}

#ifdef VISP_HAVE_OGRE

    unsigned int getNbRayCastingAttemptsForVisibility() { return nbRayAttempts; }

    vpAROgre*     getOgreContext(){return ogre;}

    double  getGoodNbRayCastingAttemptsRatio(){ return ratioVisibleRay; }
#endif

    bool          isAppearing(const unsigned int i){ return Lpol[i]->isAppearing(); }
    
    
#ifdef VISP_HAVE_OGRE

  bool            isOgreInitialised() { return ogreInitialised; }
#endif
  
    bool          isVisible(const unsigned int i){ return Lpol[i]->isVisible(); }
    
#ifdef VISP_HAVE_OGRE
    bool          isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index);
#endif
    
    inline PolygonType*        operator[](const unsigned int i)   { return Lpol[i];}
    inline const PolygonType*  operator[](const unsigned int i) const { return Lpol[i];}

    void          reset();
    
#ifdef VISP_HAVE_OGRE

    void          setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w) { ogreBackground = vpImage<unsigned char>(h, w, 0); }

    void          setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { nbRayAttempts = attempts; }

    void          setGoodNbRayCastingAttemptsRatio(const double &ratio) {ratioVisibleRay = ratio; if(ratioVisibleRay > 1.0) ratioVisibleRay = 1.0; if(ratioVisibleRay < 0.0) ratioVisibleRay = 0.0;}
    inline void setOgreShowConfigDialog(const bool showConfigDialog){
      ogreShowConfigDialog = showConfigDialog;
    }
#endif
    
    unsigned int  setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed) ;
    unsigned int  setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
    unsigned int  setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
 
#ifdef VISP_HAVE_OGRE
    unsigned int  setVisibleOgre(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
    unsigned int  setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
#endif

  inline unsigned int            size() const { return (unsigned int)Lpol.size(); }
} ;

template<class PolygonType>
vpMbHiddenFaces<PolygonType>::vpMbHiddenFaces()
  : Lpol(), nbVisiblePolygon(0), scanlineRender()
{
#ifdef VISP_HAVE_OGRE
  ogreInitialised = false;
  nbRayAttempts = 1;
  ratioVisibleRay = 1.0;
  ogreShowConfigDialog = false;
  ogre = new vpAROgre();
  ogreBackground = vpImage<unsigned char>(480, 640, 0);
#endif
}

template<class PolygonType>
vpMbHiddenFaces<PolygonType>::~vpMbHiddenFaces()
{
  for(unsigned int i = 0 ; i < Lpol.size() ; i++){
    if (Lpol[i]!=NULL){
      delete Lpol[i] ;
    }
    Lpol[i] = NULL ;
  }
  Lpol.resize(0);

#ifdef VISP_HAVE_OGRE
  if(ogre != NULL){
    delete ogre;
    ogre = NULL;
  }

  // This is already done by calling "delete ogre"
//  for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
//    if (lOgrePolygons[i]!=NULL){
//      delete lOgrePolygons[i] ;
//    }
//    lOgrePolygons[i] = NULL ;
//  }

  lOgrePolygons.resize(0);
#endif
}

template<class PolygonType>
void
vpMbHiddenFaces<PolygonType>::addPolygon(PolygonType *p)
{
  PolygonType *p_new = new PolygonType;
  p_new->index = p->index;
  p_new->setNbPoint(p->nbpt);
  p_new->isvisible = p->isvisible;
  p_new->useLod = p->useLod;
  p_new->minLineLengthThresh = p->minLineLengthThresh;
  p_new->minPolygonAreaThresh = p->minPolygonAreaThresh;
  p_new->setName(p->name);
  p_new->hasOrientation = p->hasOrientation;

  for(unsigned int i = 0; i < p->nbpt; i++)
    p_new->p[i]= p->p[i];
  Lpol.push_back(p_new);
}

template<class PolygonType>
void
vpMbHiddenFaces<PolygonType>::reset()
{
  nbVisiblePolygon = 0;
  for(unsigned int i = 0 ; i < Lpol.size() ; i++){
    if (Lpol[i]!=NULL){
      delete Lpol[i] ;
    }
    Lpol[i] = NULL ;
  }
  Lpol.resize(0);

#ifdef VISP_HAVE_OGRE
  if(ogre != NULL){
    delete ogre;
    ogre = NULL;
  }

  // This is already done by calling "delete ogre"
//  for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
//    if (lOgrePolygons[i]!=NULL){
//      delete lOgrePolygons[i] ;
//    }
//    lOgrePolygons[i] = NULL ;
//  }

  lOgrePolygons.resize(0);

  ogreInitialised = false;
  nbRayAttempts = 1;
  ratioVisibleRay = 1.0;
  ogre = new vpAROgre();
  ogreBackground = vpImage<unsigned char>(480, 640);
#endif
}

template<class PolygonType>
void
vpMbHiddenFaces<PolygonType>::computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
{
  for (unsigned int i = 0; i < Lpol.size(); i++){
    // For fast result we could just clip visible polygons.
    // However clipping all of them gives us the possibility to return more information in the scanline visibility results
//    if(Lpol[i]->isVisible())
    {
      Lpol[i]->changeFrame(cMo);
      Lpol[i]->computePolygonClipped(cam);
    }
  }
}

template<class PolygonType>
void
vpMbHiddenFaces<PolygonType>::computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h){
  std::vector<std::vector<std::pair<vpPoint, unsigned int> > > polyClipped(Lpol.size());
  std::vector<std::vector<std::pair<vpPoint, unsigned int> > * > listPolyClipped;
  std::vector<int> listPolyIndices;

  for (unsigned int i = 0; i < Lpol.size(); i++){
    // For fast result we could just use visible polygons.
    // However using all of them gives us the possibility to return more information in the scanline visibility results
//    if(Lpol[i]->isVisible())
    {
      polyClipped[i].clear();
      Lpol[i]->getPolygonClipped(polyClipped[i]);
      if(polyClipped[i].size() != 0)
      {
        listPolyClipped.push_back(&polyClipped[i]);
        listPolyIndices.push_back(Lpol[i]->getIndex());
      }
    }
  }

  scanlineRender.drawScene(listPolyClipped, listPolyIndices, cam, w, h);
}

template<class PolygonType>
void
vpMbHiddenFaces<PolygonType>::computeScanLineQuery(const vpPoint &a, const vpPoint &b,
                                                   std::vector<std::pair<vpPoint, vpPoint> > &lines,
                                                   const bool &displayResults)
{
  scanlineRender.queryLineVisibility(a,b,lines,displayResults);
}

template<class PolygonType>
unsigned int
vpMbHiddenFaces<PolygonType>::setVisiblePrivate(const vpHomogeneousMatrix &cMo,
                                                const double &angleAppears, const double &angleDisappears,
                                                bool &changed, bool useOgre, bool testRoi,
                                                const vpImage<unsigned char> &I,
                                                const vpCameraParameters &cam)
{  
  nbVisiblePolygon = 0;
  changed = false;
  
  vpTranslationVector cameraPos;
  
  if(useOgre){
#ifdef VISP_HAVE_OGRE
    cMo.inverse().extract(cameraPos);
    ogre->renderOneFrame(ogreBackground, cMo);
#else
    vpTRACE("ViSP doesn't have Ogre3D, simple visibility test used");
#endif
  }
  
  for (unsigned int i = 0; i < Lpol.size(); i++){
    //std::cout << "Calling poly: " << i << std::endl;
    if (computeVisibility(cMo, angleAppears, angleDisappears, changed, useOgre, testRoi, I, cam, cameraPos, i))
      nbVisiblePolygon ++;
  }
  return nbVisiblePolygon;
}

template<class PolygonType>
bool
vpMbHiddenFaces<PolygonType>::computeVisibility(const vpHomogeneousMatrix &cMo,
                                                const double &angleAppears, const double &angleDisappears,
                                                bool &changed, bool useOgre, bool /* testRoi */,
                                                const vpImage<unsigned char> & I,
                                                const vpCameraParameters & cam,
                                                const vpTranslationVector &
                                                #ifdef VISP_HAVE_OGRE
                                                cameraPos
                                                #endif
                                                ,
                                                unsigned int index)
{
  unsigned int i = index;
  Lpol[i]->changeFrame(cMo);
  Lpol[i]->isappearing = false;

  //Commented because we need to compute visibility
  // even when dealing with line in level of detail case
  /*if(Lpol[i]->getNbPoint() <= 2)
  {
      Lpol[i]->isvisible = true;
  }
  else*/{
  if(Lpol[i]->isVisible())
  {
    bool testDisappear = false;
    unsigned int nbCornerInsidePrev = 0;

//    if(testRoi){
//      nbCornerInsidePrev = Lpol[i]->getNbCornerInsidePrevImage();
//      if(Lpol[i]->getNbCornerInsideImage(I, cam) == 0)
//        testDisappear = true;
//    }

    if(!testDisappear){
      if(useOgre)
#ifdef VISP_HAVE_OGRE
        testDisappear = ((!Lpol[i]->isVisible(cMo, angleDisappears, true, cam, I)) || !isVisibleOgre(cameraPos,i));
#else
        testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I));
#endif
      else
        testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I));
    }

    // test if the face is still visible
    if(testDisappear){
//               std::cout << "Face " << i << " disappears" << std::endl;
      changed = true;
      Lpol[i]->isvisible = false;
    }
    else {
      //nbVisiblePolygon++;
      Lpol[i]->isvisible = true;

      if(nbCornerInsidePrev > Lpol[i]->getNbCornerInsidePrevImage())
        changed = true;
    }
  }
  else
  {
    bool testAppear = true;

//    if(testRoi && Lpol[i]->getNbCornerInsideImage(I, cam) == 0)
//      testAppear = false;

    if(testAppear){
      if(useOgre)
#ifdef VISP_HAVE_OGRE
        testAppear = ((Lpol[i]->isVisible(cMo, angleAppears, true, cam, I)) && isVisibleOgre(cameraPos,i));
#else
        testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, I));
#endif
      else
        testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, I));
    }

    if(testAppear){
//      std::cout << "Face " << i << " appears" << std::endl;
      Lpol[i]->isvisible = true;
      changed = true;
      //nbVisiblePolygon++;
    }
    else{
//      std::cout << "Problem" << std::endl;
      Lpol[i]->isvisible = false;
    }
  }
  }
  //   std::cout << "Nombre de polygones visibles: " << nbVisiblePolygon << std::endl;
  return Lpol[i]->isvisible;
}

template<class PolygonType>
unsigned int
vpMbHiddenFaces<PolygonType>::setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
{
  return setVisible(I, cam, cMo, angle, angle, changed);
}

template<class PolygonType>
unsigned int
vpMbHiddenFaces<PolygonType>::setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
{
  return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,false,true,I,cam);
}

template<class PolygonType>
unsigned int
vpMbHiddenFaces<PolygonType>::setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
{
  return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,false);
}

#ifdef VISP_HAVE_OGRE

template<class PolygonType>
void 
vpMbHiddenFaces<PolygonType>::initOgre(const vpCameraParameters &cam)
{
  ogreInitialised = true;
  ogre->setCameraParameters(cam);
  ogre->setShowConfigDialog(ogreShowConfigDialog);
  ogre->init(ogreBackground, false, true);
    
  for(unsigned int n = 0 ; n < Lpol.size(); n++){
    Ogre::ManualObject* manual = ogre->getSceneManager()->createManualObject(Ogre::StringConverter::toString(n));

    manual->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
    for(unsigned int i = 0; i < Lpol[n]->nbpt; i++){
      manual->position( (Ogre::Real)Lpol[n]->p[i].get_oX(), (Ogre::Real)Lpol[n]->p[i].get_oY(), (Ogre::Real)Lpol[n]->p[i].get_oZ());
      manual->colour(1.0, 1.0, 1.0);
      manual->index(i);
    }
    
    manual->index(0);
    manual->end();

    ogre->getSceneManager()->getRootSceneNode()->createChildSceneNode()->attachObject(manual);
    
    lOgrePolygons.push_back(manual);
  }
}

template<class PolygonType>
void
vpMbHiddenFaces<PolygonType>::displayOgre(const vpHomogeneousMatrix &cMo)
{
  if(ogreInitialised && !ogre->isWindowHidden()){
    for(unsigned int i = 0 ; i < Lpol.size() ; i++){
      if(Lpol[i]->isVisible()){
        lOgrePolygons[i]->setVisible(true);
      }
      else
        lOgrePolygons[i]->setVisible(false);
    }
    ogre->display(ogreBackground, cMo);
  }
}

template<class PolygonType>
unsigned int
vpMbHiddenFaces<PolygonType>::setVisibleOgre(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
{
  return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,true,true,I,cam);
}

template<class PolygonType>
unsigned int
vpMbHiddenFaces<PolygonType>::setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
{
  return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,true);
}

template<class PolygonType>
bool                           
vpMbHiddenFaces<PolygonType>::isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index)
{
  Ogre::Vector3 camera((Ogre::Real)cameraPos[0],(Ogre::Real)cameraPos[1],(Ogre::Real)cameraPos[2]);
  if(!ogre->getCamera()->isVisible(lOgrePolygons[index]->getBoundingBox())){
    lOgrePolygons[index]->setVisible(false);
    Lpol[index]->isvisible = false;
    return false;
  }

  //Get the center of gravity
  bool visible = false;
  unsigned int nbVisible = 0;

  for(unsigned int i = 0; i < nbRayAttempts; i++)
  {
    Ogre::Vector3 origin(0,0,0);
    Ogre::Real totalFactor = 0.0f;

    for(unsigned int j = 0 ; j < Lpol[index]->getNbPoint() ; j++)
    {
        Ogre::Real factor = 1.0f;

        if(nbRayAttempts > 1){
          int r = rand() % 101;

          if(r != 0)
            factor = ((Ogre::Real)r)/100.0f;
        }

        Ogre::Vector3 tmp((Ogre::Real)Lpol[index]->getPoint(j).get_oX(), (Ogre::Real)Lpol[index]->getPoint(j).get_oY(), (Ogre::Real)Lpol[index]->getPoint(j).get_oZ());
        tmp *= factor;
        origin += tmp;
        totalFactor += factor;
    }

    origin /= totalFactor;

    Ogre::Vector3 direction = origin - camera;
    Ogre::Real distanceCollision = direction.length();

    direction.normalise();
    Ogre::RaySceneQuery *mRaySceneQuery = ogre->getSceneManager()->createRayQuery(Ogre::Ray(camera, direction));
    mRaySceneQuery->setSortByDistance(true);

    Ogre::RaySceneQueryResult &result = mRaySceneQuery->execute();
    Ogre::RaySceneQueryResult::iterator it = result.begin();

//    while(it != result.end()){
//      std::cout << it->movable->getName() << "(" << it->distance<< ") : " << std::flush;
//      it++;
//    }
//    std::cout << std::endl;
//    it = result.begin();

    if(it != result.end())
      if(it->movable->getName().find("SimpleRenderable") != Ogre::String::npos) //Test if the ogreBackground is intersect in first
        it++;

    double distance, distancePrev;
    // In a case of a two-axis aligned segment, ray collision is not always working.
    if(Lpol[index]->getNbPoint() == 2 &&
       (((std::fabs(Lpol[index]->getPoint(0).get_oX() - Lpol[index]->getPoint(1).get_oX()) < std::numeric_limits<double>::epsilon()) +
         (std::fabs(Lpol[index]->getPoint(0).get_oY() - Lpol[index]->getPoint(1).get_oY()) < std::numeric_limits<double>::epsilon()) +
         (std::fabs(Lpol[index]->getPoint(0).get_oZ() - Lpol[index]->getPoint(1).get_oZ()) < std::numeric_limits<double>::epsilon())) >= 2 ))
    {
      if(it != result.end())
      {
        if(it->movable->getName() == Ogre::StringConverter::toString(index)){
          nbVisible++;
        }
        else
        {
          distance = it->distance;
          // Cannot use epsilon for comparison as ray lenght is slightly different from the collision distance returned by Ogre::RaySceneQueryResult.
          if(distance > distanceCollision || std::fabs(distance - distanceCollision) < 1e-6 /*std::fabs(distance) * std::numeric_limits<double>::epsilon()*/)
            nbVisible++;
        }
      }
      else
        nbVisible++; // Collision not detected but present.
    }
    else
    {
      if(it != result.end())
      {
        distance = it->distance;
        distancePrev = distance;

        //std::cout << "For " << Ogre::StringConverter::toString(index) << ": " << it->movable->getName() << " / " << std::flush;

        if(it->movable->getName() == Ogre::StringConverter::toString(index)){
          nbVisible++;
        }
        else
        {
          it++;
          while(it != result.end())
          {
            distance = it->distance;

            if(std::fabs(distance - distancePrev) < 1e-6 /*std::fabs(distance) * std::numeric_limits<double>::epsilon()*/){
              //std::cout << it->movable->getName() << " / " << std::flush;
              if(it->movable->getName() == Ogre::StringConverter::toString(index)){
                nbVisible++;
                break;
              }
              it++;
              distancePrev = distance;
            }
            else
              break;
          }
        }
      }
    }

    ogre->getSceneManager()->destroyQuery(mRaySceneQuery);

  }

  if(((double)nbVisible)/((double)nbRayAttempts) > ratioVisibleRay ||
     std::fabs(((double)nbVisible)/((double)nbRayAttempts) - ratioVisibleRay) < ratioVisibleRay * std::numeric_limits<double>::epsilon())
    visible = true;
  else
    visible = false;

  if(visible){
    lOgrePolygons[index]->setVisible(true);
    Lpol[index]->isvisible = true;
  }
  else{
    lOgrePolygons[index]->setVisible(false);
    Lpol[index]->isvisible = false;
  }

  return Lpol[index]->isvisible;
}
#endif //VISP_HAVE_OGRE

#endif // vpMbHiddenFaces