simgear/math/SGGeometryTest.cxx

   1 // Copyright (C) 2006  Mathias Froehlich - Mathias.Froehlich@web.de
   2 //
   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.
   7 //
   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.
  12 //
  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.
  16 //
  17
  18 #ifdef HAVE_CONFIG_H
  19 #  include <simgear_config.h>
  20 #endif
  21
  22 #include <cstdlib>
  23 #include <iostream>
  24
  25 #include "SGGeometry.hxx"
  26 #include "sg_random.h"
  27
  28 template<typename T>
  29 SGVec3<T> rndVec3(void)
  30 {
  31   return SGVec3<T>(sg_random(), sg_random(), sg_random());
  32 }
  33
  34 template<typename T>
  35 bool
  36 TriangleClosestPointIntersectionTest(void)
  37 {
  38   unsigned nTests = 100000;
  39   unsigned failedCount = 0;
  40   for (unsigned i = 0; i < nTests; ++i) {
  41       // test triangle
  42       SGTriangle<T> triangle(rndVec3<T>(), rndVec3<T>(), rndVec3<T>());
  43       T triangleEps = 100*SGLimits<T>::epsilon();
  44
  45       // generate random point in the triangle
  46       T u = 4*sg_random() - 2;
  47       T v = 4*sg_random() - 2;
  48       if (1 < u + v) {
  49         v = T(0.5)*(v + 1 - u);
  50         u = 1 - v;
  51       }
  52       u = SGMisc<T>::clip(u, 0, 1);
  53       v = SGMisc<T>::clip(v, 0, 1);
  54
  55       SGVec3<T> testClosest;
  56       testClosest = triangle.getBaseVertex();
  57       testClosest += u*triangle.getEdge(0) + v*triangle.getEdge(1);
  58
  59       SGVec3<T> n = triangle.getNormal();
  60       SGVec3<T> e0 = triangle.getEdge(0);
  61       SGVec3<T> e1 = triangle.getEdge(1);
  62
  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);
  67
  68       SGVec3<T> testPoint = testClosest;
  69       // Ok, if we are at some edge, go perpandicular to the triangle away
  70       if (u == 0) {
  71         if (v == 0) {
  72           testPoint += sg_random()*a0 + sg_random()*a2;
  73         } else if (v == 1) {
  74           testPoint += sg_random()*a1 + sg_random()*a2;
  75         } else {
  76           testPoint += sg_random()*a2;
  77         }
  78       } else if (u == 1) {
  79         testPoint += sg_random()*a0 + sg_random()*a1;
  80       } else {
  81         if (v == 0) {
  82           testPoint += sg_random()*a0;
  83         } else if (u + v == 1) {
  84           testPoint += sg_random()*a1;
  85         }
  86       }
  87       testPoint += (2*sg_random() - 1)*n;
  88
  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 = "
  94                   << u << ", v = " << v
  95                   << "\ntestPoint = " << testPoint
  96                   << ", testClosest = " << testClosest
  97                   << ", closest = " << closest << "\n"
  98                   << triangle << std::endl;
  99         ++failedCount;
 100       }
 101
 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);
 107
 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;
 116         ++failedCount;
 117       }
 118   }
 119
 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;
 124     return false;
 125   }
 126
 127   return true;
 128 }
 129
 130 template<typename T>
 131 bool
 132 TriangleLineIntersectionTest(void)
 133 {
 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>();
 140
 141     SGTriangle<T> tri(v0, v1, v2);
 142
 143     T triangleEps = 100*SGLimits<T>::epsilon();
 144
 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;
 149
 150     SGVec3<T> isectpt = v0 + u*(v1 - v0) + v*(v2 - v0);
 151
 152     SGLineSegment<T> lineSegment;
 153     SGVec3<T> dir = rndVec3<T>();
 154
 155     SGVec3<T> isectres;
 156     lineSegment.set(isectpt - t*dir, isectpt + (1 - t)*dir);
 157
 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;
 165           ++failedCount;
 166         }
 167       } else {
 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;
 172         ++failedCount;
 173       }
 174     } else {
 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;
 180         ++failedCount;
 181       }
 182     }
 183
 184     SGRay<T> ray;
 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;
 193           ++failedCount;
 194         }
 195       } else {
 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;
 200         ++failedCount;
 201       }
 202     } else {
 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;
 208         ++failedCount;
 209       }
 210     }
 211   }
 212
 213   if (nTests < 100*failedCount) {
 214     std::cout << "Failed ray intersection tests: " << failedCount
 215               << " tests out of " << nTests
 216               << " went wrong. Abort!" << std::endl;
 217     return false;
 218   }
 219
 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);
 224
 225   SGTriangle<T> tri(v0, v1, v2);
 226
 227   SGRay<T> ray;
 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;
 231     return false;
 232   }
 233
 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;
 237     return false;
 238   }
 239
 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;
 244     return false;
 245   }
 246
 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;
 250     return false;
 251   }
 252
 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;
 256     return false;
 257   }
 258
 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;
 262     return false;
 263   }
 264
 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;
 268     return false;
 269   }
 270
 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;
 276 //     return false;
 277 //   }
 278
 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;
 284 //     return false;
 285 //   }
 286
 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;
 292 //     return false;
 293 //   }
 294
 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;
 299     return false;
 300   }
 301
 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;
 305     return false;
 306   }
 307
 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;
 311     return false;
 312   }
 313
 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;
 317     return false;
 318   }
 319
 320   return true;
 321 }
 322
 323 template<typename T>
 324 bool
 325 SphereLineIntersectionTest(void)
 326 {
 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);
 333
 334     SGVec3<T> offset = normalize(rndVec3<T>());
 335     T t = 4*sg_random();
 336
 337     // This one is the point we use to judge if the test should fail or not
 338     SGVec3<T> base = center + t*offset;
 339
 340     SGVec3<T> per = perpendicular(offset);
 341     SGVec3<T> start = base + 4*sg_random()*per;
 342     SGVec3<T> end = base - 4*sg_random()*per;
 343
 344     SGLineSegment<T> lineSegment;
 345     lineSegment.set(start, end);
 346     if (intersects(sphere, lineSegment)) {
 347       if (radius < t) {
 348         std::cout << "Failed sphere line intersection test #" << i
 349                   << ": false positive!\nt = " << t << "\n"
 350                   << sphere << "\n" << lineSegment << std::endl;
 351         ++failedCount;
 352       }
 353     } else {
 354       if (t <= radius) {
 355         std::cout << "Failed sphere line intersection test #" << i
 356                   << ": false negative!\nt = " << t << "\n"
 357                   << sphere << "\n" << lineSegment << std::endl;
 358         ++failedCount;
 359       }
 360     }
 361
 362     SGRay<T> ray;
 363     ray.set(start, end - start);
 364     if (intersects(sphere, ray)) {
 365       if (radius < t) {
 366         std::cout << "Failed sphere line intersection test #" << i
 367                   << ": false positive!\nt = " << t << "\n"
 368                   << sphere << "\n" << ray << std::endl;
 369         ++failedCount;
 370       }
 371     } else {
 372       if (t <= radius) {
 373         std::cout << "Failed sphere line intersection test #" << i
 374                   << ": false negative!\nt = " << t << "\n"
 375                   << sphere << "\n" << ray << std::endl;
 376         ++failedCount;
 377       }
 378     }
 379   }
 380
 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;
 385     return false;
 386   }
 387
 388   failedCount = 0;
 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);
 393
 394     SGVec3<T> offset = normalize(rndVec3<T>());
 395     T t = 4*sg_random();
 396
 397     // This one is the point we use to judge if the test should fail or not
 398     SGVec3<T> base = center + t*offset;
 399
 400     SGVec3<T> start = base;
 401     SGVec3<T> end = base + 2*sg_random()*offset;
 402
 403     SGLineSegment<T> lineSegment;
 404     lineSegment.set(start, end);
 405     if (intersects(sphere, lineSegment)) {
 406       if (radius < t) {
 407         std::cout << "Failed sphere line intersection test #" << i
 408                   << ": false positive!\nt = " << t << "\n"
 409                   << sphere << "\n" << lineSegment << std::endl;
 410         ++failedCount;
 411       }
 412     } else {
 413       if (t <= radius) {
 414         std::cout << "Failed sphere line intersection test #" << i
 415                   << ": false negative!\nt = " << t << "\n"
 416                   << sphere << "\n" << lineSegment << std::endl;
 417         ++failedCount;
 418       }
 419     }
 420
 421     SGRay<T> ray;
 422     ray.set(start, end - start);
 423     if (intersects(sphere, ray)) {
 424       if (radius < t) {
 425         std::cout << "Failed sphere line intersection test #" << i
 426                   << ": false positive!\nt = " << t << "\n"
 427                   << sphere << "\n" << ray << std::endl;
 428         ++failedCount;
 429       }
 430     } else {
 431       if (t <= radius) {
 432         std::cout << "Failed sphere line intersection test #" << i
 433                   << ": false negative!\nt = " << t << "\n"
 434                   << sphere << "\n" << ray << std::endl;
 435         ++failedCount;
 436       }
 437     }
 438   }
 439
 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;
 444     return false;
 445   }
 446
 447   return true;
 448 }
 449
 450 template<typename T>
 451 bool
 452 BoxLineIntersectionTest(void)
 453 {
 454   // ok, bad test case coverage, but better than nothing ...
 455
 456   unsigned nTests = 100000;
 457   unsigned failedCount = 0;
 458   for (unsigned i = 0; i < nTests; ++i) {
 459     SGBox<T> box;
 460     box.expandBy(rndVec3<T>());
 461     box.expandBy(rndVec3<T>());
 462
 463     SGVec3<T> center = box.getCenter();
 464
 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;
 468
 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;
 476         ++failedCount;
 477       }
 478     } else {
 479       if (intersects(box, base)) {
 480         std::cout << "Failed box line intersection test #" << i
 481                   << ": false negative!\n"
 482                   << box << "\n" << lineSegment << std::endl;
 483         ++failedCount;
 484       }
 485     }
 486
 487     SGRay<T> ray;
 488     ray.set(base, dir);
 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;
 494         ++failedCount;
 495       }
 496     } else {
 497       if (intersects(box, base)) {
 498         std::cout << "Failed box line intersection test #" << i
 499                   << ": false negative!\n"
 500                   << box << "\n" << ray << std::endl;
 501         ++failedCount;
 502       }
 503     }
 504   }
 505
 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;
 510     return false;
 511   }
 512
 513   return true;
 514 }
 515
 516 int
 517 main(void)
 518 {
 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;
 522
 523   sg_srandom(17);
 524
 525   if (!TriangleClosestPointIntersectionTest<float>())
 526     return EXIT_FAILURE;
 527   if (!TriangleClosestPointIntersectionTest<double>())
 528     return EXIT_FAILURE;
 529
 530   if (!TriangleLineIntersectionTest<float>())
 531     return EXIT_FAILURE;
 532   if (!TriangleLineIntersectionTest<double>())
 533     return EXIT_FAILURE;
 534
 535   if (!SphereLineIntersectionTest<float>())
 536     return EXIT_FAILURE;
 537   if (!SphereLineIntersectionTest<double>())
 538     return EXIT_FAILURE;
 539
 540   if (!BoxLineIntersectionTest<float>())
 541     return EXIT_FAILURE;
 542   if (!BoxLineIntersectionTest<double>())
 543     return EXIT_FAILURE;
 544
 545   std::cout << "Successfully passed all tests!" << std::endl;
 546   return EXIT_SUCCESS;
 547 }