diff --git a/MathLib/MathTools.h b/MathLib/MathTools.h
index f037e46526a1a58fdf73a965a39175d91b46a325..cd9eeb80a28c4ab3f4db2426542a94210911635b 100644
--- a/MathLib/MathTools.h
+++ b/MathLib/MathTools.h
@@ -23,7 +23,7 @@
 #include <omp.h>
 #endif
 
-#include "TemplatePoint.h"
+#include "Point3d.h"
 
 namespace MathLib
 {
@@ -107,13 +107,6 @@ void crossProd (const double u[3], const double v[3], double r[3]);
 double calcProjPntToLineAndDists(const double p[3], const double a[3],
                                  const double b[3], double &lambda, double &d0);
 
-template <typename POINT_T>
-typename POINT_T::FP_T sqrDist(POINT_T const& p0, POINT_T const& p1)
-{
-	typename POINT_T::FP_T const v[3] = {p1[0]-p0[0], p1[1]-p0[1], p1[2]-p0[2]};
-	return MathLib::scalarProduct<typename POINT_T::FP_T,3>(v,v);
-}
-
 template <typename T, std::size_t DIM>
 bool operator==(TemplatePoint<T,DIM> const& a, TemplatePoint<T,DIM> const& b)
 {
@@ -121,6 +114,14 @@ bool operator==(TemplatePoint<T,DIM> const& a, TemplatePoint<T,DIM> const& b)
 	return (sqr_dist < pow(std::numeric_limits<T>::epsilon(),2));
 }
 
+/// Computes the squared dist between the two points p0 and p1.
+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 scalarProduct<double,3>(v,v);
+}
+
 /** squared dist between double arrays p0 and p1 (size of arrays is 3) */
 inline
 double sqrDist(const double* p0, const double* p1)