mesh_base.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 * Point Cloud Library (PCL) - www.pointclouds.org
 * Copyright (c) 2009-2012, Willow Garage, Inc.
 * Copyright (c) 2012-, Open Perception, Inc.
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above
 *    copyright notice, this list of conditions and the following
 *    disclaimer in the documentation and/or other materials provided
 *    with the distribution.
 *  * Neither the name of the copyright holder(s) nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */

#ifndef PCL_GEOMETRY_MESH_BASE_H
#define PCL_GEOMETRY_MESH_BASE_H

#include <vector>

#include <pcl/geometry/boost.h>
#include <pcl/geometry/eigen.h>
#include <pcl/geometry/mesh_circulators.h>
#include <pcl/geometry/mesh_indices.h>
#include <pcl/geometry/mesh_elements.h>
#include <pcl/geometry/mesh_traits.h>
#include <pcl/point_cloud.h>

// Global variables used during testing

#ifdef PCL_GEOMETRY_MESH_BASE_TEST_DELETE_FACE_MANIFOLD_2
namespace pcl
{
  namespace geometry
  {
    bool g_pcl_geometry_mesh_base_test_delete_face_manifold_2_success;
  } // End namespace geometry
} // End namespace pcl
#endif

// Forward declarations

namespace pcl
{
  namespace geometry
  {
    template <class MeshT>
    class MeshIO;
  } // End namespace geometry
} // End namespace pcl

// MeshBase

namespace pcl
{
  namespace geometry
  {
    template <class DerivedT, class MeshTraitsT, class MeshTagT>
    class MeshBase
    {
      public:

        typedef MeshBase <DerivedT, MeshTraitsT, MeshTagT> Self;
        typedef boost::shared_ptr <Self>                   Ptr;
        typedef boost::shared_ptr <const Self>             ConstPtr;

        typedef DerivedT Derived;

        // These have to be defined in the traits class.
        typedef typename MeshTraitsT::VertexData   VertexData;
        typedef typename MeshTraitsT::HalfEdgeData HalfEdgeData;
        typedef typename MeshTraitsT::EdgeData     EdgeData;
        typedef typename MeshTraitsT::FaceData     FaceData;
        typedef typename MeshTraitsT::IsManifold   IsManifold;

        // Check if the mesh traits are defined correctly.
        BOOST_CONCEPT_ASSERT ((boost::Convertible <IsManifold, bool>));

        typedef MeshTagT MeshTag;

        // Data
        typedef boost::integral_constant <bool, !boost::is_same <VertexData  , pcl::geometry::NoData>::value> HasVertexData;
        typedef boost::integral_constant <bool, !boost::is_same <HalfEdgeData, pcl::geometry::NoData>::value> HasHalfEdgeData;
        typedef boost::integral_constant <bool, !boost::is_same <EdgeData    , pcl::geometry::NoData>::value> HasEdgeData;
        typedef boost::integral_constant <bool, !boost::is_same <FaceData    , pcl::geometry::NoData>::value> HasFaceData;

        typedef pcl::PointCloud <VertexData>   VertexDataCloud;
        typedef pcl::PointCloud <HalfEdgeData> HalfEdgeDataCloud;
        typedef pcl::PointCloud <EdgeData>     EdgeDataCloud;
        typedef pcl::PointCloud <FaceData>     FaceDataCloud;

        // Indices
        typedef pcl::geometry::VertexIndex   VertexIndex;
        typedef pcl::geometry::HalfEdgeIndex HalfEdgeIndex;
        typedef pcl::geometry::EdgeIndex     EdgeIndex;
        typedef pcl::geometry::FaceIndex     FaceIndex;

        typedef std::vector <VertexIndex>   VertexIndices;
        typedef std::vector <HalfEdgeIndex> HalfEdgeIndices;
        typedef std::vector <EdgeIndex>     EdgeIndices;
        typedef std::vector <FaceIndex>     FaceIndices;

        // Circulators
        typedef pcl::geometry::VertexAroundVertexCirculator           <const Self> VertexAroundVertexCirculator;
        typedef pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator <const Self> OutgoingHalfEdgeAroundVertexCirculator;
        typedef pcl::geometry::IncomingHalfEdgeAroundVertexCirculator <const Self> IncomingHalfEdgeAroundVertexCirculator;
        typedef pcl::geometry::FaceAroundVertexCirculator             <const Self> FaceAroundVertexCirculator;
        typedef pcl::geometry::VertexAroundFaceCirculator             <const Self> VertexAroundFaceCirculator;
        typedef pcl::geometry::InnerHalfEdgeAroundFaceCirculator      <const Self> InnerHalfEdgeAroundFaceCirculator;
        typedef pcl::geometry::OuterHalfEdgeAroundFaceCirculator      <const Self> OuterHalfEdgeAroundFaceCirculator;
        typedef pcl::geometry::FaceAroundFaceCirculator               <const Self> FaceAroundFaceCirculator;

        MeshBase ()
          : vertex_data_cloud_ (),
            half_edge_data_cloud_ (),
            edge_data_cloud_ (),
            face_data_cloud_ (),
            vertices_ (),
            half_edges_ (),
            faces_ (),
            inner_he_ (),
            free_he_ (),
            is_new_ (),
            make_adjacent_ (),
            is_boundary_ (),
            delete_faces_vertex_ (),
            delete_faces_face_ ()
        {
        }

        // addVertex / addFace / deleteVertex / deleteEdge / deleteFace / cleanUp

        inline VertexIndex
        addVertex (const VertexData& vertex_data=VertexData ())
        {
          vertices_.push_back (Vertex ());
          this->addData (vertex_data_cloud_, vertex_data, HasVertexData ());
          return (VertexIndex (static_cast <int> (this->sizeVertices () - 1)));
        }

        inline FaceIndex
        addFace (const VertexIndices& vertices,
                 const FaceData&      face_data      = FaceData (),
                 const EdgeData&      edge_data      = EdgeData (),
                 const HalfEdgeData&  half_edge_data = HalfEdgeData ())
        {
          // NOTE: The derived class has to implement addFaceImpl. If needed it can use the general method addFaceImplBase.
          return (static_cast <Derived*> (this)->addFaceImpl (vertices, face_data, edge_data, half_edge_data));
        }

        void
        deleteVertex (const VertexIndex& idx_vertex)
        {
          assert (this->isValid (idx_vertex));
          if (this->isDeleted (idx_vertex)) return;

          delete_faces_vertex_.clear ();
          FaceAroundVertexCirculator       circ     = this->getFaceAroundVertexCirculator (idx_vertex);
          const FaceAroundVertexCirculator circ_end = circ;
          do
          {
            if (circ.getTargetIndex ().isValid ()) // Check for boundary.
            {
              delete_faces_vertex_.push_back (circ.getTargetIndex ());
            }
          } while (++circ!=circ_end);

          for (FaceIndices::const_iterator it = delete_faces_vertex_.begin (); it!=delete_faces_vertex_.end (); ++it)
          {
            this->deleteFace (*it);
          }
        }

        void
        deleteEdge (const HalfEdgeIndex& idx_he)
        {
          assert (this->isValid (idx_he));
          if (this->isDeleted (idx_he)) return;

          HalfEdgeIndex opposite = this->getOppositeHalfEdgeIndex (idx_he);

          if (this->isBoundary (idx_he))   this->markDeleted (idx_he);
          else                             this->deleteFace (this->getFaceIndex (idx_he));
          if (this->isBoundary (opposite)) this->markDeleted (opposite);
          else                             this->deleteFace (this->getFaceIndex (opposite));
        }

        inline void
        deleteEdge (const EdgeIndex& idx_edge)
        {
          assert (this->isValid (idx_edge));
          this->deleteEdge (pcl::geometry::toHalfEdgeIndex (idx_edge));
          assert (this->isDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, false))); // Bug in this class!
        }

        inline void
        deleteFace (const FaceIndex& idx_face)
        {
          assert (this->isValid (idx_face));
          if (this->isDeleted (idx_face)) return;

          this->deleteFace (idx_face, IsManifold ());
        }

        void
        cleanUp ()
        {
          // Copy the non-deleted mesh elements and store the index to their new position
          const VertexIndices new_vertex_indices =
              this->remove <Vertices, VertexDataCloud, VertexIndices, HasVertexData>
              (vertices_, vertex_data_cloud_);
          const HalfEdgeIndices new_half_edge_indices =
              this->remove <HalfEdges, HalfEdgeDataCloud, HalfEdgeIndices, HasHalfEdgeData>
              (half_edges_, half_edge_data_cloud_);
          const FaceIndices new_face_indices =
              this->remove <Faces, FaceDataCloud, FaceIndices, HasFaceData>
              (faces_, face_data_cloud_);

          // Remove deleted edge data
          if (HasEdgeData::value)
          {
            typename EdgeDataCloud::const_iterator it_ed_old = edge_data_cloud_.begin ();
            typename EdgeDataCloud::iterator       it_ed_new = edge_data_cloud_.begin ();

            HalfEdgeIndices::const_iterator it_ind     = new_half_edge_indices.begin ();
            HalfEdgeIndices::const_iterator it_ind_end = new_half_edge_indices.end ();

            for (; it_ind!=it_ind_end; it_ind+=2, ++it_ed_old)
            {
              if (it_ind->isValid ())
              {
                *it_ed_new++ = *it_ed_old;
              }
            }
            edge_data_cloud_.resize (this->sizeEdges ());
          }

          // Adjust the indices
          for (VertexIterator it = vertices_.begin (); it!=vertices_.end (); ++it)
          {
            if (it->idx_outgoing_half_edge_.isValid ())
            {
              it->idx_outgoing_half_edge_ = new_half_edge_indices [it->idx_outgoing_half_edge_.get ()];
            }
          }

          for (HalfEdgeIterator it = half_edges_.begin (); it!=half_edges_.end (); ++it)
          {
            it->idx_terminating_vertex_ = new_vertex_indices    [it->idx_terminating_vertex_.get ()];
            it->idx_next_half_edge_     = new_half_edge_indices [it->idx_next_half_edge_.get ()];
            it->idx_prev_half_edge_     = new_half_edge_indices [it->idx_prev_half_edge_.get ()];
            if (it->idx_face_.isValid ())
            {
              it->idx_face_ = new_face_indices [it->idx_face_.get ()];
            }
          }

          for (FaceIterator it = faces_.begin (); it!=faces_.end (); ++it)
          {
            it->idx_inner_half_edge_ = new_half_edge_indices [it->idx_inner_half_edge_.get ()];
          }
        }

        // Vertex connectivity

        inline HalfEdgeIndex
        getOutgoingHalfEdgeIndex (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (this->getVertex (idx_vertex).idx_outgoing_half_edge_);
        }

        inline HalfEdgeIndex
        getIncomingHalfEdgeIndex (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (this->getOppositeHalfEdgeIndex (this->getOutgoingHalfEdgeIndex (idx_vertex)));
        }

        // Half-edge connectivity

        inline VertexIndex
        getTerminatingVertexIndex (const HalfEdgeIndex& idx_half_edge) const
        {
          assert (this->isValid (idx_half_edge));
          return (this->getHalfEdge (idx_half_edge).idx_terminating_vertex_);
        }

        inline VertexIndex
        getOriginatingVertexIndex (const HalfEdgeIndex& idx_half_edge) const
        {
          assert (this->isValid (idx_half_edge));
          return (this->getTerminatingVertexIndex (this->getOppositeHalfEdgeIndex (idx_half_edge)));
        }

        inline HalfEdgeIndex
        getOppositeHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge) const
        {
          assert (this->isValid (idx_half_edge));
          // Check if the index is even or odd and return the other index.
          return (HalfEdgeIndex (idx_half_edge.get () & 1 ? idx_half_edge.get () - 1 : idx_half_edge.get () + 1));
        }

        inline HalfEdgeIndex
        getNextHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge) const
        {
          assert (this->isValid (idx_half_edge));
          return (this->getHalfEdge (idx_half_edge).idx_next_half_edge_);
        }

        inline HalfEdgeIndex
        getPrevHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge) const
        {
          assert (this->isValid (idx_half_edge));
          return (this->getHalfEdge (idx_half_edge).idx_prev_half_edge_);
        }

        inline FaceIndex
        getFaceIndex (const HalfEdgeIndex& idx_half_edge) const
        {
          assert (this->isValid (idx_half_edge));
          return (this->getHalfEdge (idx_half_edge).idx_face_);
        }

        inline FaceIndex
        getOppositeFaceIndex (const HalfEdgeIndex& idx_half_edge) const
        {
          assert (this->isValid (idx_half_edge));
          return (this->getFaceIndex (this->getOppositeHalfEdgeIndex (idx_half_edge)));
        }

        // Face connectivity

        inline HalfEdgeIndex
        getInnerHalfEdgeIndex (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (this->getFace (idx_face).idx_inner_half_edge_);
        }

        inline HalfEdgeIndex
        getOuterHalfEdgeIndex (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (this->getOppositeHalfEdgeIndex (this->getInnerHalfEdgeIndex (idx_face)));
        }

        // Circulators

        inline VertexAroundVertexCirculator
        getVertexAroundVertexCirculator (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (VertexAroundVertexCirculator (idx_vertex, this));
        }

        inline VertexAroundVertexCirculator
        getVertexAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge) const
        {
          assert (this->isValid (idx_outgoing_half_edge));
          return (VertexAroundVertexCirculator (idx_outgoing_half_edge, this));
        }

        inline OutgoingHalfEdgeAroundVertexCirculator
        getOutgoingHalfEdgeAroundVertexCirculator (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (OutgoingHalfEdgeAroundVertexCirculator (idx_vertex, this));
        }

        inline OutgoingHalfEdgeAroundVertexCirculator
        getOutgoingHalfEdgeAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge) const
        {
          assert (this->isValid (idx_outgoing_half_edge));
          return (OutgoingHalfEdgeAroundVertexCirculator (idx_outgoing_half_edge, this));
        }

        inline IncomingHalfEdgeAroundVertexCirculator
        getIncomingHalfEdgeAroundVertexCirculator (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (IncomingHalfEdgeAroundVertexCirculator (idx_vertex, this));
        }

        inline IncomingHalfEdgeAroundVertexCirculator
        getIncomingHalfEdgeAroundVertexCirculator (const HalfEdgeIndex& idx_incoming_half_edge) const
        {
          assert (this->isValid (idx_incoming_half_edge));
          return (IncomingHalfEdgeAroundVertexCirculator (idx_incoming_half_edge, this));
        }

        inline FaceAroundVertexCirculator
        getFaceAroundVertexCirculator (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (FaceAroundVertexCirculator (idx_vertex, this));
        }

        inline FaceAroundVertexCirculator
        getFaceAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge) const
        {
          assert (this->isValid (idx_outgoing_half_edge));
          return (FaceAroundVertexCirculator (idx_outgoing_half_edge, this));
        }

        inline VertexAroundFaceCirculator
        getVertexAroundFaceCirculator (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (VertexAroundFaceCirculator (idx_face, this));
        }

        inline VertexAroundFaceCirculator
        getVertexAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const
        {
          assert (this->isValid (idx_inner_half_edge));
          return (VertexAroundFaceCirculator (idx_inner_half_edge, this));
        }

        inline InnerHalfEdgeAroundFaceCirculator
        getInnerHalfEdgeAroundFaceCirculator (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (InnerHalfEdgeAroundFaceCirculator (idx_face, this));
        }

        inline InnerHalfEdgeAroundFaceCirculator
        getInnerHalfEdgeAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const
        {
          assert (this->isValid (idx_inner_half_edge));
          return (InnerHalfEdgeAroundFaceCirculator (idx_inner_half_edge, this));
        }

        inline OuterHalfEdgeAroundFaceCirculator
        getOuterHalfEdgeAroundFaceCirculator (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (OuterHalfEdgeAroundFaceCirculator (idx_face, this));
        }

        inline OuterHalfEdgeAroundFaceCirculator
        getOuterHalfEdgeAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const
        {
          assert (this->isValid (idx_inner_half_edge));
          return (OuterHalfEdgeAroundFaceCirculator (idx_inner_half_edge, this));
        }

        inline FaceAroundFaceCirculator
        getFaceAroundFaceCirculator (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (FaceAroundFaceCirculator (idx_face, this));
        }

        inline FaceAroundFaceCirculator
        getFaceAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const
        {
          assert (this->isValid (idx_inner_half_edge));
          return (FaceAroundFaceCirculator (idx_inner_half_edge, this));
        }

        // isEqualTopology

        bool
        isEqualTopology (const Self& other) const
        {
          if (this->sizeVertices  () != other.sizeVertices  ()) return (false);
          if (this->sizeHalfEdges () != other.sizeHalfEdges ()) return (false);
          if (this->sizeFaces     () != other.sizeFaces     ()) return (false);

          for (unsigned int i=0; i<this->sizeVertices (); ++i)
          {
            if (this->getOutgoingHalfEdgeIndex (VertexIndex (i)) !=
                other.getOutgoingHalfEdgeIndex (VertexIndex (i))) return (false);
          }

          for (unsigned int i=0; i<this->sizeHalfEdges (); ++i)
          {
            if (this->getTerminatingVertexIndex (HalfEdgeIndex (i)) !=
                other.getTerminatingVertexIndex (HalfEdgeIndex (i))) return (false);

            if (this->getNextHalfEdgeIndex (HalfEdgeIndex (i)) !=
                other.getNextHalfEdgeIndex (HalfEdgeIndex (i))) return (false);

            if (this->getPrevHalfEdgeIndex (HalfEdgeIndex (i)) !=
                other.getPrevHalfEdgeIndex (HalfEdgeIndex (i))) return (false);

            if (this->getFaceIndex (HalfEdgeIndex (i)) !=
                other.getFaceIndex (HalfEdgeIndex (i))) return (false);
          }

          for (unsigned int i=0; i<this->sizeFaces (); ++i)
          {
            if (this->getInnerHalfEdgeIndex (FaceIndex (i)) !=
                other.getInnerHalfEdgeIndex (FaceIndex (i))) return (false);
          }

          return (true);
        }

        // isValid

        inline bool
        isValid (const VertexIndex& idx_vertex) const
        {
          return (idx_vertex >= VertexIndex (0) && idx_vertex < VertexIndex (int (vertices_.size ())));
        }

        inline bool
        isValid (const HalfEdgeIndex& idx_he) const
        {
          return (idx_he >= HalfEdgeIndex (0) && idx_he < HalfEdgeIndex (half_edges_.size ()));
        }

        inline bool
        isValid (const EdgeIndex& idx_edge) const
        {
          return (idx_edge >= EdgeIndex (0) && idx_edge < EdgeIndex (half_edges_.size () / 2));
        }

        inline bool
        isValid (const FaceIndex& idx_face) const
        {
          return (idx_face >= FaceIndex (0) && idx_face < FaceIndex (faces_.size ()));
        }

        // isDeleted

        inline bool
        isDeleted (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (!this->getOutgoingHalfEdgeIndex (idx_vertex).isValid ());
        }

        inline bool
        isDeleted (const HalfEdgeIndex& idx_he) const
        {
          assert (this->isValid (idx_he));
          return (!this->getTerminatingVertexIndex (idx_he).isValid ());
        }

        inline bool
        isDeleted (const EdgeIndex& idx_edge) const
        {
          assert (this->isValid (idx_edge));
          return (this->isDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, true)) ||
                  this->isDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, false)));
        }

        inline bool
        isDeleted (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (!this->getInnerHalfEdgeIndex (idx_face).isValid ());
        }

        // isIsolated

        inline bool
        isIsolated (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (!this->getOutgoingHalfEdgeIndex (idx_vertex).isValid ());
        }

        // isBoundary

        inline bool
        isBoundary (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          if (this->isIsolated (idx_vertex)) return (true);
          return (this->isBoundary (this->getOutgoingHalfEdgeIndex (idx_vertex)));
        }

        inline bool
        isBoundary (const HalfEdgeIndex& idx_he) const
        {
          assert (this->isValid (idx_he));
          return (!this->getFaceIndex (idx_he).isValid ());
        }

        inline bool
        isBoundary (const EdgeIndex& idx_edge) const
        {
          assert (this->isValid (idx_edge));
          const HalfEdgeIndex& idx = pcl::geometry::toHalfEdgeIndex (idx_edge);
          return (this->isBoundary (idx) || this->isBoundary (this->getOppositeHalfEdgeIndex (idx)));
        }

        template <bool CheckVerticesT> inline bool
        isBoundary (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (this->isBoundary (idx_face, boost::integral_constant <bool, CheckVerticesT> ()));
        }

        inline bool
        isBoundary (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (this->isBoundary (idx_face, boost::true_type ()));
        }

        // isManifold

        inline bool
        isManifold (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          if (this->isIsolated (idx_vertex)) return (true);
          return (this->isManifold (idx_vertex, IsManifold ()));
        }

        inline bool
        isManifold () const
        {
          return (this->isManifold (IsManifold ()));
        }

        // size

        inline size_t
        sizeVertices () const
        {
          return (vertices_.size ());
        }

        inline size_t
        sizeHalfEdges () const
        {
          assert (half_edges_.size () % 2 == 0); // This would be a bug in the mesh.
          return (half_edges_.size ());
        }

        inline size_t
        sizeEdges () const
        {
          assert (half_edges_.size () % 2 == 0); // This would be a bug in the mesh.
          return (half_edges_.size () / 2);
        }

        inline size_t
        sizeFaces () const
        {
          return (faces_.size ());
        }

        // empty

        inline bool
        empty () const
        {
          return (this->emptyVertices () && this->emptyEdges () && this->emptyFaces ());
        }

        inline bool
        emptyVertices () const
        {
          return (vertices_.empty ());
        }

        inline bool
        emptyEdges () const
        {
          return (half_edges_.empty ());
        }

        inline bool
        emptyFaces () const
        {
          return (faces_.empty ());
        }

        // reserve

        inline void
        reserveVertices (const size_t n)
        {
          vertices_.reserve (n);
          this->reserveData (vertex_data_cloud_, n, HasVertexData ());
        }

        inline void
        reserveEdges (const size_t n)
        {
          half_edges_.reserve (2*n);
          this->reserveData (half_edge_data_cloud_, 2*n, HasHalfEdgeData ());
          this->reserveData (edge_data_cloud_     ,   n, HasEdgeData     ());
        }

        inline void
        reserveFaces (const size_t n)
        {
          faces_.reserve (n);
          this->reserveData (face_data_cloud_, n, HasFaceData ());
        }

        // resize

        inline void
        resizeVertices (const size_t n, const VertexData& data = VertexData ())
        {
          vertices_.resize (n);
          this->resizeData (vertex_data_cloud_, n, data, HasVertexData ());
        }

        inline void
        resizeEdges (const size_t       n,
                     const EdgeData&    edge_data = EdgeData (),
                     const HalfEdgeData he_data   = HalfEdgeData ())
        {
          half_edges_.resize (2*n);
          this->resizeData (half_edge_data_cloud_, 2*n, he_data  , HasHalfEdgeData ());
          this->resizeData (edge_data_cloud_     ,   n, edge_data, HasEdgeData     ());
        }

        inline void
        resizeFaces (const size_t n, const FaceData& data = FaceData ())
        {
          faces_.resize (n);
          this->resizeData (face_data_cloud_, n, data, HasFaceData ());
        }

        // clear

        void
        clear ()
        {
          vertices_.clear ();
          half_edges_.clear ();
          faces_.clear ();

          this->clearData (vertex_data_cloud_   , HasVertexData   ());
          this->clearData (half_edge_data_cloud_, HasHalfEdgeData ());
          this->clearData (edge_data_cloud_     , HasEdgeData     ());
          this->clearData (face_data_cloud_     , HasFaceData     ());
        }

        // get / set the vertex data cloud

        inline VertexDataCloud&
        getVertexDataCloud ()
        {
          return (vertex_data_cloud_);
        }

        inline VertexDataCloud
        getVertexDataCloud () const
        {
          return (vertex_data_cloud_);
        }

        inline bool
        setVertexDataCloud (const VertexDataCloud& vertex_data_cloud)
        {
          if (vertex_data_cloud.size () == vertex_data_cloud_.size ())
          {
            vertex_data_cloud_ = vertex_data_cloud;
            return (true);
          }
          else
          {
            return (false);
          }
        }

        // get / set the half-edge data cloud

        inline HalfEdgeDataCloud&
        getHalfEdgeDataCloud ()
        {
          return (half_edge_data_cloud_);
        }

        inline HalfEdgeDataCloud
        getHalfEdgeDataCloud () const
        {
          return (half_edge_data_cloud_);
        }

        inline bool
        setHalfEdgeDataCloud (const HalfEdgeDataCloud& half_edge_data_cloud)
        {
          if (half_edge_data_cloud.size () == half_edge_data_cloud_.size ())
          {
            half_edge_data_cloud_ = half_edge_data_cloud;
            return (true);
          }
          else
          {
            return (false);
          }
        }

        // get / set the edge data cloud

        inline EdgeDataCloud&
        getEdgeDataCloud ()
        {
          return (edge_data_cloud_);
        }

        inline EdgeDataCloud
        getEdgeDataCloud () const
        {
          return (edge_data_cloud_);
        }

        inline bool
        setEdgeDataCloud (const EdgeDataCloud& edge_data_cloud)
        {
          if (edge_data_cloud.size () == edge_data_cloud_.size ())
          {
            edge_data_cloud_ = edge_data_cloud;
            return (true);
          }
          else
          {
            return (false);
          }
        }

        // get / set the face data cloud

        inline FaceDataCloud&
        getFaceDataCloud ()
        {
          return (face_data_cloud_);
        }

        inline FaceDataCloud
        getFaceDataCloud () const
        {
          return (face_data_cloud_);
        }

        inline bool
        setFaceDataCloud (const FaceDataCloud& face_data_cloud)
        {
          if (face_data_cloud.size () == face_data_cloud_.size ())
          {
            face_data_cloud_ = face_data_cloud;
            return (true);
          }
          else
          {
            return (false);
          }
        }

        // getVertexIndex / getHalfEdgeIndex / getEdgeIndex / getFaceIndex

        inline VertexIndex
        getVertexIndex (const VertexData& vertex_data) const
        {
          if (HasVertexData::value)
          {
            assert (&vertex_data >= &vertex_data_cloud_.front () && &vertex_data <= &vertex_data_cloud_.back ());
            return (VertexIndex (std::distance (&vertex_data_cloud_.front (), &vertex_data)));
          }
          else
          {
            return (VertexIndex ());
          }
        }

        inline HalfEdgeIndex
        getHalfEdgeIndex (const HalfEdgeData& half_edge_data) const
        {
          if (HasHalfEdgeData::value)
          {
            assert (&half_edge_data >= &half_edge_data_cloud_.front () && &half_edge_data <= &half_edge_data_cloud_.back ());
            return (HalfEdgeIndex (std::distance (&half_edge_data_cloud_.front (), &half_edge_data)));
          }
          else
          {
            return (HalfEdgeIndex ());
          }
        }

        inline EdgeIndex
        getEdgeIndex (const EdgeData& edge_data) const
        {
          if (HasEdgeData::value)
          {
            assert (&edge_data >= &edge_data_cloud_.front () && &edge_data <= &edge_data_cloud_.back ());
            return (EdgeIndex (std::distance (&edge_data_cloud_.front (), &edge_data)));
          }
          else
          {
            return (EdgeIndex ());
          }
        }

        inline FaceIndex
        getFaceIndex (const FaceData& face_data) const
        {
          if (HasFaceData::value)
          {
            assert (&face_data >= &face_data_cloud_.front () && &face_data <= &face_data_cloud_.back ());
            return (FaceIndex (std::distance (&face_data_cloud_.front (), &face_data)));
          }
          else
          {
            return (FaceIndex ());
          }
        }

      protected:

        // Types

        // Elements
        typedef pcl::geometry::Vertex   Vertex;
        typedef pcl::geometry::HalfEdge HalfEdge;
        typedef pcl::geometry::Face     Face;

        typedef std::vector <Vertex>   Vertices;
        typedef std::vector <HalfEdge> HalfEdges;
        typedef std::vector <Face>     Faces;

        typedef typename Vertices::iterator  VertexIterator;
        typedef typename HalfEdges::iterator HalfEdgeIterator;
        typedef typename Faces::iterator     FaceIterator;

        typedef typename Vertices::const_iterator  VertexConstIterator;
        typedef typename HalfEdges::const_iterator HalfEdgeConstIterator;
        typedef typename Faces::const_iterator     FaceConstIterator;

        FaceIndex
        addFaceImplBase (const VertexIndices& vertices,
                         const FaceData&      face_data,
                         const EdgeData&      edge_data,
                         const HalfEdgeData&  half_edge_data)
        {
          const int n = static_cast<int> (vertices.size ());
          if (n < 3) return (FaceIndex ());

          // Check for topological errors
          inner_he_.resize      (n);
          free_he_.resize       (n);
          is_new_.resize        (n);
          make_adjacent_.resize (n);
          int i, j;
          for (i=0; i<n; ++i)
          {
            if (!this->checkTopology1 (vertices [i], vertices [(i+1)%n], inner_he_ [i], is_new_ [i], IsManifold ()))
            {
              return (FaceIndex ());
            }
          }
          for (i=0; i<n; ++i)
          {
            j = (i+1)%n;
            if (!this->checkTopology2 (inner_he_ [i], inner_he_ [j], is_new_ [i], is_new_ [j], this->isIsolated (vertices [j]), make_adjacent_ [i], free_he_ [i], IsManifold ()))
            {
              return (FaceIndex ());
            }
          }

          // Reconnect the existing half-edges if needed
          if (!IsManifold::value)
          {
            for (i=0; i<n; ++i)
            {
              if (make_adjacent_ [i])
              {
                this->makeAdjacent (inner_he_ [i], inner_he_ [(i+1)%n], free_he_ [i]);
              }
            }
          }

          // Add new half-edges if needed
          for (i=0; i<n; ++i)
          {
            if (is_new_ [i])
            {
              inner_he_ [i] = this->addEdge (vertices [i], vertices [(i+1)%n], half_edge_data, edge_data);
            }
          }

          // Connect
          for (i=0; i<n; ++i)
          {
            j = (i+1)%n;
            if      ( is_new_ [i] &&  is_new_ [j]) this->connectNewNew (inner_he_ [i], inner_he_ [j], vertices [j], IsManifold ());
            else if ( is_new_ [i] && !is_new_ [j]) this->connectNewOld (inner_he_ [i], inner_he_ [j], vertices [j]);
            else if (!is_new_ [i] &&  is_new_ [j]) this->connectOldNew (inner_he_ [i], inner_he_ [j], vertices [j]);
            else                                   this->connectOldOld (inner_he_ [i], inner_he_ [j], vertices [j], IsManifold ());
          }
          return (this->connectFace (inner_he_, face_data));
        }

        // addEdge

        HalfEdgeIndex
        addEdge (const VertexIndex&  idx_v_a,
                 const VertexIndex&  idx_v_b,
                 const HalfEdgeData& he_data,
                 const EdgeData&     edge_data)
        {
          half_edges_.push_back (HalfEdge (idx_v_b));
          half_edges_.push_back (HalfEdge (idx_v_a));

          this->addData (half_edge_data_cloud_, he_data  , HasHalfEdgeData ());
          this->addData (half_edge_data_cloud_, he_data  , HasHalfEdgeData ());
          this->addData (edge_data_cloud_     , edge_data, HasEdgeData     ());

          return (HalfEdgeIndex (static_cast <int> (half_edges_.size () - 2)));
        }

        // topology checks

        bool
        checkTopology1 (const VertexIndex&            idx_v_a,
                        const VertexIndex&            idx_v_b,
                        HalfEdgeIndex&                idx_he_ab,
                        std::vector <bool>::reference is_new_ab,
                        boost::true_type              /*is_manifold*/) const
        {
          is_new_ab = true;
          if (this->isIsolated (idx_v_a)) return (true);

          idx_he_ab = this->getOutgoingHalfEdgeIndex (idx_v_a);

          if (!this->isBoundary (idx_he_ab))                          return (false);
          if (this->getTerminatingVertexIndex (idx_he_ab) == idx_v_b) is_new_ab = false;
          return (true);
        }

        bool
        checkTopology1 (const VertexIndex&            idx_v_a,
                        const VertexIndex&            idx_v_b,
                        HalfEdgeIndex&                idx_he_ab,
                        std::vector <bool>::reference is_new_ab,
                        boost::false_type              /*is_manifold*/) const
        {
          is_new_ab = true;
          if (this->isIsolated (idx_v_a))                                   return (true);
          if (!this->isBoundary (this->getOutgoingHalfEdgeIndex (idx_v_a))) return (false);

          VertexAroundVertexCirculator       circ     = this->getVertexAroundVertexCirculator (this->getOutgoingHalfEdgeIndex (idx_v_a));
          const VertexAroundVertexCirculator circ_end = circ;

          do
          {
            if (circ.getTargetIndex () == idx_v_b)
            {
              idx_he_ab = circ.getCurrentHalfEdgeIndex ();
              if (!this->isBoundary (idx_he_ab)) return (false);

              is_new_ab = false;
              return (true);
            }
          } while (++circ!=circ_end);

          return (true);
        }

        inline bool
        checkTopology2 (const HalfEdgeIndex&          /*idx_he_ab*/,
                        const HalfEdgeIndex&          /*idx_he_bc*/,
                        const bool                    is_new_ab,
                        const bool                    is_new_bc,
                        const bool                    is_isolated_b,
                        std::vector <bool>::reference /*make_adjacent_ab_bc*/,
                        HalfEdgeIndex&                /*idx_free_half_edge*/,
                        boost::true_type              /*is_manifold*/) const
        {
          if (is_new_ab && is_new_bc && !is_isolated_b) return (false);
          else                                          return (true);
        }

        inline bool
        checkTopology2 (const HalfEdgeIndex&          idx_he_ab,
                        const HalfEdgeIndex&          idx_he_bc,
                        const bool                    is_new_ab,
                        const bool                    is_new_bc,
                        const bool                    /*is_isolated_b*/,
                        std::vector <bool>::reference make_adjacent_ab_bc,
                        HalfEdgeIndex&                idx_free_half_edge,
                        boost::false_type             /*is_manifold*/) const
        {
          if (is_new_ab || is_new_bc)
          {
            make_adjacent_ab_bc = false;
            return (true); // Make adjacent is only needed for two old half-edges
          }

          if (this->getNextHalfEdgeIndex (idx_he_ab) == idx_he_bc)
          {
            make_adjacent_ab_bc = false;
            return (true); // already adjacent
          }

          make_adjacent_ab_bc = true;

          // Find the next boundary half edge
          IncomingHalfEdgeAroundVertexCirculator circ = this->getIncomingHalfEdgeAroundVertexCirculator (this->getOppositeHalfEdgeIndex (idx_he_bc));

          do ++circ; while (!this->isBoundary (circ.getTargetIndex ()));
          idx_free_half_edge = circ.getTargetIndex ();

          // This would detach the faces around the vertex from each other.
          if (circ.getTargetIndex () == idx_he_ab) return (false);
          else                                     return (true);
        }

        void
        makeAdjacent (const HalfEdgeIndex& idx_he_ab,
                      const HalfEdgeIndex& idx_he_bc,
                      HalfEdgeIndex&       idx_free_half_edge)
        {
          // Re-link. No references!
          const HalfEdgeIndex idx_he_ab_next   = this->getNextHalfEdgeIndex (idx_he_ab);
          const HalfEdgeIndex idx_he_bc_prev   = this->getPrevHalfEdgeIndex (idx_he_bc);
          const HalfEdgeIndex idx_he_free_next = this->getNextHalfEdgeIndex (idx_free_half_edge);

          this->connectPrevNext (idx_he_ab,          idx_he_bc);
          this->connectPrevNext (idx_free_half_edge, idx_he_ab_next);
          this->connectPrevNext (idx_he_bc_prev,     idx_he_free_next);
        }

        // connect

        FaceIndex
        connectFace (const HalfEdgeIndices& inner_he,
                     const FaceData&        face_data)
        {
          faces_.push_back (Face (inner_he.back ()));
          this->addData (face_data_cloud_, face_data, HasFaceData ());

          const FaceIndex idx_face (static_cast <int> (this->sizeFaces () - 1));

          for (HalfEdgeIndices::const_iterator it=inner_he.begin (); it!=inner_he.end (); ++it)
          {
            this->setFaceIndex (*it, idx_face);
          }

          return (idx_face);
        }

        inline void
        connectPrevNext (const HalfEdgeIndex& idx_he_ab,
                         const HalfEdgeIndex& idx_he_bc)
        {
          this->setNextHalfEdgeIndex (idx_he_ab, idx_he_bc);
          this->setPrevHalfEdgeIndex (idx_he_bc, idx_he_ab);
        }

        void
        connectNewNew (const HalfEdgeIndex& idx_he_ab,
                       const HalfEdgeIndex& idx_he_bc,
                       const VertexIndex&   idx_v_b,
                       boost::true_type     /*is_manifold*/)
        {
          const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex (idx_he_ab);
          const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex (idx_he_bc);

          this->connectPrevNext (idx_he_ab, idx_he_bc);
          this->connectPrevNext (idx_he_cb, idx_he_ba);

          this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_ba);
        }

        void
        connectNewNew (const HalfEdgeIndex& idx_he_ab,
                       const HalfEdgeIndex& idx_he_bc,
                       const VertexIndex&   idx_v_b,
                       boost::false_type    /*is_manifold*/)
        {
          if (this->isIsolated (idx_v_b))
          {
            this->connectNewNew (idx_he_ab, idx_he_bc, idx_v_b, boost::true_type ());
          }
          else
          {
            const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex (idx_he_ab);
            const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex (idx_he_bc);

            // No references!
            const HalfEdgeIndex idx_he_b_out      = this->getOutgoingHalfEdgeIndex (idx_v_b);
            const HalfEdgeIndex idx_he_b_out_prev = this->getPrevHalfEdgeIndex (idx_he_b_out);

            this->connectPrevNext (idx_he_ab,         idx_he_bc);
            this->connectPrevNext (idx_he_cb,         idx_he_b_out);
            this->connectPrevNext (idx_he_b_out_prev, idx_he_ba);
          }
        }

        void
        connectNewOld (const HalfEdgeIndex& idx_he_ab,
                       const HalfEdgeIndex& idx_he_bc,
                       const VertexIndex&   idx_v_b)
        {
          const HalfEdgeIndex idx_he_ba      = this->getOppositeHalfEdgeIndex (idx_he_ab);
          const HalfEdgeIndex idx_he_bc_prev = this->getPrevHalfEdgeIndex (idx_he_bc); // No reference!

          this->connectPrevNext (idx_he_ab,      idx_he_bc);
          this->connectPrevNext (idx_he_bc_prev, idx_he_ba);

          this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_ba);
        }

        void
        connectOldNew (const HalfEdgeIndex& idx_he_ab,
                       const HalfEdgeIndex& idx_he_bc,
                       const VertexIndex&   idx_v_b)
        {
          const HalfEdgeIndex idx_he_cb      = this->getOppositeHalfEdgeIndex (idx_he_bc);
          const HalfEdgeIndex idx_he_ab_next = this->getNextHalfEdgeIndex (idx_he_ab); // No reference!

          this->connectPrevNext (idx_he_ab, idx_he_bc);
          this->connectPrevNext (idx_he_cb, idx_he_ab_next);

          this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_ab_next);
        }

        void
        connectOldOld (const HalfEdgeIndex& /*idx_he_ab*/,
                       const HalfEdgeIndex& /*idx_he_bc*/,
                       const VertexIndex&   /*idx_v_b*/,
                       boost::true_type     /*is_manifold*/)
        {
        }

        void
        connectOldOld (const HalfEdgeIndex& /*idx_he_ab*/,
                       const HalfEdgeIndex& idx_he_bc,
                       const VertexIndex&   idx_v_b,
                       boost::false_type    /*is_manifold*/)
        {
          const HalfEdgeIndex& idx_he_b_out = this->getOutgoingHalfEdgeIndex (idx_v_b);

          // The outgoing half edge MUST be a boundary half-edge (if there is one)
          if (idx_he_b_out == idx_he_bc) // he_bc is no longer on the boundary
          {
            OutgoingHalfEdgeAroundVertexCirculator       circ     = this->getOutgoingHalfEdgeAroundVertexCirculator (idx_he_b_out);
            const OutgoingHalfEdgeAroundVertexCirculator circ_end = circ;

            while (++circ!=circ_end)
            {
              if (this->isBoundary (circ.getTargetIndex ()))
              {
                this->setOutgoingHalfEdgeIndex (idx_v_b, circ.getTargetIndex ());
                return;
              }
            }
          }
        }

        // addData

        template <class DataT>
        inline void
        addData (pcl::PointCloud <DataT>& cloud, const DataT& data, boost::true_type /*has_data*/)
        {
          cloud.push_back (data);
        }

        template <class DataT>
        inline void
        addData (pcl::PointCloud <DataT>& /*cloud*/, const DataT& /*data*/, boost::false_type /*has_data*/)
        {
        }

        // deleteFace

        void
        deleteFace (const FaceIndex& idx_face,
                    boost::true_type /*is_manifold*/)
        {
          assert (this->isValid (idx_face));
          delete_faces_face_.clear ();
          delete_faces_face_.push_back (idx_face);

          while (!delete_faces_face_.empty ())
          {
            const FaceIndex idx_face_cur = delete_faces_face_.back ();
            delete_faces_face_.pop_back ();

            // This calls the non-manifold version of deleteFace, which will call the manifold version of reconnect.
            this->deleteFace (idx_face_cur, boost::false_type ());
          }
        }

        void
        deleteFace (const FaceIndex&  idx_face,
                    boost::false_type /*is_manifold*/)
        {
          assert (this->isValid (idx_face));
          if (this->isDeleted (idx_face)) return;

          // Store all half-edges in the face
          inner_he_.clear ();
          is_boundary_.clear ();
          InnerHalfEdgeAroundFaceCirculator       circ     = this->getInnerHalfEdgeAroundFaceCirculator (idx_face);
          const InnerHalfEdgeAroundFaceCirculator circ_end = circ;
          do
          {
            inner_he_.push_back (circ.getTargetIndex ());
            is_boundary_.push_back (this->isBoundary (this->getOppositeHalfEdgeIndex (circ.getTargetIndex ())));
          } while (++circ != circ_end);
          assert (inner_he_.size () >= 3); // Minimum should be a triangle.

          const int n = static_cast <int> (inner_he_.size ());
          int j;

          if (IsManifold::value)
          {
            for (int i=0; i<n; ++i)
            {
              j = (i+1)%n;
              this->reconnect (inner_he_ [i], inner_he_ [j], is_boundary_ [i], is_boundary_ [j]);
            }
            for (int i=0; i<n; ++i)
            {
              this->getHalfEdge (inner_he_ [i]).idx_face_.invalidate ();
            }
          }
          else
          {
            for (int i=0; i<n; ++i)
            {
              j = (i+1)%n;
              this->reconnect (inner_he_ [i], inner_he_ [j], is_boundary_ [i], is_boundary_ [j]);
              this->getHalfEdge (inner_he_ [i]).idx_face_.invalidate ();
            }
          }

          this->markDeleted (idx_face);
        }

        // reconnect

        void
        reconnect (const HalfEdgeIndex& idx_he_ab,
                   const HalfEdgeIndex& idx_he_bc,
                   const bool           is_boundary_ba,
                   const bool           is_boundary_cb)
        {
          const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex  (idx_he_ab);
          const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex  (idx_he_bc);
          const VertexIndex   idx_v_b   = this->getTerminatingVertexIndex (idx_he_ab);

          if (is_boundary_ba && is_boundary_cb) // boundary - boundary
          {
            const HalfEdgeIndex& idx_he_cb_next = this->getNextHalfEdgeIndex (idx_he_cb);

            if (idx_he_cb_next == idx_he_ba) // Vertex b is isolated
            {
              this->markDeleted (idx_v_b);
            }
            else
            {
              this->connectPrevNext (this->getPrevHalfEdgeIndex (idx_he_ba), idx_he_cb_next);
              this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_cb_next);
            }

            this->markDeleted (idx_he_ab);
            this->markDeleted (idx_he_ba);
          }
          else if (is_boundary_ba && !is_boundary_cb) // boundary - no boundary
          {
            this->connectPrevNext (this->getPrevHalfEdgeIndex (idx_he_ba), idx_he_bc);
            this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_bc);

            this->markDeleted (idx_he_ab);
            this->markDeleted (idx_he_ba);
          }
          else if (!is_boundary_ba && is_boundary_cb) // no boundary - boundary
          {
            const HalfEdgeIndex& idx_he_cb_next = this->getNextHalfEdgeIndex (idx_he_cb);
            this->connectPrevNext (idx_he_ab, idx_he_cb_next);
            this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_cb_next);
          }
          else // no boundary - no boundary
          {
            this->reconnectNBNB (idx_he_bc, idx_he_cb, idx_v_b, IsManifold ());
          }
        }

        void
        reconnectNBNB (const HalfEdgeIndex& idx_he_bc,
                       const HalfEdgeIndex& idx_he_cb,
                       const VertexIndex&   idx_v_b,
                       boost::true_type     /*is_manifold*/)
        {
          if (this->isBoundary (idx_v_b))
          {
            // Deletion of this face makes the mesh non-manifold
            // -> delete the neighboring faces until it is manifold again
            IncomingHalfEdgeAroundVertexCirculator circ = this->getIncomingHalfEdgeAroundVertexCirculator (idx_he_cb);

            while (!this->isBoundary (circ.getTargetIndex ()))
            {
              delete_faces_face_.push_back (this->getFaceIndex ((circ++).getTargetIndex ()));

#ifdef PCL_GEOMETRY_MESH_BASE_TEST_DELETE_FACE_MANIFOLD_2
              if (circ == this->getIncomingHalfEdgeAroundVertexCirculator (idx_he_cb)) // Abort infinity loop
              {
                // In a manifold mesh we can't invalidate the face while reconnecting!
                // See the implementation of
                // deleteFace (const FaceIndex&  idx_face,
                //             boost::false_type /*is_manifold*/)
                pcl::geometry::g_pcl_geometry_mesh_base_test_delete_face_manifold_2_success = false;
                return;
              }
#endif
            }
          }
          else
          {
            this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_bc);
          }
        }

        void
        reconnectNBNB (const HalfEdgeIndex& idx_he_bc,
                       const HalfEdgeIndex& /*idx_he_cb*/,
                       const VertexIndex&   idx_v_b,
                       boost::false_type    /*is_manifold*/)
        {
          if (!this->isBoundary (idx_v_b))
          {
            this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_bc);
          }
        }

        // markDeleted

        inline void
        markDeleted (const VertexIndex& idx_vertex)
        {
          assert (this->isValid (idx_vertex));
          this->getVertex (idx_vertex).idx_outgoing_half_edge_.invalidate ();
        }

        inline void
        markDeleted (const HalfEdgeIndex& idx_he)
        {
          assert (this->isValid (idx_he));
          this->getHalfEdge (idx_he).idx_terminating_vertex_.invalidate ();
        }

        inline void
        markDeleted (const EdgeIndex& idx_edge)
        {
          assert (this->isValid (idx_edge));
          this->markDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, true));
          this->markDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, false));
        }

        inline void
        markDeleted (const FaceIndex& idx_face)
        {
          assert (this->isValid (idx_face));
          this->getFace (idx_face).idx_inner_half_edge_.invalidate ();
        }

        // For cleanUp

        template <class ElementContainerT, class DataContainerT, class IndexContainerT, class HasDataT> IndexContainerT
        remove (ElementContainerT& elements, DataContainerT& data_cloud)
        {
          typedef typename IndexContainerT::value_type   Index;
          typedef typename ElementContainerT::value_type Element;

          if (HasDataT::value) assert (elements.size () == data_cloud.size ());
          else                 assert (data_cloud.empty ()); // Bug in this class!

          IndexContainerT new_indices (elements.size (), typename IndexContainerT::value_type ());
          Index ind_old (0), ind_new (0);

          typename ElementContainerT::const_iterator it_e_old = elements.begin ();
          typename ElementContainerT::iterator       it_e_new = elements.begin ();

          typename DataContainerT::const_iterator it_d_old = data_cloud.begin ();
          typename DataContainerT::iterator       it_d_new = data_cloud.begin ();

          typename IndexContainerT::iterator       it_ind_new     = new_indices.begin ();
          typename IndexContainerT::const_iterator it_ind_new_end = new_indices.end ();

          while (it_ind_new!=it_ind_new_end)
          {
            if (!this->isDeleted (ind_old))
            {
              *it_ind_new = ind_new++;

              // TODO: Test for self assignment?
              *it_e_new++ = *it_e_old;
              this->assignIf    (it_d_old, it_d_new, HasDataT ());
              this->incrementIf (          it_d_new, HasDataT ());
            }
            ++ind_old;
            ++it_e_old;
            this->incrementIf (it_d_old, HasDataT ());
            ++it_ind_new;
          }

          elements.resize (ind_new.get (), Element ());
          if (HasDataT::value)
          {
            data_cloud.resize (ind_new.get ());
          }
          else if (it_d_old != data_cloud.begin () || it_d_new != data_cloud.begin ())
          {
            std::cerr << "TODO: Bug in MeshBase::remove!\n";
            assert (false);
            exit (EXIT_FAILURE);
          }

          return (new_indices);
        }

        template <class IteratorT> inline void
        incrementIf (IteratorT& it, boost::true_type /*has_data*/) const
        {
          ++it;
        }

        template <class IteratorT> inline void
        incrementIf (IteratorT& /*it*/, boost::false_type /*has_data*/) const
        {
        }

        template <class ConstIteratorT, class IteratorT> inline void
        assignIf (const ConstIteratorT source, IteratorT target, boost::true_type /*has_data*/) const
        {
          *target = *source;
        }

        template <class ConstIteratorT, class IteratorT> inline void
        assignIf (const ConstIteratorT /*source*/, IteratorT /*target*/, boost::false_type /*has_data*/) const
        {
        }

        // Vertex / Half-edge / Face connectivity

        inline void
        setOutgoingHalfEdgeIndex (const VertexIndex& idx_vertex, const HalfEdgeIndex& idx_outgoing_half_edge)
        {
          assert (this->isValid (idx_vertex));
          this->getVertex (idx_vertex).idx_outgoing_half_edge_ = idx_outgoing_half_edge;
        }

        inline void
        setTerminatingVertexIndex (const HalfEdgeIndex& idx_half_edge, const VertexIndex& idx_terminating_vertex)
        {
          assert (this->isValid (idx_half_edge));
          this->getHalfEdge (idx_half_edge).idx_terminating_vertex_ = idx_terminating_vertex;
        }

        inline void
        setNextHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge, const HalfEdgeIndex& idx_next_half_edge)
        {
          assert (this->isValid (idx_half_edge));
          this->getHalfEdge (idx_half_edge).idx_next_half_edge_ = idx_next_half_edge;
        }

        inline void
        setPrevHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge,
                              const HalfEdgeIndex& idx_prev_half_edge)
        {
          assert (this->isValid (idx_half_edge));
          this->getHalfEdge (idx_half_edge).idx_prev_half_edge_ = idx_prev_half_edge;
        }

        inline void
        setFaceIndex (const HalfEdgeIndex& idx_half_edge, const FaceIndex& idx_face)
        {
          assert (this->isValid (idx_half_edge));
          this->getHalfEdge (idx_half_edge).idx_face_ = idx_face;
        }

        inline void
        setInnerHalfEdgeIndex (const FaceIndex& idx_face, const HalfEdgeIndex& idx_inner_half_edge)
        {
          assert (this->isValid (idx_face));
          this->getFace (idx_face).idx_inner_half_edge_ = idx_inner_half_edge;
        }

        // isBoundary / isManifold

        bool
        isBoundary (const FaceIndex& idx_face, boost::true_type /*check_vertices*/) const
        {
          VertexAroundFaceCirculator       circ     = this->getVertexAroundFaceCirculator (idx_face);
          const VertexAroundFaceCirculator circ_end = circ;

          do
          {
            if (this->isBoundary (circ.getTargetIndex ()))
            {
              return (true);
            }
          } while (++circ!=circ_end);

          return (false);
        }

        bool
        isBoundary (const FaceIndex& idx_face, boost::false_type /*check_vertices*/) const
        {
          OuterHalfEdgeAroundFaceCirculator       circ     = this->getOuterHalfEdgeAroundFaceCirculator (idx_face);
          const OuterHalfEdgeAroundFaceCirculator circ_end = circ;

          do
          {
            if (this->isBoundary (circ.getTargetIndex ()))
            {
              return (true);
            }
          } while (++circ!=circ_end);

          return (false);
        }

        inline bool
        isManifold (const VertexIndex&, boost::true_type /*is_manifold*/) const
        {
          return (true);
        }

        bool
        isManifold (const VertexIndex& idx_vertex, boost::false_type /*is_manifold*/) const
        {
          OutgoingHalfEdgeAroundVertexCirculator       circ     = this->getOutgoingHalfEdgeAroundVertexCirculator (idx_vertex);
          const OutgoingHalfEdgeAroundVertexCirculator circ_end = circ;

          if (!this->isBoundary ((circ++).getTargetIndex ())) return (true);
          do
          {
            if (this->isBoundary (circ.getTargetIndex ())) return (false);
          } while (++circ != circ_end);

          return (true);
        }

        inline bool
        isManifold (boost::true_type /*is_manifold*/) const
        {
          return (true);
        }

        bool
        isManifold (boost::false_type /*is_manifold*/) const
        {
          for (unsigned int i=0; i<this->sizeVertices (); ++i)
          {
            if (!this->isManifold (VertexIndex (i))) return (false);
          }
          return (true);
        }

        // reserveData / resizeData / clearData

        template <class DataCloudT> inline void
        reserveData (DataCloudT& cloud, const size_t n, boost::true_type /*has_data*/) const
        {
          cloud.reserve (n);
        }

        template <class DataCloudT> inline void
        reserveData (DataCloudT& /*cloud*/, const size_t /*n*/, boost::false_type /*has_data*/) const
        {
        }

        template <class DataCloudT> inline void
        resizeData (DataCloudT& data_cloud, const size_t n, const typename DataCloudT::value_type& data, boost::true_type /*has_data*/) const
        {
          data.resize (n, data);
        }

        template <class DataCloudT> inline void
        resizeData (DataCloudT& /*data_cloud*/, const size_t /*n*/, const typename DataCloudT::value_type& /*data*/, boost::false_type /*has_data*/) const
        {
        }

        template <class DataCloudT> inline void
        clearData (DataCloudT& cloud, boost::true_type /*has_data*/) const
        {
          cloud.clear ();
        }

        template <class DataCloudT> inline void
        clearData (DataCloudT& /*cloud*/, boost::false_type /*has_data*/) const
        {
        }

        // get / set Vertex

        inline Vertex&
        getVertex (const VertexIndex& idx_vertex)
        {
          assert (this->isValid (idx_vertex));
          return (vertices_ [idx_vertex.get ()]);
        }

        inline Vertex
        getVertex (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (vertices_ [idx_vertex.get ()]);
        }

        inline void
        setVertex (const VertexIndex& idx_vertex, const Vertex& vertex)
        {
          assert (this->isValid (idx_vertex));
          vertices_ [idx_vertex.get ()] = vertex;
        }

        // get / set HalfEdge

        inline HalfEdge&
        getHalfEdge (const HalfEdgeIndex& idx_he)
        {
          assert (this->isValid (idx_he));
          return (half_edges_ [idx_he.get ()]);
        }

        inline HalfEdge
        getHalfEdge (const HalfEdgeIndex& idx_he) const
        {
          assert (this->isValid (idx_he));
          return (half_edges_ [idx_he.get ()]);
        }

        inline void
        setHalfEdge (const HalfEdgeIndex& idx_he, const HalfEdge& half_edge)
        {
          assert (this->isValid (idx_he));
          half_edges_ [idx_he.get ()] = half_edge;
        }

        // get / set Face

        inline Face&
        getFace (const FaceIndex& idx_face)
        {
          assert (this->isValid (idx_face));
          return (faces_ [idx_face.get ()]);
        }

        inline Face
        getFace (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (faces_ [idx_face.get ()]);
        }

        inline void
        setFace (const FaceIndex& idx_face, const Face& face)
        {
          assert (this->isValid (idx_face));
          faces_ [idx_face.get ()] = face;
        }

      private:

        // Members

        VertexDataCloud vertex_data_cloud_;

        HalfEdgeDataCloud half_edge_data_cloud_;

        EdgeDataCloud edge_data_cloud_;

        FaceDataCloud face_data_cloud_;

        Vertices vertices_;

        HalfEdges half_edges_;

        Faces faces_;

        // NOTE: It is MUCH faster to store these variables permamently.

        HalfEdgeIndices inner_he_;

        HalfEdgeIndices free_he_;

        std::vector <bool> is_new_;

        std::vector <bool> make_adjacent_;

        std::vector <bool> is_boundary_;

        FaceIndices delete_faces_vertex_;

        FaceIndices delete_faces_face_;

      public:

        template <class MeshT>
        friend class pcl::geometry::MeshIO;

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
  } // End namespace geometry
} // End namespace pcl

#endif // PCL_GEOMETRY_MESH_BASE_H