Solved a bug in the angle normalization function.

src/g2_spline.cpp

 #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)