diff --git a/VirtualRobot/MathTools.cpp b/VirtualRobot/MathTools.cpp
index 111dbe8e9b79cf37f9067073977724ff294f877d..606da5a3ce6d66e1f9559e85245e8dfa63e2d820 100644
--- a/VirtualRobot/MathTools.cpp
+++ b/VirtualRobot/MathTools.cpp
@@ -1322,27 +1322,6 @@ MathTools::Line VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::intersectPlanes( const Pl
 	return MathTools::Line(pos,dir);
 }
 
-Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::nearestPointOnLine( const Line &l, const Eigen::Vector3f &p )
-{
-	if (!l.isValid())
-		return Eigen::Vector3f::Zero();
-
-	Eigen::Vector3f lp = p - l.p;
-
-	float lambda = l.d.dot(lp);
-
-	Eigen::Vector3f res = l.p + lambda*l.d;
-	return res;
-}
-
-float VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::distPointLine( const Line &l, const Eigen::Vector3f &p )
-{
-	if (!l.isValid())
-		return -1.0f;
-	Eigen::Vector3f p2 = nearestPointOnLine(l,p);
-	return (p2-p).norm();
-}
-
 float VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::rad2deg( float rad )
 {
 	static const float c = (float)(180.0/M_PI);
diff --git a/VirtualRobot/MathTools.h b/VirtualRobot/MathTools.h
index 67d8256c22babc19a93715265b5fbe9d7dbb6561..ed73d37211c58cb6239cddf9369351c51f4c5200 100644
--- a/VirtualRobot/MathTools.h
+++ b/VirtualRobot/MathTools.h
@@ -175,9 +175,12 @@ namespace MathTools
 	//! Create a floor plane
 	Plane VIRTUAL_ROBOT_IMPORT_EXPORT getFloorPlane();
 
-	struct Line
+	template<typename VectorT>
+	struct BaseLine
 	{
-		Line(const Eigen::Vector3f &point, const Eigen::Vector3f &dir)
+		BaseLine() {}
+
+		BaseLine(const VectorT &point, const VectorT &dir)
 		{
 			p = point;
 			d = dir;
@@ -185,14 +188,21 @@ namespace MathTools
 				d.normalize();
 		}
 
-		Line(const Line& line){this->p = line.p;this->d = line.d;}
+		BaseLine(const BaseLine<VectorT>& line)
+		: p(line.p)
+		, d(line.d)
+		{
+		}
 
 		bool isValid() const {return d.norm()>1e-9;}
 
-		Eigen::Vector3f	p;	// point
-		Eigen::Vector3f d;	// direction (unit length)
+		VectorT p;	// point
+		VectorT d;	// direction (unit length)
 	};
 
+    typedef BaseLine<Eigen::Vector3f> Line;
+    typedef BaseLine<Eigen::Vector2f> Line2D;
+
     struct Segment
     {
         Segment()
@@ -424,8 +434,29 @@ namespace MathTools
     IntersectionResult VIRTUAL_ROBOT_IMPORT_EXPORT intersectOOBBPlane(const OOBB &oobb, const Plane &plane, std::vector<Eigen::Vector3f> &storeResult);
 
 	//! Returns nearest point to p on line l
-	Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT nearestPointOnLine(const Line &l, const Eigen::Vector3f &p);
-	float VIRTUAL_ROBOT_IMPORT_EXPORT distPointLine(const Line &l, const Eigen::Vector3f &p);
+	template<typename VectorT>
+	inline VectorT nearestPointOnLine( const BaseLine<VectorT> &l, const VectorT &p )
+	{
+		if (!l.isValid())
+			return VectorT::Zero();
+
+		VectorT lp = p - l.p;
+
+		float lambda = l.d.dot(lp);
+
+		VectorT res = l.p + lambda*l.d;
+		return res;
+	}
+
+	//! Returns the distance of vector p to line l
+	template<typename VectorT>
+	inline float distPointLine( const BaseLine<VectorT> &l, const VectorT &p )
+	{
+		if (!l.isValid())
+			return -1.0f;
+		VectorT p2 = nearestPointOnLine<VectorT>(l, p);
+		return (p2-p).norm();
+	}
 
 	//! Check if three points are collinear
 	bool VIRTUAL_ROBOT_IMPORT_EXPORT collinear(const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p3 );