diff --git a/MathLib/Point3d.h b/MathLib/Point3d.h
index 4f548648f75d4be7a112f89a490edf7e597f661c..3a0754b0e2eb76b143ffba88f70052c0ad4bc43f 100644
--- a/MathLib/Point3d.h
+++ b/MathLib/Point3d.h
@@ -13,6 +13,7 @@
 #pragma once
 
 #include <limits>
+#include <Eigen/Dense>
 
 #include "mathlib_export.h"
 
@@ -47,8 +48,9 @@ inline MathLib::Point3d operator*(MATRIX const& mat, MathLib::Point3d const& p)
 inline
 double sqrDist(MathLib::Point3d const& p0, MathLib::Point3d const& p1)
 {
-    const double v[3] = {p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]};
-    return MathLib::scalarProduct<double,3>(v,v);
+    return (Eigen::Map<Eigen::Vector3d>(const_cast<double*>(p0.getCoords())) -
+            Eigen::Map<Eigen::Vector3d>(const_cast<double*>(p1.getCoords())))
+        .squaredNorm();
 }
 
 /// Computes the squared distance between the orthogonal projection of the two