From 360d3834ca57d89c396635b613afaeb9186c3b83 Mon Sep 17 00:00:00 2001
From: frohlich <frohlich>
Date: Tue, 30 Jan 2007 20:12:15 +0000
Subject: [PATCH] Modified Files: 	SGIntersect.hxx SGVec3.hxx SGVec4.hxx:
 Add convinience methods

---
 simgear/math/SGIntersect.hxx | 32 ++++++++++++++++++++++++++++++++
 simgear/math/SGVec3.hxx      |  2 ++
 simgear/math/SGVec4.hxx      |  2 ++
 3 files changed, 36 insertions(+)

diff --git a/simgear/math/SGIntersect.hxx b/simgear/math/SGIntersect.hxx
index 4d87d688..5951e158 100644
--- 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)
diff --git a/simgear/math/SGVec3.hxx b/simgear/math/SGVec3.hxx
index e33e8c05..9e07584e 100644
--- a/simgear/math/SGVec3.hxx
+++ b/simgear/math/SGVec3.hxx
@@ -96,6 +96,8 @@ public:
   { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; }
   explicit SGVec3(const osg::Vec3d& d)
   { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; }
+  explicit SGVec3(const SGVec2<T>& v2, const T& v3 = 0)
+  { data()[0] = v2[0]; data()[1] = v2[1]; data()[2] = v3; }
 
   /// Access by index, the index is unchecked
   const T& operator()(unsigned i) const
diff --git a/simgear/math/SGVec4.hxx b/simgear/math/SGVec4.hxx
index d610a6ca..7c714323 100644
--- a/simgear/math/SGVec4.hxx
+++ b/simgear/math/SGVec4.hxx
@@ -96,6 +96,8 @@ public:
   { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; }
   explicit SGVec4(const osg::Vec4d& d)
   { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; }
+  explicit SGVec4(const SGVec3<T>& v3, const T& v4 = 0)
+  { data()[0] = v3[0]; data()[1] = v3[1]; data()[2] = v3[2]; data()[3] = v4; }
 
 
   /// Access by index, the index is unchecked
-- 
2.39.5