添加路径规划

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -2196,8 +2196,32 @@ std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffset(std::vect
     {
         return gpsTrace;
     }
-
     std::vector<iv::Point2D> trace;
+    bool use_new_method = false;
+    if  (use_new_method)
+    {
+
+        const int val = 100;
+        double V = gpsTrace[100].y;
+        for (int j = 0; j < gpsTrace.size(); j++)
+        {
+            if (j > val) trace.push_back(gpsTrace[j]);
+            else {
+                double t = (double)j / val;
+                double s = 1. - t*t*(3.-2.*t);
+                double ox = s * offset;
+                double oy = t * V;
+
+                Point2D pt(ox, oy);
+                pt.speed = gpsTrace[j].speed;
+                pt.v1 = gpsTrace[j].v1;
+                pt.v2 = gpsTrace[j].v2;
+                trace.push_back(pt);
+            }
+         }
+        return trace;
+    }
+
     for (int j = 0; j < gpsTrace.size(); j++)
     {
         double sumx1 = 0, sumy1 = 0, count1 = 0;