Add one cusp when slot left with speed < 0

diff --git a/src/psp.cc b/src/psp.cc
index e2c6ee1585cf903bab80be200b5870ceb2aaf0f6..44c4c88fa63c00d614f1bea358a89a5017152661 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -412,6 +412,10 @@ void PSPlanner::fer_parallel()
                         this->cusps_.push_back(this->cc());
                 }
         }
+        if (this->cc().st() < 0) {
+                this->c_++;
+                this->cusps_.push_back(this->cc());
+        }
 }
 
 void PSPlanner::fer_perpendicular()