1 #include "bsol_algs.h"
2 //:
3 // \file
4
5 #ifdef _MSC_VER
6 # include "vcl_msvc_warnings.h"
7 #endif
8
9 #include "vnl/vnl_numeric_traits.h"
10 #include <vsol/vsol_point_2d_sptr.h>
11 #include <vsol/vsol_point_2d.h>
12 #include <vsol/vsol_point_3d.h>
13 #include <vsol/vsol_line_2d.h>
14 #include <vsol/vsol_box_2d.h>
15 #include <vsol/vsol_box_3d.h>
16 #include <vsol/vsol_polygon_2d.h>
17 #include <vsol/vsol_digital_curve_2d.h>
18 #include "vgl/vgl_point_2d.h"
19 #include "vgl/vgl_homg_point_2d.h"
20 #include "vgl/vgl_box_2d.h"
21 #include "vgl/vgl_intersection.h"
22 #include <vgl/algo/vgl_convex_hull_2d.h>
23
24 // Destructor
25 bsol_algs::~bsol_algs()
26 = default;
27
28 //-----------------------------------------------------------------------------
29 //: Compute a bounding box for a set of vsol_point_2ds.
30 //-----------------------------------------------------------------------------
31 vbl_bounding_box<double,2> bsol_algs::
bounding_box(std::vector<vsol_point_2d_sptr> const & points)32 bounding_box(std::vector<vsol_point_2d_sptr> const& points)
33 {
34 vbl_bounding_box<double, 2> b;
35 for (const auto & point : points)
36 b.update(point->x(), point->y());
37 return b;
38 }
39
40 //-----------------------------------------------------------------------------
41 //: Compute a bounding box for a set of vsol_line_2ds.
42 //-----------------------------------------------------------------------------
43 vbl_bounding_box<double,2> bsol_algs::
bounding_box(std::vector<vsol_line_2d_sptr> const & lines)44 bounding_box(std::vector<vsol_line_2d_sptr> const & lines)
45 {
46 vbl_bounding_box<double, 2> b;
47 for (const auto & line : lines)
48 {
49 vsol_point_2d_sptr p0 = line->p0();
50 vsol_point_2d_sptr p1 = line->p1();
51 b.update(p0->x(), p0->y());
52 b.update(p1->x(), p1->y());
53 }
54 return b;
55 }
56
57 //-----------------------------------------------------------------------------
58 //: Compute a bounding box for a set of vsol_point_3ds.
59 //-----------------------------------------------------------------------------
60 vbl_bounding_box<double,3> bsol_algs::
bounding_box(std::vector<vsol_point_3d_sptr> const & points)61 bounding_box(std::vector<vsol_point_3d_sptr> const& points)
62 {
63 vbl_bounding_box<double, 3> b;
64 for (const auto & point : points)
65 b.update(point->x(), point->y(), point->z());
66 return b;
67 }
68
69 //-----------------------------------------------------------------------------
70 //: Determine if a point is inside a bounding box
71 //-----------------------------------------------------------------------------
in(vsol_box_2d_sptr const & b,double x,double y)72 bool bsol_algs::in(vsol_box_2d_sptr const & b, double x, double y)
73 {
74 if (!b)
75 return false;
76 double xmin = b->get_min_x(), ymin = b->get_min_y();
77 double xmax = b->get_max_x(), ymax = b->get_max_y();
78 return x >= xmin && x <= xmax
79 && y >= ymin && y <= ymax;
80 }
81
82 //: returns true if the boxes a and b intersect
meet(vsol_box_2d_sptr const & a,vsol_box_2d_sptr const & b)83 bool bsol_algs::meet(vsol_box_2d_sptr const & a, vsol_box_2d_sptr const & b)
84 {
85 double min_x_a = a->get_min_x(), max_x_a = a->get_max_x();
86 double min_y_a = a->get_min_y(), max_y_a = a->get_max_y();
87 double min_x_b = b->get_min_x(), max_x_b = b->get_max_x();
88 double min_y_b = b->get_min_y(), max_y_b = b->get_max_y();
89 return min_x_b <= max_x_a && min_x_a <= max_x_b
90 && min_y_b <= max_y_a && min_y_a <= max_y_b;
91 }
92
93 //: find the intersection of two boxes. Return false if no intersection
intersection(vsol_box_2d_sptr const & a,vsol_box_2d_sptr const & b,vsol_box_2d_sptr & a_int_b)94 bool bsol_algs::intersection(vsol_box_2d_sptr const & a,
95 vsol_box_2d_sptr const & b,
96 vsol_box_2d_sptr& a_int_b)
97 {
98 vgl_point_2d<double> a_min(a->get_min_x(), a->get_min_y());
99 vgl_point_2d<double> a_max(a->get_max_x(), a->get_max_y());
100 vgl_box_2d<double> vga(a_min, a_max);
101
102 vgl_point_2d<double> b_min(b->get_min_x(), b->get_min_y());
103 vgl_point_2d<double> b_max(b->get_max_x(), b->get_max_y());
104 vgl_box_2d<double> vgb(b_min, b_max);
105 vgl_box_2d<double> temp = vgl_intersection(vga, vgb);
106 if (temp.is_empty())
107 return false;
108 a_int_b = new vsol_box_2d();
109 a_int_b->add_point(temp.min_x(), temp.min_y());
110 a_int_b->add_point(temp.max_x(), temp.max_y());
111 return true;
112 }
113
114 //: find the convex union of two boxes. Always return true
box_union(vsol_box_2d_sptr const & a,vsol_box_2d_sptr const & b,vsol_box_2d_sptr & a_union_b)115 bool bsol_algs::box_union(vsol_box_2d_sptr const & a,
116 vsol_box_2d_sptr const & b,
117 vsol_box_2d_sptr& a_union_b)
118 {
119 if (!a||!b)
120 return false;
121 double x_min_a = a->get_min_x(), y_min_a = a->get_min_y();
122 double x_max_a = a->get_max_x(), y_max_a = a->get_max_y();
123 double x_min_b = b->get_min_x(), y_min_b = b->get_min_y();
124 double x_max_b = b->get_max_x(), y_max_b = b->get_max_y();
125 double x_min=x_min_a, y_min = y_min_a;
126 double x_max=x_max_a, y_max = y_max_a;
127 if (x_min_b<x_min)
128 x_min = x_min_b;
129 if (y_min_b<y_min)
130 y_min = y_min_b;
131 if (x_max_b>x_max)
132 x_max = x_max_b;
133 if (y_max_b>y_max)
134 y_max = y_max_b;
135 a_union_b = new vsol_box_2d();
136 a_union_b->add_point(x_min, y_min);
137 a_union_b->add_point(x_max, y_max);
138 return true;
139 }
140
141 //-----------------------------------------------------------------------------
142 //: expand or contract a box with the supplied absolute margin
143 //-----------------------------------------------------------------------------
box_with_margin(vsol_box_2d_sptr const & b,const double margin,vsol_box_2d_sptr & bmod)144 bool bsol_algs::box_with_margin(vsol_box_2d_sptr const & b,
145 const double margin,
146 vsol_box_2d_sptr& bmod)
147 {
148 if (!b)
149 return false;
150 double x_min = b->get_min_x(), y_min = b->get_min_y();
151 double x_max = b->get_max_x(), y_max = b->get_max_y();
152 double width = x_max-x_min, height = y_max-y_min;
153 //See if the margin for contraction is too large, i.e. margin is negative
154 if ((width+2*margin)<0)
155 return false;
156 if ((height+2*margin)<0)
157 return false;
158 bmod = new vsol_box_2d();
159 bmod->add_point(x_min-margin, y_min-margin);
160 bmod->add_point(x_max+margin, y_max+margin);
161 return true;
162 }
163
164 //-----------------------------------------------------------------------------
165 //: Compute the convex hull of a set of polygons
166 //-----------------------------------------------------------------------------
hull_of_poly_set(std::vector<vsol_polygon_2d_sptr> const & polys,vsol_polygon_2d_sptr & hull)167 bool bsol_algs::hull_of_poly_set(std::vector<vsol_polygon_2d_sptr> const& polys,
168 vsol_polygon_2d_sptr& hull)
169 {
170 if (!polys.size())
171 return false;
172 std::vector<vgl_point_2d<double> > points;
173 for (const auto & poly : polys)
174 {
175 if (!poly)
176 return false;
177 for (unsigned int i=0; i<poly->size(); ++i)
178 points.emplace_back(poly->vertex(i)->x(),
179 poly->vertex(i)->y());
180 }
181 vgl_convex_hull_2d<double> ch(points);
182 vgl_polygon<double> h = ch.hull();
183 hull = bsol_algs::poly_from_vgl(h);
184 return true;
185 }
186
187 //-----------------------------------------------------------------------------
188 //: Determine if a point is inside a bounding box
189 //-----------------------------------------------------------------------------
in(vsol_box_3d_sptr const & b,double x,double y,double z)190 bool bsol_algs::in(vsol_box_3d_sptr const & b,
191 double x, double y, double z)
192 {
193 if (!b)
194 return false;
195 double xmin = b->get_min_x(), ymin = b->get_min_y(), zmin = b->get_min_z();
196 double xmax = b->get_max_x(), ymax = b->get_max_y(), zmax = b->get_max_z();
197 return x >= xmin && x <= xmax
198 && y >= ymin && y <= ymax
199 && z >= zmin && z <= zmax;
200 }
201
poly_from_box(vsol_box_2d_sptr const & box)202 vsol_polygon_2d_sptr bsol_algs::poly_from_box(vsol_box_2d_sptr const& box)
203 {
204 std::vector<vsol_point_2d_sptr> pts;
205 vsol_point_2d_sptr pa = new vsol_point_2d(box->get_min_x(), box->get_min_y());
206 vsol_point_2d_sptr pb = new vsol_point_2d(box->get_max_x(), box->get_min_y());
207 vsol_point_2d_sptr pc = new vsol_point_2d(box->get_max_x(), box->get_max_y());
208 vsol_point_2d_sptr pd = new vsol_point_2d(box->get_min_x(), box->get_max_y());
209 pts.push_back(pa); pts.push_back(pb);
210 pts.push_back(pc); pts.push_back(pd); //pts.push_back(pa);
211 return new vsol_polygon_2d(pts);
212 }
213
214 //: construct a vsol_polygon from a vgl_polygon
poly_from_vgl(vgl_polygon<double> const & poly)215 vsol_polygon_2d_sptr bsol_algs::poly_from_vgl(vgl_polygon<double> const& poly)
216 {
217 vsol_polygon_2d_sptr out;
218 std::vector<vsol_point_2d_sptr> pts;
219 if (poly.num_sheets() != 1)
220 return out;
221 std::vector<vgl_point_2d<double> > sheet = poly[0];
222 for (auto & pit : sheet)
223 {
224 vsol_point_2d_sptr p = new vsol_point_2d(pit.x(), pit.y());
225 pts.push_back(p);
226 }
227 out = new vsol_polygon_2d(pts);
228 return out;
229 }
230
231 vgl_polygon<double>
vgl_from_poly(vsol_polygon_2d_sptr const & poly)232 bsol_algs::vgl_from_poly(vsol_polygon_2d_sptr const& poly)
233 {
234 vgl_polygon<double> vp(1);
235 if (!poly)
236 return vp;
237 unsigned nverts = poly->size();
238 for (unsigned i = 0; i<nverts; ++i)
239 {
240 double x = poly->vertex(i)->x(), y = poly->vertex(i)->y();
241 vp.push_back(x, y);
242 }
243 return vp;
244 }
245
246 //: find the closest point to p in a set of points
247 vsol_point_2d_sptr
closest_point(vsol_point_2d_sptr const & p,std::vector<vsol_point_2d_sptr> const & point_set,double & d)248 bsol_algs::closest_point(vsol_point_2d_sptr const& p,
249 std::vector<vsol_point_2d_sptr> const& point_set,
250 double& d)
251 {
252 vsol_point_2d_sptr cp;
253 int n = point_set.size();
254 if (!p||!n)
255 return cp;
256 double dmin_sq = vnl_numeric_traits<double>::maxval;
257 double x = p->x(), y = p->y();
258 for (const auto & pit : point_set)
259 {
260 double xs = pit->x(), ys = pit->y();
261 double dsq = (x-xs)*(x-xs)+(y-ys)*(y-ys);
262 if (dsq<dmin_sq)
263 {
264 dmin_sq = dsq;
265 cp = pit;
266 }
267 }
268 d = std::sqrt(dmin_sq);
269 return cp;
270 }
271
272 vsol_point_3d_sptr
closest_point(vsol_point_3d_sptr const & p,std::vector<vsol_point_3d_sptr> const & point_set,double & d)273 bsol_algs::closest_point(vsol_point_3d_sptr const& p,
274 std::vector<vsol_point_3d_sptr> const& point_set,
275 double& d)
276 {
277 d = 0;
278 vsol_point_3d_sptr cp;
279 int n = point_set.size();
280 if (!p||!n)
281 return cp;
282 double dmin_sq = vnl_numeric_traits<double>::maxval;
283 double x = p->x(), y = p->y(), z = p->z();
284 for (const auto & pit : point_set)
285 {
286 double xs = pit->x(), ys = pit->y(), zs = pit->z();
287 double dsq = (x-xs)*(x-xs) + (y-ys)*(y-ys) + (z-zs)*(z-zs);
288 if (dsq<dmin_sq)
289 {
290 dmin_sq = dsq;
291 cp = pit;
292 }
293 }
294 d = std::sqrt(dmin_sq);
295 return cp;
296 }
297
298 //: Transform a vsol_polygon_2d with a general homography.
299 // Return false if any of the points are turned into ideal points
300 // since vsol geometry is assumed finite.
homography(vsol_polygon_2d_sptr const & p,vgl_h_matrix_2d<double> const & H,vsol_polygon_2d_sptr & Hp)301 bool bsol_algs::homography(vsol_polygon_2d_sptr const& p,
302 vgl_h_matrix_2d<double> const& H,
303 vsol_polygon_2d_sptr& Hp)
304 {
305 const int n = p->size();
306 std::vector<vsol_point_2d_sptr> pts;
307 const double tol = 1e-06;
308 for (int i = 0; i<n; i++)
309 {
310 vsol_point_2d_sptr v = p->vertex(i);
311 vgl_homg_point_2d<double> hp(v->x(), v->y());
312 vgl_homg_point_2d<double> Hhp = H(hp);
313 if (Hhp.ideal(tol))
314 return false;
315 vgl_point_2d<double> q(Hhp);
316 vsol_point_2d_sptr qs = new vsol_point_2d(q.x(), q.y());
317 pts.push_back(qs);
318 }
319 Hp = new vsol_polygon_2d(pts);
320 return true;
321 }
322
323 //: Transform a vsol_polygon_2d with a point specified as the center of the transformation.
324 // i.e. vertices of the polygon are translated so that the specified point is the origin.
325 /// The transformation is then applied and the point coordinates added back in afterwards.
326 vsol_polygon_2d_sptr bsol_algs::
transform_about_point(vsol_polygon_2d_sptr const & p,vsol_point_2d_sptr const & c,vgl_h_matrix_2d<double> const & H)327 transform_about_point(vsol_polygon_2d_sptr const& p,
328 vsol_point_2d_sptr const& c,
329 vgl_h_matrix_2d<double> const& H)
330 {
331 const int n = p->size();
332 std::vector<vsol_point_2d_sptr> pts;
333 for (int i = 0; i<n; i++)
334 {
335 vsol_point_2d_sptr v = p->vertex(i);
336 //Remove the centroid
337 vgl_homg_point_2d<double> hp(v->x() - c->x(), v->y() - c->y());
338 vgl_homg_point_2d<double> Hhp = H(hp);
339 vgl_point_2d<double> q(Hhp);
340 //add it back in
341 vsol_point_2d_sptr qs = new vsol_point_2d(q.x() + c->x(), q.y() + c->y());
342 pts.push_back(qs);
343 }
344 return new vsol_polygon_2d(pts);
345 }
346
347 //: Transform a vsol_polygon_2d with a homography
348 // Apply the transform with the centroid of the polygon as the
349 // origin and then translate by the centroid location vector
350 vsol_polygon_2d_sptr bsol_algs::
transform_about_centroid(vsol_polygon_2d_sptr const & p,vgl_h_matrix_2d<double> const & H)351 transform_about_centroid(vsol_polygon_2d_sptr const& p,
352 vgl_h_matrix_2d<double> const& H)
353 {
354 vsol_point_2d_sptr c = p->centroid();
355 return bsol_algs::transform_about_point(p, c, H);
356 }
357
homography(vsol_box_2d_sptr const & b,vgl_h_matrix_2d<double> const & H,vsol_box_2d_sptr & Hb)358 bool bsol_algs::homography(vsol_box_2d_sptr const& b,
359 vgl_h_matrix_2d<double> const& H,
360 vsol_box_2d_sptr& Hb)
361 {
362 vsol_polygon_2d_sptr p = bsol_algs::poly_from_box(b);
363 vsol_polygon_2d_sptr Hp;
364 if (!homography(p, H, Hp))
365 return false;
366 Hb = Hp->get_bounding_box();
367 return true;
368 }
369
tangent(vsol_digital_curve_2d_sptr const & dc,unsigned index,double & dx,double & dy)370 void bsol_algs::tangent(vsol_digital_curve_2d_sptr const& dc, unsigned index,
371 double& dx, double& dy)
372 {
373 dx = 0; dy = 0;
374 if (!dc)
375 return;
376 unsigned n = dc->size();
377 //cases
378 if (index>=n)
379 return;
380 if (index == 0)// first point on curve
381 {
382 vsol_point_2d_sptr p_n0 = dc->point(0);
383 vsol_point_2d_sptr p_n1 = dc->point(1);
384 dx = p_n1->x()-p_n0->x();
385 dy = p_n1->y()-p_n0->y();
386 return;
387 }
388
389 if (index == n-1)// last point on curve
390 {
391 vsol_point_2d_sptr p_n1 = dc->point(n-1);
392 vsol_point_2d_sptr p_n2 = dc->point(n-2);
393 dx = p_n1->x()-p_n2->x();
394 dy = p_n1->y()-p_n2->y();
395 return;
396 }
397 //the normal case
398 vsol_point_2d_sptr p_m1 = dc->point(index-1);
399 vsol_point_2d_sptr p_p1 = dc->point(index+1);
400 dx = p_p1->x()-p_m1->x();
401 dy = p_p1->y()-p_m1->y();
402 }
403
print(vsol_box_2d_sptr const & b)404 void bsol_algs::print(vsol_box_2d_sptr const& b)
405 {
406 if (!b)
407 return;
408 std::cout << *b << '\n';
409 }
410
print(vsol_box_3d_sptr const & b)411 void bsol_algs::print(vsol_box_3d_sptr const& b)
412 {
413 if (!b)
414 return;
415 double xmin = b->get_min_x(), ymin = b->get_min_y(), zmin = b->get_min_z();
416 double xmax = b->get_max_x(), ymax = b->get_max_y(), zmax = b->get_max_z();
417
418 std::cout << "vsol_box_2d[(" << xmin << ' ' << ymin << ' ' << zmin << ")<("
419 << xmax << ' ' << ymax << ' ' << zmax << ")]\n";
420 }
421
print(vsol_point_2d_sptr const & p)422 void bsol_algs::print(vsol_point_2d_sptr const& p)
423 {
424 if (!p)
425 return;
426 std::cout << *p << '\n';
427 }
428
print(vsol_point_3d_sptr const & p)429 void bsol_algs::print(vsol_point_3d_sptr const& p)
430 {
431 if (!p)
432 return;
433 std::cout << "vsol_point_3d[ " << p->x() << ' ' << p->y() << ' '
434 << p->z() << " ]\n";
435 }
436