X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Fmath%2FSGIntersect.hxx;h=46bdb427d0e027c388be42a3997858eeed1ec4ac;hb=3bcd0bafd5fba1eebadfd1cb8a7294d665cf1932;hp=bdc350f2351282d038d81c10719f7b0db380a91e;hpb=d11954e80c9e62f87da087b29f6bfb7a6d7e84b5;p=simgear.git

diff --git a/simgear/math/SGIntersect.hxx b/simgear/math/SGIntersect.hxx
index bdc350f2..46bdb427 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
@@ -18,6 +18,8 @@
 #ifndef SGIntersect_HXX
 #define SGIntersect_HXX
 
+#include <algorithm>
+
 template<typename T>
 inline bool
 intersects(const SGSphere<T>& s1, const SGSphere<T>& s2)
@@ -38,25 +40,11 @@ intersects(const SGBox<T1>& box, const SGSphere<T2>& sphere)
 {
   if (sphere.empty())
     return false;
-  // Is more or less trivially included in the next tests
-  // if (box.empty())
-  //   return false;
-
-  if (sphere.getCenter().x() < box.getMin().x() - sphere.getRadius())
-    return false;
-  if (sphere.getCenter().y() < box.getMin().y() - sphere.getRadius())
-    return false;
-  if (sphere.getCenter().z() < box.getMin().z() - sphere.getRadius())
-    return false;
-
-  if (box.getMax().x() + sphere.getRadius() < sphere.getCenter().x())
-    return false;
-  if (box.getMax().y() + sphere.getRadius() < sphere.getCenter().y())
-    return false;
-  if (box.getMax().z() + sphere.getRadius() < sphere.getCenter().z())
+  if (box.empty())
     return false;
 
-  return true;
+  SGVec3<T1> closest = box.getClosestPoint(sphere.getCenter());
+  return distSqr(closest, SGVec3<T1>(sphere.getCenter())) <= sphere.getRadius2();
 }
 // make it symmetric
 template<typename T1, typename T2>
@@ -265,11 +253,11 @@ intersects(SGVec3<T>& dst, const SGLineSegment<T>& lineSegment, const SGPlane<T>
 
   // The negative numerator for the \alpha expression
   T num = plane.getPositiveDist();
-  num -= dot(plane.getNormal(), lineSegment.getOrigin());
+  num -= dot(plane.getNormal(), lineSegment.getStart());
   
   // If the numerator is zero, we have the lines origin included in the plane
   if (fabs(num) <= SGLimits<T>::min()) {
-    dst = lineSegment.getOrigin();
+    dst = lineSegment.getStart();
     return true;
   }
 
@@ -291,7 +279,7 @@ intersects(SGVec3<T>& dst, const SGLineSegment<T>& lineSegment, const SGPlane<T>
   if (1 < alpha)
     return false;
 
-  dst = lineSegment.getOrigin() + alpha*lineSegment.getDirection();
+  dst = lineSegment.getStart() + alpha*lineSegment.getDirection();
   return true;
 }
 // make it symmetric
@@ -550,6 +538,420 @@ 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, typename T2>
+inline bool
+intersects(const SGTriangle<T>& tri, const SGSphere<T2>& 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() - SGVec3<T>(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 T1, typename T2>
+inline bool
+intersects(const SGSphere<T1>& sphere, const SGTriangle<T2>& tri)
+{ return intersects(tri, sphere); }
+
+
 template<typename T>
 inline bool
 intersects(const SGVec3<T>& v, const SGSphere<T>& sphere)