diff --git a/src/TMS_Kalman.cpp b/src/TMS_Kalman.cpp
index 005d3f03..fe7f8e41 100644
--- a/src/TMS_Kalman.cpp
+++ b/src/TMS_Kalman.cpp
@@ -396,3 +396,32 @@ TVectorD TMS_Kalman::GetNoiseVector(TMS_KalmanNode Node) {
 
   return toy;
 }
+
+
+void TMS_Kalman::SetStartDirection(double ax, double ay)
+{
+  // Defining a vector by two slopes is a bit odd;
+  // it's not possible to define vectors in the x/y plane for example.
+  // Consider the dector (dx/dz, dy/dz, dz/dz), the z component is 1 by construction,
+  // and thus we normalise this
+  double mag  = sqrt(ax*ax + ay*ay + 1);
+  double mag2 = ax*ax + ay*ay + 1; // square of mag
+
+  StartDirection[0]=ax/mag;
+  StartDirection[1]=ay/mag;
+  StartDirection[2]=sqrt(1 - ax*ax/mag2 - ay*ay/mag2);
+}
+
+void TMS_Kalman::SetEndDirection(double ax, double ay)
+{
+  // Defining a vector by two slopes is a bit odd;
+  // it's not possible to define vectors in the x/y plane for example.
+  // Consider the dector (dx/dz, dy/dz, dz/dz), the z component is 1 by construction,
+  // and thus we normalise this
+  double mag  = sqrt(ax*ax + ay*ay + 1);
+  double mag2 = ax*ax + ay*ay + 1; // square of mag
+
+  StartDirection[0]=ax/mag;
+  StartDirection[1]=ay/mag;
+  StartDirection[2]=sqrt(1 - ax*ax/mag2 - ay*ay/mag2);
+}
diff --git a/src/TMS_Kalman.h b/src/TMS_Kalman.h
index d1a35c9f..ff6eb4fe 100644
--- a/src/TMS_Kalman.h
+++ b/src/TMS_Kalman.h
@@ -214,8 +214,8 @@ class TMS_Kalman {
 
     void SetMomentum(double mom) {momentum = mom;}
     // Set direction unit vectors from only x and y slope
-    void SetStartDirection(double ax, double ay) {StartDirection[0]=ax; StartDirection[1]=ay; StartDirection[2]=sqrt(1 - ax*ax - ay*ay);};
-    void SetEndDirection  (double ax, double ay) {EndDirection[0]=ax;   EndDirection[1]=ay;   EndDirection[2]=sqrt(1 - ax*ax - ay*ay);};
+    void SetStartDirection(double ax, double ay);// {StartDirection[0]=ax; StartDirection[1]=ay; StartDirection[2]=sqrt(1 - ax*ax - ay*ay);};
+    void SetEndDirection  (double ax, double ay);// {EndDirection[0]=ax;   EndDirection[1]=ay;   EndDirection[2]=sqrt(1 - ax*ax - ay*ay);};
 
     // Set position unit vectors
     void SetStartPosition(double ax, double ay, double az) {Start[0]=ax; Start[1]=ay; Start[2]=az;};