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