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