diff --git a/src/g2_spline.cpp b/src/g2_spline.cpp
index 90846f31ab52e34eaca9f39b9fef12399cfebed9..acb107ca394e750d47e5a172a0d31b97b0f31fbf 100644
--- a/src/g2_spline.cpp
+++ b/src/g2_spline.cpp
@@ -1,7 +1,6 @@
 #include "g2_spline.h"
 #include <math.h>
 #include <boost/bind.hpp>
-#include <iostream>
 
 CG2Spline::CG2Spline()
 {
@@ -114,9 +113,9 @@ CG2Spline::CG2Spline(TPoint &start,TPoint &end)
 void CG2Spline::normalize_angle(double &angle)
 {
   if(angle>3.14159)
-    angle-=3.14159;
+    angle-=2*3.14159;
   else if(angle<-3.14159)
-    angle+=3.14159;
+    angle+=2*3.14159;
 }
 
 double CG2Spline::find_parameter(double length)