diff --git a/src/DotRecast.Core/Numerics/RcVec3f.cs b/src/DotRecast.Core/Numerics/RcVec3f.cs
index f558d4c..bde01a5 100644
--- a/src/DotRecast.Core/Numerics/RcVec3f.cs
+++ b/src/DotRecast.Core/Numerics/RcVec3f.cs
@@ -274,12 +274,17 @@ namespace DotRecast.Core.Numerics
         /// @param[in] v2 A point. [(x, y, z)]
         /// @return The distance between the two points.
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static float Distance(RcVec3f v1, RcVec3f v2)
+        public static float Distance(RcVec3f value1, RcVec3f value2)
         {
-            float dx = v2.X - v1.X;
-            float dy = v2.Y - v1.Y;
-            float dz = v2.Z - v1.Z;
-            return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
+            float distanceSquared = DistanceSquared(value1, value2);
+            return MathF.Sqrt(distanceSquared);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static float DistanceSquared(RcVec3f value1, RcVec3f value2)
+        {
+            var difference = value1 - value2;
+            return Dot(difference, difference);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
diff --git a/test/DotRecast.Core.Test/Vector3Tests.cs b/test/DotRecast.Core.Test/Vector3Tests.cs
index bc9a155..e2e111a 100644
--- a/test/DotRecast.Core.Test/Vector3Tests.cs
+++ b/test/DotRecast.Core.Test/Vector3Tests.cs
@@ -91,7 +91,6 @@ public class Vector3Tests
         Assert.That(v3.X, Is.EqualTo(v33.X));
         Assert.That(v3.Y, Is.EqualTo(v33.Y));
         Assert.That(v3.Z, Is.EqualTo(v33.Z));
-        
     }
 
     [Test]
@@ -101,12 +100,47 @@ public class Vector3Tests
         var v1 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle());
         var array1 = new float[3];
         v1.CopyTo(array1);
-        
+
         var v11 = new RcVec3f(v1.X, v1.Y, v1.Z);
         var array11 = new float[3];
         v11.CopyTo(array11);
-        
+
         Assert.That(array1, Is.EqualTo(array11));
     }
 
+    [Test]
+    [Repeat(100000)]
+    public void TestVectorDistance()
+    {
+        var v1 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle());
+        var v2 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle());
+        var d3 = Vector3.Distance(v1, v2);
+        var d4 = Vector3.DistanceSquared(v1, v2);
+
+        var v11 = new RcVec3f(v1.X, v1.Y, v1.Z);
+        var v22 = new RcVec3f(v2.X, v2.Y, v2.Z);
+        var d33 = RcVec3f.Distance(v11, v22);
+        var d44 = RcVec3f.DistanceSquared(v11, v22);
+
+        Assert.That(d3, Is.EqualTo(d33));
+        Assert.That(d4, Is.EqualTo(d44));
+    }
+    
+    // [Test]
+    // [Repeat(100000)]
+    // public void TestVectorLerp()
+    // {
+    //     var amt = Random.Shared.NextSingle();
+    //     var v1 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle());
+    //     var v2 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle());
+    //     var v3 = Vector3.Lerp(v1, v2, amt);
+    //
+    //     var v11 = new RcVec3f(v1.X, v1.Y, v1.Z);
+    //     var v22 = new RcVec3f(v2.X, v2.Y, v2.Z);
+    //     var v33 = RcVec3f.Lerp(v11, v22, amt);
+    //
+    //     Assert.That(v3.X, Is.EqualTo(v33.X));
+    //     Assert.That(v3.Y, Is.EqualTo(v33.Y));
+    //     Assert.That(v3.Z, Is.EqualTo(v33.Z));
+    // }
 }
\ No newline at end of file