Add guess goal car declaration, ut

[hubacji1/psp.git] / src / psp.cc

diff --git a/src/psp.cc b/src/psp.cc
index f2ff07e3d0dedd619611cebad283ddcae9181d4f..f43b9ef246ffd1770ab1cdd2d05a8fecc3640acf 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -19,6 +19,24 @@ bool PSPlanner::collide()
         return std::get<0>(::collide(bc, ps));
 }
 
+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;
+}
+
+void PSPlanner::guess_gc()
+{
+}
+
 bool PSPlanner::left()
 {
        double lfx = this->cc().lfx();
@@ -286,20 +304,6 @@ 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()
 {
 }