From 92b36a8943dd942ef5603d4dbbd298c7fe2ce4b1 Mon Sep 17 00:00:00 2001
From: Jan Mrna <mj370@protonmail.com>
Date: Thu, 2 Oct 2025 20:48:05 +0200
Subject: [PATCH] Added DistanceTo to vec class

---
 cpp/src/math.hpp  |  5 +++++
 cpp/test/test.cpp | 32 ++++++++++++++++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/cpp/src/math.hpp b/cpp/src/math.hpp
index ad5e5c4..4f148c5 100644
--- a/cpp/src/math.hpp
+++ b/cpp/src/math.hpp
@@ -153,6 +153,11 @@ public:
     return std::sqrt(LengthSquared());
   }
 
+  T DistanceTo(const vec& b) const {
+    const vec& a = *this;
+    return (a - b).Length();
+  } 
+
   //
   // In-place vector operations
   //
diff --git a/cpp/test/test.cpp b/cpp/test/test.cpp
index b4b2ae5..c7d4523 100644
--- a/cpp/test/test.cpp
+++ b/cpp/test/test.cpp
@@ -376,6 +376,38 @@ TEST(vec, GetOrthogonal)
   ASSERT_FLOAT_EQ(v2[1], 5.0f);
 }
 
+TEST(vec, DistanceTo)
+{
+  // Test DistanceTo with 3D vectors
+  vec3 v1{0.0f, 0.0f, 0.0f};
+  vec3 v2{3.0f, 4.0f, 0.0f};
+  
+  float distance = v1.DistanceTo(v2);
+  ASSERT_FLOAT_EQ(distance, 5.0f); // 3-4-5 triangle
+  
+  // Distance should be symmetric
+  ASSERT_FLOAT_EQ(v2.DistanceTo(v1), distance);
+  
+  // Test with 2D vectors
+  vec2 a{1.0f, 1.0f};
+  vec2 b{4.0f, 5.0f};
+  
+  float distance_2d = a.DistanceTo(b);
+  ASSERT_FLOAT_EQ(distance_2d, 5.0f); // sqrt((4-1)² + (5-1)²) = sqrt(9+16) = 5
+  
+  // Distance to self should be zero
+  ASSERT_FLOAT_EQ(v1.DistanceTo(v1), 0.0f);
+  ASSERT_FLOAT_EQ(a.DistanceTo(a), 0.0f);
+  
+  // Test that original vectors are unchanged
+  ASSERT_FLOAT_EQ(v1[0], 0.0f);
+  ASSERT_FLOAT_EQ(v1[1], 0.0f);
+  ASSERT_FLOAT_EQ(v1[2], 0.0f);
+  ASSERT_FLOAT_EQ(v2[0], 3.0f);
+  ASSERT_FLOAT_EQ(v2[1], 4.0f);
+  ASSERT_FLOAT_EQ(v2[2], 0.0f);
+}
+
 TEST(vec, ChainedOperations) {
   vec2 a{1.0f, 2.0f};
   vec2 b{3.0f, 4.0f};