SGGeometryTest.cxx

// Copyright (C) 2006  Mathias Froehlich - Mathias.Froehlich@web.de
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Library General Public
// License as published by the Free Software Foundation; either
// version 2 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Library General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
//

#ifdef HAVE_CONFIG_H
#  include <simgear_config.h>
#endif

#include <cstdlib>
#include <iostream>

#include "SGGeometry.hxx"
#include "sg_random.h"

template<typename T>
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)
{
  unsigned nTests = 100000;
  unsigned failedCount = 0;
  for (unsigned i = 0; i < nTests; ++i) {
    SGVec3<T> v0 = rndVec3<T>();
    SGVec3<T> v1 = rndVec3<T>();
    SGVec3<T> v2 = rndVec3<T>();
    
    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;
    T t = 4*sg_random() - 2;

    SGVec3<T> isectpt = v0 + u*(v1 - v0) + v*(v2 - v0);

    SGLineSegment<T> lineSegment;
    SGVec3<T> dir = rndVec3<T>();

    SGVec3<T> isectres;
    lineSegment.set(isectpt - t*dir, isectpt + (1 - t)*dir);
    
    if (intersects(isectres, tri, lineSegment)) {
      if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t && t <= 1) {
        if (!equivalent(isectres, isectpt, triangleEps)) {
          std::cout << "Failed line segment intersection test #" << i
                    << ": not equivalent!\nu = "
                    << u << ", v = " << v << ", t = " << t
                    << "\n" << tri << "\n" << lineSegment << std::endl;
          ++failedCount;
        }
      } else {
        std::cout << "Failed line segment intersection test #" << i
                  << ": false positive!\nu = "
                  << u << ", v = " << v << ", t = " << t
                  << "\n" << tri << "\n" << lineSegment << std::endl;
        ++failedCount;
      }
    } else {
      if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t && t <= 1) {
        std::cout << "Failed line segment intersection test #" << i
                  << ": false negative!\nu = "
                  << u << ", v = " << v << ", t = " << t
                  << "\n" << tri << "\n" << lineSegment << std::endl;
        ++failedCount;
      }
    }

    SGRay<T> ray;
    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, triangleEps)) {
          std::cout << "Failed ray intersection test #" << i
                    << ": not equivalent!\nu = "
                    << u << ", v = " << v << ", t = " << t
                    << "\n" << tri << "\n" << ray << std::endl;
          ++failedCount;
        }
      } else {
        std::cout << "Failed ray intersection test #" << i
                  << ": false positive!\nu = "
                  << u << ", v = " << v << ", t = " << t
                  << "\n" << tri << "\n" << ray << std::endl;
        ++failedCount;
      }
    } else {
      if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t) {
        std::cout << "Failed ray intersection test #" << i
                  << ": false negative !\nu = "
                  << u << ", v = " << v << ", t = " << t
                  << "\n" << tri << "\n" << ray << std::endl;
        ++failedCount;
      }
    }
  }

  if (nTests < 100*failedCount) {
    std::cout << "Failed ray intersection tests: " << failedCount
              << " tests out of " << nTests
              << " went wrong. Abort!" << std::endl;
    return false;
  }

  SGVec3<T> v0 = SGVec3<T>(0, 0, 0);
  SGVec3<T> v1 = SGVec3<T>(1, 0, 0);
  SGVec3<T> v2 = SGVec3<T>(0, 1, 0);

  SGTriangle<T> tri(v0, v1, v2);

  SGRay<T> ray;
  ray.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0.1, 0.1, -1));
  if (!intersects(tri, ray)) {
    std::cout << "Failed test #1!" << std::endl;
    return false;
  }

  ray.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, 0, -1));
  if (!intersects(tri, ray)) {
    std::cout << "Failed test #2!" << std::endl;
    return false;
  }

  SGLineSegment<T> lineSegment;
  lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0.1, 0.1, -1));
  if (!intersects(tri, lineSegment)) {
    std::cout << "Failed test #3!" << std::endl;
    return false;
  }

  lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, 0, -1));
  if (!intersects(tri, lineSegment)) {
    std::cout << "Failed test #4!" << std::endl;
    return false;
  }

  lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, 1, -1));
  if (!intersects(tri, lineSegment)) {
    std::cout << "Failed test #5!" << std::endl;
    return false;
  }

  lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(1, 0, -1));
  if (!intersects(tri, lineSegment)) {
    std::cout << "Failed test #6!" << std::endl;
    return false;
  }

  lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(1, 1, -1));
  if (!intersects(tri, lineSegment)) {
    std::cout << "Failed test #7!" << std::endl;
    return false;
  }

  // is exactly in the plane
  // FIXME: cannot detect that yet ??
//   lineSegment.set(SGVec3<T>(0, 0, 0), SGVec3<T>(1, 0, 0));
//   if (!intersects(tri, lineSegment)) {
//     std::cout << "Failed test #8!" << std::endl;
//     return false;
//   }

  // is exactly in the plane
  // FIXME: cannot detect that yet ??
//   lineSegment.set(SGVec3<T>(-1, 0, 0), SGVec3<T>(1, 0, 0));
//   if (!intersects(tri, lineSegment)) {
//     std::cout << "Failed test #9!" << std::endl;
//     return false;
//   }

  // is exactly paralell to the plane
  // FIXME: cannot detect that yet ??
//   lineSegment.set(SGVec3<T>(-1, 1, 0), SGVec3<T>(1, 1, 0));
//   if (intersects(tri, lineSegment)) {
//     std::cout << "Failed test #10!" << std::endl;
//     return false;
//   }

  // should fail since the line segment poins slightly beyond the triangle
  lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(1, 1, -0.9));
  if (intersects(tri, lineSegment)) {
    std::cout << "Failed test #11!" << std::endl;
    return false;
  }

  lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, -0.1, -1));
  if (intersects(tri, lineSegment)) {
    std::cout << "Failed test #12!" << std::endl;
    return false;
  }

  lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(-0.1, -0.1, -1));
  if (intersects(tri, lineSegment)) {
    std::cout << "Failed test #13!" << std::endl;
    return false;
  }

  lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(-0.1, 0, -1));
  if (intersects(tri, lineSegment)) {
    std::cout << "Failed test #14!" << std::endl;
    return false;
  }

  return true;
}

template<typename T>
bool
SphereLineIntersectionTest(void)
{
  unsigned nTests = 100000;
  unsigned failedCount = 0;
  for (unsigned i = 0; i < nTests; ++i) {
    SGVec3<T> center = rndVec3<T>();
    T radius = 2*sg_random();
    SGSphere<T> sphere(center, radius);

    SGVec3<T> offset = normalize(rndVec3<T>());
    T t = 4*sg_random();

    // This one is the point we use to judge if the test should fail or not
    SGVec3<T> base = center + t*offset;

    SGVec3<T> per = perpendicular(offset);
    SGVec3<T> start = base + 4*sg_random()*per;
    SGVec3<T> end = base - 4*sg_random()*per;

    SGLineSegment<T> lineSegment;
    lineSegment.set(start, end);
    if (intersects(sphere, lineSegment)) {
      if (radius < t) {
        std::cout << "Failed sphere line intersection test #" << i
                  << ": false positive!\nt = " << t << "\n"
                  << sphere << "\n" << lineSegment << std::endl;
        ++failedCount;
      }
    } else {
      if (t <= radius) {
        std::cout << "Failed sphere line intersection test #" << i
                  << ": false negative!\nt = " << t << "\n"
                  << sphere << "\n" << lineSegment << std::endl;
        ++failedCount;
      }
    }

    SGRay<T> ray;
    ray.set(start, end - start);
    if (intersects(sphere, ray)) {
      if (radius < t) {
        std::cout << "Failed sphere line intersection test #" << i
                  << ": false positive!\nt = " << t << "\n"
                  << sphere << "\n" << ray << std::endl;
        ++failedCount;
      }
    } else {
      if (t <= radius) {
        std::cout << "Failed sphere line intersection test #" << i
                  << ": false negative!\nt = " << t << "\n"
                  << sphere << "\n" << ray << std::endl;
        ++failedCount;
      }
    }
  }

  if (nTests < 100*failedCount) {
    std::cout << "Failed sphere line intersection tests: " << failedCount
              << " tests out of " << nTests
              << " went wrong. Abort!" << std::endl;
    return false;
  }

  failedCount = 0;
  for (unsigned i = 0; i < nTests; ++i) {
    SGVec3<T> center = rndVec3<T>();
    T radius = 2*sg_random();
    SGSphere<T> sphere(center, radius);

    SGVec3<T> offset = normalize(rndVec3<T>());
    T t = 4*sg_random();

    // This one is the point we use to judge if the test should fail or not
    SGVec3<T> base = center + t*offset;

    SGVec3<T> start = base;
    SGVec3<T> end = base + 2*sg_random()*offset;

    SGLineSegment<T> lineSegment;
    lineSegment.set(start, end);
    if (intersects(sphere, lineSegment)) {
      if (radius < t) {
        std::cout << "Failed sphere line intersection test #" << i
                  << ": false positive!\nt = " << t << "\n"
                  << sphere << "\n" << lineSegment << std::endl;
        ++failedCount;
      }
    } else {
      if (t <= radius) {
        std::cout << "Failed sphere line intersection test #" << i
                  << ": false negative!\nt = " << t << "\n"
                  << sphere << "\n" << lineSegment << std::endl;
        ++failedCount;
      }
    }

    SGRay<T> ray;
    ray.set(start, end - start);
    if (intersects(sphere, ray)) {
      if (radius < t) {
        std::cout << "Failed sphere line intersection test #" << i
                  << ": false positive!\nt = " << t << "\n"
                  << sphere << "\n" << ray << std::endl;
        ++failedCount;
      }
    } else {
      if (t <= radius) {
        std::cout << "Failed sphere line intersection test #" << i
                  << ": false negative!\nt = " << t << "\n"
                  << sphere << "\n" << ray << std::endl;
        ++failedCount;
      }
    }
  }

  if (nTests < 100*failedCount) {
    std::cout << "Failed sphere line intersection tests: " << failedCount
              << " tests out of " << nTests
              << " went wrong. Abort!" << std::endl;
    return false;
  }

  return true;
}

template<typename T>
bool
BoxLineIntersectionTest(void)
{
  // ok, bad test case coverage, but better than nothing ...

  unsigned nTests = 100000;
  unsigned failedCount = 0;
  for (unsigned i = 0; i < nTests; ++i) {
    SGBox<T> box;
    box.expandBy(rndVec3<T>());
    box.expandBy(rndVec3<T>());

    SGVec3<T> center = box.getCenter();
    
    // This one is the point we use to judge if the test should fail or not
    SGVec3<T> base = rndVec3<T>();
    SGVec3<T> dir = base - center;

    SGLineSegment<T> lineSegment;
    lineSegment.set(base, base + dir);
    if (intersects(box, lineSegment)) {
      if (!intersects(box, base)) {
        std::cout << "Failed box line intersection test #" << i
                  << ": false positive!\n"
                  << box << "\n" << lineSegment << std::endl;
        ++failedCount;
      }
    } else {
      if (intersects(box, base)) {
        std::cout << "Failed box line intersection test #" << i
                  << ": false negative!\n"
                  << box << "\n" << lineSegment << std::endl;
        ++failedCount;
      }
    }

    SGRay<T> ray;
    ray.set(base, dir);
    if (intersects(box, ray)) {
      if (!intersects(box, base)) {
        std::cout << "Failed box line intersection test #" << i
                  << ": false positive!\n"
                  << box << "\n" << ray << std::endl;
        ++failedCount;
      }
    } else {
      if (intersects(box, base)) {
        std::cout << "Failed box line intersection test #" << i
                  << ": false negative!\n"
                  << box << "\n" << ray << 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;
}

int
main(void)
{
  std::cout << "Testing Geometry intersection routines.\n"
            << "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>())
    return EXIT_FAILURE;

  if (!SphereLineIntersectionTest<float>())
    return EXIT_FAILURE;
  if (!SphereLineIntersectionTest<double>())
    return EXIT_FAILURE;
  
  if (!BoxLineIntersectionTest<float>())
    return EXIT_FAILURE;
  if (!BoxLineIntersectionTest<double>())
    return EXIT_FAILURE;
  
  std::cout << "Successfully passed all tests!" << std::endl;
  return EXIT_SUCCESS;
}

---