diff --git a/src/commons/MathUtil.h b/src/commons/MathUtil.h
index 6808cf85..dc03bccc 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);
+    }
 
 };