1 // Copyright (C) 2006 Mathias Froehlich - Mathias.Froehlich@web.de
3 // This library is free software; you can redistribute it and/or
4 // modify it under the terms of the GNU Library General Public
5 // License as published by the Free Software Foundation; either
6 // version 2 of the License, or (at your option) any later version.
8 // This library is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 // Library General Public License for more details.
13 // You should have received a copy of the GNU General Public License
14 // along with this program; if not, write to the Free Software
15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19 # include <simgear_config.h>
25 #include "SGGeometry.hxx"
26 #include "sg_random.h"
29 SGVec3<T> rndVec3(void)
31 return SGVec3<T>(sg_random(), sg_random(), sg_random());
36 TriangleClosestPointIntersectionTest(void)
38 unsigned nTests = 100000;
39 unsigned failedCount = 0;
40 for (unsigned i = 0; i < nTests; ++i) {
42 SGTriangle<T> triangle(rndVec3<T>(), rndVec3<T>(), rndVec3<T>());
43 T triangleEps = 100*SGLimits<T>::epsilon();
45 // generate random point in the triangle
46 T u = 4*sg_random() - 2;
47 T v = 4*sg_random() - 2;
49 v = T(0.5)*(v + 1 - u);
52 u = SGMisc<T>::clip(u, 0, 1);
53 v = SGMisc<T>::clip(v, 0, 1);
55 SGVec3<T> testClosest;
56 testClosest = triangle.getBaseVertex();
57 testClosest += u*triangle.getEdge(0) + v*triangle.getEdge(1);
59 SGVec3<T> n = triangle.getNormal();
60 SGVec3<T> e0 = triangle.getEdge(0);
61 SGVec3<T> e1 = triangle.getEdge(1);
63 // The normals to the edges
64 SGVec3<T> a0 = cross(e0, n);
65 SGVec3<T> a1 = cross(e1 - e0, n);
66 SGVec3<T> a2 = cross(n, e1);
68 SGVec3<T> testPoint = testClosest;
69 // Ok, if we are at some edge, go perpandicular to the triangle away
72 testPoint += sg_random()*a0 + sg_random()*a2;
74 testPoint += sg_random()*a1 + sg_random()*a2;
76 testPoint += sg_random()*a2;
79 testPoint += sg_random()*a0 + sg_random()*a1;
82 testPoint += sg_random()*a0;
83 } else if (u + v == 1) {
84 testPoint += sg_random()*a1;
87 testPoint += (2*sg_random() - 1)*n;
89 // Test the closest point function
90 SGVec3<T> closest = closestPoint(triangle, testPoint);
91 if (!equivalent(closest, testClosest, triangleEps)) {
92 std::cout << "Failed closest point test #" << i
93 << ": not equivalent!\nu = "
95 << "\ntestPoint = " << testPoint
96 << ", testClosest = " << testClosest
97 << ", closest = " << closest << "\n"
98 << triangle << std::endl;
102 // Test the triangle sphere intersection
103 SGSphere<T> sphere(testPoint, sg_random());
104 bool exactIntersection = intersects(sphere, testClosest);
105 // bool exactIntersection = intersects(sphere, closest);
106 bool sphereTriangleIntersection = intersects(sphere, triangle);
108 if (sphereTriangleIntersection != exactIntersection) {
109 std::cout << "Failed triangle sphere intersection test #" << i
110 << ": not equivalent!\nu = "
111 << u << ", v = " << v
112 << "\ntestPoint = " << testPoint
113 << ", testClosest = " << testClosest
114 << ", closest = " << closest << "\n"
115 << triangle << std::endl;
120 if (nTests < 100*failedCount) {
121 std::cout << "Failed box line intersection tests: " << failedCount
122 << " tests out of " << nTests
123 << " went wrong. Abort!" << std::endl;
132 TriangleLineIntersectionTest(void)
134 unsigned nTests = 100000;
135 unsigned failedCount = 0;
136 for (unsigned i = 0; i < nTests; ++i) {
137 SGVec3<T> v0 = rndVec3<T>();
138 SGVec3<T> v1 = rndVec3<T>();
139 SGVec3<T> v2 = rndVec3<T>();
141 SGTriangle<T> tri(v0, v1, v2);
143 T triangleEps = 100*SGLimits<T>::epsilon();
145 // generate random coeficients
146 T u = 4*sg_random() - 2;
147 T v = 4*sg_random() - 2;
148 T t = 4*sg_random() - 2;
150 SGVec3<T> isectpt = v0 + u*(v1 - v0) + v*(v2 - v0);
152 SGLineSegment<T> lineSegment;
153 SGVec3<T> dir = rndVec3<T>();
156 lineSegment.set(isectpt - t*dir, isectpt + (1 - t)*dir);
158 if (intersects(isectres, tri, lineSegment)) {
159 if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t && t <= 1) {
160 if (!equivalent(isectres, isectpt, triangleEps)) {
161 std::cout << "Failed line segment intersection test #" << i
162 << ": not equivalent!\nu = "
163 << u << ", v = " << v << ", t = " << t
164 << "\n" << tri << "\n" << lineSegment << std::endl;
168 std::cout << "Failed line segment intersection test #" << i
169 << ": false positive!\nu = "
170 << u << ", v = " << v << ", t = " << t
171 << "\n" << tri << "\n" << lineSegment << std::endl;
175 if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t && t <= 1) {
176 std::cout << "Failed line segment intersection test #" << i
177 << ": false negative!\nu = "
178 << u << ", v = " << v << ", t = " << t
179 << "\n" << tri << "\n" << lineSegment << std::endl;
185 ray.set(isectpt - t*dir, dir);
186 if (intersects(isectres, tri, ray)) {
187 if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t) {
188 if (!equivalent(isectres, isectpt, triangleEps)) {
189 std::cout << "Failed ray intersection test #" << i
190 << ": not equivalent!\nu = "
191 << u << ", v = " << v << ", t = " << t
192 << "\n" << tri << "\n" << ray << std::endl;
196 std::cout << "Failed ray intersection test #" << i
197 << ": false positive!\nu = "
198 << u << ", v = " << v << ", t = " << t
199 << "\n" << tri << "\n" << ray << std::endl;
203 if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t) {
204 std::cout << "Failed ray intersection test #" << i
205 << ": false negative !\nu = "
206 << u << ", v = " << v << ", t = " << t
207 << "\n" << tri << "\n" << ray << std::endl;
213 if (nTests < 100*failedCount) {
214 std::cout << "Failed ray intersection tests: " << failedCount
215 << " tests out of " << nTests
216 << " went wrong. Abort!" << std::endl;
220 /// Some crude handmade test
221 SGVec3<T> v0 = SGVec3<T>(0, 0, 0);
222 SGVec3<T> v1 = SGVec3<T>(1, 0, 0);
223 SGVec3<T> v2 = SGVec3<T>(0, 1, 0);
225 SGTriangle<T> tri(v0, v1, v2);
228 ray.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0.1, 0.1, -1));
229 if (!intersects(tri, ray)) {
230 std::cout << "Failed test #1!" << std::endl;
234 ray.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, 0, -1));
235 if (!intersects(tri, ray)) {
236 std::cout << "Failed test #2!" << std::endl;
240 SGLineSegment<T> lineSegment;
241 lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0.1, 0.1, -1));
242 if (!intersects(tri, lineSegment)) {
243 std::cout << "Failed test #3!" << std::endl;
247 lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, 0, -1));
248 if (!intersects(tri, lineSegment)) {
249 std::cout << "Failed test #4!" << std::endl;
253 lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, 1, -1));
254 if (!intersects(tri, lineSegment)) {
255 std::cout << "Failed test #5!" << std::endl;
259 lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(1, 0, -1));
260 if (!intersects(tri, lineSegment)) {
261 std::cout << "Failed test #6!" << std::endl;
265 lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(1, 1, -1));
266 if (!intersects(tri, lineSegment)) {
267 std::cout << "Failed test #7!" << std::endl;
271 // is exactly in the plane
272 // FIXME: cannot detect that yet ??
273 // lineSegment.set(SGVec3<T>(0, 0, 0), SGVec3<T>(1, 0, 0));
274 // if (!intersects(tri, lineSegment)) {
275 // std::cout << "Failed test #8!" << std::endl;
279 // is exactly in the plane
280 // FIXME: cannot detect that yet ??
281 // lineSegment.set(SGVec3<T>(-1, 0, 0), SGVec3<T>(1, 0, 0));
282 // if (!intersects(tri, lineSegment)) {
283 // std::cout << "Failed test #9!" << std::endl;
287 // is exactly paralell to the plane
288 // FIXME: cannot detect that yet ??
289 // lineSegment.set(SGVec3<T>(-1, 1, 0), SGVec3<T>(1, 1, 0));
290 // if (intersects(tri, lineSegment)) {
291 // std::cout << "Failed test #10!" << std::endl;
295 // should fail since the line segment poins slightly beyond the triangle
296 lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(1, 1, -0.9));
297 if (intersects(tri, lineSegment)) {
298 std::cout << "Failed test #11!" << std::endl;
302 lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, -0.1, -1));
303 if (intersects(tri, lineSegment)) {
304 std::cout << "Failed test #12!" << std::endl;
308 lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(-0.1, -0.1, -1));
309 if (intersects(tri, lineSegment)) {
310 std::cout << "Failed test #13!" << std::endl;
314 lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(-0.1, 0, -1));
315 if (intersects(tri, lineSegment)) {
316 std::cout << "Failed test #14!" << std::endl;
325 SphereLineIntersectionTest(void)
327 unsigned nTests = 100000;
328 unsigned failedCount = 0;
329 for (unsigned i = 0; i < nTests; ++i) {
330 SGVec3<T> center = rndVec3<T>();
331 T radius = 2*sg_random();
332 SGSphere<T> sphere(center, radius);
334 SGVec3<T> offset = normalize(rndVec3<T>());
337 // This one is the point we use to judge if the test should fail or not
338 SGVec3<T> base = center + t*offset;
340 SGVec3<T> per = perpendicular(offset);
341 SGVec3<T> start = base + 4*sg_random()*per;
342 SGVec3<T> end = base - 4*sg_random()*per;
344 SGLineSegment<T> lineSegment;
345 lineSegment.set(start, end);
346 if (intersects(sphere, lineSegment)) {
348 std::cout << "Failed sphere line intersection test #" << i
349 << ": false positive!\nt = " << t << "\n"
350 << sphere << "\n" << lineSegment << std::endl;
355 std::cout << "Failed sphere line intersection test #" << i
356 << ": false negative!\nt = " << t << "\n"
357 << sphere << "\n" << lineSegment << std::endl;
363 ray.set(start, end - start);
364 if (intersects(sphere, ray)) {
366 std::cout << "Failed sphere line intersection test #" << i
367 << ": false positive!\nt = " << t << "\n"
368 << sphere << "\n" << ray << std::endl;
373 std::cout << "Failed sphere line intersection test #" << i
374 << ": false negative!\nt = " << t << "\n"
375 << sphere << "\n" << ray << std::endl;
381 if (nTests < 100*failedCount) {
382 std::cout << "Failed sphere line intersection tests: " << failedCount
383 << " tests out of " << nTests
384 << " went wrong. Abort!" << std::endl;
389 for (unsigned i = 0; i < nTests; ++i) {
390 SGVec3<T> center = rndVec3<T>();
391 T radius = 2*sg_random();
392 SGSphere<T> sphere(center, radius);
394 SGVec3<T> offset = normalize(rndVec3<T>());
397 // This one is the point we use to judge if the test should fail or not
398 SGVec3<T> base = center + t*offset;
400 SGVec3<T> start = base;
401 SGVec3<T> end = base + 2*sg_random()*offset;
403 SGLineSegment<T> lineSegment;
404 lineSegment.set(start, end);
405 if (intersects(sphere, lineSegment)) {
407 std::cout << "Failed sphere line intersection test #" << i
408 << ": false positive!\nt = " << t << "\n"
409 << sphere << "\n" << lineSegment << std::endl;
414 std::cout << "Failed sphere line intersection test #" << i
415 << ": false negative!\nt = " << t << "\n"
416 << sphere << "\n" << lineSegment << std::endl;
422 ray.set(start, end - start);
423 if (intersects(sphere, ray)) {
425 std::cout << "Failed sphere line intersection test #" << i
426 << ": false positive!\nt = " << t << "\n"
427 << sphere << "\n" << ray << std::endl;
432 std::cout << "Failed sphere line intersection test #" << i
433 << ": false negative!\nt = " << t << "\n"
434 << sphere << "\n" << ray << std::endl;
440 if (nTests < 100*failedCount) {
441 std::cout << "Failed sphere line intersection tests: " << failedCount
442 << " tests out of " << nTests
443 << " went wrong. Abort!" << std::endl;
452 BoxLineIntersectionTest(void)
454 // ok, bad test case coverage, but better than nothing ...
456 unsigned nTests = 100000;
457 unsigned failedCount = 0;
458 for (unsigned i = 0; i < nTests; ++i) {
460 box.expandBy(rndVec3<T>());
461 box.expandBy(rndVec3<T>());
463 SGVec3<T> center = box.getCenter();
465 // This one is the point we use to judge if the test should fail or not
466 SGVec3<T> base = rndVec3<T>();
467 SGVec3<T> dir = base - center;
469 SGLineSegment<T> lineSegment;
470 lineSegment.set(base, base + dir);
471 if (intersects(box, lineSegment)) {
472 if (!intersects(box, base)) {
473 std::cout << "Failed box line intersection test #" << i
474 << ": false positive!\n"
475 << box << "\n" << lineSegment << std::endl;
479 if (intersects(box, base)) {
480 std::cout << "Failed box line intersection test #" << i
481 << ": false negative!\n"
482 << box << "\n" << lineSegment << std::endl;
489 if (intersects(box, ray)) {
490 if (!intersects(box, base)) {
491 std::cout << "Failed box line intersection test #" << i
492 << ": false positive!\n"
493 << box << "\n" << ray << std::endl;
497 if (intersects(box, base)) {
498 std::cout << "Failed box line intersection test #" << i
499 << ": false negative!\n"
500 << box << "\n" << ray << std::endl;
506 if (nTests < 100*failedCount) {
507 std::cout << "Failed box line intersection tests: " << failedCount
508 << " tests out of " << nTests
509 << " went wrong. Abort!" << std::endl;
519 std::cout << "Testing Geometry intersection routines.\n"
520 << "Some of these tests can fail due to roundoff problems...\n"
521 << "Dont worry if only a few of them fail..." << std::endl;
525 if (!TriangleClosestPointIntersectionTest<float>())
527 if (!TriangleClosestPointIntersectionTest<double>())
530 if (!TriangleLineIntersectionTest<float>())
532 if (!TriangleLineIntersectionTest<double>())
535 if (!SphereLineIntersectionTest<float>())
537 if (!SphereLineIntersectionTest<double>())
540 if (!BoxLineIntersectionTest<float>())
542 if (!BoxLineIntersectionTest<double>())
545 std::cout << "Successfully passed all tests!" << std::endl;