From c1c8a7224a2322d84bc5a70d5a6aecb204502daf Mon Sep 17 00:00:00 2001
From: lianglihui <lianglihui@zhidaoauto.com>
Date: Wed, 2 Mar 2022 14:15:14 +0800
Subject: [PATCH] =?UTF-8?q?254=20=E4=BF=AE=E6=94=B9=E8=BD=A6=E5=89=8D?=
 =?UTF-8?q?=E5=BC=95=E5=AF=BC=E7=BA=BF=E7=BB=98=E5=88=B6=E9=80=BB=E8=BE=91?=
 =?UTF-8?q?=20=20=E4=BB=A5=E4=BC=98=E5=8C=96=E8=BD=A6=E5=89=8D=E5=BC=95?=
 =?UTF-8?q?=E5=AF=BC=E7=BA=BF=E5=B1=95=E7=8E=B0=E6=95=88=E6=9E=9C?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../routeoverlay/MogoRouteOverlayManager.java | 39 +++++++++++++++++--
 1 file changed, 35 insertions(+), 4 deletions(-)

diff --git a/modules/mogo-module-service/src/main/java/com/mogo/module/service/routeoverlay/MogoRouteOverlayManager.java b/modules/mogo-module-service/src/main/java/com/mogo/module/service/routeoverlay/MogoRouteOverlayManager.java
index d580ac9aa1..cfe28af1b8 100644
--- a/modules/mogo-module-service/src/main/java/com/mogo/module/service/routeoverlay/MogoRouteOverlayManager.java
+++ b/modules/mogo-module-service/src/main/java/com/mogo/module/service/routeoverlay/MogoRouteOverlayManager.java
@@ -1,7 +1,6 @@
 package com.mogo.module.service.routeoverlay;
 
 import android.content.Context;
-import android.util.Log;
 
 import com.mogo.eagle.core.data.autopilot.ADASTrajectoryInfo;
 import com.mogo.eagle.core.data.autopilot.AutopilotGuardianStatusInfo;
@@ -19,6 +18,7 @@ import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotPlanningListen
 import com.mogo.eagle.core.function.call.map.CallerMapLocationListenerManager;
 import com.mogo.eagle.core.utilcode.mogo.logger.Logger;
 import com.mogo.module.common.MogoApisHandler;
+import com.mogo.module.common.utils.DrivingDirectionUtils;
 
 import org.jetbrains.annotations.NotNull;
 import org.jetbrains.annotations.Nullable;
@@ -36,6 +36,8 @@ public class MogoRouteOverlayManager implements
     private int STATUS_AUTOPILOT = 0;//0 非自动驾驶 ; 1 自动驾驶
     private MogoLatLng mEnding;
     private MogoLocation mLocation;
+    private List<MogoLatLng> mTrajectoryList = new ArrayList<>();
+
     private MogoRouteOverlayManager(Context context) {
         mContext = context;
     }
@@ -92,9 +94,8 @@ public class MogoRouteOverlayManager implements
                 mogoLatLngs.add(new MogoLatLng(a.getLat(), a.getLon()));
             }
         }
-        if (FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData || STATUS_AUTOPILOT == 1){
-            RouteOverlayDrawer.getInstance(mContext).drawTrajectoryList(mogoLatLngs);
-        }
+
+        mTrajectoryList = mogoLatLngs;
         builder.append("}");
         //Log.d(TAG,builder.toString());
     }
@@ -157,6 +158,36 @@ public class MogoRouteOverlayManager implements
     @Override
     public void onLocationChanged(@Nullable MogoLocation location) {
         mLocation = location;
+        if (mTrajectoryList.isEmpty()){
+            return;
+        }
+        ArrayList  list = new ArrayList();
+        for (MogoLatLng latLng:mTrajectoryList) {
+            if(!isPointOnCarFront(mLocation,latLng)){
+                list.add(latLng);
+            }
+        }
+        if (FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData || STATUS_AUTOPILOT == 1){
+            RouteOverlayDrawer.getInstance(mContext).drawTrajectoryList(list);
+        }
+    }
+
+    public boolean isPointOnCarFront(MogoLocation carLocal, MogoLatLng pointLocal) {
+        double carLon = carLocal.getLongitude();
+        double carLat = carLocal.getLatitude();
+        double poiLon = pointLocal.lon;
+        double poiLat = pointLocal.lat;
+        float carAngle = carLocal.getBearing();
+
+        // 计算车辆与点之间的夹角
+        int diffAngle = DrivingDirectionUtils.getDegreeOfCar2Poi(
+                carLon, carLat, poiLon, poiLat, (int) carAngle);
+//        Log.e(TAG,"diffAngle:"+diffAngle);
+        if (diffAngle <= 90) {
+            return true;
+        } else {
+            return false;
+        }
     }
 
 }