Merge branch 'maint' into next

[simgear.git] / simgear / math / SGIntersect.hxx

diff --git a/simgear/math/SGIntersect.hxx b/simgear/math/SGIntersect.hxx
index 18a49e8d366a2d4478375b2ffe568ff374ffc3d7..5951e158fafcfea6f81268a3affe04ca4666b2fa 100644 (file)
--- a/simgear/math/SGIntersect.hxx
+++ b/simgear/math/SGIntersect.hxx
@@ -287,6 +287,38 @@ intersects(SGVec3<T>& dst, const SGPlane<T>& plane, const SGLineSegment<T>& line
 { return intersects(dst, lineSegment, plane); }
 
 
+// Distance of a line segment to a point
+template<typename T>
+inline T
+distSqr(const SGLineSegment<T>& lineSeg, const SGVec3<T>& p)
+{
+  SGVec3<T> ps = p - lineSeg.getStart();
+
+  T psdotdir = dot(ps, lineSeg.getDirection());
+  if (psdotdir <= 0)
+    return dot(ps, ps);
+
+  SGVec3<T> pe = p - lineSeg.getEnd();
+  if (0 <= dot(pe, lineSeg.getDirection()))
+    return dot(pe, pe);
+ 
+  return dot(ps, ps) - psdotdir*psdotdir/dot(lineSeg.getDirection(), lineSeg.getDirection());
+}
+// make it symmetric
+template<typename T>
+inline T
+distSqr(const SGVec3<T>& p, const SGLineSegment<T>& lineSeg)
+{ return distSqr(lineSeg, p); }
+// with sqrt
+template<typename T>
+inline T
+dist(const SGVec3<T>& p, const SGLineSegment<T>& lineSeg)
+{ return sqrt(distSqr(lineSeg, p)); }
+template<typename T>
+inline T
+dist(const SGLineSegment<T>& lineSeg, const SGVec3<T>& p)
+{ return sqrt(distSqr(lineSeg, p)); }
+
 template<typename T>
 inline bool
 intersects(const SGRay<T>& ray, const SGSphere<T>& sphere)
@@ -570,17 +602,17 @@ intersects(const SGBox<T>& box, const SGRay<T>& ray)
         return false;
     }
 
-    T near = - SGLimits<T>::max();
-    T far = SGLimits<T>::max();
+    T nearr = - SGLimits<T>::max();
+    T farr = SGLimits<T>::max();
 
     T T1 = (cMin - cOrigin) / cDir;
     T T2 = (cMax - cOrigin) / cDir;
     if (T1 > T2) std::swap (T1, T2);/* since T1 intersection with near plane */
-    if (T1 > near) near = T1; /* want largest Tnear */
-    if (T2 < far) far = T2; /* want smallest Tfar */
-    if (near > far) // far box is missed
+    if (T1 > nearr) nearr = T1; /* want largest Tnear */
+    if (T2 < farr) farr = T2; /* want smallest Tfarr */
+    if (nearr > farr) // farr box is missed
       return false;
-    if (far < 0) // box is behind ray
+    if (farr < 0) // box is behind ray
       return false;
   }