From 9c102e1390e908b70c08d462a0fb2870de4a0d58 Mon Sep 17 00:00:00 2001
From: Sooyoung Cha <ellen2g@snu.ac.kr>
Date: Mon, 2 Dec 2024 17:51:32 +0900
Subject: [PATCH] MathUtil squaredist (#910)

---
 src/commons/MathUtil.h | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/commons/MathUtil.h b/src/commons/MathUtil.h
index 6808cf857..dc03bccc8 100644
--- a/src/commons/MathUtil.h
+++ b/src/commons/MathUtil.h
@@ -256,6 +256,13 @@ class MathUtil {
         return sum;
     }
 
+    static float squareDist(const float xx, const float xy, const float xz,
+            const float yx, const float yy, const float yz){
+        float d1 = xx - yx;
+        float d2 = xy - yy;
+        float d3 = xz - yz;
+        return (d1 * d1 + d2 * d2 + d3 * d3);
+    }
 
 };