From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 22 Jul 2019 14:08:53 +0000 (+0200)
Subject: Add forward method implementation
X-Git-Tag: SOLID~6^2~4
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/commitdiff_plain/9f054880ed32fefb0d44cd70464e55d3ba6eca67

Add forward method implementation
---

diff --git a/src/psp.cc b/src/psp.cc
index feb8b16..d64a343 100644
--- 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()
 {
 }