From 4f9e057358b1d3a6e0f15a4274fb68ba207e338a Mon Sep 17 00:00:00 2001
From: Fraser Waters <frassle@gmail.com>
Date: Sat, 26 Aug 2017 13:12:12 +0100
Subject: [PATCH] Add Distance[Squared] methods to Vector3[d]

---
 src/OpenTK/Math/Vector3.cs         | 48 ++++++++++++++++++++++++++++++
 src/OpenTK/Math/Vector3d.cs        | 48 ++++++++++++++++++++++++++++++
 tests/OpenTK.Tests/Vector3Tests.fs | 10 +++++++
 3 files changed, 106 insertions(+)

diff --git a/src/OpenTK/Math/Vector3.cs b/src/OpenTK/Math/Vector3.cs
index 6e2d1c11..24a0a342 100644
--- a/src/OpenTK/Math/Vector3.cs
+++ b/src/OpenTK/Math/Vector3.cs
@@ -563,6 +563,54 @@ namespace OpenTK
             result.Z = vec.Z < min.Z ? min.Z : vec.Z > max.Z ? max.Z : vec.Z;
         }
 
+        /// <summary>
+        /// Compute the euclidean distance between two vectors.
+        /// </summary>
+        /// <param name="vec1">The first vector</param>
+        /// <param name="vec2">The second vector</param>
+        /// <returns>The distance</returns>
+        public static float Distance(Vector3 vec1, Vector3 vec2)
+        {
+            float result;
+            Distance(ref vec1, ref vec2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Compute the euclidean distance between two vectors.
+        /// </summary>
+        /// <param name="vec1">The first vector</param>
+        /// <param name="vec2">The second vector</param>
+        /// <param name="result">The distance</param>
+        public static void Distance(ref Vector3 vec1, ref Vector3 vec2, out float result)
+        {
+            result = (float)Math.Sqrt((vec2.X - vec1.X) * (vec2.X - vec1.X) + (vec2.Y - vec1.Y) * (vec2.Y - vec1.Y) + (vec2.Z - vec1.Z) * (vec2.Z - vec1.Z));
+        }
+
+        /// <summary>
+        /// Compute the squared euclidean distance between two vectors.
+        /// </summary>
+        /// <param name="vec1">The first vector</param>
+        /// <param name="vec2">The second vector</param>
+        /// <returns>The squared distance</returns>
+        public static float DistanceSquared(Vector3 vec1, Vector3 vec2)
+        {
+            float result;
+            DistanceSquared(ref vec1, ref vec2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Compute the squared euclidean distance between two vectors.
+        /// </summary>
+        /// <param name="vec1">The first vector</param>
+        /// <param name="vec2">The second vector</param>
+        /// <param name="result">The squared distance</param>
+        public static void DistanceSquared(ref Vector3 vec1, ref Vector3 vec2, out float result)
+        {
+            result = (vec2.X - vec1.X) * (vec2.X - vec1.X) + (vec2.Y - vec1.Y) * (vec2.Y - vec1.Y) + (vec2.Z - vec1.Z) * (vec2.Z - vec1.Z);
+        }
+
         /// <summary>
         /// Scale a vector to unit length
         /// </summary>
diff --git a/src/OpenTK/Math/Vector3d.cs b/src/OpenTK/Math/Vector3d.cs
index 9100f9cd..0216e460 100644
--- a/src/OpenTK/Math/Vector3d.cs
+++ b/src/OpenTK/Math/Vector3d.cs
@@ -557,6 +557,54 @@ namespace OpenTK
             result.Z = vec.Z < min.Z ? min.Z : vec.Z > max.Z ? max.Z : vec.Z;
         }
 
+        /// <summary>
+        /// Compute the euclidean distance between two vectors.
+        /// </summary>
+        /// <param name="vec1">The first vector</param>
+        /// <param name="vec2">The second vector</param>
+        /// <returns>The distance</returns>
+        public static double Distance(Vector3d vec1, Vector3d vec2)
+        {
+            double result;
+            Distance(ref vec1, ref vec2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Compute the euclidean distance between two vectors.
+        /// </summary>
+        /// <param name="vec1">The first vector</param>
+        /// <param name="vec2">The second vector</param>
+        /// <param name="result">The distance</param>
+        public static void Distance(ref Vector3d vec1, ref Vector3d vec2, out double result)
+        {
+            result = Math.Sqrt((vec2.X - vec1.X) * (vec2.X - vec1.X) + (vec2.Y - vec1.Y) * (vec2.Y - vec1.Y) + (vec2.Z - vec1.Z) * (vec2.Z - vec1.Z));
+        }
+
+        /// <summary>
+        /// Compute the squared euclidean distance between two vectors.
+        /// </summary>
+        /// <param name="vec1">The first vector</param>
+        /// <param name="vec2">The second vector</param>
+        /// <returns>The squared distance</returns>
+        public static double DistanceSquared(Vector3d vec1, Vector3d vec2)
+        {
+            double result;
+            DistanceSquared(ref vec1, ref vec2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Compute the squared euclidean distance between two vectors.
+        /// </summary>
+        /// <param name="vec1">The first vector</param>
+        /// <param name="vec2">The second vector</param>
+        /// <param name="result">The squared distance</param>
+        public static void DistanceSquared(ref Vector3d vec1, ref Vector3d vec2, out double result)
+        {
+            result = (vec2.X - vec1.X) * (vec2.X - vec1.X) + (vec2.Y - vec1.Y) * (vec2.Y - vec1.Y) + (vec2.Z - vec1.Z) * (vec2.Z - vec1.Z);
+        }
+
         /// <summary>
         /// Scale a vector to unit length
         /// </summary>
diff --git a/tests/OpenTK.Tests/Vector3Tests.fs b/tests/OpenTK.Tests/Vector3Tests.fs
index c12c0f18..18cb89a9 100644
--- a/tests/OpenTK.Tests/Vector3Tests.fs
+++ b/tests/OpenTK.Tests/Vector3Tests.fs
@@ -124,6 +124,16 @@ module Vector3 =
 
             Assert.Equal(lsq, v.LengthSquared)
 
+    [<Properties(Arbitrary = [| typeof<OpenTKGen> |])>]
+    module Distance =
+        [<Property>]
+        let ``Distance(a, b) = (b - a).Length`` (a : Vector3, b : Vector3) =
+            Assert.ApproximatelyEqual(Vector3.Distance(a, b), (b - a).Length)
+
+        [<Property>]
+        let ``DistanceSquared(a, b) = (b - a).LengthSquared`` (a : Vector3, b : Vector3) =
+            Assert.ApproximatelyEqual(Vector3.DistanceSquared(a, b), (b - a).LengthSquared)
+
     [<Properties(Arbitrary = [| typeof<OpenTKGen> |])>]
     module Normalization =
         //