text: #include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
  #include <CGAL/Surface_mesh.h>
  #include <CGAL/Polyhedron_3.h>
  
  #include <CGAL/Rigid_triangle_mesh_collision_detection.h>
  
  #include <iostream>
  #include <fstream>
  
  typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
  typedef CGAL::Surface_mesh<K::Point_3>             Surface_mesh;
  typedef CGAL::Polyhedron_3<K>                      Polyhedron_3;
  namespace params = CGAL::parameters;
  
  void test_remove()
  {
    std::cout << "test_remove()" << std::endl;
    Surface_mesh sm;
    CGAL::Rigid_triangle_mesh_collision_detection<Surface_mesh> collision_detection;
    collision_detection.add_mesh(sm); // 0
    collision_detection.add_mesh(sm); // 1
    collision_detection.add_mesh(sm); // 2
    collision_detection.add_mesh(sm); // 3
    collision_detection.add_mesh(sm); // 4
  
    assert( collision_detection.is_valid_index(0) );
    assert( collision_detection.is_valid_index(1) );
    assert( collision_detection.is_valid_index(2) );
    assert( collision_detection.is_valid_index(3) );
    assert( collision_detection.is_valid_index(4) );
    assert(!collision_detection.is_valid_index(5) );
  
    assert(collision_detection.size() == 5);
  
    collision_detection.remove_mesh(4);
    assert(collision_detection.size() == 4);
    assert(!collision_detection.is_valid_index(4));
  
    std::size_t id = collision_detection.add_mesh(sm);
    assert(id == 4);
  
    collision_detection.remove_mesh(2);
    assert(collision_detection.size() == 4);
    assert(!collision_detection.is_valid_index(2));
    id = collision_detection.add_mesh(sm);
    assert(id == 2);
  
    assert( collision_detection.is_valid_index(0) );
    assert( collision_detection.is_valid_index(1) );
    assert( collision_detection.is_valid_index(2) );
    assert( collision_detection.is_valid_index(3) );
    assert( collision_detection.is_valid_index(4) );
  
    collision_detection.remove_mesh(3);
    assert( collision_detection.size()==4);
    assert( collision_detection.size_of_garbage()==1);
    collision_detection.remove_mesh(3);
    assert( collision_detection.size()==4);
    assert( collision_detection.size_of_garbage()==1);
    collision_detection.remove_mesh(2);
    assert( collision_detection.size()==3);
    assert( collision_detection.size_of_garbage()==2);
    collision_detection.remove_mesh(1);
    assert( collision_detection.size()==2);
    assert( collision_detection.size_of_garbage()==3);
    collision_detection.remove_mesh(4);
    assert( collision_detection.size()==1);
    assert( collision_detection.size_of_garbage()==4);
    collision_detection.remove_mesh(0);
    assert( collision_detection.size()==0);
    assert( collision_detection.size_of_garbage()==5);
  }
  
  template <class TriangleMesh, class Index>
  void test_intersections(Index index, const char* type)
  {
    std::cout << "test_intersections<"<<type<<">()" << std::endl;
    TriangleMesh tm1, tm2, tm3;
    std::ifstream input("data/small_spheres.off");
    assert(input);
    input >> tm1;
    input.close();
    input.open(CGAL::data_file_path("meshes/blobby.off"));
    assert(input);
    input >> tm2;
    input.close();
    input.open("data-coref/large_cube_coplanar.off");
    assert(input);
    input >> tm3;
    input.close();
  
    CGAL::Rigid_triangle_mesh_collision_detection<TriangleMesh> collision_detection;
    collision_detection.add_mesh(tm1, params::face_index_map(get(index, tm1))); // 0
    // add tm1 using an external tree
    typename CGAL::Rigid_triangle_mesh_collision_detection<TriangleMesh>::AABB_tree
      tm1_tree(boost::begin(faces(tm1)), boost::end(faces(tm1)), tm1);
    collision_detection.add_mesh(tm1_tree, tm1, params::face_index_map(get(index, tm1))); // 1 small_spheres
    collision_detection.add_mesh(tm2, params::face_index_map(get(index, tm2))); // 2 blobby
    collision_detection.add_mesh(tm3, params::face_index_map(get(index, tm3))); // 3 large_cube_coplanar
    // pool is 0 1 2 3
    collision_detection.remove_mesh(0);
    // pool is now 3 1 2
    std::vector<std::size_t> inter_res;
    std::vector< std::pair<std::size_t, bool> > inter_and_inclus_res;
  
    // spheres intersects both cube and blobby
    inter_res = collision_detection.get_all_intersections(1);
    assert(inter_res.size() == 2);
    assert(inter_res[0] == 3);
    assert(inter_res[1] == 2);
  
    // blobby intersects only spheres
    inter_res = collision_detection.get_all_intersections(2);
    assert(inter_res.size() == 1);
    assert(inter_res[0] == 1);
  
    // blobby is included into cube
    inter_and_inclus_res = collision_detection.get_all_intersections_and_inclusions(2);
    assert(inter_and_inclus_res.size() == 2);
    assert(inter_and_inclus_res[0].first == 3 && inter_and_inclus_res[0].second);
    assert(inter_and_inclus_res[1].first == 1 && !inter_and_inclus_res[1].second);
  
    // set transformations
    K::Aff_transformation_3 tm1_transf(-0.22115682390903474, 0.97263618730220114, 0.071193443439028392,
                                       -2.3805504333121519, 0.37717228816945586, 0.017983434697950496,
                                       0.92596849898551192, -4.6543355030238258, 0.89935016777420218,
                                       0.23163644624001295, -0.37082857562196114, -5.4867495857613582);
  
    K::Aff_transformation_3 tm2_transf(-0.10062050938738221, -0.92610032901754502, -0.36361200981845854,
                                       -2.1218726603809985, 0.98718082597554757, -0.13844084127856282,
                                       0.079423864753106477, -4.6627185424110094, -0.1238932198179533,
                                       -0.35095913445824267, 0.92815847570523291, -5.349009707055858);
  
    K::Aff_transformation_3 tm3_transf(0.8523140340417823, -0.41651563577194606, -0.3163471392835962,
                                       -2.0549216027623891, 0.34883891716890358, 0.90335137983769587,
                                       -0.24953495629622252, -5.8394794922248607, 0.3897078357485903,
                                       0.10232795171810326, 0.91523592207328008, -5.2436990668816268);
  
    collision_detection.set_transformation(1, tm1_transf);
    collision_detection.set_transformation(2, tm2_transf);
    collision_detection.set_transformation(3, tm3_transf);
  
    // spheres intersects blobby
    inter_res = collision_detection.get_all_intersections(1);
    assert(inter_res.size() == 1);
    assert(inter_res[0] == 2);
  
    // blobby intersect spheres and cube
    inter_res = collision_detection.get_all_intersections(2);
    assert(inter_res.size() == 2);
    assert(inter_res[0] == 3);
    assert(inter_res[1] == 1);
  
    // cube contains one CC of spheres
    inter_and_inclus_res = collision_detection.get_all_intersections_and_inclusions(3);
    assert(inter_and_inclus_res.size() == 2);
    assert(inter_and_inclus_res[0].first == 1 && inter_and_inclus_res[0].second);
    assert(inter_and_inclus_res[1].first == 2 && !inter_and_inclus_res[1].second);
  
    // one CC of spheres is contained by cube
    inter_and_inclus_res = collision_detection.get_all_intersections_and_inclusions(1);
    assert(inter_and_inclus_res.size() == 2);
    assert(inter_and_inclus_res[0].first == 3 && inter_and_inclus_res[0].second);
    assert(inter_and_inclus_res[1].first == 2 && !inter_and_inclus_res[1].second);
  }
  
  int main()
  {
    test_remove();
    test_intersections<Surface_mesh>(boost::face_index, "Surface_mesh");
    test_intersections<Polyhedron_3>(boost::face_external_index, "Polyhedron_3");
  
    return 0;
  }
  