snap_borders_onto_target.cpp

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polyhedron_items_with_id_3.h>
#include <CGAL/IO/Polyhedron_iostream.h>

#include <CGAL/box_intersection_d.h>
#include <CGAL/boost/graph/helpers.h>

#include <iostream>
#include <fstream>


typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Polyhedron_3<K, CGAL::Polyhedron_items_with_id_3>  Face_graph;

typedef boost::graph_traits<Face_graph>::halfedge_descriptor halfedge_descriptor;
typedef boost::graph_traits<Face_graph>::edge_descriptor edge_descriptor;
typedef boost::graph_traits<Face_graph>::face_descriptor face_descriptor;
typedef boost::graph_traits<Face_graph>::vertex_descriptor vertex_descriptor;

typedef CGAL::Box_intersection_d::Box_with_handle_d<double,3,vertex_descriptor> Box;

struct Vertex_proximity_report{
  double m_epsilon_2;
  boost::unordered_map<vertex_descriptor, vertex_descriptor>& m_vertex_map;
  boost::unordered_map<vertex_descriptor, vertex_descriptor> m_reversed_vertex_map;
  Vertex_proximity_report(double epsilon,
                          boost::unordered_map<vertex_descriptor, vertex_descriptor>& vertex_map)
  
    : m_epsilon_2( epsilon * epsilon )
    , m_vertex_map(vertex_map)
  {}

  // callback functor that reports all truly intersecting triangles
  void operator()(const Box& a, const Box& b)
  {
    if ( CGAL::compare_squared_distance(a.handle()->point(), b.handle()->point(), m_epsilon_2) != CGAL::LARGER )
    {
      boost::unordered_map<vertex_descriptor, vertex_descriptor>::iterator it = 
        m_reversed_vertex_map.insert( std::make_pair(a.handle(), b.handle()) ).first;
      if (it->second!=b.handle())
      {
        // conflict!
        m_vertex_map.erase(it->second);
        return;
      }
      m_vertex_map.insert( std::make_pair(b.handle(), a.handle()) );
    }
  }
};


int main(int argc, char** argv)
{
  if (argc!=5)
  {
    std::cerr << "Usage: " << argv[0] << " input_to_snap.off input_to_snap_onto.off output.off epsilon\n";
    return 1;
  }

  Face_graph fg, fg_target;
  std::ifstream input(argv[1]);
  if (!input)
  {
    std::cerr << "Cannot open " << argv[1] << "\n";
    return 1;
  }
  input >> fg;
  input.close();
  input.open(argv[2]);
  if (!input)
  {
    std::cerr << "Cannot open " << argv[2] << "\n";
    return 1;
  }
  input >> fg_target;
  std::ofstream output(argv[3]);
  if (!output)
  {
    std::cerr << "Cannot open " << argv[3] << "\n";
    return 1;
  }

  double epsilon = atof(argv[4]);


// Extract border vertices
  std::vector<halfedge_descriptor> border_vertices;
  for(edge_descriptor ed : edges(fg))
    if (is_border(ed, fg) )
    {
      halfedge_descriptor hd=halfedge(ed, fg);
      if ( !is_border(hd, fg) ) hd=opposite(hd, fg);
      border_vertices.push_back(hd);
    }

// Try to snap vertices
  std::vector<Box> boxes;
  for(halfedge_descriptor hd : border_vertices)
  {
    vertex_descriptor vd = target(hd, fg);
    const K::Point_3& p = vd->point();
    boxes.push_back(
      Box( CGAL::Bbox_3( p.x()-epsilon, p.y()-epsilon, p.z()-epsilon,
                         p.x()+epsilon, p.y()+epsilon, p.z()+epsilon ), vd)
    );
  }

  std::vector<Box> target_boxes;
  for(vertex_descriptor vh : vertices(fg_target))
  {
    target_boxes.push_back(Box( vh->point().bbox(), vh ));
  }

  //map fg_target -> fg
  boost::unordered_map<vertex_descriptor, vertex_descriptor> vertex_map;
  CGAL::box_intersection_d(
    boxes.begin(), boxes.end(),
    target_boxes.begin(), target_boxes.end(),
    Vertex_proximity_report(epsilon, vertex_map));

  std::cout << "found " << vertex_map.size() << " points to snap\n";
  
  for(std::pair<vertex_descriptor, vertex_descriptor>  p : vertex_map)
  {
    p.second->point() = p.first->point();
  }
  
// write output
  output << std::setprecision(17) << fg;

  return 0;
}