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 TriangleLineIntersectionTest(void)
  37 {
  38   unsigned nTests = 100000;
  39   unsigned failedCount = 0;
  40   for (unsigned i = 0; i < nTests; ++i) {
  41     SGVec3<T> v0 = rndVec3<T>();
  42     SGVec3<T> v1 = rndVec3<T>();
  43     SGVec3<T> v2 = rndVec3<T>();
  44
  45     SGTriangle<T> tri(v0, v1, v2);
  46
  47     // generate random coeficients
  48     T u = 4*sg_random() - 2;
  49     T v = 4*sg_random() - 2;
  50     T t = 4*sg_random() - 2;
  51
  52     SGVec3<T> isectpt = v0 + u*(v1 - v0) + v*(v2 - v0);
  53
  54     SGLineSegment<T> lineSegment;
  55     SGVec3<T> dir = rndVec3<T>();
  56
  57     SGVec3<T> isectres;
  58     lineSegment.set(isectpt - t*dir, isectpt + (1 - t)*dir);
  59
  60     if (intersects(isectres, tri, lineSegment)) {
  61       if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t && t <= 1) {
  62         if (!equivalent(isectres, isectpt)) {
  63           std::cout << "Failed line segment intersection test #" << i
  64                     << ": not equivalent!\nu = "
  65                     << u << ", v = " << v << ", t = " << t
  66                     << "\n" << tri << "\n" << lineSegment << std::endl;
  67           ++failedCount;
  68         }
  69       } else {
  70         std::cout << "Failed line segment intersection test #" << i
  71                   << ": false positive!\nu = "
  72                   << u << ", v = " << v << ", t = " << t
  73                   << "\n" << tri << "\n" << lineSegment << std::endl;
  74         ++failedCount;
  75       }
  76     } else {
  77       if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t && t <= 1) {
  78         std::cout << "Failed line segment intersection test #" << i
  79                   << ": false negative!\nu = "
  80                   << u << ", v = " << v << ", t = " << t
  81                   << "\n" << tri << "\n" << lineSegment << std::endl;
  82         ++failedCount;
  83       }
  84     }
  85
  86     SGRay<T> ray;
  87     ray.set(isectpt - t*dir, dir);
  88     if (intersects(isectres, tri, ray)) {
  89       if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t) {
  90         if (!equivalent(isectres, isectpt)) {
  91           std::cout << "Failed ray intersection test #" << i
  92                     << ": not equivalent!\nu = "
  93                     << u << ", v = " << v << ", t = " << t
  94                     << "\n" << tri << "\n" << ray << std::endl;
  95           ++failedCount;
  96         }
  97       } else {
  98         std::cout << "Failed ray intersection test #" << i
  99                   << ": false positive!\nu = "
 100                   << u << ", v = " << v << ", t = " << t
 101                   << "\n" << tri << "\n" << ray << std::endl;
 102         ++failedCount;
 103       }
 104     } else {
 105       if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t) {
 106         std::cout << "Failed ray intersection test #" << i
 107                   << ": false negative !\nu = "
 108                   << u << ", v = " << v << ", t = " << t
 109                   << "\n" << tri << "\n" << ray << std::endl;
 110         ++failedCount;
 111       }
 112     }
 113   }
 114
 115   if (nTests < 100*failedCount) {
 116     std::cout << "Failed ray intersection tests: " << failedCount
 117               << " tests out of " << nTests
 118               << " went wrong. Abort!" << std::endl;
 119     return false;
 120   }
 121
 122   /// Some crude handmade test
 123   SGVec3<T> v0 = SGVec3<T>(0, 0, 0);
 124   SGVec3<T> v1 = SGVec3<T>(1, 0, 0);
 125   SGVec3<T> v2 = SGVec3<T>(0, 1, 0);
 126
 127   SGTriangle<T> tri(v0, v1, v2);
 128
 129   SGRay<T> ray;
 130   ray.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0.1, 0.1, -1));
 131   if (!intersects(tri, ray)) {
 132     std::cout << "Failed test #1!" << std::endl;
 133     return false;
 134   }
 135
 136   ray.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, 0, -1));
 137   if (!intersects(tri, ray)) {
 138     std::cout << "Failed test #2!" << std::endl;
 139     return false;
 140   }
 141
 142   SGLineSegment<T> lineSegment;
 143   lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0.1, 0.1, -1));
 144   if (!intersects(tri, lineSegment)) {
 145     std::cout << "Failed test #3!" << std::endl;
 146     return false;
 147   }
 148
 149   lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, 0, -1));
 150   if (!intersects(tri, lineSegment)) {
 151     std::cout << "Failed test #4!" << std::endl;
 152     return false;
 153   }
 154
 155   lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, 1, -1));
 156   if (!intersects(tri, lineSegment)) {
 157     std::cout << "Failed test #5!" << std::endl;
 158     return false;
 159   }
 160
 161   lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(1, 0, -1));
 162   if (!intersects(tri, lineSegment)) {
 163     std::cout << "Failed test #6!" << std::endl;
 164     return false;
 165   }
 166
 167   lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(1, 1, -1));
 168   if (!intersects(tri, lineSegment)) {
 169     std::cout << "Failed test #7!" << std::endl;
 170     return false;
 171   }
 172
 173   // is exactly in the plane
 174   // FIXME: cannot detect that yet ??
 175 //   lineSegment.set(SGVec3<T>(0, 0, 0), SGVec3<T>(1, 0, 0));
 176 //   if (!intersects(tri, lineSegment)) {
 177 //     std::cout << "Failed test #8!" << std::endl;
 178 //     return false;
 179 //   }
 180
 181   // is exactly in the plane
 182   // FIXME: cannot detect that yet ??
 183 //   lineSegment.set(SGVec3<T>(-1, 0, 0), SGVec3<T>(1, 0, 0));
 184 //   if (!intersects(tri, lineSegment)) {
 185 //     std::cout << "Failed test #9!" << std::endl;
 186 //     return false;
 187 //   }
 188
 189   // is exactly paralell to the plane
 190   // FIXME: cannot detect that yet ??
 191 //   lineSegment.set(SGVec3<T>(-1, 1, 0), SGVec3<T>(1, 1, 0));
 192 //   if (intersects(tri, lineSegment)) {
 193 //     std::cout << "Failed test #10!" << std::endl;
 194 //     return false;
 195 //   }
 196
 197   // should fail since the line segment poins slightly beyond the triangle
 198   lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(1, 1, -0.9));
 199   if (intersects(tri, lineSegment)) {
 200     std::cout << "Failed test #11!" << std::endl;
 201     return false;
 202   }
 203
 204   lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, -0.1, -1));
 205   if (intersects(tri, lineSegment)) {
 206     std::cout << "Failed test #12!" << std::endl;
 207     return false;
 208   }
 209
 210   lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(-0.1, -0.1, -1));
 211   if (intersects(tri, lineSegment)) {
 212     std::cout << "Failed test #13!" << std::endl;
 213     return false;
 214   }
 215
 216   lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(-0.1, 0, -1));
 217   if (intersects(tri, lineSegment)) {
 218     std::cout << "Failed test #14!" << std::endl;
 219     return false;
 220   }
 221
 222   return true;
 223 }
 224
 225 template<typename T>
 226 bool
 227 SphereLineIntersectionTest(void)
 228 {
 229   unsigned nTests = 100000;
 230   unsigned failedCount = 0;
 231   for (unsigned i = 0; i < nTests; ++i) {
 232     SGVec3<T> center = rndVec3<T>();
 233     T radius = 2*sg_random();
 234     SGSphere<T> sphere(center, radius);
 235
 236     SGVec3<T> offset = normalize(rndVec3<T>());
 237     T t = 4*sg_random();
 238
 239     // This one is the point we use to judge if the test should fail or not
 240     SGVec3<T> base = center + t*offset;
 241
 242     SGVec3<T> per = perpendicular(offset);
 243     SGVec3<T> start = base + 4*sg_random()*per;
 244     SGVec3<T> end = base - 4*sg_random()*per;
 245
 246     SGLineSegment<T> lineSegment;
 247     lineSegment.set(start, end);
 248     if (intersects(sphere, lineSegment)) {
 249       if (radius < t) {
 250         std::cout << "Failed sphere line intersection test #" << i
 251                   << ": false positive!\nt = " << t << "\n"
 252                   << sphere << "\n" << lineSegment << std::endl;
 253         ++failedCount;
 254       }
 255     } else {
 256       if (t <= radius) {
 257         std::cout << "Failed sphere line intersection test #" << i
 258                   << ": false negative!\nt = " << t << "\n"
 259                   << sphere << "\n" << lineSegment << std::endl;
 260         ++failedCount;
 261       }
 262     }
 263
 264     SGRay<T> ray;
 265     ray.set(start, end - start);
 266     if (intersects(sphere, ray)) {
 267       if (radius < t) {
 268         std::cout << "Failed sphere line intersection test #" << i
 269                   << ": false positive!\nt = " << t << "\n"
 270                   << sphere << "\n" << ray << std::endl;
 271         ++failedCount;
 272       }
 273     } else {
 274       if (t <= radius) {
 275         std::cout << "Failed sphere line intersection test #" << i
 276                   << ": false negative!\nt = " << t << "\n"
 277                   << sphere << "\n" << ray << std::endl;
 278         ++failedCount;
 279       }
 280     }
 281   }
 282
 283   if (nTests < 100*failedCount) {
 284     std::cout << "Failed sphere line intersection tests: " << failedCount
 285               << " tests out of " << nTests
 286               << " went wrong. Abort!" << std::endl;
 287     return false;
 288   }
 289
 290   failedCount = 0;
 291   for (unsigned i = 0; i < nTests; ++i) {
 292     SGVec3<T> center = rndVec3<T>();
 293     T radius = 2*sg_random();
 294     SGSphere<T> sphere(center, radius);
 295
 296     SGVec3<T> offset = normalize(rndVec3<T>());
 297     T t = 4*sg_random();
 298
 299     // This one is the point we use to judge if the test should fail or not
 300     SGVec3<T> base = center + t*offset;
 301
 302     SGVec3<T> start = base;
 303     SGVec3<T> end = base + 2*sg_random()*offset;
 304
 305     SGLineSegment<T> lineSegment;
 306     lineSegment.set(start, end);
 307     if (intersects(sphere, lineSegment)) {
 308       if (radius < t) {
 309         std::cout << "Failed sphere line intersection test #" << i
 310                   << ": false positive!\nt = " << t << "\n"
 311                   << sphere << "\n" << lineSegment << std::endl;
 312         ++failedCount;
 313       }
 314     } else {
 315       if (t <= radius) {
 316         std::cout << "Failed sphere line intersection test #" << i
 317                   << ": false negative!\nt = " << t << "\n"
 318                   << sphere << "\n" << lineSegment << std::endl;
 319         ++failedCount;
 320       }
 321     }
 322
 323     SGRay<T> ray;
 324     ray.set(start, end - start);
 325     if (intersects(sphere, ray)) {
 326       if (radius < t) {
 327         std::cout << "Failed sphere line intersection test #" << i
 328                   << ": false positive!\nt = " << t << "\n"
 329                   << sphere << "\n" << ray << std::endl;
 330         ++failedCount;
 331       }
 332     } else {
 333       if (t <= radius) {
 334         std::cout << "Failed sphere line intersection test #" << i
 335                   << ": false negative!\nt = " << t << "\n"
 336                   << sphere << "\n" << ray << std::endl;
 337         ++failedCount;
 338       }
 339     }
 340   }
 341
 342   if (nTests < 100*failedCount) {
 343     std::cout << "Failed sphere line intersection tests: " << failedCount
 344               << " tests out of " << nTests
 345               << " went wrong. Abort!" << std::endl;
 346     return false;
 347   }
 348
 349   return true;
 350 }
 351
 352 template<typename T>
 353 bool
 354 BoxLineIntersectionTest(void)
 355 {
 356   // ok, bad test case coverage, but better than nothing ...
 357
 358   unsigned nTests = 100000;
 359   unsigned failedCount = 0;
 360   for (unsigned i = 0; i < nTests; ++i) {
 361     SGBox<T> box;
 362     box.expandBy(rndVec3<T>());
 363     box.expandBy(rndVec3<T>());
 364
 365     SGVec3<T> center = box.getCenter();
 366
 367     // This one is the point we use to judge if the test should fail or not
 368     SGVec3<T> base = rndVec3<T>();
 369     SGVec3<T> dir = base - center;
 370
 371     SGLineSegment<T> lineSegment;
 372     lineSegment.set(base, base + dir);
 373     if (intersects(box, lineSegment)) {
 374       if (!intersects(box, base)) {
 375         std::cout << "Failed box line intersection test #" << i
 376                   << ": false positive!\n"
 377                   << box << "\n" << lineSegment << std::endl;
 378         ++failedCount;
 379       }
 380     } else {
 381       if (intersects(box, base)) {
 382         std::cout << "Failed box line intersection test #" << i
 383                   << ": false negative!\n"
 384                   << box << "\n" << lineSegment << std::endl;
 385         ++failedCount;
 386       }
 387     }
 388
 389     SGRay<T> ray;
 390     ray.set(base, dir);
 391     if (intersects(box, ray)) {
 392       if (!intersects(box, base)) {
 393         std::cout << "Failed box line intersection test #" << i
 394                   << ": false positive!\n"
 395                   << box << "\n" << ray << std::endl;
 396         ++failedCount;
 397       }
 398     } else {
 399       if (intersects(box, base)) {
 400         std::cout << "Failed box line intersection test #" << i
 401                   << ": false negative!\n"
 402                   << box << "\n" << ray << std::endl;
 403         ++failedCount;
 404       }
 405     }
 406   }
 407
 408   if (nTests < 100*failedCount) {
 409     std::cout << "Failed box line intersection tests: " << failedCount
 410               << " tests out of " << nTests
 411               << " went wrong. Abort!" << std::endl;
 412     return false;
 413   }
 414
 415   return true;
 416 }
 417
 418 int
 419 main(void)
 420 {
 421   std::cout << "Testing Geometry intersection routines.\n"
 422             << "Some of these tests can fail due to roundoff problems...\n"
 423             << "Dont worry if only a few of them fail..." << std::endl;
 424
 425   if (!TriangleLineIntersectionTest<float>())
 426     return EXIT_FAILURE;
 427   if (!TriangleLineIntersectionTest<double>())
 428     return EXIT_FAILURE;
 429
 430   if (!SphereLineIntersectionTest<float>())
 431     return EXIT_FAILURE;
 432   if (!SphereLineIntersectionTest<double>())
 433     return EXIT_FAILURE;
 434
 435   if (!BoxLineIntersectionTest<float>())
 436     return EXIT_FAILURE;
 437   if (!BoxLineIntersectionTest<double>())
 438     return EXIT_FAILURE;
 439
 440   std::cout << "Successfully passed all tests!" << std::endl;
 441   return EXIT_SUCCESS;
 442 }