MathUtil squaredist (#910)

Author: sooyoung-cha

Diff for: 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);
+    }
 
 };