1 // Some tests for vgl_triangle_3d_line_intersection
2 
3 #include <iostream>
4 #ifdef _MSC_VER
5 #  include "vcl_msvc_warnings.h"
6 #endif
7 #include "testlib/testlib_test.h"
8 #include "vgl/vgl_triangle_3d.h"
9 #include "vgl/vgl_point_3d.h"
10 #include "vgl/vgl_line_segment_3d.h"
11 
12 static const double tol = 1e-9;
13 
14 
15 //========================================================================
16 // Test vgl_triangle_3d_test_inside() function
17 //========================================================================
18 static void
test_point_inside()19 test_point_inside()
20 {
21   std::cout << '\n'
22             << "*************************************\n"
23             << " Testing vgl_triangle_3d_test_inside\n"
24             << "*************************************\n";
25 
26   // Define a triangle - arbitrary choice
27   vgl_point_3d<double> p1(2, 1, -1), p2(1, -2, 0), p3(-1, 2, 2);
28   vgl_vector_3d<double> v1(2, 1, -1), v2(1, -2, 0), v3(-1, 2, 2);
29   vgl_point_3d<double> o(0, 0, 0);
30 
31   vgl_point_3d<double> q;
32   double a1, a2, a3;
33 
34   // test centroid
35   {
36     a1 = 1.0 / 3.0;
37     a2 = 1.0 / 3.0;
38     a3 = 1.0 / 3.0;
39     q = o + a1 * v1 + a2 * v2 + a3 * v3;
40     TEST("Centroid inside?", vgl_triangle_3d_test_inside(q, p1, p2, p3), true);
41   }
42 
43   // test off-centre point
44   {
45     a1 = 0.25;
46     a2 = 0.25;
47     a3 = 0.5;
48     q = o + a1 * v1 + a2 * v2 + a3 * v3;
49     vgl_triangle_3d_test_inside(q, p1, p2, p3);
50     TEST("Off-centre point inside?", vgl_triangle_3d_test_inside(q, p1, p2, p3), true);
51   }
52 
53   // test edge point
54   {
55     a1 = 0.0;
56     a2 = 0.5;
57     a3 = 0.5;
58     q = o + a1 * v1 + a2 * v2 + a3 * v3;
59     vgl_triangle_3d_test_inside(q, p1, p2, p3);
60     TEST("Edge point inside?", vgl_triangle_3d_test_inside(q, p1, p2, p3), true);
61   }
62 
63   // test vertex point
64   {
65     a1 = 0.0;
66     a2 = 0.0;
67     a3 = 1.0;
68     q = o + a1 * v1 + a2 * v2 + a3 * v3;
69     vgl_triangle_3d_test_inside(q, p1, p2, p3);
70     TEST("Vertex point inside?", vgl_triangle_3d_test_inside(q, p1, p2, p3), true);
71   }
72 
73   // test outside point, near edge
74   {
75     a1 = 0.0;
76     a2 = 0.501;
77     a3 = 0.5;
78     q = o + a1 * v1 + a2 * v2 + a3 * v3;
79     vgl_triangle_3d_test_inside(q, p1, p2, p3);
80     TEST("Outside edge point outside?", vgl_triangle_3d_test_inside(q, p1, p2, p3), false);
81   }
82 
83   // test outside point, near vertex
84   {
85     a1 = 0.0;
86     a2 = 0.0;
87     a3 = 1.01;
88     q = o + a1 * v1 + a2 * v2 + a3 * v3;
89     vgl_triangle_3d_test_inside(q, p1, p2, p3);
90     TEST("Outside vertex point outside?", vgl_triangle_3d_test_inside(q, p1, p2, p3), false);
91   }
92 
93   // test off-plane point (but would project into triangle)
94   {
95     a1 = 1.0 / 3.0;
96     a2 = 1.0 / 3.0;
97     a3 = 1.0 / 3.0;
98     q = o + a1 * v1 + a2 * v2 + a3 * v3 + vgl_vector_3d<double>(0, 0, 2);
99     TEST("Off-plane point outside?", vgl_triangle_3d_test_inside(q, p1, p2, p3), false);
100   }
101 }
102 
103 //========================================================================
104 // Test vgl_triangle_3d_line_intersection() function
105 //========================================================================
106 static void
test_line_intersection()107 test_line_intersection()
108 {
109   std::cout << '\n'
110             << "*******************************************\n"
111             << " Testing vgl_triangle_3d_line_intersection\n"
112             << "*******************************************\n";
113   {
114     // Define a triangle - arbitrary choice
115     vgl_point_3d<double> p1(2, 1, -1), p2(1, -2, 0), p3(-1, 2, 2);
116     vgl_vector_3d<double> v1(2, 1, -1), v2(1, -2, 0), v3(-1, 2, 2);
117     vgl_point_3d<double> o(0, 0, 0);
118 
119     // Define a line - arbitrary choice
120     vgl_point_3d<double> centroid = o + v1 / 3.0 + v2 / 3.0 + v3 / 3.0;
121     vgl_line_segment_3d<double> lineseg(o, o + 2 * (centroid - o));
122 
123     vgl_point_3d<double> q; // intersection point - should be centroid
124     TEST("line intersects?", (Skew == vgl_triangle_3d_line_intersection(lineseg, p1, p2, p3, q)), true);
125     TEST("intersection point correct?", (q - centroid).length() < tol, true);
126   }
127   {
128     bool outside_fail = false, inside_fail = false;
129     constexpr double rad = 0.01745329251994329577; // radians per degree, i.e., pi/180
130     vgl_point_3d<double> i_pnt;
131     for (double e = 1e-12; e < 0.0002; e *= 9.0) // height above/below triangle edge p1-p2
132     {
133       vgl_point_3d<double> l1_in(1001, 900, 1000 - e), l2_in(1001, 1100, 1000 - e);
134       vgl_point_3d<double> l1_out(1001, 900, 1000 + e), l2_out(1001, 1100, 1000 + e);
135       vgl_line_segment_3d<double> l_in(l1_in, l2_in), l_out(l1_out, l2_out);
136       for (double a = -89.9; a < 89.95; a += 0.1) // angle of triangle normal w.r.t to l1-l2
137       {
138         // Define a triangle that wobbles about p1-p2
139         vgl_point_3d<double> p1(1000, 1000, 1000), p2(1002, 1000, 1000),
140           p3(1001, 1000 + (20 * std::sin(a * rad)), 1000 - (20 * std::cos(a * rad)));
141         vgl_triangle_3d_intersection_t in_result1 = vgl_triangle_3d_line_intersection(l_in, p1, p2, p3, i_pnt);
142         vgl_triangle_3d_intersection_t in_result2 = vgl_triangle_3d_line_intersection(l_in, p1, p3, p2, i_pnt);
143         vgl_triangle_3d_intersection_t in_result3 = vgl_triangle_3d_line_intersection(l_in, p3, p1, p2, i_pnt);
144         vgl_triangle_3d_intersection_t in_result4 = vgl_triangle_3d_line_intersection(l_in, p3, p2, p1, i_pnt);
145         vgl_triangle_3d_intersection_t in_result5 = vgl_triangle_3d_line_intersection(l_in, p2, p3, p1, i_pnt);
146         vgl_triangle_3d_intersection_t in_result6 = vgl_triangle_3d_line_intersection(l_in, p2, p1, p3, i_pnt);
147         vgl_triangle_3d_intersection_t out_result1 = vgl_triangle_3d_line_intersection(l_out, p1, p2, p3, i_pnt);
148         vgl_triangle_3d_intersection_t out_result2 = vgl_triangle_3d_line_intersection(l_out, p1, p3, p2, i_pnt);
149         vgl_triangle_3d_intersection_t out_result3 = vgl_triangle_3d_line_intersection(l_out, p3, p1, p2, i_pnt);
150         vgl_triangle_3d_intersection_t out_result4 = vgl_triangle_3d_line_intersection(l_out, p3, p2, p1, i_pnt);
151         vgl_triangle_3d_intersection_t out_result5 = vgl_triangle_3d_line_intersection(l_out, p2, p3, p1, i_pnt);
152         vgl_triangle_3d_intersection_t out_result6 = vgl_triangle_3d_line_intersection(l_out, p2, p1, p3, i_pnt);
153 
154         if (in_result1 != Skew || in_result2 != Skew || in_result3 != Skew || in_result4 != Skew ||
155             in_result5 != Skew || in_result6 != Skew || out_result1 != None || out_result2 != None ||
156             out_result3 != None || out_result4 != None || out_result5 != None || out_result6 != None)
157         {
158           std::cout << "normal_distance: " << e << " angle: " << a;
159           if (out_result1 != None)
160           {
161             outside_fail = true;
162             std::cout << " outside 1 fail";
163           }
164           if (out_result2 != None)
165           {
166             outside_fail = true;
167             std::cout << " outside 2 fail";
168           }
169           if (out_result3 != None)
170           {
171             outside_fail = true;
172             std::cout << " outside 3 fail";
173           }
174           if (out_result4 != None)
175           {
176             outside_fail = true;
177             std::cout << " outside 4 fail";
178           }
179           if (out_result5 != None)
180           {
181             outside_fail = true;
182             std::cout << " outside 5 fail";
183           }
184           if (out_result6 != None)
185           {
186             outside_fail = true;
187             std::cout << " outside 6 fail";
188           }
189           if (in_result1 != Skew)
190           {
191             inside_fail = true;
192             std::cout << " inside 1 fail";
193           }
194           if (in_result2 != Skew)
195           {
196             inside_fail = true;
197             std::cout << " inside 2 fail";
198           }
199           if (in_result3 != Skew)
200           {
201             inside_fail = true;
202             std::cout << " inside 3 fail";
203           }
204           if (in_result4 != Skew)
205           {
206             inside_fail = true;
207             std::cout << " inside 4 fail";
208           }
209           if (in_result5 != Skew)
210           {
211             inside_fail = true;
212             std::cout << " inside 5 fail";
213           }
214           if (in_result6 != Skew)
215           {
216             inside_fail = true;
217             std::cout << " inside 6 fail";
218           }
219           std::cout << std::endl;
220         }
221       }
222     }
223     TEST("All inside rays ok", inside_fail, false);
224     TEST("All outside rays ok", outside_fail, false);
225   }
226 }
227 
228 
229 //========================================================================
230 // Main testing function
231 //========================================================================
232 void
test_triangle_3d_line_intersection()233 test_triangle_3d_line_intersection()
234 {
235   test_point_inside();
236   test_line_intersection();
237 }
238 
239 
240 //========================================================================
241 // Define main()
242 //========================================================================
243 TESTMAIN(test_triangle_3d_line_intersection);
244