Use planned entries

diff --git a/src/uniform-sampling-template.cc b/src/uniform-sampling-template.cc
index 1ca82f69b46aadac47c4424f05e5e9be21ebbf31..346a2350e0c2705c84c96d6cf51a5868df51568b 100644 (file)
--- a/src/uniform-sampling-template.cc
+++ b/src/uniform-sampling-template.cc
@@ -9,6 +9,11 @@
         #define EPP RRTS
 #endif
 
+double edist(double x1, double y1, double x2, double y2)
+{
+        return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
+}
+
 std::chrono::high_resolution_clock::time_point TSTART_;
 std::chrono::high_resolution_clock::time_point TEND_;
 inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); }
@@ -48,7 +53,21 @@ int main()
                 jvi["slot"][0][3][0].asDouble(),
                 jvi["slot"][0][3][1].asDouble()
         );
-
+        if (psp.ps().parallel()) {
+                auto s = psp.ps();
+                double len1 = edist(s.x1(), s.y1(), s.x4(), s.y4());
+                if (len1 < 7.0) { // sc4_1
+                        jvi["entry"][0] = -744205.8107096809;
+                        jvi["entry"][1] = -1044324.0805424277;
+                        jvi["entry"][2] = 0.407050423340469;
+                        jvi["entry"][3] = 0.40715042334046897;
+                } else { // sc1_0
+                        jvi["entry"][0] = 6.688127114360318;
+                        jvi["entry"][1] = 0.9001931028769716;
+                        jvi["entry"][2] = 0.3416999999999787;
+                        jvi["entry"][3] = 0.40749999999997144;
+                }
+        }
         psp.fe();
 
         auto lm = psp.last_maneuver();