VTK polydata iterator for CGAL AABB_tree

#include <iostream>
#include <list>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/AABB_triangle_primitive.h>

#include <vtkXMLPolyDataReader.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkCutter.h>
#include <vtkPlane.h>
#include <vtkIdList.h>
#include <vtkCell.h>

typedef CGAL::Simple_cartesian<double> K;
typedef K::FT FT;
typedef K::Point_3 Point;
typedef K::Vector_3 Vector;
typedef K::Plane_3 Plane;
typedef K::Segment_3 Segment;
typedef K::Triangle_3 Triangle;

// defines the iterator over triangles needed by the tree:
class Triangles_const_iterator
    : public boost::iterator_facade<
      Triangles_const_iterator
    , K::Triangle_3 const           // Value
    , boost::forward_traversal_tag  // CategoryOrTraversal
    >
{
public:
    Triangles_const_iterator()
        : data(nullptr), cellId(-1) {}

    static Triangles_const_iterator begin(vtkPolyData* data)
    {
        return Triangles_const_iterator(data, 0);
    }

    static Triangles_const_iterator end(vtkPolyData* data)
    {
        return Triangles_const_iterator(data, data->GetNumberOfCells());
    }

private:
    explicit Triangles_const_iterator(vtkPolyData* data, int index)
        : data(data), cellId(index)
    {
    }


    friend class boost::iterator_core_access;
    void increment() {
        ++cellId;
    }

    K::Triangle_3 convertVTKtoCGAL() const
    {
        if (cellId < data->GetNumberOfCells()) {
            vtkCell* cell = data->GetCell(cellId);

            if (cell->GetNumberOfPoints() == 3) {
                K::Point_3 p[3];
                for (int i = 0; i < 3; ++i) {
                    double* pt = data->GetPoints()->GetPoint(cell->GetPointId(i));
                    p[i] = K::Point_3(pt[0], pt[1], pt[2]);
                }

                // Convert triangle to CGAL
                return K::Triangle_3(p[0], p[1], p[2]);
            }
        }
        return K::Triangle_3();
    }

    K::Triangle_3 dereference() const { return convertVTKtoCGAL(); }

    bool equal(Triangles_const_iterator const& other) const
    {
        return data == other.data && cellId == other.cellId;
    }

    int cellId;
    vtkPolyData* data;
};

typedef CGAL::AABB_triangle_primitive<K, Triangles_const_iterator> Primitive;
typedef CGAL::AABB_traits<K, Primitive> Traits;
typedef CGAL::AABB_tree<Traits> Tree;


int main()
{
    auto reader = vtkPolyDataReader::New();
    reader->SetFileName("mesh.vtk");
    reader->Update();

    auto polyData = reader->GetOutput();

    std::ifstream planeIn("plane.txt");

    double coords[9];
    for (int i = 0; i < 9; ++i) {
        planeIn >> coords[i];
    }
    Tree tree(Triangles_const_iterator::begin(polyData), Triangles_const_iterator::end(polyData));
    tree.accelerate_distance_queries();

    Point points[] = { { coords[0], coords[1], coords[2] }, { coords[3], coords[4], coords[5] }, { coords[6], coords[7], coords[8] } };
    Plane plane_query(points[0], points[1], points[2]);

    std::vector<Tree::Intersection_and_primitive_id<Plane>::Type> segments;
    tree.all_intersections(plane_query, std::back_inserter(segments));

    std::cout << segments.size() << " intersections(s) with plane\n";

    return EXIT_SUCCESS;
}