Add dist and distSqr functions

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

diff --git a/simgear/math/SGVec3.hxx b/simgear/math/SGVec3.hxx
index afe7049d80e125038e9096d55b41c62e1ea13058..e6ea50aa8ec00d909953cfb2e36afba4f4675c40 100644 (file)
--- a/simgear/math/SGVec3.hxx
+++ b/simgear/math/SGVec3.hxx
@@ -10,10 +10,9 @@
 // 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 Library General Public
-// License along with this library; if not, write to the
-// Free Software Foundation, Inc., 59 Temple Place - Suite 330,
-// Boston, MA  02111-1307, USA.
+// 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.
 //
 
 #ifndef SGVec3_H
@@ -44,14 +43,6 @@ public:
   /// make sure it has at least 3 elements
   explicit SGVec3(const T* data)
   { _data[0] = data[0]; _data[1] = data[1]; _data[2] = data[2]; }
-  /// Constructor. Initialize by a geodetic coordinate
-  /// Note that this conversion is relatively expensive to compute
-  SGVec3(const SGGeod& geod)
-  { SGGeodesy::SGGeodToCart(geod, *this); }
-  /// Constructor. Initialize by a geocentric coordinate
-  /// Note that this conversion is relatively expensive to compute
-  SGVec3(const SGGeoc& geoc)
-  { SGGeodesy::SGGeocToCart(geoc, *this); }
 
   /// Access by index, the index is unchecked
   const T& operator()(unsigned i) const
@@ -126,11 +117,58 @@ public:
   static SGVec3 e3(void)
   { return SGVec3(0, 0, 1); }
 
+  /// Constructor. Initialize by a geodetic coordinate
+  /// Note that this conversion is relatively expensive to compute
+  static SGVec3 fromGeod(const SGGeod& geod);
+  /// Constructor. Initialize by a geocentric coordinate
+  /// Note that this conversion is relatively expensive to compute
+  static SGVec3 fromGeoc(const SGGeoc& geoc);
+
 private:
   /// The actual data
   T _data[3];
 };
 
+template<>
+inline
+SGVec3<double>
+SGVec3<double>::fromGeod(const SGGeod& geod)
+{
+  SGVec3<double> cart;
+  SGGeodesy::SGGeodToCart(geod, cart);
+  return cart;
+}
+
+template<>
+inline
+SGVec3<float>
+SGVec3<float>::fromGeod(const SGGeod& geod)
+{
+  SGVec3<double> cart;
+  SGGeodesy::SGGeodToCart(geod, cart);
+  return SGVec3<float>(cart(0), cart(1), cart(2));
+}
+
+template<>
+inline
+SGVec3<double>
+SGVec3<double>::fromGeoc(const SGGeoc& geoc)
+{
+  SGVec3<double> cart;
+  SGGeodesy::SGGeocToCart(geoc, cart);
+  return cart;
+}
+
+template<>
+inline
+SGVec3<float>
+SGVec3<float>::fromGeoc(const SGGeoc& geoc)
+{
+  SGVec3<double> cart;
+  SGGeodesy::SGGeocToCart(geoc, cart);
+  return SGVec3<float>(cart(0), cart(1), cart(2));
+}
+
 /// Unary +, do nothing ...
 template<typename T>
 inline
@@ -258,6 +296,20 @@ equivalent(const SGVec3<T>& v1, const SGVec3<T>& v2)
   return equivalent(v1, v2, tol, tol);
 }
 
+/// The euclidean distance of the two vectors
+template<typename T>
+inline
+T
+dist(const SGVec3<T>& v1, const SGVec3<T>& v2)
+{ return norm(v1 - v2); }
+
+/// The squared euclidean distance of the two vectors
+template<typename T>
+inline
+T
+distSqr(const SGVec3<T>& v1, const SGVec3<T>& v2)
+{ SGVec3<T> tmp = v1 - v2; return dot(tmp, tmp); }
+
 #ifndef NDEBUG
 template<typename T>
 inline