diff --git a/pcbnew/tools/drawing_tool.cpp b/pcbnew/tools/drawing_tool.cpp
index fd00149b8d..a059366663 100644
--- a/pcbnew/tools/drawing_tool.cpp
+++ b/pcbnew/tools/drawing_tool.cpp
@@ -693,23 +693,9 @@ int DRAWING_TOOL::drawSegment( int aShape, bool aContinous )
         {
             // 45 degree lines
             if( linesAngle45 && aShape == S_SEGMENT )
-            {
-                VECTOR2D lineVector( wxPoint( cursorPos.x, cursorPos.y ) - graphic.GetStart() );
-                double angle = lineVector.Angle();
-
-                double newAngle = round( angle / ( M_PI / 4.0 ) ) * M_PI / 4.0;
-                VECTOR2D newLineVector = lineVector.Rotate( newAngle - angle );
-
-                // Snap the new line to the grid // TODO fix it, does not work good..
-                VECTOR2D newLineEnd = VECTOR2D( graphic.GetStart() ) + newLineVector;
-                VECTOR2D snapped = m_view->GetGAL()->GetGridPoint( newLineEnd );
-
-                graphic.SetEnd( wxPoint( snapped.x, snapped.y ) );
-            }
+                make45DegLine( &graphic );
             else
-            {
                 graphic.SetEnd( wxPoint( cursorPos.x, cursorPos.y ) );
-            }
 
             // Show a preview of the item
             preview.ViewUpdate( KIGFX::VIEW_ITEM::GEOMETRY );
@@ -847,27 +833,11 @@ int DRAWING_TOOL::drawZone( bool aKeepout )
 
         else if( evt->IsMotion() )
         {
-            helperLine->SetEnd( wxPoint( cursorPos.x, cursorPos.y ) );
-
             // 45 degree lines
             if( linesAngle45 )
-            {
-                VECTOR2D lineVector( wxPoint( cursorPos.x, cursorPos.y ) - helperLine->GetStart() );
-                double angle = lineVector.Angle();
-
-                double newAngle = round( angle / ( M_PI / 4.0 ) ) * M_PI / 4.0;
-                VECTOR2D newLineVector = lineVector.Rotate( newAngle - angle );
-
-                // Snap the new line to the grid // TODO fix it, does not work good..
-                VECTOR2D newLineEnd = VECTOR2D( helperLine->GetStart() ) + newLineVector;
-                VECTOR2D snapped = m_view->GetGAL()->GetGridPoint( newLineEnd );
-
-                helperLine->SetEnd( wxPoint( snapped.x, snapped.y ) );
-            }
+                make45DegLine( helperLine );
             else
-            {
                 helperLine->SetEnd( wxPoint( cursorPos.x, cursorPos.y ) );
-            }
 
             // Show a preview of the item
             preview.ViewUpdate( KIGFX::VIEW_ITEM::GEOMETRY );
@@ -888,6 +858,27 @@ int DRAWING_TOOL::drawZone( bool aKeepout )
 }
 
 
+void DRAWING_TOOL::make45DegLine( DRAWSEGMENT* aSegment ) const
+{
+    VECTOR2I cursorPos = m_controls->GetCursorPosition();
+
+    // Current line vector
+    VECTOR2D lineVector( wxPoint( cursorPos.x, cursorPos.y ) - aSegment->GetStart() );
+    double angle = lineVector.Angle();
+
+    // Find the closest angle, which is a multiple of 45 degrees
+    double newAngle = round( angle / ( M_PI / 4.0 ) ) * M_PI / 4.0;
+
+    VECTOR2D newLineVector = lineVector.Rotate( newAngle - angle );
+    VECTOR2D newLineEnd = VECTOR2D( aSegment->GetStart() ) + newLineVector;
+
+    // Snap the new line to the grid
+    newLineEnd = m_view->GetGAL()->GetGridPoint( newLineEnd );
+
+    aSegment->SetEnd( wxPoint( newLineEnd.x, newLineEnd.y ) );
+}
+
+
 void DRAWING_TOOL::setTransitions()
 {
     Go( &DRAWING_TOOL::DrawLine, COMMON_ACTIONS::drawLine.MakeEvent() );
diff --git a/pcbnew/tools/drawing_tool.h b/pcbnew/tools/drawing_tool.h
index baafa9dbc9..d34113e126 100644
--- a/pcbnew/tools/drawing_tool.h
+++ b/pcbnew/tools/drawing_tool.h
@@ -34,6 +34,7 @@ namespace KIGFX
 }
 class BOARD;
 class PCB_EDIT_FRAME;
+class DRAWSEGMENT;
 
 /**
  * Class DRAWING_TOOL
@@ -84,6 +85,11 @@ private:
     ///> @param aKeepout decides if the drawn polygon is a zone or a keepout area.
     int drawZone( bool aKeepout );
 
+    ///> Forces a DRAWSEGMENT to be drawn at multiple of 45 degrees. The origin
+    ///> stays the same, the end of the aSegment is modified according to the
+    ///> current cursor position.
+    void make45DegLine( DRAWSEGMENT* aSegment ) const;
+
     ///> Sets up handlers for various events.
     void setTransitions();