X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Fmath%2FSGGeometryTest.cxx;h=77bf14c69a7c8790b4b464450daa80d987f85fa1;hb=7bdb530440d1dadc991f305edb1b70ec85f27451;hp=cd2383ee954f5bfd771a64f9168c18694cc0aff6;hpb=de020ee69524393daf11200aa0a46bfd5aa2409a;p=simgear.git

diff --git a/simgear/math/SGGeometryTest.cxx b/simgear/math/SGGeometryTest.cxx
index cd2383ee..77bf14c6 100644
--- a/simgear/math/SGGeometryTest.cxx
+++ b/simgear/math/SGGeometryTest.cxx
@@ -31,6 +31,102 @@ SGVec3<T> rndVec3(void)
   return SGVec3<T>(sg_random(), sg_random(), sg_random());
 }
 
+template<typename T>
+bool
+TriangleClosestPointIntersectionTest(void)
+{
+  unsigned nTests = 100000;
+  unsigned failedCount = 0;
+  for (unsigned i = 0; i < nTests; ++i) {
+      // test triangle
+      SGTriangle<T> triangle(rndVec3<T>(), rndVec3<T>(), rndVec3<T>());
+      T triangleEps = 100*SGLimits<T>::epsilon();
+
+      // generate random point in the triangle
+      T u = 4*sg_random() - 2;
+      T v = 4*sg_random() - 2;
+      if (1 < u + v) {
+        v = T(0.5)*(v + 1 - u);
+        u = 1 - v;
+      }
+      u = SGMisc<T>::clip(u, 0, 1);
+      v = SGMisc<T>::clip(v, 0, 1);
+
+      SGVec3<T> testClosest;
+      testClosest = triangle.getBaseVertex();
+      testClosest += u*triangle.getEdge(0) + v*triangle.getEdge(1);
+
+      SGVec3<T> n = triangle.getNormal();
+      SGVec3<T> e0 = triangle.getEdge(0);
+      SGVec3<T> e1 = triangle.getEdge(1);
+
+      // The normals to the edges
+      SGVec3<T> a0 = cross(e0, n);
+      SGVec3<T> a1 = cross(e1 - e0, n);
+      SGVec3<T> a2 = cross(n, e1);
+
+      SGVec3<T> testPoint = testClosest;
+      // Ok, if we are at some edge, go perpandicular to the triangle away
+      if (u == 0) {
+        if (v == 0) {
+          testPoint += sg_random()*a0 + sg_random()*a2;
+        } else if (v == 1) {
+          testPoint += sg_random()*a1 + sg_random()*a2;
+        } else {
+          testPoint += sg_random()*a2;
+        }
+      } else if (u == 1) {
+        testPoint += sg_random()*a0 + sg_random()*a1;
+      } else {
+        if (v == 0) {
+          testPoint += sg_random()*a0;
+        } else if (u + v == 1) {
+          testPoint += sg_random()*a1;
+        }
+      }
+      testPoint += (2*sg_random() - 1)*n;
+
+      // Test the closest point function
+      SGVec3<T> closest = closestPoint(triangle, testPoint);
+      if (!equivalent(closest, testClosest, triangleEps)) {
+        std::cout << "Failed closest point test #" << i
+                  << ": not equivalent!\nu = "
+                  << u << ", v = " << v
+                  << "\ntestPoint = " << testPoint
+                  << ", testClosest = " << testClosest
+                  << ", closest = " << closest << "\n"
+                  << triangle << std::endl;
+        ++failedCount;
+      }
+
+      // Test the triangle sphere intersection
+      SGSphere<T> sphere(testPoint, sg_random());
+      bool exactIntersection = intersects(sphere, testClosest);
+//       bool exactIntersection = intersects(sphere, closest);
+      bool sphereTriangleIntersection = intersects(sphere, triangle);
+    
+      if (sphereTriangleIntersection != exactIntersection) {
+        std::cout << "Failed triangle sphere intersection test #" << i
+                  << ": not equivalent!\nu = "
+                  << u << ", v = " << v
+                  << "\ntestPoint = " << testPoint
+                  << ", testClosest = " << testClosest
+                  << ", closest = " << closest << "\n"
+                  << triangle << std::endl;
+        ++failedCount;
+      }
+  }
+
+  if (nTests < 100*failedCount) {
+    std::cout << "Failed box line intersection tests: " << failedCount
+              << " tests out of " << nTests
+              << " went wrong. Abort!" << std::endl;
+    return false;
+  }
+
+  return true;
+}
+
 template<typename T>
 bool
 TriangleLineIntersectionTest(void)
@@ -44,6 +140,8 @@ TriangleLineIntersectionTest(void)
     
     SGTriangle<T> tri(v0, v1, v2);
 
+    T triangleEps = 100*SGLimits<T>::epsilon();
+
     // generate random coeficients
     T u = 4*sg_random() - 2;
     T v = 4*sg_random() - 2;
@@ -59,7 +157,7 @@ TriangleLineIntersectionTest(void)
     
     if (intersects(isectres, tri, lineSegment)) {
       if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t && t <= 1) {
-        if (!equivalent(isectres, isectpt)) {
+        if (!equivalent(isectres, isectpt, triangleEps)) {
           std::cout << "Failed line segment intersection test #" << i
                     << ": not equivalent!\nu = "
                     << u << ", v = " << v << ", t = " << t
@@ -87,7 +185,7 @@ TriangleLineIntersectionTest(void)
     ray.set(isectpt - t*dir, dir);
     if (intersects(isectres, tri, ray)) {
       if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t) {
-        if (!equivalent(isectres, isectpt)) {
+        if (!equivalent(isectres, isectpt, triangleEps)) {
           std::cout << "Failed ray intersection test #" << i
                     << ": not equivalent!\nu = "
                     << u << ", v = " << v << ", t = " << t
@@ -422,6 +520,13 @@ main(void)
             << "Some of these tests can fail due to roundoff problems...\n"
             << "Dont worry if only a few of them fail..." << std::endl;
 
+  sg_srandom(17);
+
+  if (!TriangleClosestPointIntersectionTest<float>())
+    return EXIT_FAILURE;
+  if (!TriangleClosestPointIntersectionTest<double>())
+    return EXIT_FAILURE;
+
   if (!TriangleLineIntersectionTest<float>())
     return EXIT_FAILURE;
   if (!TriangleLineIntersectionTest<double>())