Add forward method implementation

diff --git a/src/psp.cc b/src/psp.cc
index feb8b167567b6662c1a4dc9ea00213660b906cff..d64a343a8ea188db515c1b56cdc5e0f11d8a3132 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -164,6 +164,20 @@ void PSPlanner::fer_perpendicular()
 {
 }
 
+bool PSPlanner::forward()
+{
+        double heading = this->ps().heading();
+        while (heading < 0) heading += 2 * M_PI;
+        if (!this->ps().parallel())
+                heading -= M_PI / 2;
+        double h = this->gc().h();
+        while (h < 0) h += 2 * M_PI;
+        if (-0.00001 < heading - h && heading - h < 0.00001)
+                return true;
+        else
+                return false;
+}
+
 PSPlanner::PSPlanner()
 {
 }