vpMbtHiddenFace.cpp

/****************************************************************************
 *
 * $Id: vpMbtHiddenFace.cpp 3530 2012-01-03 10:52:12Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2012 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://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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:
 * Make the complete tracking of an object by using its CAD model
 *
 * Authors:
 * Nicolas Melchior
 * Romain Tallonneau
 * Eric Marchand
 *
 *****************************************************************************/
#include <visp/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS

#include <visp/vpMbtHiddenFace.h>

vpMbtPolygon::vpMbtPolygon()
{
  nbpt = 0 ;
  p = NULL ;
  isappearing = false;
  negative = 0;
  angle_1 = -1e6;
}

vpMbtPolygon::~vpMbtPolygon()
{
//  cout << "Deleting Polygon "  << index  <<  endl ;
  if (p!=NULL)
  {
    delete[] p;
    p = NULL;
  }
}

void
vpMbtPolygon::setNbPoint(const unsigned int nb)
{
  nbpt = nb ;
  if (p != NULL)
    delete[] p;
  p = new vpPoint[nb] ;
}

void
vpMbtPolygon::addPoint(const unsigned int n, const vpPoint &P)
{
  //if( p!NULL && n < nbpt )
    p[n] = P ;
}

void
vpMbtPolygon::changeFrame(const vpHomogeneousMatrix &cMo)
{
  for (unsigned int i = 0 ; i < nbpt ; i++)
  {
    p[i].changeFrame(cMo) ;
    p[i].projection() ;
  }
}

bool
vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo)
{
  changeFrame(cMo) ;
  
  if(nbpt <= 2){
    /* a line is allways visible */
    isvisible = true;
    isappearing = false;
    return  true ;
  }

  vpColVector e1(3) ;
  vpColVector e2(3) ;
  vpColVector facenormal(3) ;

  e1[0] = p[1].get_X() - p[0].get_X() ;
  e1[1] = p[1].get_Y() - p[0].get_Y() ;
  e1[2] = p[1].get_Z() - p[0].get_Z() ;

  e2[0] = p[2].get_X() - p[1].get_X() ;
  e2[1] = p[2].get_Y() - p[1].get_Y() ;
  e2[2] = p[2].get_Z() - p[1].get_Z() ;

  facenormal = vpColVector::crossProd(e1,e2) ;

  double angle = p[0].get_X()*facenormal[0] +  p[0].get_Y()*facenormal[1]  +  p[0].get_Z()*facenormal[2]  ;
  
  double diff = angle - angle_1;
  if (diff < 0) negative++;
  else negative =0;

  if(angle < -0.00001 )
  {
    isvisible = true;
    isappearing = false;
    return  true ;
  }
  else
  {
    if (angle < 0.0000001 )//&& negative >=1)
      isappearing = true;
    else
      isappearing = false;
    isvisible = false ;
    return false ;
  }
}



bool 
vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha)
{
  changeFrame(cMo) ;
  
  if(nbpt <= 2){
    /* a line is allways visible */
    return  true ;
  }

  vpColVector e1(3) ;
  vpColVector e2(3) ;
  vpColVector facenormal(3) ;

  e1[0] = p[1].get_X() - p[0].get_X() ;
  e1[1] = p[1].get_Y() - p[0].get_Y() ;
  e1[2] = p[1].get_Z() - p[0].get_Z() ;

  e2[0] = p[2].get_X() - p[1].get_X() ;
  e2[1] = p[2].get_Y() - p[1].get_Y() ;
  e2[2] = p[2].get_Z() - p[1].get_Z() ;

  facenormal = vpColVector::crossProd(e1,e2) ;

  vpColVector n_plan(3);
  n_plan[0] = 0;
  n_plan[1] = 0;
  n_plan[2] = 1;
  
  vpColVector n_cam(3);
  n_cam = facenormal;
  
  double angle = p[0].get_X()*facenormal[0] +  p[0].get_Y()*facenormal[1]  +  p[0].get_Z()*facenormal[2]  ;
  
  double n_cam_dot_n_plan = vpColVector::dotProd (n_cam, n_plan);
  double cos_angle = n_cam_dot_n_plan * (1 / ( n_cam.euclideanNorm() * n_plan.euclideanNorm() ));
  double my_angle = acos(cos_angle);
  
  if(angle < 0 && ( my_angle > static_cast<double>(M_PI - alpha) || my_angle < static_cast<double>(-M_PI + alpha) ) ){
    return true;
  }  
  return false;
}


vpMbtHiddenFaces::vpMbtHiddenFaces()
{
}


vpMbtHiddenFaces::~vpMbtHiddenFaces()
{
//  cout << "Deleting Hidden Face struxture "<<endl  ;
  vpMbtPolygon *p ;

  for(std::list<vpMbtPolygon*>::const_iterator it=Lpol.begin(); it!=Lpol.end(); ++it){
    p = *it;
    if (p!=NULL){
      delete p ;
    }
    p = NULL ;
  }

  Lpol.clear() ;
}

void
vpMbtHiddenFaces::addPolygon(vpMbtPolygon *p)
{
  vpMbtPolygon *p_new = new vpMbtPolygon;
  p_new->index = p->index;
  p_new->setNbPoint(p->nbpt);
  p_new->isvisible = p->isvisible;
  for(unsigned int i = 0; i < p->nbpt; i++)
    p_new->p[i]= p->p[i];
  Lpol.push_back(p_new);
}


unsigned int
vpMbtHiddenFaces::setVisible(const vpHomogeneousMatrix &cMo)
{
  unsigned int nbvisiblepolygone = 0 ;
  vpMbtPolygon *p ;

  for(std::list<vpMbtPolygon*>::const_iterator it=Lpol.begin(); it!=Lpol.end(); ++it){
    p = *it;
    if (p->isVisible(cMo))
      nbvisiblepolygone++;
  }
  return nbvisiblepolygone ;
}

bool
vpMbtHiddenFaces::isVisible(const int index)
{
  vpMbtPolygon *p ;
  for(std::list<vpMbtPolygon*>::const_iterator it=Lpol.begin(); it!=Lpol.end(); ++it){
    p = *it;
    if (p->getIndex() == index) return p->isVisible() ;
  }
  return false ;
}

bool
vpMbtHiddenFaces::isAppearing(const int index)
{
  vpMbtPolygon *p ;
  for(std::list<vpMbtPolygon*>::const_iterator it=Lpol.begin(); it!=Lpol.end(); ++it){
    p = *it;
    if (p->getIndex() == index) return p->isAppearing() ;
  }
  return false ;
}

void
vpMbtHiddenFaces::reset()
{
  vpMbtPolygon *p ;

  for(std::list<vpMbtPolygon*>::const_iterator it=Lpol.begin(); it!=Lpol.end(); ++it){
    p = *it;
    if (p!=NULL){
      delete p ;
    }
    p = NULL ;
  }

  Lpol.clear();
}

#endif