/*
 * 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$
 *
 */

#pragma once

#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/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>

#include <vector>
#include <type_traits>

////////////////////////////////////////////////////////////////////////////////
// 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
  {
    /** \brief Base class for the half-edge mesh.
      * \tparam DerivedT Has to implement the method 'addFaceImpl'. Please have a look at pcl::geometry::TriangleMesh, pcl::geometry::QuadMesh and pcl::geometry::PolygonMesh.
      * \tparam MeshTraitsT Please have a look at pcl::geometry::DefaultMeshTraits.
      * \tparam MeshTagT Tag describing the type of the mesh, e.g. TriangleMeshTag, QuadMeshTag, PolygonMeshTag.
      * \author Martin Saelzle
      * \ingroup geometry
      * \todo Add documentation
      */
    template <class DerivedT, class MeshTraitsT, class MeshTagT>
    class MeshBase
    {
      public:

        using Self = MeshBase <DerivedT, MeshTraitsT, MeshTagT>;
        using Ptr = shared_ptr<Self>;
        using ConstPtr = shared_ptr<const Self>;

        using Derived = DerivedT;

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

        // Check if the mesh traits are defined correctly.
        static_assert (std::is_convertible<IsManifold, bool>::value, "MeshTraitsT::IsManifold is not convertible to bool");

        using MeshTag = MeshTagT;

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

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

        // Indices
        using VertexIndex = pcl::geometry::VertexIndex;
        using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex;
        using EdgeIndex = pcl::geometry::EdgeIndex;
        using FaceIndex = pcl::geometry::FaceIndex;

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

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

        /** \brief Constructor. */
        MeshBase ()
          : vertex_data_cloud_ (),
            half_edge_data_cloud_ (),
            edge_data_cloud_ (),
            face_data_cloud_ ()
        {
        }

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

        /** \brief Add a vertex to the mesh.
          * \param[in] vertex_data Data that is stored in the vertex. This is only added if the mesh has data associated with the vertices.
          * \return Index to the new vertex.
          */
        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)));
        }

        /** \brief Add a face to the mesh. Data is only added if it is associated with the elements. The last vertex is connected with the first one.
          * \param[in] vertices       Indices to the vertices of the new face.
          * \param[in] face_data      Data that is set for the face.
          * \param[in] half_edge_data Data that is set for all added half-edges.
          * \param[in] edge_data      Data that is set for all added edges.
          * \return Index to the new face. Failure is signaled by returning an invalid face index.
          * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior!
          */
        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));
        }

        /** \brief Mark the given vertex and all connected half-edges and faces as deleted.
          * \note Call cleanUp () to finally delete all mesh-elements.
          */
        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);
          }
        }

        /** \brief Mark the given half-edge, the opposite half-edge and the associated faces as deleted.
          * \note Call cleanUp () to finally delete all mesh-elements.
          */
        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));
        }

        /** \brief Mark the given edge (both half-edges) and the associated faces as deleted.
          * \note Call cleanUp () to finally delete all mesh-elements.
          */
        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!
        }

        /** \brief Mark the given face as deleted. More faces are deleted if the manifold mesh would become non-manifold.
          * \note Call cleanUp () to finally delete all mesh-elements.
          */
        inline void
        deleteFace (const FaceIndex& idx_face)
        {
          assert (this->isValid (idx_face));
          if (this->isDeleted (idx_face)) return;

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

        /** \brief Removes all mesh elements and data that are marked as deleted.
          * \note This removes all isolated vertices as well.
          */
        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)
          {
            auto it_ed_old = edge_data_cloud_.begin ();
            auto it_ed_new = edge_data_cloud_.begin ();

            for (auto it_ind = new_half_edge_indices.cbegin (), it_ind_end = new_half_edge_indices.cend (); 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
        ////////////////////////////////////////////////////////////////////////

        /** \brief Get the outgoing half-edge index to a given vertex. */
        inline HalfEdgeIndex
        getOutgoingHalfEdgeIndex (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (this->getVertex (idx_vertex).idx_outgoing_half_edge_);
        }

        /** \brief Get the incoming half-edge index to a given vertex. */
        inline HalfEdgeIndex
        getIncomingHalfEdgeIndex (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (this->getOppositeHalfEdgeIndex (this->getOutgoingHalfEdgeIndex (idx_vertex)));
        }

        ////////////////////////////////////////////////////////////////////////
        // Half-edge connectivity
        ////////////////////////////////////////////////////////////////////////

        /** \brief Get the terminating vertex index to a given half-edge. */
        inline VertexIndex
        getTerminatingVertexIndex (const HalfEdgeIndex& idx_half_edge) const
        {
          assert (this->isValid (idx_half_edge));
          return (this->getHalfEdge (idx_half_edge).idx_terminating_vertex_);
        }

        /** \brief Get the originating vertex index to a given half-edge. */
        inline VertexIndex
        getOriginatingVertexIndex (const HalfEdgeIndex& idx_half_edge) const
        {
          assert (this->isValid (idx_half_edge));
          return (this->getTerminatingVertexIndex (this->getOppositeHalfEdgeIndex (idx_half_edge)));
        }

        /** \brief Get the opposite half-edge index to a given 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));
        }

        /** \brief Get the next half-edge index to a given half-edge. */
        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_);
        }

        /** \brief Get the previous half-edge index to a given 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_);
        }

        /** \brief Get the face index to a given 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_);
        }

        /** \brief Get the face index to a given half-edge. */
        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
        ////////////////////////////////////////////////////////////////////////

        /** \brief Get the inner half-edge index to a given face. */
        inline HalfEdgeIndex
        getInnerHalfEdgeIndex (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (this->getFace (idx_face).idx_inner_half_edge_);
        }

        /** \brief Get the outer half-edge inex to a given face. */
        inline HalfEdgeIndex
        getOuterHalfEdgeIndex (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (this->getOppositeHalfEdgeIndex (this->getInnerHalfEdgeIndex (idx_face)));
        }

        ////////////////////////////////////////////////////////////////////////
        // Circulators
        ////////////////////////////////////////////////////////////////////////

        /** \see pcl::geometry::VertexAroundVertexCirculator */
        inline VertexAroundVertexCirculator
        getVertexAroundVertexCirculator (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (VertexAroundVertexCirculator (idx_vertex, this));
        }

        /** \see pcl::geometry::VertexAroundVertexCirculator */
        inline VertexAroundVertexCirculator
        getVertexAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge) const
        {
          assert (this->isValid (idx_outgoing_half_edge));
          return (VertexAroundVertexCirculator (idx_outgoing_half_edge, this));
        }

        /** \see pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator */
        inline OutgoingHalfEdgeAroundVertexCirculator
        getOutgoingHalfEdgeAroundVertexCirculator (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (OutgoingHalfEdgeAroundVertexCirculator (idx_vertex, this));
        }

        /** \see pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator */
        inline OutgoingHalfEdgeAroundVertexCirculator
        getOutgoingHalfEdgeAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge) const
        {
          assert (this->isValid (idx_outgoing_half_edge));
          return (OutgoingHalfEdgeAroundVertexCirculator (idx_outgoing_half_edge, this));
        }

        /** \see pcl::geometry::IncomingHalfEdgeAroundVertexCirculator */
        inline IncomingHalfEdgeAroundVertexCirculator
        getIncomingHalfEdgeAroundVertexCirculator (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (IncomingHalfEdgeAroundVertexCirculator (idx_vertex, this));
        }

        /** \see pcl::geometry::IncomingHalfEdgeAroundVertexCirculator */
        inline IncomingHalfEdgeAroundVertexCirculator
        getIncomingHalfEdgeAroundVertexCirculator (const HalfEdgeIndex& idx_incoming_half_edge) const
        {
          assert (this->isValid (idx_incoming_half_edge));
          return (IncomingHalfEdgeAroundVertexCirculator (idx_incoming_half_edge, this));
        }

        /** \see pcl::geometry::FaceAroundVertexCirculator */
        inline FaceAroundVertexCirculator
        getFaceAroundVertexCirculator (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (FaceAroundVertexCirculator (idx_vertex, this));
        }

        /** \see pcl::geometry::FaceAroundVertexCirculator */
        inline FaceAroundVertexCirculator
        getFaceAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge) const
        {
          assert (this->isValid (idx_outgoing_half_edge));
          return (FaceAroundVertexCirculator (idx_outgoing_half_edge, this));
        }

        /** \see pcl::geometry::VertexAroundFaceCirculator */
        inline VertexAroundFaceCirculator
        getVertexAroundFaceCirculator (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (VertexAroundFaceCirculator (idx_face, this));
        }

        /** \see pcl::geometry::VertexAroundFaceCirculator */
        inline VertexAroundFaceCirculator
        getVertexAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const
        {
          assert (this->isValid (idx_inner_half_edge));
          return (VertexAroundFaceCirculator (idx_inner_half_edge, this));
        }

        /** \see pcl::geometry::InnerHalfEdgeAroundFaceCirculator */
        inline InnerHalfEdgeAroundFaceCirculator
        getInnerHalfEdgeAroundFaceCirculator (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (InnerHalfEdgeAroundFaceCirculator (idx_face, this));
        }

        /** \see pcl::geometry::InnerHalfEdgeAroundFaceCirculator */
        inline InnerHalfEdgeAroundFaceCirculator
        getInnerHalfEdgeAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const
        {
          assert (this->isValid (idx_inner_half_edge));
          return (InnerHalfEdgeAroundFaceCirculator (idx_inner_half_edge, this));
        }

        /** \see pcl::geometry::OuterHalfEdgeAroundFaceCirculator */
        inline OuterHalfEdgeAroundFaceCirculator
        getOuterHalfEdgeAroundFaceCirculator (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (OuterHalfEdgeAroundFaceCirculator (idx_face, this));
        }

        /** \see pcl::geometry::OuterHalfEdgeAroundFaceCirculator */
        inline OuterHalfEdgeAroundFaceCirculator
        getOuterHalfEdgeAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const
        {
          assert (this->isValid (idx_inner_half_edge));
          return (OuterHalfEdgeAroundFaceCirculator (idx_inner_half_edge, this));
        }

        /** \see pcl::geometry::FaceAroundFaceCirculator */
        inline FaceAroundFaceCirculator
        getFaceAroundFaceCirculator (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (FaceAroundFaceCirculator (idx_face, this));
        }

        /** \see pcl::geometry::FaceAroundFaceCirculator */
        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
        //////////////////////////////////////////////////////////////////////////

        /** \brief Check if the other mesh has the same topology as this mesh. */
        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 (std::size_t i=0; i<this->sizeVertices (); ++i)
          {
            if (this->getOutgoingHalfEdgeIndex (VertexIndex (i)) !=
                other.getOutgoingHalfEdgeIndex (VertexIndex (i))) return (false);
          }

          for (std::size_t 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 (std::size_t i=0; i<this->sizeFaces (); ++i)
          {
            if (this->getInnerHalfEdgeIndex (FaceIndex (i)) !=
                other.getInnerHalfEdgeIndex (FaceIndex (i))) return (false);
          }

          return (true);
        }

        ////////////////////////////////////////////////////////////////////////
        // isValid
        ////////////////////////////////////////////////////////////////////////

        /** \brief Check if the given vertex index is a valid index into the mesh. */
        inline bool
        isValid (const VertexIndex& idx_vertex) const
        {
          return (idx_vertex >= VertexIndex (0) && idx_vertex < VertexIndex (int (vertices_.size ())));
        }

        /** \brief Check if the given half-edge index is a valid index into the mesh.  */
        inline bool
        isValid (const HalfEdgeIndex& idx_he) const
        {
          return (idx_he >= HalfEdgeIndex (0) && idx_he < HalfEdgeIndex (half_edges_.size ()));
        }

        /** \brief Check if the given edge index is a valid index into the mesh. */
        inline bool
        isValid (const EdgeIndex& idx_edge) const
        {
          return (idx_edge >= EdgeIndex (0) && idx_edge < EdgeIndex (half_edges_.size () / 2));
        }

        /** \brief Check if the given face index is a valid index into the mesh.  */
        inline bool
        isValid (const FaceIndex& idx_face) const
        {
          return (idx_face >= FaceIndex (0) && idx_face < FaceIndex (faces_.size ()));
        }

        ////////////////////////////////////////////////////////////////////////
        // isDeleted
        ////////////////////////////////////////////////////////////////////////

        /** \brief Check if the given vertex is marked as deleted. */
        inline bool
        isDeleted (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (!this->getOutgoingHalfEdgeIndex (idx_vertex).isValid ());
        }

        /** \brief Check if the given half-edge is marked as deleted. */
        inline bool
        isDeleted (const HalfEdgeIndex& idx_he) const
        {
          assert (this->isValid (idx_he));
          return (!this->getTerminatingVertexIndex (idx_he).isValid ());
        }

        /** \brief Check if the given edge (any of the two half-edges) is marked as deleted. */
        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)));
        }

        /** \brief Check if the given face is marked as deleted. */
        inline bool
        isDeleted (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (!this->getInnerHalfEdgeIndex (idx_face).isValid ());
        }

        ////////////////////////////////////////////////////////////////////////
        // isIsolated
        ////////////////////////////////////////////////////////////////////////

        /** \brief Check if the given vertex is isolated (not connected to other elements). */
        inline bool
        isIsolated (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (!this->getOutgoingHalfEdgeIndex (idx_vertex).isValid ());
        }

        ////////////////////////////////////////////////////////////////////////
        // isBoundary
        ////////////////////////////////////////////////////////////////////////

        /** \brief Check if the given vertex lies on the boundary. Isolated vertices are considered to be on the boundary. */
        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)));
        }

        /** \brief Check if the given half-edge lies on the bounddary. */
        inline bool
        isBoundary (const HalfEdgeIndex& idx_he) const
        {
          assert (this->isValid (idx_he));
          return (!this->getFaceIndex (idx_he).isValid ());
        }

        /** \brief Check if the given edge lies on the boundary (any of the two half-edges lies on the boundary. */
        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)));
        }

        /** \brief Check if the given face lies on the boundary. There are two versions of this method, selected by the template parameter.
          * \tparam CheckVerticesT Check if any vertex lies on the boundary (true) or check if any edge lies on the boundary (false).
          */
        template <bool CheckVerticesT> inline bool
        isBoundary (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (this->isBoundary (idx_face, std::integral_constant <bool, CheckVerticesT> ()));
        }

        /** \brief Check if the given face lies on the boundary. This method uses isBoundary \c true which checks if any vertex lies on the boundary. */
        inline bool
        isBoundary (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (this->isBoundary (idx_face, std::true_type ()));
        }

        ////////////////////////////////////////////////////////////////////////
        // isManifold
        ////////////////////////////////////////////////////////////////////////

        /** \brief Check if the given vertex is manifold. Isolated vertices are manifold. */
        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 ()));
        }

        /** \brief Check if the mesh is manifold. */
        inline bool
        isManifold () const
        {
          return (this->isManifold (IsManifold ()));
        }

        ////////////////////////////////////////////////////////////////////////
        // size
        ////////////////////////////////////////////////////////////////////////

        /** \brief Get the number of the vertices. */
        inline std::size_t
        sizeVertices () const
        {
          return (vertices_.size ());
        }

        /** \brief Get the number of the half-edges. */
        inline std::size_t
        sizeHalfEdges () const
        {
          assert (half_edges_.size () % 2 == 0); // This would be a bug in the mesh.
          return (half_edges_.size ());
        }

        /** \brief Get the number of the edges. */
        inline std::size_t
        sizeEdges () const
        {
          assert (half_edges_.size () % 2 == 0); // This would be a bug in the mesh.
          return (half_edges_.size () / 2);
        }

        /** \brief Get the number of the faces. */
        inline std::size_t
        sizeFaces () const
        {
          return (faces_.size ());
        }

        ////////////////////////////////////////////////////////////////////////
        // empty
        ////////////////////////////////////////////////////////////////////////

        /** \brief Check if the mesh is empty. */
        inline bool
        empty () const
        {
          return (this->emptyVertices () && this->emptyEdges () && this->emptyFaces ());
        }

        /** \brief Check if the vertices are empty. */
        inline bool
        emptyVertices () const
        {
          return (vertices_.empty ());
        }

        /** \brief Check if the edges are empty. */
        inline bool
        emptyEdges () const
        {
          return (half_edges_.empty ());
        }

        /** \brief Check if the faces are empty. */
        inline bool
        emptyFaces () const
        {
          return (faces_.empty ());
        }

        ////////////////////////////////////////////////////////////////////////
        // reserve
        ////////////////////////////////////////////////////////////////////////

        /** \brief Reserve storage space n vertices. */
        inline void
        reserveVertices (const std::size_t n)
        {
          vertices_.reserve (n);
          this->reserveData (vertex_data_cloud_, n, HasVertexData ());
        }

        /** \brief Reserve storage space for n edges (2*n storage space is reserved for the half-edges). */
        inline void
        reserveEdges (const std::size_t n)
        {
          half_edges_.reserve (2*n);
          this->reserveData (half_edge_data_cloud_, 2*n, HasHalfEdgeData ());
          this->reserveData (edge_data_cloud_     ,   n, HasEdgeData     ());
        }

        /** \brief Reserve storage space for n faces. */
        inline void
        reserveFaces (const std::size_t n)
        {
          faces_.reserve (n);
          this->reserveData (face_data_cloud_, n, HasFaceData ());
        }

        ////////////////////////////////////////////////////////////////////////
        // resize
        ////////////////////////////////////////////////////////////////////////

        /** \brief Resize the the vertices to n elements. */
        inline void
        resizeVertices (const std::size_t n, const VertexData& data = VertexData ())
        {
          vertices_.resize (n);
          this->resizeData (vertex_data_cloud_, n, data, HasVertexData ());
        }

        /** \brief Resize the edges to n elements (half-edges will hold 2*n elements). */
        inline void
        resizeEdges (const std::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     ());
        }

        /** \brief Resize the faces to n elements. */
        inline void
        resizeFaces (const std::size_t n, const FaceData& data = FaceData ())
        {
          faces_.resize (n);
          this->resizeData (face_data_cloud_, n, data, HasFaceData ());
        }

        ////////////////////////////////////////////////////////////////////////
        // clear
        ////////////////////////////////////////////////////////////////////////

        /** \brief Clear all mesh elements and data. */
        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
        ////////////////////////////////////////////////////////////////////////

        /** \brief Get access to the stored vertex data.
          * \warning Please make sure to NOT add or remove elements from the cloud.
          */
        inline VertexDataCloud&
        getVertexDataCloud ()
        {
          return (vertex_data_cloud_);
        }

        /** \brief Get the stored vertex data. */
        inline VertexDataCloud
        getVertexDataCloud () const
        {
          return (vertex_data_cloud_);
        }

        /** \brief Change the stored vertex data.
          * \param[in] vertex_data_cloud The new vertex data. Must be the same as the current data.
          * \return true if the cloud could be set.
          */
        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);
          }
          return (false);
        }

        ////////////////////////////////////////////////////////////////////////
        // get / set the half-edge data cloud
        ////////////////////////////////////////////////////////////////////////

        /** \brief Get access to the stored half-edge data.
          * \warning Please make sure to NOT add or remove elements from the cloud.
          */
        inline HalfEdgeDataCloud&
        getHalfEdgeDataCloud ()
        {
          return (half_edge_data_cloud_);
        }

        /** \brief Get the stored half-edge data. */
        inline HalfEdgeDataCloud
        getHalfEdgeDataCloud () const
        {
          return (half_edge_data_cloud_);
        }

        /** \brief Change the stored half-edge data.
          * \param[in] half_edge_data_cloud The new half-edge data. Must be the same as the current data.
          * \return true if the cloud could be set.
          */
        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);
          }
          return (false);
        }

        ////////////////////////////////////////////////////////////////////////
        // get / set the edge data cloud
        ////////////////////////////////////////////////////////////////////////

        /** \brief Get access to the stored edge data.
          * \warning Please make sure to NOT add or remove elements from the cloud.
          */
        inline EdgeDataCloud&
        getEdgeDataCloud ()
        {
          return (edge_data_cloud_);
        }

        /** \brief Get the stored edge data. */
        inline EdgeDataCloud
        getEdgeDataCloud () const
        {
          return (edge_data_cloud_);
        }

        /** \brief Change the stored edge data.
          * \param[in] edge_data_cloud The new edge data. Must be the same as the current data.
          * \return true if the cloud could be set.
          */
        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);
          }
          return (false);
        }

        ////////////////////////////////////////////////////////////////////////
        // get / set the face data cloud
        ////////////////////////////////////////////////////////////////////////

        /** \brief Get access to the stored face data.
          * \warning Please make sure to NOT add or remove elements from the cloud.
          */
        inline FaceDataCloud&
        getFaceDataCloud ()
        {
          return (face_data_cloud_);
        }

        /** \brief Get the stored face data. */
        inline FaceDataCloud
        getFaceDataCloud () const
        {
          return (face_data_cloud_);
        }

        /** \brief Change the stored face data.
          * \param[in] face_data_cloud The new face data. Must be the same as the current data.
          * \return true if the cloud could be set.
          */
        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);
          }
          return (false);
        }

        ////////////////////////////////////////////////////////////////////////
        // getVertexIndex / getHalfEdgeIndex / getEdgeIndex / getFaceIndex
        ////////////////////////////////////////////////////////////////////////

        /** \brief Get the index associated to the given vertex data.
          * \return Invalid index if the mesh does not have associated vertex data.
          */
        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)));
          }
          return (VertexIndex ());
        }

        /** \brief Get the index associated to the given half-edge data. */
        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)));
          }
          return (HalfEdgeIndex ());
        }

        /** \brief Get the index associated to the given edge data. */
        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)));
          }
          return (EdgeIndex ());
        }

        /** \brief Get the index associated to the given face data. */
        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)));
          }
          return (FaceIndex ());
        }

      protected:

        ////////////////////////////////////////////////////////////////////////
        // Types
        ////////////////////////////////////////////////////////////////////////

        // Elements
        using Vertex = pcl::geometry::Vertex;
        using HalfEdge = pcl::geometry::HalfEdge;
        using Face = pcl::geometry::Face;

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

        using VertexIterator = typename Vertices::iterator;
        using HalfEdgeIterator = typename HalfEdges::iterator;
        using FaceIterator = typename Faces::iterator;

        using VertexConstIterator = typename Vertices::const_iterator;
        using HalfEdgeConstIterator = typename HalfEdges::const_iterator;
        using FaceConstIterator = typename Faces::const_iterator;

        /** \brief General implementation of addFace. */
        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);
          for (int i=0; i<n; ++i)
          {
            if (!this->checkTopology1 (vertices [i], vertices [(i+1)%n], inner_he_ [i], is_new_ [i], IsManifold ()))
            {
              return (FaceIndex ());
            }
          }
          for (int i=0; i<n; ++i)
          {
            int 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 (int 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 (int 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 (int i=0; i<n; ++i)
          {
            int 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
        ////////////////////////////////////////////////////////////////////////

        /** \brief Add an edge between the two given vertices and connect them with the vertices.
          * \param[in]  idx_v_a   The first vertex index
          * \param[in]  idx_v_b   The second vertex index
          * \param[in]  he_data   Data associated with the half-edges. This is only added if the mesh has data associated with the half-edges.
          * \param[in]  edge_data Data associated with the edge. This is only added if the mesh has data associated with the edges.
          * \return Index to the half-edge from vertex a to vertex b.
          */
        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
        ////////////////////////////////////////////////////////////////////////

        /** \brief Check if the edge between the two vertices can be added.
          * \param[in]  idx_v_a   Index to the first vertex.
          * \param[in]  idx_v_b   Index to the second vertex.
          * \param[out] idx_he_ab Index to the half-edge ab if is_new_ab=false.
          * \param[out] is_new_ab true if the edge between the vertices exists already. Must be initialized with true!
          * \return true if the half-edge may be added.
          */
        bool
        checkTopology1 (const VertexIndex&            idx_v_a,
                        const VertexIndex&            idx_v_b,
                        HalfEdgeIndex&                idx_he_ab,
                        std::vector <bool>::reference is_new_ab,
                        std::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);
        }

        /** \brief Non manifold version of checkTopology1 */
        bool
        checkTopology1 (const VertexIndex&            idx_v_a,
                        const VertexIndex&            idx_v_b,
                        HalfEdgeIndex&                idx_he_ab,
                        std::vector <bool>::reference is_new_ab,
                        std::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);
        }

        /** \brief Check if the face may be added (mesh does not become non-manifold). */
        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*/,
                        std::true_type              /*is_manifold*/) const
        {
          return !(is_new_ab && is_new_bc && !is_isolated_b);
        }

        /** \brief Check if the half-edge bc is the next half-edge of ab.
          * \param[in]  idx_he_ab           Index to the half-edge between the vertices a and b.
          * \param[in]  idx_he_bc           Index to the half-edge between the vertices b and c.
          * \param[in]  is_new_ab           Half-edge ab is new.
          * \param[in]  is_new_bc           Half-edge bc is new.
          * \param[out] make_adjacent_ab_bc Half-edges ab and bc need to be made adjacent.
          * \param[out] idx_free_half_edge  Free half-edge (needed for makeAdjacent)
          * \return true if addFace may be continued.
          */
        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,
                        std::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.
          return (circ.getTargetIndex () != idx_he_ab);
        }

        /** \brief Make the half-edges bc the next half-edge of ab.
          * \param[in]      idx_he_ab          Index to the half-edge between the vertices a and b.
          * \param[in]      idx_he_bc          Index to the half-edge between the vertices b and c.
          * \param[in, out] idx_free_half_edge Free half-edge needed to re-connect the half-edges around vertex b.
          */
        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
        ////////////////////////////////////////////////////////////////////////

        /** \brief Add a face to the mesh and connect it to the half-edges.
          * \param[in] inner_he  Inner half-edges of the face.
          * \param[in] face_data Data that is stored in the face. This is only added if the mesh has data associated with the faces.
          * \return Index to the new face.
          */
        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 (const auto &idx_half_edge : inner_he)
          {
            this->setFaceIndex (idx_half_edge, idx_face);
          }

          return (idx_face);
        }

        /** \brief Connect the next and prev indices of the two half-edges with each other. */
        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);
        }

        /** \brief Both half-edges are new (manifold version). */
        void
        connectNewNew (const HalfEdgeIndex& idx_he_ab,
                       const HalfEdgeIndex& idx_he_bc,
                       const VertexIndex&   idx_v_b,
                       std::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);
        }

        /** \brief Both half-edges are new (non-manifold version). */
        void
        connectNewNew (const HalfEdgeIndex& idx_he_ab,
                       const HalfEdgeIndex& idx_he_bc,
                       const VertexIndex&   idx_v_b,
                       std::false_type    /*is_manifold*/)
        {
          if (this->isIsolated (idx_v_b))
          {
            this->connectNewNew (idx_he_ab, idx_he_bc, idx_v_b, std::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);
          }
        }

        /** \brief The first half-edge is new. */
        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);
        }

        /** \brief The second half-edge is new. */
        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);
        }

        /** \brief Both half-edges are old (manifold version). */
        void
        connectOldOld (const HalfEdgeIndex& /*idx_he_ab*/,
                       const HalfEdgeIndex& /*idx_he_bc*/,
                       const VertexIndex&   /*idx_v_b*/,
                       std::true_type     /*is_manifold*/)
        {
        }

        /** \brief Both half-edges are old (non-manifold version). */
        void
        connectOldOld (const HalfEdgeIndex& /*idx_he_ab*/,
                       const HalfEdgeIndex& idx_he_bc,
                       const VertexIndex&   idx_v_b,
                       std::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
        ////////////////////////////////////////////////////////////////////////

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

        /** \brief Does nothing. */
        template <class DataT>
        inline void
        addData (pcl::PointCloud <DataT>& /*cloud*/, const DataT& /*data*/, std::false_type /*has_data*/)
        {
        }

        ////////////////////////////////////////////////////////////////////////
        // deleteFace
        ////////////////////////////////////////////////////////////////////////

        /** \brief Manifold version of deleteFace. If the mesh becomes non-manifold due to the delete operation the faces around the non-manifold vertex are deleted until the mesh becomes manifold again. */
        void
        deleteFace (const FaceIndex& idx_face,
                    std::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, std::false_type ());
          }
        }

        /** \brief Non-manifold version of deleteFace. */
        void
        deleteFace (const FaceIndex&  idx_face,
                    std::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
        ////////////////////////////////////////////////////////////////////////

        /** \brief Deconnect the input half-edges from the mesh and adjust the indices of the connected half-edges. */
        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 ());
          }
        }

        /** \brief Both edges are not on the boundary. Manifold version. */
        void
        reconnectNBNB (const HalfEdgeIndex& idx_he_bc,
                       const HalfEdgeIndex& idx_he_cb,
                       const VertexIndex&   idx_v_b,
                       std::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,
                //             std::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);
          }
        }

        /** \brief Both edges are not on the boundary. Non-manifold version. */
        void
        reconnectNBNB (const HalfEdgeIndex& idx_he_bc,
                       const HalfEdgeIndex& /*idx_he_cb*/,
                       const VertexIndex&   idx_v_b,
                       std::false_type    /*is_manifold*/)
        {
          if (!this->isBoundary (idx_v_b))
          {
            this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_bc);
          }
        }

        ////////////////////////////////////////////////////////////////////////
        // markDeleted
        ////////////////////////////////////////////////////////////////////////

        /** \brief Mark the given vertex as deleted. */
        inline void
        markDeleted (const VertexIndex& idx_vertex)
        {
          assert (this->isValid (idx_vertex));
          this->getVertex (idx_vertex).idx_outgoing_half_edge_.invalidate ();
        }

        /** \brief Mark the given half-edge as deleted. */
        inline void
        markDeleted (const HalfEdgeIndex& idx_he)
        {
          assert (this->isValid (idx_he));
          this->getHalfEdge (idx_he).idx_terminating_vertex_.invalidate ();
        }

        /** \brief Mark the given edge (both half-edges) as deleted. */
        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));
        }

        /** \brief Mark the given face as deleted. */
        inline void
        markDeleted (const FaceIndex& idx_face)
        {
          assert (this->isValid (idx_face));
          this->getFace (idx_face).idx_inner_half_edge_.invalidate ();
        }

        ////////////////////////////////////////////////////////////////////////
        // For cleanUp
        ////////////////////////////////////////////////////////////////////////

        /** \brief Removes mesh elements and data that are marked as deleted from the container.
          * \tparam ElementContainerT e.g. std::vector \<Vertex\>
          * \tparam DataContainerT    e.g. std::vector \<VertexData\>
          * \tparam IndexContainerT   e.g. std::vector \<VertexIndex\>
          * \tparam HasDataT          Integral constant specifying if the mesh has data associated with the elements.
          *
          * \param[in, out] elements Container for the mesh elements. Resized to the new size.
          * \param[in, out] data_cloud Container for the mesh data. Resized to the new size.
          * \return Container with the same size as the old input data. Holds the indices to the new elements for each non-deleted element and an invalid index if it is deleted.
          */
        template <class ElementContainerT, class DataContainerT, class IndexContainerT, class HasDataT> IndexContainerT
        remove (ElementContainerT& elements, DataContainerT& data_cloud)
        {
          using Index = typename IndexContainerT::value_type;
          using Element = typename ElementContainerT::value_type;

          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);
        }

        /** \brief Increment the iterator. */
        template <class IteratorT> inline void
        incrementIf (IteratorT& it, std::true_type /*has_data*/) const
        {
          ++it;
        }

        /** \brief Does nothing. */
        template <class IteratorT> inline void
        incrementIf (IteratorT& /*it*/, std::false_type /*has_data*/) const
        {
        }

        /** \brief Assign the source iterator to the target iterator. */
        template <class ConstIteratorT, class IteratorT> inline void
        assignIf (const ConstIteratorT source, IteratorT target, std::true_type /*has_data*/) const
        {
          *target = *source;
        }

        /** \brief Does nothing. */
        template <class ConstIteratorT, class IteratorT> inline void
        assignIf (const ConstIteratorT /*source*/, IteratorT /*target*/, std::false_type /*has_data*/) const
        {
        }

        ////////////////////////////////////////////////////////////////////////
        // Vertex / Half-edge / Face connectivity
        ////////////////////////////////////////////////////////////////////////

        /** \brief Set the outgoing half-edge index to a given vertex. */
        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;
        }

        /** \brief Set the terminating vertex index to a given 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;
        }

        /** \brief Set the next half_edge index to a given half-edge. */
        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;
        }

        /** \brief Set the previous half-edge index to a given 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;
        }

        /** \brief Set the face index to a given 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;
        }

        /** \brief Set the inner half-edge index to a given 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
        ////////////////////////////////////////////////////////////////////////

        /** \brief Check if any vertex of the face lies on the boundary. */
        bool
        isBoundary (const FaceIndex& idx_face, std::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);
        }

        /** \brief Check if any edge of the face lies on the boundary. */
        bool
        isBoundary (const FaceIndex& idx_face, std::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);
        }

        /** \brief Always manifold. */
        inline bool
        isManifold (const VertexIndex&, std::true_type /*is_manifold*/) const
        {
          return (true);
        }

        /** \brief Check if the given vertex is manifold. */
        bool
        isManifold (const VertexIndex& idx_vertex, std::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);
        }

        /** \brief Always manifold. */
        inline bool
        isManifold (std::true_type /*is_manifold*/) const
        {
          return (true);
        }

        /** \brief Check if all vertices in the mesh are manifold. */
        bool
        isManifold (std::false_type /*is_manifold*/) const
        {
          for (std::size_t i=0; i<this->sizeVertices (); ++i)
          {
            if (!this->isManifold (VertexIndex (i))) return (false);
          }
          return (true);
        }

        ////////////////////////////////////////////////////////////////////////
        // reserveData / resizeData / clearData
        ////////////////////////////////////////////////////////////////////////

        /** \brief Reserve storage space for the mesh data. */
        template <class DataCloudT> inline void
        reserveData (DataCloudT& cloud, const std::size_t n, std::true_type /*has_data*/) const
        {
          cloud.reserve (n);
        }

        /** \brief Does nothing */
        template <class DataCloudT> inline void
        reserveData (DataCloudT& /*cloud*/, const std::size_t /*n*/, std::false_type /*has_data*/) const
        {
        }

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

        /** \brief Does nothing. */
        template <class DataCloudT> inline void
        resizeData (DataCloudT& /*data_cloud*/, const std::size_t /*n*/, const typename DataCloudT::value_type& /*data*/, std::false_type /*has_data*/) const
        {
        }

        /** \brief Clear the mesh data. */
        template <class DataCloudT> inline void
        clearData (DataCloudT& cloud, std::true_type /*has_data*/) const
        {
          cloud.clear ();
        }

        /** \brief Does nothing. */
        template <class DataCloudT> inline void
        clearData (DataCloudT& /*cloud*/, std::false_type /*has_data*/) const
        {
        }

        ////////////////////////////////////////////////////////////////////////
        // get / set Vertex
        ////////////////////////////////////////////////////////////////////////

        /** \brief Get the vertex for the given index. */
        inline Vertex&
        getVertex (const VertexIndex& idx_vertex)
        {
          assert (this->isValid (idx_vertex));
          return (vertices_ [idx_vertex.get ()]);
        }

        /** \brief Get the vertex for the given index. */
        inline Vertex
        getVertex (const VertexIndex& idx_vertex) const
        {
          assert (this->isValid (idx_vertex));
          return (vertices_ [idx_vertex.get ()]);
        }

        /** \brief Set the vertex at the given index. */
        inline void
        setVertex (const VertexIndex& idx_vertex, const Vertex& vertex)
        {
          assert (this->isValid (idx_vertex));
          vertices_ [idx_vertex.get ()] = vertex;
        }

        ////////////////////////////////////////////////////////////////////////
        // get / set HalfEdge
        ////////////////////////////////////////////////////////////////////////

        /** \brief Get the half-edge for the given index. */
        inline HalfEdge&
        getHalfEdge (const HalfEdgeIndex& idx_he)
        {
          assert (this->isValid (idx_he));
          return (half_edges_ [idx_he.get ()]);
        }

        /** \brief Get the half-edge for the given index. */
        inline HalfEdge
        getHalfEdge (const HalfEdgeIndex& idx_he) const
        {
          assert (this->isValid (idx_he));
          return (half_edges_ [idx_he.get ()]);
        }

        /** \brief Set the half-edge at the given index. */
        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
        ////////////////////////////////////////////////////////////////////////

        /** \brief Get the face for the given index. */
        inline Face&
        getFace (const FaceIndex& idx_face)
        {
          assert (this->isValid (idx_face));
          return (faces_ [idx_face.get ()]);
        }

        /** \brief Get the face for the given index. */
        inline Face
        getFace (const FaceIndex& idx_face) const
        {
          assert (this->isValid (idx_face));
          return (faces_ [idx_face.get ()]);
        }

        /** \brief Set the face at the given index. */
        inline void
        setFace (const FaceIndex& idx_face, const Face& face)
        {
          assert (this->isValid (idx_face));
          faces_ [idx_face.get ()] = face;
        }

      private:

        ////////////////////////////////////////////////////////////////////////
        // Members
        ////////////////////////////////////////////////////////////////////////

        /** \brief Data stored for the vertices. */
        VertexDataCloud vertex_data_cloud_;

        /** \brief Data stored for the half-edges. */
        HalfEdgeDataCloud half_edge_data_cloud_;

        /** \brief Data stored for the edges. */
        EdgeDataCloud edge_data_cloud_;

        /** \brief Data stored for the faces. */
        FaceDataCloud face_data_cloud_;

        /** \brief Connectivity information for the vertices. */
        Vertices vertices_;

        /** \brief Connectivity information for the half-edges. */
        HalfEdges half_edges_;

        /** \brief Connectivity information for the faces. */
        Faces faces_;

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

        /** \brief Storage for addFaceImplBase and deleteFace. */
        HalfEdgeIndices inner_he_;

        /** \brief Storage for addFaceImplBase. */
        HalfEdgeIndices free_he_;

        /** \brief Storage for addFaceImplBase. */
        std::vector <bool> is_new_;

        /** \brief Storage for addFaceImplBase. */
        std::vector <bool> make_adjacent_;

        /** \brief Storage for deleteFace. */
        std::vector <bool> is_boundary_;

        /** \brief Storage for deleteVertex. */
        FaceIndices delete_faces_vertex_;

        /** \brief Storage for deleteFace. */
        FaceIndices delete_faces_face_;

      public:

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

        PCL_MAKE_ALIGNED_OPERATOR_NEW
    };
  } // End namespace geometry
} // End namespace pcl
