1 // Some tests for vgl_closest_point
2 // Peter Vanroose, 5 June 2003
3 #include <iostream>
4 #include <cmath>
5 #include <limits>
6 #include "testlib/testlib_test.h"
7 #include "vgl/vgl_closest_point.h"
8 #include "vgl/vgl_distance.h"
9 #include "vgl/vgl_homg_point_2d.h"
10 #include "vgl/vgl_homg_line_2d.h"
11 #include "vgl/vgl_homg_point_3d.h"
12 #include "vgl/vgl_homg_plane_3d.h"
13 #include "vgl/vgl_homg_line_3d_2_points.h"
14 #include "vgl/vgl_point_2d.h"
15 #include "vgl/vgl_line_2d.h"
16 #include "vgl/vgl_point_3d.h"
17 #include "vgl/vgl_line_3d_2_points.h"
18 #include "vgl/vgl_line_segment_2d.h"
19 #include "vgl/vgl_line_segment_3d.h"
20 #include "vgl/vgl_infinite_line_3d.h"
21 #include "vgl/vgl_plane_3d.h"
22 #include "vgl/vgl_sphere_3d.h"
23 #include "vgl/vgl_cylinder_3d.h"
24 #ifdef _MSC_VER
25 # include "vcl_msvc_warnings.h"
26 #endif
27 #include "vgl/vgl_cubic_spline_3d.h"
28
29 static void
testHomgLine2DClosestPoint()30 testHomgLine2DClosestPoint()
31 {
32 vgl_homg_point_2d<double> p, q;
33 vgl_homg_line_2d<double> l;
34
35 // test for coincident
36 l.set(0, 1, -1);
37 q.set(0, 1);
38 p = vgl_closest_point(l, q);
39 TEST("2D coincident test", p, q);
40 TEST("2D coincident test", vgl_closest_point(q, l), q);
41 TEST_NEAR("Distance test", vgl_distance(l, q), 0.0, 1e-8);
42 TEST_NEAR("Distance test", vgl_distance(q, l), 0.0, 1e-8);
43 q.set(5, 1);
44 p = vgl_closest_point(l, q);
45 TEST("2D coincident test", p, q);
46
47 // test for non-coincident
48 q.set(0, 2);
49 p.set(0, 1);
50 TEST("2D non-coincident test", vgl_closest_point(l, q), p);
51 TEST_NEAR("Distance test", vgl_distance(l, q), 1.0, 1e-8);
52 q.set(5, 2);
53 p.set(5, 1);
54 TEST("2D non-coincident test", vgl_closest_point(q, l), p);
55 TEST_NEAR("Distance test", vgl_distance(q, l), 1.0, 1e-8);
56 }
57
58 // Test for closest points on two 3D lines, by Brendan McCane
59 static void
testHomgLine3DClosestPoints()60 testHomgLine3DClosestPoints()
61 {
62 vgl_homg_point_3d<double> p1, p2;
63 vgl_homg_line_3d_2_points<double> l1;
64 vgl_homg_line_3d_2_points<double> l2;
65 std::pair<vgl_homg_point_3d<double>, vgl_homg_point_3d<double>> pts;
66
67 // test for parallel lines
68 p1.set(0, 1, 0);
69 p2.set(1, 0, 0, 0);
70 l1.set(p1, p2);
71 p1.set(1, 4, 4);
72 p2.set(1, 0, 0, 0);
73 l2.set(p1, p2);
74 pts = vgl_closest_points(l1, l2);
75 // result should be (1,0,0,0) for both pts
76 if (pts.first != pts.second)
77 std::cout << "parallel test failed, points should be equal.\n"
78 << "points are: " << pts.first << ' ' << pts.second << std::endl;
79 else if (pts.first != p2)
80 std::cout << "parallel test failed, points should be " << p2 << '\n'
81 << "points are: " << pts.first << ' ' << pts.second << std::endl;
82 TEST("Parallel test", pts.first == pts.second && pts.first == p2, true);
83 TEST_NEAR("Parallel distance test", vgl_distance(l1, l2), 5.0, 1e-8);
84 TEST_NEAR("Parallel distance test", vgl_distance(l1, p1), 5.0, 1e-8);
85 p1.set(0, 1, 0);
86 TEST_NEAR("Parallel distance test", vgl_distance(p1, l2), 5.0, 1e-8);
87
88 // test for intersecting lines.
89 p1.set(0, 0, 0);
90 p2.set(1, 1, 1);
91 l1.set(p1, p2);
92 p1.set(2, 0, 0);
93 p2.set(1, 1, 1);
94 l2.set(p1, p2);
95 pts = vgl_closest_points(l1, l2);
96 // result should be (1,1,1) for both pts
97 if (pts.first != pts.second)
98 std::cout << "Intersect test failed, points should be equal\n"
99 << "points are: " << pts.first << ' ' << pts.second << std::endl;
100 else if (pts.first != p2)
101 std::cout << "Intersect test failed, points should be " << p2 << '\n'
102 << "points are: " << pts.first << ' ' << pts.second << std::endl;
103 TEST("Intersect test", pts.first == pts.second && pts.first == p2, true);
104 TEST_NEAR("Intersect distance test", vgl_distance(l1, l2), 0.0, 1e-8);
105
106 // now test for skew lines
107 //
108 // The lines are diagonals on neighbouring faces of a unit cube.
109 // The diagonals are chosen so they do not meet. The closest
110 // distance between the two lines is 1/sqrt(3). There is actually
111 // a way of visualising the problem that makes the answer obvious
112 // (see visualisation course by Geoff Wyvill and Bob Parslow).
113
114 p1.set(0, 0, 0);
115 p2.set(1, 1, 0);
116 l1.set(p1, p2);
117 p1.set(1, 0, 0);
118 p2.set(1, 1, 1);
119 l2.set(p1, p2);
120 pts = vgl_closest_points(l1, l2);
121 std::cout << "Closest points are: " << pts.first << ' ' << pts.second << '\n';
122 TEST("Skew lines test", pts.first, vgl_homg_point_3d<double>(2, 2, 0, 3));
123 TEST("Skew lines test", pts.second, vgl_homg_point_3d<double>(3, 1, 1, 3));
124 TEST_NEAR("Skew lines distance test", vgl_distance(pts.first, pts.second), 1 / std::sqrt(3.0), 1e-8);
125 TEST_NEAR("Skew lines distance test", vgl_distance(l1, pts.second), 1 / std::sqrt(3.0), 1e-8);
126 TEST_NEAR("Skew lines distance test", vgl_distance(pts.first, l2), 1 / std::sqrt(3.0), 1e-8);
127 }
128
129 static void
testHomgPlane3DClosestPoint()130 testHomgPlane3DClosestPoint()
131 {
132 vgl_homg_point_3d<double> p, q;
133 vgl_homg_plane_3d<double> l;
134
135 // test for coincident
136 l.set(0, 1, 0, -1);
137 q.set(0, 1, 2);
138 p = vgl_closest_point(l, q);
139 TEST("3D coincident test", p, q);
140 TEST("3D coincident test", vgl_closest_point(q, l), q);
141 TEST_NEAR("Distance test", vgl_distance(l, q), 0.0, 1e-8);
142 TEST_NEAR("Distance test", vgl_distance(q, l), 0.0, 1e-8);
143 q.set(5, 1, 3);
144 p = vgl_closest_point(l, q);
145 TEST("3D coincident test", p, q);
146
147 // test for non-coincident
148 q.set(0, 2, 3);
149 p.set(0, 1, 3);
150 TEST("3D non-coincident test", vgl_closest_point(l, q), p);
151 TEST_NEAR("Distance test", vgl_distance(l, q), 1.0, 1e-8);
152 q.set(5, 2, 3);
153 p.set(5, 1, 3);
154 TEST("3D non-coincident test", vgl_closest_point(q, l), p);
155 TEST_NEAR("Distance test", vgl_distance(q, l), 1.0, 1e-8);
156 }
157
158 static void
testLine2DClosestPoint()159 testLine2DClosestPoint()
160 {
161 vgl_point_2d<double> p, q;
162 vgl_line_2d<double> l;
163
164 // test for coincident
165 l.set(0, 1, -1);
166 q.set(0, 1);
167 p = vgl_closest_point(l, q);
168 TEST("2D coincident test", p, q);
169 TEST("2D coincident test", vgl_closest_point(q, l), q);
170 TEST_NEAR("Distance test", vgl_distance(l, q), 0.0, 1e-8);
171 TEST_NEAR("Distance test", vgl_distance(q, l), 0.0, 1e-8);
172 q.set(5, 1);
173 p = vgl_closest_point(l, q);
174 TEST("2D coincident test", p, q);
175
176 // test for non-coincident
177 q.set(0, 2);
178 p.set(0, 1);
179 TEST("2D non-coincident test", vgl_closest_point(l, q), p);
180 TEST_NEAR("Distance test", vgl_distance(l, q), 1.0, 1e-8);
181 q.set(5, 2);
182 p.set(5, 1);
183 TEST("2D non-coincident test", vgl_closest_point(q, l), p);
184 TEST_NEAR("Distance test", vgl_distance(q, l), 1.0, 1e-8);
185 }
186
187 static void
testLine3DClosestPoint()188 testLine3DClosestPoint()
189 {
190 vgl_point_3d<double> p, q;
191 vgl_line_3d_2_points<double> l;
192
193 // test for coincident
194 p.set(3, 4, -1);
195 q.set(-3, -2, 5);
196 l.set(p, q);
197 q.set(0, 1, 2);
198 p = vgl_closest_point(l, q);
199 TEST("3D coincident test", p, q);
200 TEST("3D coincident test", vgl_closest_point(q, l), q);
201 TEST_NEAR("Distance test", vgl_distance(l, q), 0.0, 1e-8);
202 TEST_NEAR("Distance test", vgl_distance(q, l), 0.0, 1e-8);
203
204 // test for non-coincident
205 q.set(0, 2, 7);
206 p.set(0, 5, 3);
207 l.set(p, vgl_point_3d<double>(0, 1, 0));
208 TEST("3D non-coincident test", vgl_closest_point(l, q), p);
209 TEST_NEAR("Distance test", vgl_distance(l, q), 5.0, 1e-8);
210 q.set(1, 2, 7);
211 TEST("3D non-coincident test", vgl_closest_point(q, l), p);
212 TEST_NEAR("Distance test", vgl_distance(q, l), std::sqrt(26.0), 1e-8);
213 }
214
215 // Similar to test above, but uses parametric value.
216 static void
test_line_3d_2_points_closest_point_t()217 test_line_3d_2_points_closest_point_t()
218 {
219 std::cout << "------------------------------------------------\n"
220 << " Testing vgl_closest_point_t(line_3d_2_points):\n"
221 << "------------------------------------------------\n";
222
223 vgl_point_3d<double> p, q, r;
224 vgl_line_3d_2_points<double> l;
225 double t;
226
227 // test for coincident
228 p.set(3, 4, -1);
229 q.set(-3, -2, 5);
230 l.set(p, q);
231 r.set(0, 1, 2); // midpt of line
232 t = vgl_closest_point_t(l, r);
233 TEST_NEAR("3D coincident test", t, 0.5, 1e-8);
234
235 // test for non-coincident
236 p.set(0, 0, 0);
237 q.set(1, 0, 0);
238 l.set(p, q);
239 r.set(0.5, 2, 3);
240 t = vgl_closest_point_t(l, r);
241 TEST_NEAR("3D non-coincident test", t, 0.5, 1e-8);
242 }
243
244 static void
test_line_segment_2d_closest_point()245 test_line_segment_2d_closest_point()
246 {
247 std::cout << "-----------------------------------------------------\n"
248 << " Testing vgl_closest_point(line_segment_2d, point_2d)\n"
249 << "-----------------------------------------------------\n";
250
251 vgl_point_2d<double> p, q;
252 vgl_line_segment_2d<double> l(vgl_point_2d<double>(0, 0), vgl_point_2d<double>(0, 1));
253
254 // test for coincident
255 q.set(0, 1);
256 p = vgl_closest_point(l, q);
257 TEST("2D coincident test", p, q);
258 TEST("2D coincident test", vgl_closest_point(q, l), q);
259 TEST_NEAR("Distance test", vgl_distance(l, q), 0.0, 1e-8);
260 TEST_NEAR("Distance test", vgl_distance(q, l), 0.0, 1e-8);
261
262 // test for non-coincident
263 q.set(0, 2);
264 p.set(0, 1);
265 TEST("2D non-coincident test", vgl_closest_point(l, q), p);
266 TEST_NEAR("Distance test", vgl_distance(l, q), 1.0, 1e-8);
267 q.set(5, 0);
268 p.set(0, 0);
269 TEST("2D non-coincident test", vgl_closest_point(q, l), p);
270 TEST_NEAR("Distance test", vgl_distance(q, l), 5.0, 1e-8);
271 }
272
273 static void
test_line_segment_3d_closest_point()274 test_line_segment_3d_closest_point()
275 {
276 std::cout << "-----------------------------------------------------\n"
277 << " Testing vgl_closest_point(line_segment_3d, point_3d)\n"
278 << "-----------------------------------------------------\n";
279
280 vgl_point_3d<double> p, q;
281 vgl_line_segment_3d<double> l(vgl_point_3d<double>(0, 0, 0), vgl_point_3d<double>(0, 0, 1));
282
283 // test for coincident
284 p.set(3, 4, -1);
285 q.set(-3, -2, 5);
286 l.set(p, q);
287 q.set(0, 1, 2);
288 p = vgl_closest_point(l, q);
289 TEST("3D coincident test", p, q);
290 TEST("3D coincident test", vgl_closest_point(q, l), q);
291 TEST_NEAR("Distance test", vgl_distance(l, q), 0.0, 1e-8);
292 TEST_NEAR("Distance test", vgl_distance(q, l), 0.0, 1e-8);
293
294 // test for non-coincident
295 q.set(0, 2, 7);
296 p.set(0, 5, 3);
297 l.set(p, vgl_point_3d<double>(0, 1, 0));
298 TEST("3D non-coincident test", vgl_closest_point(l, q), p);
299 TEST_NEAR("Distance test", vgl_distance(l, q), 5.0, 1e-8);
300 q.set(1, 2, 7);
301 TEST("3D non-coincident test", vgl_closest_point(q, l), p);
302 TEST_NEAR("Distance test", vgl_distance(q, l), std::sqrt(26.0), 1e-8);
303 }
304
305
306 static void
testPlane3DClosestPoint()307 testPlane3DClosestPoint()
308 {
309 std::cout << "------------------------------------------\n"
310 << " Testing vgl_closest_point(vgl_plane_3d):\n"
311 << "------------------------------------------\n";
312
313 vgl_point_3d<double> p, q;
314 vgl_plane_3d<double> l;
315
316 // test for coincident
317 l.set(0, 1, 0, -1);
318 q.set(0, 1, 2);
319 p = vgl_closest_point(l, q);
320 TEST("3D coincident test", p, q);
321 TEST("3D coincident test", vgl_closest_point(q, l), q);
322 TEST_NEAR("Distance test", vgl_distance(l, q), 0.0, 1e-8);
323 TEST_NEAR("Distance test", vgl_distance(q, l), 0.0, 1e-8);
324 q.set(5, 1, 3);
325 p = vgl_closest_point(l, q);
326 TEST("3D coincident test", p, q);
327
328 // test for non-coincident
329 q.set(0, 2, 3);
330 p.set(0, 1, 3);
331 TEST("3D non-coincident test", vgl_closest_point(l, q), p);
332 TEST_NEAR("Distance test", vgl_distance(l, q), 1.0, 1e-8);
333 q.set(5, 2, 3);
334 p.set(5, 1, 3);
335 TEST("3D non-coincident test", vgl_closest_point(q, l), p);
336 TEST_NEAR("Distance test", vgl_distance(q, l), 1.0, 1e-8);
337 }
338
339 static void
testPoly2DClosestPoint()340 testPoly2DClosestPoint()
341 {
342 double poly_x[] = { 0.0, 0.0, 2.0, 2.0 };
343 double poly_y[] = { 0.0, 1.0, 1.0, 0.0 }; // rectangle
344 double px, py;
345
346 // test for coincident with non-closed polygon
347 int idx = vgl_closest_point_to_non_closed_polygon(px, py, poly_x, poly_y, 4, 1.5, 1.0);
348 TEST("2D non-closed polygon coincident test: index", idx, 1);
349 TEST("2D non-closed polygon coincident test: point x", px, 1.5);
350 TEST("2D non-closed polygon coincident test: point y", py, 1.0);
351 TEST_NEAR("Distance test", vgl_distance_to_non_closed_polygon(poly_x, poly_y, 4, 1.5, 1.0), 0.0, 1e-8);
352
353 // test for non-coincident with non-closed polygon
354 idx = vgl_closest_point_to_non_closed_polygon(px, py, poly_x, poly_y, 4, 1.5, 0.25);
355 TEST("2D non-closed polygon non-coincident test: index", idx, 2);
356 TEST("2D non-closed polygon non-coincident test: point x", px, 2.0);
357 TEST("2D non-closed polygon non-coincident test: point y", py, 0.25);
358 TEST_NEAR("Distance test", vgl_distance_to_non_closed_polygon(poly_x, poly_y, 4, 1.5, 0.25), 0.5, 1e-8);
359
360 // test for coincident with closed polygon
361 idx = vgl_closest_point_to_closed_polygon(px, py, poly_x, poly_y, 4, 1.5, 1.0);
362 TEST("2D closed polygon coincident test: index", idx, 1);
363 TEST("2D closed polygon coincident test: point x", px, 1.5);
364 TEST("2D closed polygon coincident test: point y", py, 1.0);
365 TEST_NEAR("Distance test", vgl_distance_to_closed_polygon(poly_x, poly_y, 4, 1.5, 1.0), 0.0, 1e-8);
366
367 // test for non-coincident with closed polygon
368 idx = vgl_closest_point_to_closed_polygon(px, py, poly_x, poly_y, 4, 1.5, 0.25);
369 TEST("2D closed polygon non-coincident test: index", idx, 3);
370 TEST("2D closed polygon non-coincident test: point x", px, 1.5);
371 TEST("2D closed polygon non-coincident test: point y", py, 0.0);
372 TEST_NEAR("Distance test", vgl_distance_to_closed_polygon(poly_x, poly_y, 4, 1.5, 0.25), 0.25, 1e-8);
373 }
374
375 static void
testPoly3DClosestPoint()376 testPoly3DClosestPoint()
377 {
378 double poly_x[] = { 0.0, 0.0, 2.0, 2.0 };
379 double poly_y[] = { 0.0, 0.0, 2.0, 2.0 };
380 double poly_z[] = { 0.0, 1.0, 1.0, 0.0 }; // rectangle
381 double px, py, pz;
382
383 // test for coincident with non-closed polygon
384 int idx = vgl_closest_point_to_non_closed_polygon(px, py, pz, poly_x, poly_y, poly_z, 4, 1.5, 1.5, 1.0);
385 TEST("3D non-closed polygon coincident test: index", idx, 1);
386 TEST("3D non-closed polygon coincident test: point x", px, 1.5);
387 TEST("3D non-closed polygon coincident test: point y", py, 1.5);
388 TEST("3D non-closed polygon coincident test: point z", pz, 1.0);
389 TEST_NEAR("Distance test", vgl_distance_to_non_closed_polygon(poly_x, poly_y, poly_z, 4, 1.5, 1.5, 1.0), 0.0, 1e-8);
390
391 // test for non-coincident with non-closed polygon
392 idx = vgl_closest_point_to_non_closed_polygon(px, py, pz, poly_x, poly_y, poly_z, 4, 1.5, 1.5, 0.25);
393 TEST("3D non-closed polygon non-coincident test: index", idx, 2);
394 TEST("3D non-closed polygon non-coincident test: point x", px, 2.0);
395 TEST("3D non-closed polygon non-coincident test: point y", py, 2.0);
396 TEST("3D non-closed polygon non-coincident test: point z", pz, 0.25);
397 TEST_NEAR("Distance test",
398 vgl_distance_to_non_closed_polygon(poly_x, poly_y, poly_z, 4, 1.5, 1.5, 0.25),
399 std::sqrt(0.5),
400 1e-8);
401
402 // test for coincident with closed polygon
403 idx = vgl_closest_point_to_closed_polygon(px, py, pz, poly_x, poly_y, poly_z, 4, 1.5, 1.5, 1.0);
404 TEST("3D closed polygon coincident test: index", idx, 1);
405 TEST("3D closed polygon coincident test: point x", px, 1.5);
406 TEST("3D closed polygon coincident test: point y", py, 1.5);
407 TEST("3D closed polygon coincident test: point z", pz, 1.0);
408 TEST_NEAR("Distance test", vgl_distance_to_closed_polygon(poly_x, poly_y, poly_z, 4, 1.5, 1.5, 1.0), 0.0, 1e-8);
409
410 // test for non-coincident with closed polygon
411 idx = vgl_closest_point_to_closed_polygon(px, py, pz, poly_x, poly_y, poly_z, 4, 1.5, 1.5, 0.25);
412 TEST("3D closed polygon non-coincident test: index", idx, 3);
413 TEST("3D closed polygon non-coincident test: point x", px, 1.5);
414 TEST("3D closed polygon non-coincident test: point y", py, 1.5);
415 TEST("3D closed polygon non-coincident test: point z", pz, 0.0);
416 TEST_NEAR("Distance test", vgl_distance_to_closed_polygon(poly_x, poly_y, poly_z, 4, 1.5, 1.5, 0.25), 0.25, 1e-8);
417 }
418
419 // Test for closest points on two 3D (non-homogeneous) lines
420 static void
testLine3DClosestPoints()421 testLine3DClosestPoints()
422 {
423 std::cout << "--------------------------------------------------\n"
424 << " Testing vgl_closest_points(3D lines, non-homog):\n"
425 << "--------------------------------------------------\n";
426
427 // Test general case of non-parallel, non-intersecting lines
428 {
429 vgl_line_3d_2_points<double> l1(vgl_point_3d<double>(0, 0, 0), vgl_point_3d<double>(1, 0, 0));
430 vgl_line_3d_2_points<double> l2(vgl_point_3d<double>(0, 0, 1), vgl_point_3d<double>(0, 1, 1));
431 bool u = false;
432 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
433 bool success = (c.first == vgl_point_3d<double>(0, 0, 0) && c.second == vgl_point_3d<double>(0, 0, 1) && u);
434 TEST("Non-parallel, non-intersecting", success, true);
435 }
436
437 // Test common case of non-parallel, intersecting lines
438 {
439 vgl_line_3d_2_points<double> l1(vgl_point_3d<double>(0, 0, 0), vgl_point_3d<double>(1, 0, 0));
440 vgl_line_3d_2_points<double> l2(vgl_point_3d<double>(0, 0, 0), vgl_point_3d<double>(0, 1, 0));
441 bool u = false;
442 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
443 bool success = (c.first == vgl_point_3d<double>(0, 0, 0) && c.second == vgl_point_3d<double>(0, 0, 0) && u);
444 TEST("Non-parallel, intersecting", success, true);
445 }
446
447 // Test special case of parallel, non-collinear lines
448 {
449 vgl_line_3d_2_points<double> l1(vgl_point_3d<double>(0, 0, 0), vgl_point_3d<double>(1, 0, 0));
450 vgl_line_3d_2_points<double> l2(vgl_point_3d<double>(0, 0, 1), vgl_point_3d<double>(1, 0, 1));
451 bool u = true;
452 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
453 bool success = (c.first == vgl_point_3d<double>(0, 0, 0) && c.second == vgl_point_3d<double>(0, 0, 1) && !u);
454 TEST("Parallel, non-collinear", success, true);
455 }
456
457 // Test special case of collinear lines
458 {
459 vgl_line_3d_2_points<double> l1(vgl_point_3d<double>(0, 0, 0), vgl_point_3d<double>(1, 0, 0));
460 vgl_line_3d_2_points<double> l2(vgl_point_3d<double>(0, 0, 0), vgl_point_3d<double>(1, 0, 0));
461 bool u = true;
462 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
463 bool success = (c.first == vgl_point_3d<double>(0, 0, 0) && c.second == vgl_point_3d<double>(0, 0, 0) && !u);
464 TEST("Collinear", success, true);
465 }
466 }
467
468 // Test for closest points on two 3D line segments
469 static void
testLineSegment3DClosestPoints()470 testLineSegment3DClosestPoints()
471 {
472 std::cout << "-----------------------------------------------\n"
473 << " Testing vgl_closest_points(3D line segments):\n"
474 << "-----------------------------------------------\n";
475
476 // Test general case of non-parallel, non-intersecting lines, with internal points closest
477 {
478 vgl_line_segment_3d<double> l1(vgl_point_3d<double>(-1, 0, 0), vgl_point_3d<double>(1, 0, 0));
479 vgl_line_segment_3d<double> l2(vgl_point_3d<double>(0, -1, 1), vgl_point_3d<double>(0, 1, 1));
480 bool u = false;
481 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
482 bool success = (c.first == vgl_point_3d<double>(0, 0, 0) && c.second == vgl_point_3d<double>(0, 0, 1) && u);
483 TEST("Non-parallel, non-intersecting, internal points", success, true);
484 }
485
486 // Test general case of non-parallel, non-intersecting lines, with end points closest
487 {
488 vgl_line_segment_3d<double> l1(vgl_point_3d<double>(1, 0, 0), vgl_point_3d<double>(2, 0, 0));
489 vgl_line_segment_3d<double> l2(vgl_point_3d<double>(0, 1, 1), vgl_point_3d<double>(0, 2, 1));
490 bool u = false;
491 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
492 bool success = (c.first == vgl_point_3d<double>(1, 0, 0) && c.second == vgl_point_3d<double>(0, 1, 1) && u);
493 TEST("Non-parallel, non-intersecting, end points", success, true);
494 }
495
496 // Test common case of non-parallel, non-intersecting lines, with 1 end point/ 1 internal point
497 {
498 vgl_line_segment_3d<double> l1(vgl_point_3d<double>(1, 0, 0), vgl_point_3d<double>(2, 0, 0));
499 vgl_line_segment_3d<double> l2(vgl_point_3d<double>(0, -1, 1), vgl_point_3d<double>(0, +1, 1));
500 bool u = false;
501 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
502 bool success = (c.first == vgl_point_3d<double>(1, 0, 0) && c.second == vgl_point_3d<double>(0, 0, 1) && u);
503 TEST("Non-parallel, non-intersecting, endpoint/internal", success, true);
504 }
505
506 // Test common case of non-parallel, intersecting lines
507 {
508 vgl_line_segment_3d<double> l1(vgl_point_3d<double>(0, 0, 0), vgl_point_3d<double>(1, 0, 0));
509 vgl_line_segment_3d<double> l2(vgl_point_3d<double>(0, 0, 0), vgl_point_3d<double>(0, 1, 0));
510 bool u = false;
511 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
512 bool success = (c.first == vgl_point_3d<double>(0, 0, 0) && c.second == vgl_point_3d<double>(0, 0, 0) && u);
513 TEST("Non-parallel, intersecting", success, true);
514 }
515
516 // Test special case of parallel, non-collinear lines with endpoints closest
517 {
518 vgl_line_segment_3d<double> l1(vgl_point_3d<double>(1, 0, 0), vgl_point_3d<double>(2, 0, 0));
519 vgl_line_segment_3d<double> l2(vgl_point_3d<double>(-2, 0, 1), vgl_point_3d<double>(-1, 0, 1));
520 bool u = false;
521 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
522 bool success = (c.first == vgl_point_3d<double>(1, 0, 0) && c.second == vgl_point_3d<double>(-1, 0, 1) && u);
523 TEST("Parallel, non-collinear, endpoints", success, true);
524 }
525
526 // Test special case of parallel, non-collinear lines with non-unique internal points closest
527 {
528 vgl_line_segment_3d<double> l1(vgl_point_3d<double>(0, 0, 0), vgl_point_3d<double>(2, 0, 0));
529 vgl_line_segment_3d<double> l2(vgl_point_3d<double>(-2, 0, 1), vgl_point_3d<double>(1, 0, 1));
530 bool u = true;
531 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
532 bool success = (c.first == vgl_point_3d<double>(0, 0, 0) && c.second == vgl_point_3d<double>(0, 0, 1) && !u);
533 TEST("Parallel, non-collinear, non-unique internal points", success, true);
534 }
535
536 // Test special case of collinear lines, non-overlapping
537 {
538 vgl_line_segment_3d<double> l1(vgl_point_3d<double>(1, 0, 0), vgl_point_3d<double>(2, 0, 0));
539 vgl_line_segment_3d<double> l2(vgl_point_3d<double>(-2, 0, 0), vgl_point_3d<double>(-1, 0, 0));
540 bool u = false;
541 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
542 bool success = (c.first == vgl_point_3d<double>(1, 0, 0) && c.second == vgl_point_3d<double>(-1, 0, 0) && u);
543 TEST("Collinear, non-overlapping", success, true);
544 }
545
546 // Test special case of collinear lines, touching at endpoints
547 {
548 vgl_line_segment_3d<double> l1(vgl_point_3d<double>(0, 0, 0), vgl_point_3d<double>(2, 0, 0));
549 vgl_line_segment_3d<double> l2(vgl_point_3d<double>(-2, 0, 0), vgl_point_3d<double>(0, 0, 0));
550 bool u = false;
551 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
552 bool success = (c.first == vgl_point_3d<double>(0, 0, 0) && c.second == vgl_point_3d<double>(0, 0, 0) && u);
553 TEST("Collinear, touching at endpoints", success, true);
554 }
555
556 // Test special case of collinear lines, overlapping
557 {
558 vgl_line_segment_3d<double> l1(vgl_point_3d<double>(-1, 0, 0), vgl_point_3d<double>(2, 0, 0));
559 vgl_line_segment_3d<double> l2(vgl_point_3d<double>(-2, 0, 0), vgl_point_3d<double>(1, 0, 0));
560 bool u = true;
561 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
562 bool success = (c.first == vgl_point_3d<double>(-1, 0, 0) && c.second == vgl_point_3d<double>(-1, 0, 0) && !u);
563 TEST("Collinear, overlapping", success, true);
564 }
565 }
566
567 // Test for closest points on two 3D line segments
568 static void
test_infinite_line_3d_closest_points()569 test_infinite_line_3d_closest_points()
570 {
571 std::cout << "-------------------------------------------------\n"
572 << " Testing vgl_closest_points(3-d infinite lines):\n"
573 << "-------------------------------------------------\n";
574
575 // Test general case of non-parallel, non-intersecting lines, with internal points closest
576
577 vgl_infinite_line_3d<double> l1(vgl_point_3d<double>(-1, 0, 0), vgl_point_3d<double>(1, 0, 0));
578 vgl_infinite_line_3d<double> l2(vgl_point_3d<double>(0, 2, 1), vgl_point_3d<double>(0, 3, 1));
579 bool u = false;
580 std::pair<vgl_point_3d<double>, vgl_point_3d<double>> c = vgl_closest_points(l1, l2, &u);
581 bool success = (c.first == vgl_point_3d<double>(0, 0, 0) && c.second == vgl_point_3d<double>(0, 0, 1) && u);
582 TEST("Non-parallel, non-intersecting infinite lines", success, true);
583 }
584
585 static void
test_infinite_line_point()586 test_infinite_line_point()
587 {
588 // tests a previous problem case where a point on a line generated from two points is checked against itself
589
590 vgl_point_3d<double> p1(17.7751, 22.1568, 28.3506);
591 vgl_point_3d<double> p2(22.894, 16.3913, 30.9384);
592 vgl_infinite_line_3d<double> l1(p1, p2);
593 vgl_point_3d<double> cp = vgl_closest_point(p1, l1);
594 TEST_NEAR("Infinite line, point on the line", vgl_distance(p1, cp), 0.0, 1e-8);
595 }
596
597 static void
test_sphere_3d_closest_point()598 test_sphere_3d_closest_point()
599 {
600 std::cout << "-------------------------------------------------\n"
601 << " Testing vgl_closest_point(3-d sphere):\n"
602 << "-------------------------------------------------\n";
603 vgl_sphere_3d<double> s(1.0, 2.0, 3.0, 4.0);
604 vgl_point_3d<double> p(5.0, 5.0, 5.0);
605 vgl_point_3d<double> cp = vgl_closest_point(s, p);
606 double x = 3.97113 - cp.x(), y = 4.22834 - cp.y(), z = 4.48556 - cp.z();
607 double er = std::sqrt(x * x + y * y + z * z);
608 TEST_NEAR("Closest point on sphere", er, 0.0, 1.e-5);
609 }
610
611 static void
test_cylinder_3d_closest_point()612 test_cylinder_3d_closest_point()
613 {
614 std::cout << "-------------------------------------------------\n"
615 << " Testing vgl_closest_point(3-d cylinder):\n"
616 << "-------------------------------------------------\n";
617 vgl_vector_3d<double> orient(0.0, 0.0, 1.0);//z axis
618 vgl_point_3d<double> cent(0.0, 0.0, 0.0);
619 double radius = 1.0;
620 // infinite cylinder
621 vgl_cylinder_3d<double> c(cent, radius, std::numeric_limits<double>::max());
622 vgl_point_3d<double> p(5.0, 5.0, 5.0);
623 vgl_point_3d<double> cp = vgl_closest_point(c, p);
624 double sq2over2 = sqrt(2.0)/2.0;
625 vgl_point_3d<double> gt(sq2over2, sq2over2, 5.0);
626 double er = (cp-gt).length();
627 TEST_NEAR("Closest point on cylinder", er, 0.0, 1.e-5);
628 }
629
630 // Test for closest point on a cubic spline
631 static void
test_spline_3d_closest_point()632 test_spline_3d_closest_point()
633 {
634 std::cout << "-------------------------------------------------\n"
635 << " Testing vgl_closest_points(3-d spline):\n"
636 << "-------------------------------------------------\n";
637 vgl_point_3d<double> pm1(1.0, 0.0, 0.0);
638 vgl_point_3d<double> p0(1.0, 3.0, 2.0);
639 vgl_point_3d<double> p1(2.0, 2.0, 4.0);
640 std::vector<vgl_point_3d<double>> knots;
641 knots.push_back(pm1);
642 knots.push_back(p0);
643 knots.push_back(p1);
644 vgl_cubic_spline_3d<double> spl(knots, 0.5, true);
645 vgl_point_3d<double> p(1.5, 2.5, 3.0);
646 vgl_point_3d<double> cp = vgl_closest_point(spl, p);
647 vgl_point_3d<double> actual_cp = spl(1.43);
648 double dist = (cp - actual_cp).length();
649 TEST_NEAR("Closest point on spline", dist, 0.0, 0.002);
650 }
651
652 static void
test_closest_point()653 test_closest_point()
654 {
655 testHomgLine2DClosestPoint();
656 testHomgLine3DClosestPoints();
657 testHomgPlane3DClosestPoint();
658 testLine2DClosestPoint();
659 testLine3DClosestPoint();
660 test_line_3d_2_points_closest_point_t();
661 test_line_segment_2d_closest_point();
662 test_line_segment_3d_closest_point();
663 testPlane3DClosestPoint();
664 testPoly2DClosestPoint();
665 testPoly3DClosestPoint();
666 testLine3DClosestPoints();
667 testLineSegment3DClosestPoints();
668 test_infinite_line_3d_closest_points();
669 test_infinite_line_point();
670 test_sphere_3d_closest_point();
671 test_spline_3d_closest_point();
672 }
673
674 TESTMAIN(test_closest_point);
675