PolyhedralCone_Rn.h

Go to the documentation of this file.

// politopix allows to make computations on polytopes such as finding vertices, intersecting, Minkowski sums, ...
//     Copyright (C) 2011-2022 : Delos Vincent
//
//     This program 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 3 of the License, or
//     (at your option) any later version.
//
//     This program is distributed in the hope that it will be useful,
//     but WITHOUT ANY WARRANTY; without even the implied warranty of
//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//     GNU General Public License for more details.
//
//     You should have received a copy of the GNU General Public License
//     along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
// I2M (UMR CNRS 5295 / University of Bordeaux)
 
#ifndef POLYHEDRALCONE_Rn
#define POLYHEDRALCONE_Rn
 
#include <boost/numeric/ublas/io.hpp>
#include <boost/shared_ptr.hpp>
#include <stdexcept>
#include <exception>
#include <iterator>
#include <numeric>
#include <vector>
#include <set>
#include "polito_Export.h"
#include "GeometricObjectIterator_Rn.h"
#include "Neighbours_Rn.h"
#include "Generator_Rn.h"
#include "HalfSpace_Rn.h"
#include "Rn.h"
 
#undef min  
#undef max 
using namespace boost::numeric::ublas;
 
 
class polito_EXPORT PolyhedralCone_Rn {
  friend class lexIteratorOfListOfHalfSpaces;
  friend class constIteratorOfListOfHalfSpaces;
 
 public:
  PolyhedralCone_Rn() {}
 
  PolyhedralCone_Rn(const PolyhedralCone_Rn& A) {
    _listOfHalfSpaces = A._listOfHalfSpaces;
    _listOfGenerators = A._listOfGenerators;
  }
 
  virtual ~PolyhedralCone_Rn() {}
 
  virtual unsigned int dimension() const {
    if (numberOfHalfSpaces()==0 && numberOfGenerators()==0)
      return Rn::getDimension();
    else if (numberOfGenerators()==0) {
      constIteratorOfListOfGeometricObjects< boost::shared_ptr<HalfSpace_Rn> > iteHS(getListOfHalfSpaces());
      const boost::shared_ptr<HalfSpace_Rn>& currentHalfSpace = iteHS.current();
      return currentHalfSpace->dimension();
    }
    else {
      constIteratorOfListOfGeometricObjects< boost::shared_ptr<Generator_Rn> > iteGN(getListOfGenerators());
      const boost::shared_ptr<Generator_Rn>& currentGenerator = iteGN.current();
      return currentGenerator->dimension();
    }
  }
 
  virtual bool isBounded() const {return false;}
 
  virtual unsigned int neigbourhoodCondition() const {return (dimension()-2);}
 
  virtual unsigned int numberOfGeneratorsPerFacet() const {return (dimension()-1);}
 
  void reset() {_listOfHalfSpaces.clear(); _listOfGenerators.clear();}
 
  // HALF-SPACES
 
  unsigned int numberOfHalfSpaces() const {return _listOfHalfSpaces.size();}
 
  boost::shared_ptr<HalfSpace_Rn> addHalfSpace(boost::shared_ptr<HalfSpace_Rn> hs, bool check=false);
 
  void removeHalfSpaceFromListAndGenerators(const boost::shared_ptr<HalfSpace_Rn>& hs);
 
  void removeHalfSpace(unsigned int j) {_listOfHalfSpaces.removeGeometricObject(j);}
 
  void removeHalfSpaces(const std::set< boost::shared_ptr<HalfSpace_Rn> >& setOfRedundantHS) {
    _listOfHalfSpaces.removeGeometricObjects( setOfRedundantHS );
  }
 
  unsigned int getHalfSpaceNumber(const boost::shared_ptr<HalfSpace_Rn>& F) const {
    unsigned int index;
    try {
      index = _listOfHalfSpaces.find(F);
    }
    catch(std::out_of_range& excep) {
      F->dump(std::cerr);
      std::cerr << std::endl;
      std::string ex(excep.what());
      ex += std::string(" : Catch in PolyhedralCone_Rn::getHalfSpaceNumber()");
      throw std::out_of_range(ex);
    }
    return index;
  }
 
  const boost::shared_ptr<HalfSpace_Rn>& getHalfSpace(unsigned int i) const;
 
  listOfGeometricObjects< boost::shared_ptr<HalfSpace_Rn> >& getListOfHalfSpaces() {return _listOfHalfSpaces;}
 
  const listOfGeometricObjects< boost::shared_ptr<HalfSpace_Rn> >& getListOfHalfSpaces() const {return _listOfHalfSpaces;}
 
  void setListOfHalfSpaces(const listOfGeometricObjects< boost::shared_ptr<HalfSpace_Rn> >& hsList) {_listOfHalfSpaces = hsList;}
 
 
  // CONVEX HULL
 
  unsigned int numberOfGenerators() const {return _listOfGenerators.size();}
 
  void addGenerator(boost::shared_ptr<Generator_Rn> vx) {_listOfGenerators.push_back(vx);}
 
  const boost::shared_ptr<Generator_Rn>& getGenerator(unsigned int i) const;
 
  unsigned int getGeneratorNumber(boost::shared_ptr<Generator_Rn> G) const;
 
  const listOfGeometricObjects< boost::shared_ptr<Generator_Rn> >& getListOfGenerators() const {return _listOfGenerators;}
 
  unsigned int getListOfGeneratorsSD(std::vector< boost::shared_ptr<Generator_Rn_SD> >& currentListOfGeneratorsSD) const {
    unsigned int generatorNumber=0;
    constIteratorOfListOfGeometricObjects< boost::shared_ptr<Generator_Rn> > iteGN(getListOfGenerators());
    {
      for (iteGN.begin(); iteGN.end()!=true; iteGN.next()) {
        // By default, Generator_Rn_SD are created with the status UNCHANGED.
        boost::shared_ptr<Generator_Rn_SD> currentGeneratorSD(new Generator_Rn_SD(*iteGN.current(), generatorNumber, Generator_Rn_SD::UNCHANGED));
        {
          for (unsigned int ii=0; ii<iteGN.current()->numberOfFacets(); ii++) {
            unsigned int fctNumber = getHalfSpaceNumber( iteGN.current()->getFacet(ii) );
            currentGeneratorSD->setFacet(fctNumber);
          }
        }
        currentGeneratorSD->orderFacets();
        currentListOfGeneratorsSD.push_back(currentGeneratorSD);
        ++generatorNumber;
      }
    }
    return generatorNumber;
  }
 
  void setListOfGeneratorsSD(const std::vector< boost::shared_ptr<Generator_Rn_SD> >& gnList) {
    listOfGeometricObjects< boost::shared_ptr<Generator_Rn> > listOfGenerators;
    std::vector< boost::shared_ptr<Generator_Rn_SD> >::const_iterator iteGN;
    {
      for (iteGN=gnList.begin(); iteGN!=gnList.end(); ++iteGN) {
        if ((*iteGN)->getStatus() ==  Generator_Rn_SD::CREATED || (*iteGN)->getStatus() ==  Generator_Rn_SD::CREATED_AND_MODIFIED) {
          boost::shared_ptr<Generator_Rn> currentGenerator = (*iteGN)->makeGenerator_Rn();
          {
            for (unsigned int ii=0; ii<(*iteGN)->numberOfFacets(); ii++)
              currentGenerator->setFacet( _listOfHalfSpaces[(*iteGN)->getFacet(ii)] );
          }
          listOfGenerators.push_back( currentGenerator );
        }
        else if ((*iteGN)->getStatus() ==  Generator_Rn_SD::MODIFIED) {
          // Substitute all old half-spaces with the new ones.
          unsigned int nbGN = (*iteGN)->getGeneratorNumber();
          boost::shared_ptr<Generator_Rn> currentGenerator = _listOfGenerators[nbGN];
          currentGenerator->clearFacets();
          {
            for (unsigned int ii=0; ii<(*iteGN)->numberOfFacets(); ii++)
              currentGenerator->setFacet( _listOfHalfSpaces[(*iteGN)->getFacet(ii)] );
          }
          listOfGenerators.push_back(_listOfGenerators[nbGN]);
        }
        else if ((*iteGN)->getStatus() ==  Generator_Rn_SD::UNCHANGED) {
          // Substitute all old half-spaces with the new ones.
          unsigned int nbGN = (*iteGN)->getGeneratorNumber();
          listOfGenerators.push_back(_listOfGenerators[nbGN]);
        }
        // If it was deleted we obviously don't need it.
      }
    }
    _listOfGenerators.clear();
    _listOfGenerators = listOfGenerators;
  }
 
  void relocateGenerators();
 
  void setListOfGenerators(const listOfGeometricObjects< boost::shared_ptr<Generator_Rn> >& gnList) {_listOfGenerators = gnList;}
 
 
  // CHECK POLYHEDRON
 
  bool checkNeighbours(const boost::shared_ptr<Generator_Rn>& V1,
                       const boost::shared_ptr<Generator_Rn>& V2,
                       std::vector< boost::shared_ptr<HalfSpace_Rn> >& commonFacets,
                       const std::set< boost::shared_ptr<HalfSpace_Rn> >& listOfRedundantHS) const {
    unsigned int sameHyperplane=0;
    for (unsigned int i=0; i<V1->numberOfFacets(); i++) {
      for (unsigned int j=0; j<V2->numberOfFacets(); j++) {
        if (V1->getRawFacet(i)==V2->getRawFacet(j) && listOfRedundantHS.find(V1->getFacet(i))==listOfRedundantHS.end()) {
          // Check the facet is not declared redundant !
          sameHyperplane++;
          commonFacets.push_back(V1->getFacet(i));
        }
      }
    }
    // For polyhedral cones it is Rn::getDimension()-1, for polytopes it is Rn::getDimension()-2.
    if (sameHyperplane >= neigbourhoodCondition())
      return true;
    return false;
  }
 
  bool isIncluded(const boost::shared_ptr<PolyhedralCone_Rn>& B) const;
 
  bool checkNeighbours(const boost::shared_ptr<Generator_Rn>& V1,
                       const boost::shared_ptr<Generator_Rn>& V2,
                       std::vector< boost::shared_ptr<HalfSpace_Rn> >& commonFacets,
                       unsigned int topologicalCode=1) const {
    unsigned int sameHyperplane=0;
    for (unsigned int i=0; i<V1->numberOfFacets(); i++) {
      for (unsigned int j=0; j<V2->numberOfFacets(); j++) {
        if (V1->getRawFacet(i) == V2->getRawFacet(j)) {
          // Check the facet is not declared redundant !
          sameHyperplane++;
          commonFacets.push_back(V1->getFacet(i));
        }
      }
    }
    // For polyhedral cones neigbourhoodCondition() is Rn::getDimension()-1, for polytopes it is Rn::getDimension()-2.
    if (sameHyperplane >= neigbourhoodCondition()-topologicalCode+1) {
      return true;
    }
    return false;
  }
 
  bool checkNeighbours(
      const boost::shared_ptr<Generator_Rn>& V1,
      const boost::shared_ptr<Generator_Rn>& V2,
      std::vector< HalfSpace_Rn* >& commonFacets,
      unsigned int topologicalCode=1) const {
    unsigned int sameHyperplane=0;
    for (unsigned int i=0; i<V1->numberOfFacets(); ++i) {
      for (unsigned int j=0; j<V2->numberOfFacets(); ++j) {
        if (V1->getRawFacet(i)==V2->getRawFacet(j)) {
          // Check the facet is not declared redundant !
          sameHyperplane++;
          commonFacets.push_back(V1->getRawFacet(i));
        }
      }
    }
    // For polyhedral cones neigbourhoodCondition() is Rn::getDimension()-1, for polytopes it is Rn::getDimension()-2.
    if (sameHyperplane >= neigbourhoodCondition()-topologicalCode+1) {
      return true;
    }
    return false;
  }
 
  bool checkNeighbours(
    const boost::shared_ptr<Generator_Rn_SD>& genIn,
    const boost::shared_ptr<Generator_Rn_SD>& genOut,
    std::vector< unsigned int >& commonFacets) const {
    // Compute the intersection on sorted lists of facets.
    std::set_intersection(genIn->facetsBegin(), genIn->facetsEnd(), genOut->facetsBegin(), genOut->facetsEnd(),
      std::inserter(commonFacets, commonFacets.end()));
    // For polyhedral cones neigbourhoodCondition() is Rn::getDimension()-1, for polytopes it is Rn::getDimension()-2.
    if (commonFacets.size() >= neigbourhoodCondition())
      return true;
 
    return false;
  }
 
 
  bool checkNeighboursWithHSnumbers(
      const boost::shared_ptr<Generator_Rn>& V1,
      const boost::shared_ptr<Generator_Rn>& V2,
      std::vector< HalfSpace_Rn* >& commonFacets,
      const std::set< boost::shared_ptr<HalfSpace_Rn> >& listOfRedundantHS,
      unsigned int topologicalCode=1) const {
    unsigned int sameHyperplane=0;
    for (unsigned int i=0; i<V1->numberOfFacets(); ++i) {
      for (unsigned int j=0; j<V2->numberOfFacets(); ++j) {
        if (V1->getRawFacet(i)==V2->getRawFacet(j) &&
            listOfRedundantHS.find(V1->getFacet(i))==listOfRedundantHS.end()) {
          // Check the facet is not declared redundant !
          sameHyperplane++;
          commonFacets.push_back(V1->getRawFacet(i));
       }
      }
    }
    // For polyhedral cones neigbourhoodCondition() is Rn::getDimension()-1, for polytopes it is Rn::getDimension()-2.
    if (sameHyperplane >= neigbourhoodCondition()-topologicalCode+1) {
      return true;
    }
    return false;
  }
 
  void fillNeighbourMatrix(std::vector< std::vector<unsigned int> >& neighboursA, unsigned int topologicalCode=1) const {
    if (neighboursA.size() != numberOfGenerators()) {
      std::string errorMessage("PolyhedralCone_Rn::fillNeighbourMatrix wrong matrix size");
      throw std::out_of_range(errorMessage);
    }
    constIteratorOfListOfGeometricObjects< boost::shared_ptr<Generator_Rn> > iteGN_A1(getListOfGenerators());
    //Neighbours_Rn newGeneratorsTool;
    {for (iteGN_A1.begin(); iteGN_A1.end()!=true; iteGN_A1.next()) {
      const boost::shared_ptr<Generator_Rn>& v1_A = iteGN_A1.current();
      constIteratorOfListOfGeometricObjects< boost::shared_ptr<Generator_Rn> > iteGN_A2(getListOfGenerators());
      {for (iteGN_A2.begin(); iteGN_A2.end()!=true; iteGN_A2.next()) {
        const boost::shared_ptr<Generator_Rn>& v2_A = iteGN_A2.current();
        std::vector< HalfSpace_Rn* > commonFacets;
        if (v1_A != v2_A) {
          //neighboursA(iteGN_A1.currentIteratorNumber(), iteGN_A2.currentIteratorNumber()) =
          if (checkNeighbours(v1_A, v2_A, commonFacets, topologicalCode) == true) {
            neighboursA[iteGN_A1.currentIteratorNumber()].push_back(iteGN_A2.currentIteratorNumber());
            //newGeneratorsTool.addGenerator(
            //    commonFacets,
            //    v1_A,
            //    v2_A,
            //    iteGN_A1.currentIteratorNumber(),
            //    iteGN_A2.currentIteratorNumber());
          }
        }
        else {
          //std::cout << "v1=" << iteGN_A1.currentIteratorNumber() << ", v2=" << iteGN_A2.currentIteratorNumber() << std::endl;
          neighboursA[iteGN_A1.currentIteratorNumber()].push_back(iteGN_A2.currentIteratorNumber());
        }
      }}
    }}
    //newGeneratorsTool.dump(std::cout);
    //for (newGeneratorsTool.begin(); newGeneratorsTool.end()!=true; newGeneratorsTool.next()) {
    //  neighboursA(newGeneratorsTool.currentGenInNumber(),  newGeneratorsTool.currentGenOutNumber()) = 1;
    //  neighboursA(newGeneratorsTool.currentGenOutNumber(), newGeneratorsTool.currentGenInNumber()) = 1;
    //}
  }
 
  void fillIrredundantNeighbourMatrix(std::vector< std::vector<unsigned int> >& neighboursA) const {
    if (neighboursA.size() != numberOfGenerators()) {
      std::string errorMessage("PolyhedralCone_Rn::fillIrredundantNeighbourMatrix wrong matrix size");
      throw std::out_of_range(errorMessage);
    }
    constIteratorOfListOfGeometricObjects< boost::shared_ptr<Generator_Rn> > iteGN_A1(getListOfGenerators());
    std::vector< boost::shared_ptr<Generator_Rn_SD> > listOfGenSD;
    // Generator_Rn => Generator_Rn_SD
    // From the current generators make a list of generators dedicated to work in the data structure algorithm.
    // Make sure the list of half-space numbers is sorted as we want to use algorithms further.
    getListOfGeneratorsSD(listOfGenSD);
    Neighbours_Rn newGeneratorsTool;
    unsigned int count_OUT=0;
    std::vector< boost::shared_ptr<Generator_Rn_SD> >::const_iterator iteGN_OUT;
    {for (iteGN_OUT=listOfGenSD.begin(); iteGN_OUT!=listOfGenSD.end(); ++iteGN_OUT, ++count_OUT) {
        unsigned int count_IN=0;
        std::vector< boost::shared_ptr<Generator_Rn_SD> >::const_iterator iteGN_IN;
        {for (iteGN_IN=listOfGenSD.begin(); iteGN_IN!=listOfGenSD.end(); ++iteGN_IN, ++count_IN) {
            std::vector< unsigned int > commonFacets;
            if (count_IN!=count_OUT && checkNeighbours(*iteGN_IN, *iteGN_OUT, commonFacets) == true)
              newGeneratorsTool.addGenerator(commonFacets,
                                             count_IN,
                                             count_OUT,
                                             HalfSpace_Rn::hs_IN_OR_OUT);
          }
        }
      }
    }
    for (newGeneratorsTool.begin(); newGeneratorsTool.end()!=true; newGeneratorsTool.next()) {
      neighboursA[newGeneratorsTool.currentGenInNumber()].push_back(newGeneratorsTool.currentGenOutNumber());
      neighboursA[newGeneratorsTool.currentGenOutNumber()].push_back(newGeneratorsTool.currentGenInNumber());
    }
 
  }
 
  virtual bool checkTopologyAndGeometry(bool check_all=false) const {
    std::cerr.precision(15);
    std::cout.precision(15);
    std::cout << "# Dimension NumberOfHalfspaces NumberOfGenerators" << std::endl;
    std::cout << dimension() << " " << numberOfHalfSpaces() << " " << numberOfGenerators() << std::endl;
 
    // Check that all generators have at least (n-1) facets in the case of a polyhedral cone and are inside the H-representation.
    bool checkGeneratorsOK = checkGenerators(_listOfGenerators, _listOfHalfSpaces, check_all);
 
    // Check that all facets have at least (n-1) generators in the case of a polyhedral cone.
    bool checkFacetsOK = checkFacets();
 
    return (checkGeneratorsOK==true && checkFacetsOK==true);
  }
 
  HalfSpace_Rn::State checkPoint(const Point_Rn& thisPoint) const;
 
  HalfSpace_Rn::State checkGenerator(const boost::shared_ptr<Generator_Rn>& thisGen) const {
    unsigned int dim = thisGen->dimension();
    Point_Rn thisPoint(dim);
    {for (unsigned int i=0; i<dim; ++i) {
      thisPoint.setCoordinate( i, thisGen->getCoordinate(i) );
    }}
    return checkPoint(thisPoint);
  }
 
  HalfSpace_Rn::State checkPoint(
      const boost::shared_ptr<Generator_Rn>& point,
      const boost::shared_ptr<HalfSpace_Rn>& halfSpace,
      double halfSpaceNorm) const;
 
  bool checkDuplicateGenerators(unsigned int& a, unsigned int& b);
 
  void checkGenerator(unsigned int vtxNumber, std::ostream &this_ostream) const {
    if (vtxNumber >= _listOfGenerators.size())
      throw std::out_of_range("PolyhedralCone_Rn::checkGenerator(unsigned int fctNumber, std::ostream &this_ostream) inadequate generator number!");
    this_ostream.precision(15);
    double TOL=Rn::getTolerance();
    this_ostream << "Check generator " << vtxNumber << std::endl;
    _listOfGenerators[vtxNumber]->dump(std::cout);
    this_ostream << std::endl;
    {for (unsigned int i=0; i< _listOfGenerators[vtxNumber]->numberOfFacets(); ++i) {
      this_ostream << getHalfSpaceNumber( _listOfGenerators[vtxNumber]->getFacet(i) ) << " ";
    }}
    this_ostream << std::endl;
    this_ostream << std::endl;
    {for (unsigned int i=0; i< _listOfGenerators[vtxNumber]->numberOfFacets(); ++i) {
      this_ostream << "H" << getHalfSpaceNumber( _listOfGenerators[vtxNumber]->getFacet(i) ) << std::endl;
      _listOfGenerators[vtxNumber]->getFacet(i)->dump(this_ostream);
      this_ostream << std::endl;
    }}
    this_ostream << std::endl;
    this_ostream << "Check generator " << vtxNumber << std::endl;
    constIteratorOfListOfGeometricObjects< boost::shared_ptr<HalfSpace_Rn> > iteHS_B(_listOfHalfSpaces);
    {for (iteHS_B.begin(); iteHS_B.end()!=true; iteHS_B.next()) {
      double halfSpaceNorm = std::inner_product(iteHS_B.current()->begin(), iteHS_B.current()->end(), iteHS_B.current()->begin(), 0.);
      halfSpaceNorm = sqrt(halfSpaceNorm);
      double scalarProduct = std::inner_product(_listOfGenerators[vtxNumber]->begin(), _listOfGenerators[vtxNumber]->end(), iteHS_B.current()->begin(), 0.);
      double distanceToHyperplane = (scalarProduct+iteHS_B.current()->getConstant()) / halfSpaceNorm;
      this_ostream << "H" << iteHS_B.currentIteratorNumber() << "\t";
      this_ostream << ": " << distanceToHyperplane;
      if (distanceToHyperplane<TOL && distanceToHyperplane>-TOL)
        this_ostream << " (*)";
      this_ostream << std::endl;
    }}
  }
 
  bool checkGenerators(
      const listOfGeometricObjects< boost::shared_ptr<Generator_Rn> >& listGenA,
      const listOfGeometricObjects< boost::shared_ptr<HalfSpace_Rn> >& listHSB, bool check_all=false) const {
    std::cout << "Check generators..... ";
    double TOL = Rn::getTolerance();
    std::vector< std::vector<int> > listOfFacetsPerGenerator(listGenA.size());
    constIteratorOfListOfGeometricObjects< boost::shared_ptr<HalfSpace_Rn> > iteHS_B(listHSB);
    {for (iteHS_B.begin(); iteHS_B.end()!=true; iteHS_B.next()) {
      double halfSpaceNorm =
        std::inner_product(iteHS_B.current()->begin(), iteHS_B.current()->end(), iteHS_B.current()->begin(), 0.);
      halfSpaceNorm = sqrt(halfSpaceNorm);
      constIteratorOfListOfGeometricObjects< boost::shared_ptr<Generator_Rn> > iteGN_A_(listGenA);
      {for (iteGN_A_.begin(); iteGN_A_.end()!=true; iteGN_A_.next()) {
        double scalarProduct = std::inner_product(iteGN_A_.current()->begin(), iteGN_A_.current()->end(), iteHS_B.current()->begin(), 0.);
        double distanceToHyperplane = (scalarProduct+iteHS_B.current()->getConstant()) / halfSpaceNorm;
        if (distanceToHyperplane > TOL) {
          // HalfSpace_Rn::hs_IN: nothing to do.
        }
        else if (distanceToHyperplane < -TOL) {
          // HalfSpace_Rn::hs_OUT
          std::cerr << "\t### G" << iteGN_A_.currentIteratorNumber() << " is out for half-space ";
          std::cerr << iteHS_B.currentIteratorNumber() << std::endl;
          std::cerr << "G" << iteGN_A_.currentIteratorNumber() << " = ";
          iteGN_A_.current()->dump(std::cerr);
          std::cerr << " (d=" << distanceToHyperplane << ")" << std::endl;
        }
        else {
          // HalfSpace_Rn::hs_ON
          listOfFacetsPerGenerator[ iteGN_A_.currentIteratorNumber() ].push_back( iteHS_B.currentIteratorNumber() );
        }
      }}
    }}
    bool checkGeneratorsOK=true;
    constIteratorOfListOfGeometricObjects< boost::shared_ptr<Generator_Rn> > iteGN_A(listGenA);
    {for (iteGN_A.begin(); iteGN_A.end()!=true; iteGN_A.next()) {
      if (listOfFacetsPerGenerator[ iteGN_A.currentIteratorNumber() ].empty()==true ||
          listOfFacetsPerGenerator[ iteGN_A.currentIteratorNumber() ].size() < numberOfGeneratorsPerFacet())
        checkGeneratorsOK = false;
      if (check_all &&
          listOfFacetsPerGenerator[ iteGN_A.currentIteratorNumber() ].size() != iteGN_A.current()->numberOfFacets())
        checkGeneratorsOK = false;
    }}
    if (checkGeneratorsOK == true)
      std::cout << "OK" << std::endl;
    else {
      std::cout << "KO" << std::endl;
      {for (iteGN_A.begin(); iteGN_A.end()!=true; iteGN_A.next()) {
        if (listOfFacetsPerGenerator[ iteGN_A.currentIteratorNumber() ].empty()==true ||
            listOfFacetsPerGenerator[ iteGN_A.currentIteratorNumber() ].size() < numberOfGeneratorsPerFacet())
          std::cerr << "\t### G"  <<  iteGN_A.currentIteratorNumber() << " has only "
            << listOfFacetsPerGenerator[ iteGN_A.currentIteratorNumber() ].size() << " facets." << std::endl;
        if (listOfFacetsPerGenerator[ iteGN_A.currentIteratorNumber() ].size() != iteGN_A.current()->numberOfFacets()) {
          std::cerr << "\t### G" << iteGN_A.currentIteratorNumber() << " has different facet size "
            << listOfFacetsPerGenerator[ iteGN_A.currentIteratorNumber() ].size() << " and "
            << iteGN_A.current()->numberOfFacets() << std::endl;
          //std::copy(
            //listOfFacetsPerGenerator[ iteGN_A.currentIteratorNumber() ].begin(),
            //listOfFacetsPerGenerator[ iteGN_A.currentIteratorNumber() ].end(), std::ostream_iterator<unsigned int>(std::cout, " ") );
        }
      }}
    }
    return checkGeneratorsOK;
  }
 
  double checkVolumeEquality(const boost::shared_ptr<PolyhedralCone_Rn>& B) const {
//     bool res1, res2;
//     std::cout << "Check generators of A inside the half-spaces of B ..... ";
//     res1 = checkGenerators(   _listOfGenerators, B->_listOfHalfSpaces, false);
//     std::cout << "Check generators of B inside the half-spaces of A ..... ";
//     res2 = checkGenerators(B->_listOfGenerators,    _listOfHalfSpaces, false);
//     return (res1 || res2);
    double TOL = Rn::getTolerance();
    bool isOutFromB = false;
    double distFromB = std::numeric_limits<int>::max();
    {
      std::vector< std::vector<int> > listOfFacetsPerGenerator(_listOfGenerators.size());
      constIteratorOfListOfGeometricObjects< boost::shared_ptr<HalfSpace_Rn> > iteHS_B(B->_listOfHalfSpaces);
      {for (iteHS_B.begin(); iteHS_B.end()!=true; iteHS_B.next()) {
        double halfSpaceNorm =
          std::inner_product(iteHS_B.current()->begin(), iteHS_B.current()->end(), iteHS_B.current()->begin(), 0.);
        halfSpaceNorm = sqrt(halfSpaceNorm);
        constIteratorOfListOfGeometricObjects< boost::shared_ptr<Generator_Rn> > iteGN_A_(_listOfGenerators);
        {for (iteGN_A_.begin(); iteGN_A_.end()!=true; iteGN_A_.next()) {
          double scalarProduct = std::inner_product(iteGN_A_.current()->begin(), iteGN_A_.current()->end(), iteHS_B.current()->begin(), 0.);
          double distanceToHyperplane = (scalarProduct+iteHS_B.current()->getConstant()) / halfSpaceNorm;
          if (distanceToHyperplane > TOL) {
            // HalfSpace_Rn::hs_IN: nothing to do.
          }
          else if (distanceToHyperplane < -TOL) {
            // HalfSpace_Rn::hs_OUT
            //std::cerr << "\t### G" << iteGN_A_.currentIteratorNumber() << " is out for half-space ";
            //std::cerr << iteHS_B.currentIteratorNumber() << std::endl;
            //std::cerr << "G" << iteGN_A_.currentIteratorNumber() << " = ";
            //iteGN_A_.current()->dump(std::cerr);
            //std::cerr << " (d=" << distanceToHyperplane << ")" << std::endl;
            isOutFromB = true;
          }
          else {
            // HalfSpace_Rn::hs_ON
            listOfFacetsPerGenerator[ iteGN_A_.currentIteratorNumber() ].push_back( iteHS_B.currentIteratorNumber() );
          }
          if (distanceToHyperplane < distFromB)
            distFromB = distanceToHyperplane;
        }}
      }}
    }
    bool isOutFromA = false;
    double distFromA = std::numeric_limits<int>::max();
    {
      std::vector< std::vector<int> > listOfFacetsPerGenerator(B->_listOfGenerators.size());
      constIteratorOfListOfGeometricObjects< boost::shared_ptr<HalfSpace_Rn> > iteHS_A(_listOfHalfSpaces);
      {for (iteHS_A.begin(); iteHS_A.end()!=true; iteHS_A.next()) {
        double halfSpaceNorm =
          std::inner_product(iteHS_A.current()->begin(), iteHS_A.current()->end(), iteHS_A.current()->begin(), 0.);
        halfSpaceNorm = sqrt(halfSpaceNorm);
        constIteratorOfListOfGeometricObjects< boost::shared_ptr<Generator_Rn> > iteGN_B_(B->_listOfGenerators);
        {for (iteGN_B_.begin(); iteGN_B_.end()!=true; iteGN_B_.next()) {
          double scalarProduct = std::inner_product(iteGN_B_.current()->begin(), iteGN_B_.current()->end(), iteHS_A.current()->begin(), 0.);
          double distanceToHyperplane = (scalarProduct+iteHS_A.current()->getConstant()) / halfSpaceNorm;
          if (distanceToHyperplane > TOL) {
            // HalfSpace_Rn::hs_IN: nothing to do.
          }
          else if (distanceToHyperplane < -TOL) {
            // HalfSpace_Rn::hs_OUT
            //std::cerr << "\t### G" << iteGN_A_.currentIteratorNumber() << " is out for half-space ";
            //std::cerr << iteHS_B.currentIteratorNumber() << std::endl;
            //std::cerr << "G" << iteGN_A_.currentIteratorNumber() << " = ";
            //iteGN_A_.current()->dump(std::cerr);
            //std::cerr << " (d=" << distanceToHyperplane << ")" << std::endl;
            isOutFromA = true;
          }
          else {
            // HalfSpace_Rn::hs_ON
            listOfFacetsPerGenerator[ iteGN_B_.currentIteratorNumber() ].push_back( iteHS_A.currentIteratorNumber() );
          }
          if (distanceToHyperplane < distFromA)
            distFromA = distanceToHyperplane;
        }}
      }}
    }
    if (isOutFromA == true)
      std::cout << "Outside A : distance = " << distFromA << std::endl;
    if (isOutFromB == true)
      std::cout << "Outside B : distance = " << distFromB << std::endl;
    return std::min(distFromA, distFromB);
  }
 
  void removeGenerator(unsigned int j) {_listOfGenerators.removeGeometricObject(j);}
 
  void removeGenerators(const std::set< boost::shared_ptr<Generator_Rn> >& setToRemove) {_listOfGenerators.removeGeometricObjects(setToRemove);}
 
  virtual bool checkEdges() const {return true;}
 
  void checkFacet(unsigned int fctNumber, std::ostream &this_ostream) const {
    if (fctNumber >= _listOfHalfSpaces.size())
      throw std::out_of_range("PolyhedralCone_Rn::checkFacet(unsigned int fctNumber, std::ostream &this_ostream) inadequate facet number!");
    this_ostream.precision(15);
    this_ostream << "Check facet " << fctNumber << std::endl;
    _listOfHalfSpaces[fctNumber]->dump(this_ostream);
    this_ostream << std::endl;
    constIteratorOfListOfGeometricObjects< boost::shared_ptr<Generator_Rn> > iteGN(_listOfGenerators);
    double halfSpaceNorm = std::inner_product(
        _listOfHalfSpaces[fctNumber]->begin(), _listOfHalfSpaces[fctNumber]->end(), _listOfHalfSpaces[fctNumber]->begin(), 0.);
    halfSpaceNorm = sqrt(halfSpaceNorm);
    {for (iteGN.begin(); iteGN.end()!=true; iteGN.next()) {
      HalfSpace_Rn::State currentState = checkPoint(iteGN.current(), _listOfHalfSpaces[fctNumber], halfSpaceNorm);
      if (currentState == HalfSpace_Rn::hs_ON)
        this_ostream << iteGN.currentIteratorNumber() << " ";
    }}
    //std::vector< boost::shared_ptr<Generator_Rn> > arrayOfGen;
    //this_ostream << "Maximize" << std::endl << "obj:";
    //this_ostream << "x_1" << std::endl << std::endl << "Subject To" << std::endl;
    //{for (unsigned int j=0; j<Rn::getDimension(); ++j) {
      //this_ostream << "r" << j << ":" << std::endl;
      //{for (unsigned int i=0; i<arrayOfGen.size(); ++i) {
        //if (arrayOfGen[i]->getCoordinate(j) > 0.)
          //this_ostream << " + ";
        //else
          //this_ostream << " - ";
        //this_ostream << fabs(arrayOfGen[i]->getCoordinate(j)) << "x_" << i << " ";
      //}}
      //this_ostream << " = " << std::endl;
    //}}
    //this_ostream << "r" << Rn::getDimension() << ":" << std::endl;
    //{for (unsigned int i=0; i<arrayOfGen.size(); ++i) {
      //this_ostream << " + x_" << i;
    //}}
    //this_ostream << " = 1." << std::endl << std::endl;
    //this_ostream << "Bounds" << std::endl;
    //{for (unsigned int i=0; i<arrayOfGen.size(); ++i) {
      //this_ostream << 0. << " <= x_" << i << std::endl;
    //}}
    //this_ostream << std::endl;
    //this_ostream << std::endl << "End" << std::endl;
    this_ostream << std::endl;
    double TOL=Rn::getTolerance();
    {for (iteGN.begin(); iteGN.end()!=true; iteGN.next()) {
      double scalarProduct = std::inner_product(iteGN.current()->begin(),iteGN.current()->end(), _listOfHalfSpaces[fctNumber]->begin(), 0.);
      double distanceToHyperplane = (scalarProduct+ _listOfHalfSpaces[fctNumber]->getConstant()) / halfSpaceNorm;
      this_ostream << "V" << iteGN.currentIteratorNumber() << "\t";
      this_ostream << ": " << distanceToHyperplane;
      if (distanceToHyperplane<TOL && distanceToHyperplane>-TOL)
        this_ostream << " (*)";
      this_ostream << std::endl;
    }}
  }
 
  bool checkFacets() const {
    std::cout << "Check facets......... ";
    bool checkFacetsOK=true;
    unsigned int j=0;
    std::vector<int> listOfGeneratorsPerFacetON(numberOfGenerators());
    std::vector<int> listOfGeneratorsPerFacetOUT(numberOfGenerators());
    constIteratorOfListOfGeometricObjects< boost::shared_ptr<HalfSpace_Rn> > iteHS(_listOfHalfSpaces);
    {for (iteHS.begin(); iteHS.end()!=true; iteHS.next()) {
      listOfGeneratorsPerFacetON.clear();
      listOfGeneratorsPerFacetOUT.clear();
      constIteratorOfListOfGeometricObjects< boost::shared_ptr<Generator_Rn> > iteGN(_listOfGenerators);
      double halfSpaceNorm = std::inner_product(iteHS.current()->begin(), iteHS.current()->end(), iteHS.current()->begin(), 0.);
      halfSpaceNorm = sqrt(halfSpaceNorm);
      {for (iteGN.begin(); iteGN.end()!=true; iteGN.next()) {
        HalfSpace_Rn::State currentState = checkPoint(iteGN.current(), iteHS.current(), halfSpaceNorm);
        unsigned int i=iteGN.currentIteratorNumber();
        if (currentState == HalfSpace_Rn::hs_ON)
          listOfGeneratorsPerFacetON.push_back(i);
        else if (currentState == HalfSpace_Rn::hs_OUT)
          listOfGeneratorsPerFacetOUT.push_back(i);
      }}
      if (listOfGeneratorsPerFacetON.empty() == true || listOfGeneratorsPerFacetON.size() < numberOfGeneratorsPerFacet())
        std::cout<< std::endl << "\t=== Facet " << j << " has only " << listOfGeneratorsPerFacetON.size() << " generators."<< std::endl;
      if (listOfGeneratorsPerFacetOUT.empty() != true) {
        {for (unsigned int i=0; i<listOfGeneratorsPerFacetOUT.size(); i++) {
          checkFacetsOK = false;
          std::cout<< std::endl;
          std::cout << "\t### Facet " << j << " excludes generator " << listOfGeneratorsPerFacetOUT[i] << std::endl;
        }}
      }
      j++;
    }}
    if (checkFacetsOK == true)
      std::cout << "OK" << std::endl;
    return checkFacetsOK;
  }
 
  bool checkEquality(const boost::shared_ptr<PolyhedralCone_Rn>& B, bool getFaceMapping=false) const;
 
  void getGeneratorsPerFacet(std::vector< std::vector< unsigned int > >& listOfGeneratorsPerFacet) const {
    listOfGeneratorsPerFacet.clear();
    listOfGeneratorsPerFacet.resize(numberOfHalfSpaces());
    constIteratorOfListOfGeometricObjects< boost::shared_ptr<HalfSpace_Rn> > iteHS(getListOfHalfSpaces());
    {for (iteHS.begin(); iteHS.end()!=true; iteHS.next()) {
      unsigned int facetNb = iteHS.currentIteratorNumber();
      const boost::shared_ptr<HalfSpace_Rn>& thisFacet = iteHS.current();
      constIteratorOfListOfGeometricObjects< boost::shared_ptr<Generator_Rn> > iteGN(getListOfGenerators());
      {for (iteGN.begin(); iteGN.end()!=true; iteGN.next()) {
        {for (unsigned int i=0; i<iteGN.current()->numberOfFacets(); ++i) {
          if (iteGN.current()->getFacet(i) == thisFacet) {
            unsigned int vtxNb = iteGN.currentIteratorNumber();
            listOfGeneratorsPerFacet[facetNb].push_back(vtxNb);
          }
        }}
      }}
    }}
  }
 
 
  // ALGORITHMS
 
  void negate() {_listOfHalfSpaces.negate(); _listOfGenerators.negate();}
 
  virtual double createTruncatedGenerator(
      const boost::shared_ptr<Generator_Rn_SD>& y,
      const boost::shared_ptr<Generator_Rn_SD>& z,
      boost::shared_ptr<Generator_Rn_SD> newG, double ay, double az, double b=0.) const;
 
  //virtual void createTruncatedGenerator(
  //  const Generator_Rn_SD& y,
  //  const Generator_Rn_SD& z,
  //  Generator_Rn_SD& newG, double ay, double az, double b=0.) const;
 
  boost::shared_ptr<PolyhedralCone_Rn> computeDualPolyhedralCone() const;
 
  virtual void dump(std::ostream &this_ostream) const;
 
  void dumpScilab(std::ostream &this_ostream) const;
 
  virtual void createBoundingBox(double) {}
 
  virtual void createBoundingSimplex(double) {}
 
 
 protected:
  listOfGeometricObjects< boost::shared_ptr<HalfSpace_Rn> > _listOfHalfSpaces;
  listOfGeometricObjects< boost::shared_ptr<Generator_Rn> > _listOfGenerators;
};
 
 
#endif // POLYHEDRALCONE_Rn