From e417903c231bd63da3e988113b43dcfc0b0abe18 Mon Sep 17 00:00:00 2001
From: frohlich <frohlich>
Date: Sun, 1 Mar 2009 12:22:56 +0000
Subject: [PATCH] Additions for the boundingvolumes Modified Files: 
 SGGeometryTest.cxx SGIntersect.hxx SGLineSegment.hxx 	SGTriangle.hxx

---
 simgear/math/SGGeometryTest.cxx | 101 ++++++++
 simgear/math/SGIntersect.hxx    | 412 +++++++++++++++++++++++++++++++-
 simgear/math/SGLineSegment.hxx  |   8 +
 simgear/math/SGTriangle.hxx     |  10 +
 4 files changed, 530 insertions(+), 1 deletion(-)

diff --git a/simgear/math/SGGeometryTest.cxx b/simgear/math/SGGeometryTest.cxx
index 432e87b6..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)
@@ -426,6 +522,11 @@ main(void)
 
   sg_srandom(17);
 
+  if (!TriangleClosestPointIntersectionTest<float>())
+    return EXIT_FAILURE;
+  if (!TriangleClosestPointIntersectionTest<double>())
+    return EXIT_FAILURE;
+
   if (!TriangleLineIntersectionTest<float>())
     return EXIT_FAILURE;
   if (!TriangleLineIntersectionTest<double>())
diff --git a/simgear/math/SGIntersect.hxx b/simgear/math/SGIntersect.hxx
index bdc350f2..b2438dc6 100644
--- a/simgear/math/SGIntersect.hxx
+++ b/simgear/math/SGIntersect.hxx
@@ -1,4 +1,4 @@
-// Copyright (C) 2006  Mathias Froehlich - Mathias.Froehlich@web.de
+// Copyright (C) 2006-2009  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
@@ -550,6 +550,416 @@ intersects(const SGTriangle<T>& tri, const SGLineSegment<T>& lineSegment, T eps
 }
 
 
+template<typename T>
+inline SGVec3<T>
+closestPoint(const SGTriangle<T>& tri, const SGVec3<T>& p)
+{
+  // This method minimizes the distance function Q(u, v) = || p - x ||
+  // where x is a point in the trialgle given by the vertices v_i
+  //  x = v_0 + u*(v_1 - v_0) + v*(v_2 - v_0)
+  // The theoretical analysis is somehow too long for a comment.
+  // May be it is sufficient to see that this code passes all the tests.
+
+  SGVec3<T> off = tri.getBaseVertex() - p;
+  T a = dot(tri.getEdge(0), tri.getEdge(0));
+  T b = dot(tri.getEdge(0), tri.getEdge(1));
+  T c = dot(tri.getEdge(1), tri.getEdge(1));
+  T d = dot(tri.getEdge(0), off);
+  T e = dot(tri.getEdge(1), off);
+  
+  T det = a*c - b*b;
+
+  T u = b*e - c*d;
+  T v = b*d - a*e;
+
+  // Regions
+  // \2|
+  //  \|
+  //   |\
+  // 3 |0\ 1
+  //----------
+  // 4 | 5 \ 6
+
+  if (u + v <= det) {
+    if (u < 0) {
+      if (v < 0) {
+        // region 4
+        if (d < 0) {
+          if (a <= -d) {
+            // u = 1;
+            // v = 0;
+            return tri.getBaseVertex() + tri.getEdge(0);
+          } else {
+            u = -d/a;
+            // v = 0;
+            return tri.getBaseVertex() + u*tri.getEdge(0);
+          }
+        } else {
+          if (0 < e) {
+            // u = 0;
+            // v = 0;
+            return tri.getBaseVertex();
+          } else if (c <= -e) {
+            // u = 0;
+            // v = 1;
+            return tri.getBaseVertex() + tri.getEdge(1);
+          } else {
+            // u = 0;
+            v = -e/c;
+            return tri.getBaseVertex() + v*tri.getEdge(1);
+          }
+        }
+      } else {
+        // region 3
+        if (0 <= e) {
+          // u = 0;
+          // v = 0;
+          return tri.getBaseVertex();
+        } else if (c <= -e) {
+          // u = 0;
+          // v = 1;
+          return tri.getBaseVertex() + tri.getEdge(1);
+        } else {
+          // u = 0;
+          v = -e/c;
+          return tri.getBaseVertex() + v*tri.getEdge(1);
+        }
+      }
+    } else if (v < 0) {
+      // region 5
+      if (0 <= d) {
+        // u = 0;
+        // v = 0;
+        return tri.getBaseVertex();
+      } else if (a <= -d) {
+        // u = 1;
+        // v = 0;
+        return tri.getBaseVertex() + tri.getEdge(0);
+      } else {
+        u = -d/a;
+        // v = 0;
+        return tri.getBaseVertex() + u*tri.getEdge(0);
+      }
+    } else {
+      // region 0
+      if (det <= SGLimits<T>::min()) {
+        u = 0;
+        v = 0;
+        return tri.getBaseVertex();
+      } else {
+        T invDet = 1/det;
+        u *= invDet;
+        v *= invDet;
+        return tri.getBaseVertex() + u*tri.getEdge(0) + v*tri.getEdge(1);
+      }
+    }
+  } else {
+    if (u < 0) {
+      // region 2
+      T tmp0 = b + d;
+      T tmp1 = c + e;
+      if (tmp0 < tmp1) {
+        T numer = tmp1 - tmp0;
+        T denom = a - 2*b + c;
+        if (denom <= numer) {
+          // u = 1;
+          // v = 0;
+          return tri.getBaseVertex() + tri.getEdge(0);
+        } else {
+          u = numer/denom;
+          v = 1 - u;
+          return tri.getBaseVertex() + u*tri.getEdge(0) + v*tri.getEdge(1);
+        }
+      } else {
+        if (tmp1 <= 0) {
+          // u = 0;
+          // v = 1;
+          return tri.getBaseVertex() + tri.getEdge(1);
+        } else if (0 <= e) {
+          // u = 0;
+          // v = 0;
+          return tri.getBaseVertex();
+        } else {
+          // u = 0;
+          v = -e/c;
+          return tri.getBaseVertex() + v*tri.getEdge(1);
+        }
+      }
+    } else if (v < 0) {
+      // region 6
+      T tmp0 = b + e;
+      T tmp1 = a + d;
+      if (tmp0 < tmp1) {
+        T numer = tmp1 - tmp0;
+        T denom = a - 2*b + c;
+        if (denom <= numer) {
+          // u = 0;
+          // v = 1;
+          return tri.getBaseVertex() + tri.getEdge(1);
+        } else {
+          v = numer/denom;
+          u = 1 - v;
+          return tri.getBaseVertex() + u*tri.getEdge(0) + v*tri.getEdge(1);
+        }
+      } else {
+        if (tmp1 < 0) {
+          // u = 1;
+          // v = 0;
+          return tri.getBaseVertex() + tri.getEdge(0);
+        } else if (0 <= d) {
+          // u = 0;
+          // v = 0;
+          return tri.getBaseVertex();
+        } else {
+          u = -d/a;
+          // v = 0;
+          return tri.getBaseVertex() + u*tri.getEdge(0);
+        }
+      }
+    } else {
+      // region 1
+      T numer = c + e - b - d;
+      if (numer <= 0) {
+        // u = 0;
+        // v = 1;
+        return tri.getVertex(2);
+      } else {
+        T denom = a - 2*b + c;
+        if (denom <= numer) {
+          // u = 1;
+          // v = 0;
+          return tri.getBaseVertex() + tri.getEdge(0);
+        } else {
+          u = numer/denom;
+          v = 1 - u;
+          return tri.getBaseVertex() + u*tri.getEdge(0) + v*tri.getEdge(1);
+        }
+      }
+    }
+  }
+}
+template<typename T>
+inline SGVec3<T>
+closestPoint(const SGVec3<T>& p, const SGTriangle<T>& tri)
+{ return closestPoint(tri, p); }
+
+template<typename T>
+inline bool
+intersects(const SGTriangle<T>& tri, const SGSphere<T>& sphere)
+{
+  // This method minimizes the distance function Q(u, v) = || p - x ||
+  // where x is a point in the trialgle given by the vertices v_i
+  //  x = v_0 + u*(v_1 - v_0) + v*(v_2 - v_0)
+  // The theoretical analysis is somehow too long for a comment.
+  // May be it is sufficient to see that this code passes all the tests.
+
+  SGVec3<T> off = tri.getBaseVertex() - sphere.getCenter();
+  T baseDist2 = dot(off, off);
+  T a = dot(tri.getEdge(0), tri.getEdge(0));
+  T b = dot(tri.getEdge(0), tri.getEdge(1));
+  T c = dot(tri.getEdge(1), tri.getEdge(1));
+  T d = dot(tri.getEdge(0), off);
+  T e = dot(tri.getEdge(1), off);
+  
+  T det = a*c - b*b;
+
+  T u = b*e - c*d;
+  T v = b*d - a*e;
+
+  // Regions
+  // \2|
+  //  \|
+  //   |\
+  // 3 |0\ 1
+  //----------
+  // 4 | 5 \ 6
+
+  if (u + v <= det) {
+    if (u < 0) {
+      if (v < 0) {
+        // region 4
+        if (d < 0) {
+          if (a <= -d) {
+            // u = 1;
+            // v = 0;
+            T sqrDist = a + 2*d + baseDist2;
+            return sqrDist <= sphere.getRadius2();
+          } else {
+            u = -d/a;
+            // v = 0;
+            T sqrDist = d*u + baseDist2;
+            return sqrDist <= sphere.getRadius2();
+          }
+        } else {
+          if (0 < e) {
+            // u = 0;
+            // v = 0;
+            return baseDist2 <= sphere.getRadius2();
+          } else if (c <= -e) {
+            // u = 0;
+            // v = 1;
+            T sqrDist = c + 2*e + baseDist2;
+            return sqrDist <= sphere.getRadius2();
+          } else {
+            // u = 0;
+            v = -e/c;
+            T sqrDist = e*v + baseDist2;
+            return sqrDist <= sphere.getRadius2();
+          }
+        }
+      } else {
+        // region 3
+        if (0 <= e) {
+          // u = 0;
+          // v = 0;
+          return baseDist2 <= sphere.getRadius2();
+        } else if (c <= -e) {
+          // u = 0;
+          // v = 1;
+          T sqrDist = c + 2*e + baseDist2;
+          return sqrDist <= sphere.getRadius2();
+        } else {
+          // u = 0;
+          v = -e/c;
+          T sqrDist = e*v + baseDist2;
+          return sqrDist <= sphere.getRadius2();
+        }
+      }
+    } else if (v < 0) {
+      // region 5
+      if (0 <= d) {
+        // u = 0;
+        // v = 0;
+        return baseDist2 <= sphere.getRadius2();
+      } else if (a <= -d) {
+        // u = 1;
+        // v = 0;
+        T sqrDist = a + 2*d + baseDist2;
+        return sqrDist <= sphere.getRadius2();
+      } else {
+        u = -d/a;
+        // v = 0;
+        T sqrDist = d*u + baseDist2;
+        return sqrDist <= sphere.getRadius2();
+      }
+    } else {
+      // region 0
+      if (det <= SGLimits<T>::min()) {
+        // sqrDist = baseDist2;
+        u = 0;
+        v = 0;
+        return baseDist2 <= sphere.getRadius2();
+      } else {
+        T invDet = 1/det;
+        u *= invDet;
+        v *= invDet;
+        T sqrDist = u*(a*u + b*v + 2*d) + v*(b*u + c*v + 2*e) + baseDist2;
+        return sqrDist <= sphere.getRadius2();
+      }
+    }
+  } else {
+    if (u < 0) {
+      // region 2
+      T tmp0 = b + d;
+      T tmp1 = c + e;
+      if (tmp0 < tmp1) {
+        T numer = tmp1 - tmp0;
+        T denom = a - 2*b + c;
+        if (denom <= numer) {
+          // u = 1;
+          // v = 0;
+          T sqrDist = a + 2*d + baseDist2;
+          return sqrDist <= sphere.getRadius2();
+        } else {
+          u = numer/denom;
+          v = 1 - u;
+          T sqrDist = u*(a*u + b*v + 2*d) + v*(b*u + c*v + 2*e) + baseDist2;
+          return sqrDist <= sphere.getRadius2();
+        }
+      } else {
+        if (tmp1 <= 0) {
+          // u = 0;
+          // v = 1;
+          T sqrDist = c + 2*e + baseDist2;
+          return sqrDist <= sphere.getRadius2();
+        } else if (0 <= e) {
+          // u = 0;
+          // v = 0;
+          return baseDist2 <= sphere.getRadius2();
+        } else {
+          // u = 0;
+          v = -e/c;
+          T sqrDist = e*v + baseDist2;
+          return sqrDist <= sphere.getRadius2();
+        }
+      }
+    } else if (v < 0) {
+      // region 6
+      T tmp0 = b + e;
+      T tmp1 = a + d;
+      if (tmp0 < tmp1) {
+        T numer = tmp1 - tmp0;
+        T denom = a - 2*b + c;
+        if (denom <= numer) {
+          // u = 0;
+          // v = 1;
+          T sqrDist = c + 2*e + baseDist2;
+          return sqrDist <= sphere.getRadius2();
+        } else {
+          v = numer/denom;
+          u = 1 - v;
+          T sqrDist = u*(a*u + b*v + 2*d) + v*(b*u + c*v + 2*e)+baseDist2;
+          return sqrDist <= sphere.getRadius2();
+        }
+      } else {
+        if (tmp1 < 0) {
+          // u = 1;
+          // v = 0;
+          T sqrDist = a + 2*d + baseDist2;
+          return sqrDist <= sphere.getRadius2();
+        } else if (0 <= d) {
+          // sqrDist = baseDist2;
+          // u = 0;
+          // v = 0;
+          return baseDist2 <= sphere.getRadius2();
+        } else {
+          u = -d/a;
+          // v = 0;
+          T sqrDist = d*u + baseDist2;
+          return sqrDist <= sphere.getRadius2();
+        }
+      }
+    } else {
+      // region 1
+      T numer = c + e - b - d;
+      if (numer <= 0) {
+        // u = 0;
+        // v = 1;
+        T sqrDist = c + 2*e + baseDist2;
+        return sqrDist <= sphere.getRadius2();
+      } else {
+        T denom = a - 2*b + c;
+        if (denom <= numer) {
+          // u = 1;
+          // v = 0;
+          T sqrDist = a + 2*d + baseDist2;
+          return sqrDist <= sphere.getRadius2();
+        } else {
+          u = numer/denom;
+          v = 1 - u;
+          T sqrDist = u*(a*u + b*v + 2*d) + v*(b*u + c*v + 2*e) + baseDist2;
+          return sqrDist <= sphere.getRadius2();
+        }
+      }
+    }
+  }
+}
+template<typename T>
+inline bool
+intersects(const SGSphere<T>& sphere, const SGTriangle<T>& tri)
+{ return intersects(tri, sphere); }
+
+
 template<typename T>
 inline bool
 intersects(const SGVec3<T>& v, const SGSphere<T>& sphere)
diff --git a/simgear/math/SGLineSegment.hxx b/simgear/math/SGLineSegment.hxx
index 0e509621..c3121c62 100644
--- a/simgear/math/SGLineSegment.hxx
+++ b/simgear/math/SGLineSegment.hxx
@@ -48,6 +48,14 @@ public:
   SGVec3<T> getCenter() const
   { return _start + T(0.5)*_direction; }
 
+  SGLineSegment<T> transform(const SGMatrix<T>& matrix) const
+  {
+    SGLineSegment<T> lineSegment;
+    lineSegment._start = matrix.xformPt(_start);
+    lineSegment._direction = matrix.xformVec(_direction);
+    return lineSegment;
+  }
+
 private:
   SGVec3<T> _start;
   SGVec3<T> _direction;
diff --git a/simgear/math/SGTriangle.hxx b/simgear/math/SGTriangle.hxx
index bc395ab9..af6f32f1 100644
--- a/simgear/math/SGTriangle.hxx
+++ b/simgear/math/SGTriangle.hxx
@@ -71,6 +71,16 @@ public:
     _d[0] = _d[1];
     _d[1] = tmp;
   }
+
+  SGTriangle<T> transform(const SGMatrix<T>& matrix) const
+  {
+    SGTriangle<T> triangle;
+    triangle._v0 = matrix.xformPt(_v0);
+    triangle._d[0] = matrix.xformVec(_d[0]);
+    triangle._d[1] = matrix.xformVec(_d[1]);
+    return triangle;
+  }
+
 private:
   /// Store one vertex directly, _d is the offset of the other two
   /// vertices wrt the base vertex
-- 
2.39.5