diff --git a/MathLib/Point3d.cpp b/MathLib/Point3d.cpp
index 43a2fb6c69be07f44d45e32a4137076b9beb39e5..935f0ebef6fae48593bd60eb7876438f1db9ee6b 100644
--- a/MathLib/Point3d.cpp
+++ b/MathLib/Point3d.cpp
@@ -16,5 +16,12 @@ Point3d::Point3d() : x_({0, 0, 0}) {}
 
 Point3d::Point3d(std::array<double, 3> x) : x_(x[0], x[1], x[2]) {}
 
+double sqrDist(MathLib::Point3d const& p0, MathLib::Point3d const& p1)
+{
+    auto const v = Eigen::Map<Eigen::Vector3d const>(p0.data());
+    auto const u = Eigen::Map<Eigen::Vector3d const>(p1.data());
+    return (v - u).squaredNorm();
+}
+
 extern const Point3d ORIGIN{{{0.0, 0.0, 0.0}}};
 }  // namespace MathLib
diff --git a/MathLib/Point3d.h b/MathLib/Point3d.h
index c490cccf9e9ea89ee99b6f8960c1bad1043d6314..03f83e956390d4c2b1fe0e1b70175b425a63e564 100644
--- a/MathLib/Point3d.h
+++ b/MathLib/Point3d.h
@@ -140,12 +140,7 @@ inline MathLib::Point3d operator*(MATRIX const& mat, MathLib::Point3d const& p)
 
 /** Computes the squared dist between the two points p0 and p1.
  */
-inline double sqrDist(MathLib::Point3d const& p0, MathLib::Point3d const& p1)
-{
-    return (p0[0] - p1[0]) * (p0[0] - p1[0]) +
-           (p0[1] - p1[1]) * (p0[1] - p1[1]) +
-           (p0[2] - p1[2]) * (p0[2] - p1[2]);
-}
+double sqrDist(MathLib::Point3d const& p0, MathLib::Point3d const& p1);
 
 /** Equality of Point3d's up to an epsilon.
  */