1 #include <iostream>
2 #include <cmath>
3 #include <limits>
4 #include <utility>
5 #include "bvgl_gen_cylinder.h"
6 #include <bvrml/bvrml_write.h>
7 #include "vgl/vgl_intersection.h"
8 #include "vgl/vgl_distance.h"
9 #include "vgl/vgl_box_3d.h"
10 #include "vgl/vgl_bounding_box.h"
11 #ifdef _MSC_VER
12 #  include "vcl_msvc_warnings.h"
13 #endif
14 bvgl_gen_cylinder
bvgl_gen_cylinder(vgl_cubic_spline_3d<double> axis,std::vector<bvgl_cross_section> const & cross_sects,double cross_section_interval)15 ::bvgl_gen_cylinder(vgl_cubic_spline_3d<double>  axis,
16                     std::vector<bvgl_cross_section> const& cross_sects,
17                     double cross_section_interval):
18   cross_section_interval_(cross_section_interval),
19   axis_(std::move(axis)),
20   cross_sections_(cross_sects)
21 {
22   for(const auto & cross_sect : cross_sects)
23     bbox_.add(cross_sect.bounding_box());
24 }
25 #if 0
26 void bvgl_gen_cylinder::load_cross_section_pointsets(std::ifstream& istr, double distance){
27   cross_sections_.clear();
28   // load the entire pointset
29   vgl_pointset_3d<double> ptset;
30   istr >> ptset;
31   std::cout << "original pointset size " << ptset.npts() << '\n';
32    // assign points to cross sections
33   // scan through the axis knots
34   unsigned nn = 0;
35   double mt = axis_.max_t();
36   for(double t = 0.0; t<=mt; t += cross_section_interval_){
37     vgl_point_3d<double> p = axis_(t);
38     vgl_point_3d<double> pc;
39     if((t + cross_section_interval_) > mt)
40       pc = axis_(t-cross_section_interval_);
41     else
42       pc = axis_(t+cross_section_interval_);
43     double d = 0.6*((p-pc).length());
44     vgl_plane_3d<double> norm_plane = axis_.normal_plane(t);
45     vgl_pointset_3d<double> cpts = vgl_intersection(norm_plane, ptset, d), final_cpts;
46     unsigned npts = cpts.npts();
47     for(unsigned i = 0; i<npts; ++i){
48       vgl_point_3d<double> pi = cpts.p(i);
49       double r = (pi-p).length();
50       if(r<=distance)
51         if(cpts.has_normals())
52           final_cpts.add_point_with_normal(pi, cpts.n(i));
53         else
54           final_cpts.add_point(pi);
55     }
56     bvgl_cross_section cs(t, p, norm_plane, final_cpts);
57     nn +=final_cpts.npts();
58     cross_sections_.push_back(cs);
59     bbox_.add(cs.bounding_box());
60   }
61   std::cout << "cumulative cross section pointset size " << nn << '\n';
62 }
63 void bvgl_gen_cylinder::load_cross_section_pointsets(std::ifstream& istr, double distance){
64   cross_sections_.clear();
65   vgl_pointset_3d<double> ptset;
66   // load the entire pointset
67   istr >> ptset;
68   if(!ptset.has_normals()){
69     std::cout << "FATAL input pointset must have normals! \n";
70     return;
71   }
72   std::vector<vgl_pointset_3d<double> > ptsets;
73   std::vector<vgl_plane_3d<double> > cross_planes;
74   std::vector<double> tset;
75   std::vector<vgl_point_3d<double> > pset;
76   // scan through the axis knots
77   double mt = axis_.max_t();
78   for(double t = 0.0; t<=mt; t += cross_section_interval_){
79     vgl_point_3d<double> p = axis_(t);
80     vgl_plane_3d<double> norm_plane = axis_.normal_plane(t);
81     ptsets.push_back(vgl_pointset_3d<double>());
82     cross_planes.push_back(norm_plane);
83     tset.push_back(t);
84     pset.push_back(p);
85   }
86   // scan through the pointset and assign each point to a cross section
87   unsigned np = ptset.npts(), nc = static_cast<unsigned>(ptsets.size());
88   for(unsigned i =0; i<np; ++i){
89     vgl_point_3d<double> pi = ptset.p(i);
90     vgl_vector_3d<double> ni = ptset.n(i);
91     double min_dist = std::numeric_limits<double>::max();
92     unsigned min_j = 0;
93     for(unsigned j = 0; j<nc; ++j){
94       double d = vgl_distance(pi, cross_planes[j]);
95       if(d<min_dist){
96         min_dist = d;
97         min_j = j;
98       }
99     }
100     ptsets[min_j].add_point_with_normal(pi, ni);
101   }
102   for(unsigned j = 0; j<nc; ++j){
103     bvgl_cross_section cs(tset[j], pset[j], cross_planes[j], ptsets[j]);
104     cross_sections_.push_back(cs);
105     bbox_.add(cs.bounding_box());
106   }
107 }
108 #endif
109 
load_cross_section_pointsets(std::ifstream & istr)110 void bvgl_gen_cylinder::load_cross_section_pointsets(std::ifstream& istr){
111   cross_sections_.clear();
112   vgl_pointset_3d<double> ptset;
113   // load the entire pointset
114   istr >> ptset;
115   if(!ptset.has_normals()){
116     std::cout << "FATAL input pointset must have normals! \n";
117     return;
118   }
119   std::vector<vgl_pointset_3d<double> > ptsets;
120   std::vector<vgl_plane_3d<double> > cross_planes;
121   std::vector<double> tset;
122   std::vector<vgl_point_3d<double> > pset;
123   // scan through the axis knots and set up elements of the cross section data
124   double mt = axis_.max_t();
125   for(double t = 0.0; t<=mt; t += cross_section_interval_){
126     vgl_point_3d<double> p = axis_(t);
127     vgl_plane_3d<double> norm_plane = axis_.normal_plane(t);
128     ptsets.emplace_back();
129     cross_planes.push_back(norm_plane);
130     tset.push_back(t);
131     pset.push_back(p);
132   }
133 
134   // scan through the pointset and assign each point to a cross section
135   unsigned np = ptset.npts(), nc = static_cast<unsigned>(ptsets.size());
136   for(unsigned i =0; i<np; ++i){
137     vgl_point_3d<double> pi = ptset.p(i);
138     vgl_vector_3d<double> ni = ptset.n(i);
139     double min_dist = std::numeric_limits<double>::max();
140     unsigned min_j = 0;
141     double dp, dk, d;
142     for(unsigned j = 0; j<nc; ++j){
143       dp = vgl_distance(pi, cross_planes[j]);
144       dk = vgl_distance(pi, pset[j]);
145       d = dp+ dk;
146       if(d<min_dist){
147         min_dist = d;
148         min_j = j;
149       }
150     }
151     ptsets[min_j].add_point_with_normal(pi, ni);
152   }
153   for(unsigned j = 0; j<nc; ++j){
154     bvgl_cross_section cs(tset[j], pset[j], cross_planes[j], ptsets[j]);
155     cross_sections_.push_back(cs);
156     bbox_.add(cs.bounding_box());
157   }
158 }
cross_section_contains(vgl_point_3d<double> const & p) const159 std::vector<unsigned> bvgl_gen_cylinder::cross_section_contains(vgl_point_3d<double> const& p) const{
160   std::vector<unsigned> ret;
161   auto nc = static_cast<unsigned>(cross_sections_.size());
162   for(unsigned i = 0; i<nc; ++i)
163     if(cross_sections_[i].contains(p))
164       ret.push_back(i);
165   return ret;
166 }
167 
closest_point(vgl_point_3d<double> const & p,vgl_point_3d<double> & pc,double dist_thresh) const168   bool bvgl_gen_cylinder::closest_point(vgl_point_3d<double> const& p, vgl_point_3d<double>& pc, double dist_thresh) const{
169   double big = std::numeric_limits<double>::max();
170   pc.set(big, big, big);
171    std::vector<unsigned> crx_indices = this->cross_section_contains(p);
172    if(!crx_indices.size())
173      return false;
174    double d_close = big;
175    vgl_point_3d<double> closest_pt;
176    for(unsigned int & crx_indice : crx_indices){
177      vgl_point_3d<double> cp = cross_sections_[crx_indice].closest_point(p, dist_thresh);
178      double d = (p-cp).length();
179      if(d<d_close){
180        d_close = d;
181        closest_pt = cp;
182      }
183    }
184    pc = closest_pt;
185    return true;
186 }
187 
distance(vgl_point_3d<double> const & p,double dist_thresh) const188 double bvgl_gen_cylinder::distance(vgl_point_3d<double> const& p, double dist_thresh) const{
189   vgl_point_3d<double> cp;
190   if(!this->closest_point(p, cp, dist_thresh))
191     return std::numeric_limits<double>::max();
192   return (p-cp).length();
193 }
194 
aggregate_pointset() const195 vgl_pointset_3d<double> bvgl_gen_cylinder::aggregate_pointset() const{
196   auto n = static_cast<unsigned>(cross_sections_.size());
197   vgl_pointset_3d<double> aggregate_ptset;
198   for(unsigned i = 0; i<n; i++){
199     vgl_pointset_3d<double> ps = cross_sections_[i].pts();
200     unsigned np = ps.npts();
201     for(unsigned j = 0; j<np; ++j)
202       aggregate_ptset.add_point_with_normal(ps.p(j), ps.n(j));
203   }
204   return aggregate_ptset;
205 }
206 
display_axis_spline(std::ofstream & ostr) const207 void bvgl_gen_cylinder::display_axis_spline(std::ofstream& ostr) const{
208   bvrml_write::write_vrml_header(ostr);
209   // display the knots
210   std::vector<vgl_point_3d<double> > knots = axis_.knots();
211   auto n = static_cast<unsigned>(knots.size());
212   auto nd = static_cast<double>(n-1);
213   float r = 1.0f;
214   for(unsigned i =0; i<n; ++i){
215     vgl_point_3d<double> p = knots[i];
216     vgl_point_3d<float> pf(static_cast<float>(p.x()), static_cast<float>(p.y()), static_cast<float>(p.z()));
217     vgl_sphere_3d<float> sp(pf, r);
218     bvrml_write::write_vrml_sphere(ostr, sp, 0.0f, 1.0f, 0.0f);
219   }
220   // display the spline points
221   for(double t = 0; t<=nd; t+=0.05){
222     vgl_point_3d<double> p = axis_(t);
223     vgl_point_3d<float> pf(static_cast<float>(p.x()), static_cast<float>(p.y()), static_cast<float>(p.z()));
224     vgl_sphere_3d<float> sp(pf, 0.25f);
225     bvrml_write::write_vrml_sphere(ostr, sp, 1.0f, 1.0f, 0.0f);
226   }
227   ostr.close();
228 }
display_cross_section_planes(std::ofstream & ostr) const229 void bvgl_gen_cylinder::display_cross_section_planes(std::ofstream& ostr) const{
230   bvrml_write::write_vrml_header(ostr);
231   auto n = static_cast<unsigned>(cross_sections_.size());
232   for(unsigned i = 0; i<n; i++)
233     cross_sections_[i].display_cross_section_plane(ostr);
234   ostr.close();
235 }
236 
display_cross_section_pointsets(std::ofstream & ostr) const237 void bvgl_gen_cylinder::display_cross_section_pointsets(std::ofstream& ostr) const{
238   bvrml_write::write_vrml_header(ostr);
239   auto n = static_cast<unsigned>(cross_sections_.size());
240   for(unsigned i = 0; i<n; i++)
241     cross_sections_[i].display_cross_section_pts(ostr);
242   ostr.close();
243 }
244 
display_surface_disks(std::ofstream & ostr) const245 void bvgl_gen_cylinder::display_surface_disks(std::ofstream& ostr) const{
246   bvrml_write::write_vrml_header(ostr);
247   auto n = static_cast<unsigned>(cross_sections_.size());
248   for(unsigned i = 0; i<n; i++)
249     cross_sections_[i].display_cross_section_normal_disks(ostr);
250   ostr.close();
251 }
252