[RouterOpt]车前引导线拖尾问题优化2

core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java

@@ -141,7 +141,7 @@ public class IdentifyDataDrawer {
                     CallerLogger.INSTANCE.d(M_HMI + "arrow47", "uuid: " + data.getUuid() + " , 40~90差值范围 , 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading());
                 }
                 if (Math.abs(cacheData.getHeading() - data.getHeading()) > 90) {
-                    int degree = DrivingDirectionUtils.getDegreeOfCar2Poi(cacheData.getLongitude(), cacheData.getLatitude(), data.getLongitude(), data.getLatitude(), Double.valueOf(cacheData.getHeading()).intValue());
+                    double degree = DrivingDirectionUtils.getDegreeOfCar2Poi(cacheData.getLongitude(), cacheData.getLatitude(), data.getLongitude(), data.getLatitude(), cacheData.getHeading());
                     if (degree > 90) {
                         CallerLogger.INSTANCE.d(M_HMI + "arrow47", "uuid: " + data.getUuid() + " , 夹角 : " + degree + " , 修正 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading());
                         correctData = data.toBuilder().setHeading(cacheData.getHeading()).build();

core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/smp/SmallMapDirectionView.java

@@ -222,11 +222,9 @@ public class SmallMapDirectionView
         double poiLon = pointLocal.longitude;
         double poiLat = pointLocal.latitude;
         float carAngle = carLocal.getBearing();
-
         // 计算车辆与点之间的夹角
-        int diffAngle = DrivingDirectionUtils.getDegreeOfCar2Poi(
+        long diffAngle = DrivingDirectionUtils.getDegreeOfCar2Poi(
                 carLon, carLat, poiLon, poiLat, (int) carAngle);
-
         return diffAngle <= 90;
     }
 

core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/map/MogoLatLng.java

@@ -26,6 +26,8 @@ public class MogoLatLng implements Parcelable {
 
     public String provider;
 
+    public double acceleration;
+
     public MogoLatLng( double lat, double lon ) {
         this.lat = lat;
         this.lon = lon;

core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/map/MogoLocation.java

@@ -31,6 +31,8 @@ public class MogoLocation implements Cloneable, Parcelable {
     private String floor = "";
     private int gpsAccuracyStatus = 0;
     private int satellite = 0;
+    private long duration = 0;
+    private float acceleration = 0f;
 
     public float getBearing() {
         return bearing;
@@ -152,6 +154,22 @@ public class MogoLocation implements Cloneable, Parcelable {
         this.gpsAccuracyStatus = gpsAccuracyStatus;
     }
 
+    public long getDuration() {
+        return duration;
+    }
+
+    public void setDuration(long duration) {
+        this.duration = duration;
+    }
+
+    public void setAcceleration(float acceleration) {
+        this.acceleration = acceleration;
+    }
+
+    public float getAcceleration() {
+        return this.acceleration;
+    }
+
     public int getSatellite() {
         return satellite;
     }

libraries/mogo-map/src/main/java/com/mogo/map/AMapViewWrapper.java

@@ -12,6 +12,9 @@ import android.graphics.Point;
 import android.graphics.Rect;
 import android.location.Location;
 import android.os.Bundle;
+import android.os.Handler;
+import android.os.Looper;
+import android.os.SystemClock;
 import android.os.Trace;
 import android.text.TextUtils;
 import android.util.Log;
@@ -286,6 +289,9 @@ public class AMapViewWrapper implements IMogoMapView,
             mMapView.setOnCameraChangeListener(null);
             CallerLogger.INSTANCE.d(TAG, "map onDestroy");
         }
+        if (mLocationTask != null) {
+            mainHandler.removeCallbacks(mLocationTask);
+        }
     }
 
     @Override
@@ -666,18 +672,52 @@ public class AMapViewWrapper implements IMogoMapView,
         return ObjectUtils.transformCenterLine(MapDataApi.INSTANCE.getCenterLineInfo(lon, lat, angle));
     }
 
+
+
+
+    private static class LocationTask implements Runnable {
+
+        private MogoLocation location;
+
+        public void setMoGoLocation(MogoLocation location) {
+            this.location = location;
+        }
+
+        @Override
+        public void run() {
+            if (location != null) {
+                CallerMapLocationListenerManager.INSTANCE.invokeMapLocationChangeListener(location);
+                location = null;
+            }
+        }
+    }
+
+    private volatile LocationTask mLocationTask;
+
+    private final Handler mainHandler = new Handler(Looper.getMainLooper());
+
     @Override
     public void onLocationChanged(@NotNull com.zhidaoauto.map.sdk.open.location.MogoLocation location) {
-        MogoLocation mLastLocation = ObjectUtils.fromLocation(location);
+        MogoLocation currentLocation = ObjectUtils.fromLocation(location);
-        UiThreadHandler.post(() -> CallerMapLocationListenerManager.INSTANCE.invokeMapLocationChangeListener(mLastLocation));
+        if (Looper.myLooper() == Looper.getMainLooper()) {
+            CallerMapLocationListenerManager.INSTANCE.invokeMapLocationChangeListener(currentLocation);
+        } else  {
+            if (mLocationTask == null) {
+                mLocationTask = new LocationTask();
+            }
+            mLocationTask.setMoGoLocation(currentLocation);
+            mainHandler.removeCallbacks(mLocationTask);
+            mainHandler.post(mLocationTask);
+        }
 
+        long start = SystemClock.elapsedRealtime();
         Location sysLocation = new Location(location.getProvider());
         sysLocation.setAltitude(location.getAltitude());
         sysLocation.setLatitude(location.getLat());
         sysLocation.setLongitude(location.getLon());
         sysLocation.setProvider(location.getProvider());
         sysLocation.setAccuracy(location.getAcceleration());
-        sysLocation.setTime(location.getDuration());
+        sysLocation.setTime(location.duration);
         sysLocation.setBearing((float) location.getHeading());
         sysLocation.setSpeed(location.getSpeed());
 
@@ -691,7 +731,6 @@ public class AMapViewWrapper implements IMogoMapView,
                     .putString(SharedPrefsConstants.LOCATION_LONGITUDE, String.valueOf(location.getLon()));
         }
 
-
         if (MogoCarLocationChangedListenerRegister.getInstance().getListener() != null) {
             MogoCarLocationChangedListenerRegister.getInstance().getListener().onCarLocationChanged2(sysLocation);
         }
@@ -711,6 +750,7 @@ public class AMapViewWrapper implements IMogoMapView,
                 CallerLogger.INSTANCE.d(TAG, "同步定位：" + GsonUtils.toJson(location));
             }
         }
+        Log.d("TTTTT", "xxxxx:" + (SystemClock.elapsedRealtime() - start));
     }
 
     @Override

libraries/mogo-map/src/main/java/com/mogo/map/utils/ObjectUtils.java

@@ -171,6 +171,8 @@ public class ObjectUtils {
         location.setDistrict(aLocation.getDistrict());
         location.setProvince(aLocation.getProvince());
         location.setAdCode(aLocation.getAdCode());
+        location.setDuration(aLocation.getDuration());
+        location.setAcceleration(aLocation.getAcceleration());
         //        location.setAccuracy( aLocation.getAccuracy() );
         //        location.setTime( aLocation.getTime() );
 //        location.setLocationDetail( aLocation.getLocationDetail() );

modules/mogo-module-common/src/main/java/com/mogo/module/common/utils/DrivingDirectionUtils.java

@@ -2,6 +2,8 @@ package com.mogo.module.common.utils;
 
 import static java.lang.Math.PI;
 
+import android.util.Pair;
+
 /**
  * author : donghongyu
  * e-mail : 1358506549@qq.com
@@ -11,40 +13,35 @@ import static java.lang.Math.PI;
  */
 public class DrivingDirectionUtils {
 
-    /**
+
-     * 计算车辆行驶方向 与 poi点到车辆的连线 间的夹角
+    public static long getDegreeOfCar2Poi(double x1, double y1, double x2, double y2, double x1_angle) {
-     *
+        Pair<Double, Double> newPoint = calculateNewPoint(x1, y1, x1_angle, 10);
-     * @param carLon   车辆位置 lon
+        if (newPoint != null) {
-     * @param carLat   车辆位置 lat
+            double angle = getAngle(x1, y1, newPoint.first, newPoint.second, x2, y2);
-     * @param poiLon   poi 位置 lon
+            return Math.round(angle + 0.5);
-     * @param poiLat   poi 位置 lat
-     * @param carAngle 车辆行驶方向
-     * @return
-     */
-    public static int getDegreeOfCar2Poi(double carLon, double carLat, double poiLon, double poiLat, int carAngle) {
-        int poiAngle = 0;
-        // 以子午线作为y轴 计算两点的余切 再将余切值转化为角度
-        double _angle = Math.atan2(Math.abs(carLon - poiLon), Math.abs(carLat - poiLat)) * (180 / PI);
-        if (poiLon > carLon) {
-            // poi 在 车辆位置的第1象限
-            if (poiLat > carLat) {
-                poiAngle = (int) _angle;
-            }
-            // poi 在 车辆位置的第2象限
-            else {
-                poiAngle = 180 - (int) _angle;
-            }
-        } else {
-            // poi 在 车辆位置的第3象限
-            if (poiLat < carLat) {
-                poiAngle = (int) _angle + 180;
-            }
-            // poi 在 车辆位置的第4象限
-            else {
-                poiAngle = 360 - (int) _angle;
-            }
         }
-        return calculationAngle(poiAngle, carAngle);
+        return 0;
+    }
+
+    private static double getAngle(double sx, double sy, double x1, double y1, double x2, double y2) {
+        x1 = x1 - sx;
+        y1 = y1 - sy;
+        x2 = x2 - sx;
+        y2 = y2 - sy;
+        double product = x1 * x2 + y1 * y2;
+        double radians = Math.acos(product / (Math.sqrt(Math.pow(x1, 2.0) + Math.pow(y1, 2.0)) * Math.sqrt(Math.pow(x2, 2.0) + Math.pow(y2, 2.0))));
+        return Math.toDegrees(radians);
+    }
+
+    public static Pair<Double, Double> calculateNewPoint(double x, double y, double angle, double distance) {
+        if (distance == 0) {
+            return null;
+        }
+        double radian = Math.toRadians(angle);
+        double radianCandle = Math.toRadians(90.0 - angle);
+        double nX = x + distance * Math.sin(radian) / 100000.0;
+        double nY = y + distance * Math.sin(radianCandle) / 100000.0;
+        return Pair.create(nX, nY);
     }
 
     /**

modules/mogo-module-service/src/main/java/com/mogo/module/service/routeoverlay/MogoRouteOverlayManager.java

@@ -3,6 +3,7 @@ package com.mogo.module.service.routeoverlay;
 import static com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant.M_OLD_ROUTE;
 
 import android.content.Context;
+import android.os.SystemClock;
 
 import androidx.annotation.NonNull;
 
@@ -17,12 +18,12 @@ import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListener
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotPlanningListenerManager;
 import com.mogo.eagle.core.function.call.map.CallerMapLocationListenerManager;
 import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger;
-import com.mogo.module.common.utils.DrivingDirectionUtils;
 
 import org.jetbrains.annotations.NotNull;
 import org.jetbrains.annotations.Nullable;
 
 import java.util.ArrayList;
+import java.util.LinkedList;
 import java.util.List;
 
 import mogo.telematics.pad.MessagePad;
@@ -38,7 +39,7 @@ public class MogoRouteOverlayManager implements
     private int STATUS_AUTOPILOT = 0;//0 非自动驾驶 ; 1 自动驾驶
     private MogoLatLng mEnding;
     private MogoLocation mLocation;
-    private List<MogoLatLng> mTrajectoryList = new ArrayList<>();
+    private final List<MogoLatLng> mTrajectoryList = new ArrayList<>();
 
     private MogoRouteOverlayManager(Context context) {
         mContext = context;
@@ -62,37 +63,52 @@ public class MogoRouteOverlayManager implements
         return sInstance;
     }
 
-    private void intiDrawer() {
+    private  StringBuilder trajectoryMsg = null;
-        RouteOverlayDrawer.getInstance(mContext).initDraw();
-    }
 
     @Override
-    public void onAutopilotTrajectory(List<MessagePad.TrajectoryPoint> trajectoryInfos) {
+    public void onAutopilotTrajectory(List<MessagePad.TrajectoryPoint> items) {
-        if (trajectoryInfos == null || trajectoryInfos.size() == 0) {
+        long start = SystemClock.elapsedRealtime();
-            return;
+        try {
-        }
+            if (items == null || items.size() == 0) {
-        StringBuilder builder = new StringBuilder();
+                return;
-        builder.append("{");
-        builder.append(System.currentTimeMillis()).append(";");
-        builder.append(mLocation.getLongitude()).append(";");
-        builder.append(mLocation.getLatitude()).append(";");
-        builder.append(mLocation.getAltitude()).append(";");
-        builder.append(mLocation.getBearing()).append(";");
-        builder.append(mLocation.getSpeed()).append(";");
-        List<MogoLatLng> mogoLatLngs = new ArrayList<>();
-        for (int i = 0; i < trajectoryInfos.size(); i++) {
-            // 临时解决车尾拖线问题，丢弃距离车最近的几个经纬度，原因是惯性导航的中心靠近车尾，会导致经纬度靠近尾部，且两个数据不同频
-            if (i > 5) {
-
-                MessagePad.TrajectoryPoint a = trajectoryInfos.get(i);
-                builder.append(a.getLongitude()).append(",");
-                builder.append(a.getLatitude()).append(",");
-                mogoLatLngs.add(new MogoLatLng(a.getLatitude(), a.getLongitude()));
             }
+            if (trajectoryMsg == null) {
+                trajectoryMsg = new StringBuilder(256);
+            }
+            if (trajectoryMsg.length() > 0) {
+                trajectoryMsg.setLength(0);
+            }
+            if (mLocation == null) {
+                return;
+            }
+            trajectoryMsg.append("{");
+            trajectoryMsg.append(SystemClock.elapsedRealtime()).append(";");
+            trajectoryMsg.append(mLocation.getLongitude()).append(";");
+            trajectoryMsg.append(mLocation.getLatitude()).append(";");
+            trajectoryMsg.append(mLocation.getAltitude()).append(";");
+            trajectoryMsg.append(mLocation.getBearing()).append(";");
+            trajectoryMsg.append(mLocation.getSpeed()).append(";");
+            List<MogoLatLng> points = new ArrayList<>();
+            for (int i = 0; i < items.size(); i++) {
+                // 临时解决车尾拖线问题，丢弃距离车最近的几个经纬度，原因是惯性导航的中心靠近车尾，会导致经纬度靠近尾部，且两个数据不同频
+                MessagePad.TrajectoryPoint a = items.get(i);
+                double lon = a.getLongitude();
+                double lat = a.getLatitude();
+                trajectoryMsg.append(lon).append(",");
+                trajectoryMsg.append(lat).append(",");
+                MogoLatLng latLng = new MogoLatLng(lat, lon);
+                latLng.acceleration = a.getAcceleration();
+                points.add(latLng);
+            }
+            synchronized (mTrajectoryList) {
+                mTrajectoryList.clear();
+                mTrajectoryList.addAll(points);
+            }
+            trajectoryMsg.append("}");
+            CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, trajectoryMsg.toString());
+        } finally {
+            CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "--- onAutopilotTrajectory -- cost:" + (SystemClock.elapsedRealtime() - start) + " ms");
         }
-
-        mTrajectoryList = mogoLatLngs;
-        builder.append("}");
     }
 
     @Override
@@ -136,7 +152,7 @@ public class MogoRouteOverlayManager implements
         if (arrivalNotification == null) {
             return;
         }
-        CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "onArriveAt data : " + arrivalNotification.toString());
+        CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "onArriveAt data : " + arrivalNotification);
 //        //演示模式下 到达终点将忽略 引导线绘制 选项关闭
 //        FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData = false;
     }
@@ -149,66 +165,37 @@ public class MogoRouteOverlayManager implements
 
     @Override
     public void onLocationChanged(@Nullable MogoLocation location) {
-//        mLocation = location;
+        long start = SystemClock.elapsedRealtime();
-//        if (mTrajectoryList.isEmpty()) {
+        boolean isExcept = false;
-//            return;
+        try {
-//        }
+            if (location == null) {
-//        CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG , "onLocationChanged : " + mTrajectoryList.size());
+                isExcept = true;
-//        ArrayList list = new ArrayList();
+                return;
-//        for (MogoLatLng latLng : mTrajectoryList) {
+            }
-//            if (!isPointOnCarFront(mLocation, latLng)) {
+            if (mLocation != null && mLocation.getLongitude() == location.getLongitude() && mLocation.getLatitude() == location.getLatitude()) {
-//                list.add(latLng);
+                return;
-//            }
+            }
-//        }
+            LinkedList<MogoLatLng> points;
-//
+            synchronized (mTrajectoryList) {
-//        StringBuilder builder = new StringBuilder();
+                points = new LinkedList<>(mTrajectoryList);
-//        for (int i = 0; i < list.size(); i++) {
+            }
-//            MogoLatLng latLng = (MogoLatLng) list.get(i);
+            if (points.isEmpty()) {
-//            builder.append(latLng.getLon()).append(",");
+                isExcept = true;
-//            builder.append(latLng.getLat()).append(",");
+                return;
-//        }
+            }
-//        CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG , "onLocationChanged : " + GsonUtils.toJson(builder.toString()));
+            if (points.size() < 2) {
-//        CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG ,"onLocationChanged size = " + list.size() + "---TrajectoryData = " + FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData + "----" + STATUS_AUTOPILOT);
+                isExcept = true;
-//        if (FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData || STATUS_AUTOPILOT == 1) {
+                return;
-//            RouteOverlayDrawer.getInstance(mContext).drawTrajectoryList(list);
+            }
-//        }
+            if (FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData || STATUS_AUTOPILOT == 1) {
-
+                RouteOverlayDrawer.getInstance(mContext).drawTrajectoryList(location, points);
-        mLocation = location;
+            }
-        List<MogoLatLng> temp = mTrajectoryList;
+        } finally {
-        if (temp.isEmpty()) {
+            mLocation = location;
-            return;
+            if (isExcept) {
-        }
+                RouteOverlayDrawer.getInstance(mContext).setVisible(false);
-//        CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG , "onLocationChanged: size = "+ mTrajectoryList.size()+" ----- "+mLocation.getLongitude()+"-"+mLocation.getLatitude());
+            }
-        ArrayList list = new ArrayList();
+            CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "--- onLocationChanged -- cost:" + (SystemClock.elapsedRealtime() - start) + " ms");
-        MogoLatLng carlatLng = new MogoLatLng(CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84Lat(),CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84Lon());
-        list.add(carlatLng);
-        for (MogoLatLng latLng : temp) {
-//            if(!isPointOnCarFront(mLocation,latLng)){
-            list.add(latLng);
-//            }
-        }
-//        CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "TrajectoryData = " + FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData + "---status = " + STATUS_AUTOPILOT + "----size = " + list.size());
-        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);
-//        CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG,"diffAngle:"+diffAngle);
-        if (diffAngle <= 90) {
-            return true;
-        } else {
-            return false;
         }
     }
 

modules/mogo-module-service/src/main/java/com/mogo/module/service/routeoverlay/RouteOverlayDrawer.java

@@ -6,8 +6,12 @@ import android.content.Context;
 import android.graphics.Bitmap;
 import android.graphics.BitmapFactory;
 import android.graphics.Color;
+import android.os.Handler;
+import android.os.HandlerThread;
+import android.os.SystemClock;
 
 import com.mogo.eagle.core.data.map.MogoLatLng;
+import com.mogo.eagle.core.data.map.MogoLocation;
 import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager;
 import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger;
 import com.mogo.eagle.core.utilcode.util.ColorUtils;
@@ -18,20 +22,23 @@ import com.mogo.map.marker.MogoMarkerOptions;
 import com.mogo.map.overlay.IMogoOverlayManager;
 import com.mogo.map.overlay.IMogoPolyline;
 import com.mogo.map.overlay.MogoPolylineOptions;
+import com.mogo.module.common.utils.DrivingDirectionUtils;
 import com.mogo.module.service.R;
 
 import java.util.ArrayList;
+import java.util.LinkedList;
 import java.util.List;
 
 public class RouteOverlayDrawer {
 
     private static final String TAG = "MogoRouteOverlayManager";
 
-    private IMogoPolyline mMoGoPolyline;
+    private volatile IMogoPolyline mMoGoPolyline;
     // 连接线参数
     private final MogoPolylineOptions mPolylineOptions;
-    // 线路径集合
+
-    private final List<MogoLatLng> mPolylinePointList;
+    private Handler mRenderHandler;
+
     // 渐变色
     private final List<Integer> mPolylineColors;
     private final Bitmap endingBitmap;
@@ -46,14 +53,30 @@ public class RouteOverlayDrawer {
         mPolylineOptions = new MogoPolylineOptions();
         mPolylineOptions.zIndex(75000f);
         mPolylineOptions.setGps(true);
-        // 绘制路径集合
-        mPolylinePointList = new ArrayList<>();
         // 引导线颜色,
         mPolylineColors = new ArrayList<>();
         mContext = context;
         mogoOverlayManager = MogoOverlayManager.getInstance();
         endingBitmap = BitmapFactory.decodeResource(context.getResources(),
                 R.drawable.icon_route_ending);
+
+        List<Integer> list = new ArrayList<>();
+        int[] startColor = ColorUtils.hexToArgb("#CC64C3EA");
+        int[] endColor = ColorUtils.hexToArgb("#0064C3EA");
+        list.add(Color.argb(startColor[0], startColor[1], startColor[2], startColor[3]));
+        list.add(Color.argb(endColor[0], endColor[1], endColor[2], endColor[3]));
+        mPolylineColors.addAll(list);
+
+        // 线条粗细，渐变，渐变色值
+        mPolylineOptions.width(20).useGradient(true).colorValues(mPolylineColors);
+        HandlerThread renderTask = new HandlerThread("xxxx") {
+            @Override
+            protected void onLooperPrepared() {
+                super.onLooperPrepared();
+                mRenderHandler = new Handler(getLooper());
+            }
+        };
+        renderTask.start();
     }
 
     public static RouteOverlayDrawer getInstance(Context context) {
@@ -103,7 +126,6 @@ public class RouteOverlayDrawer {
         if (mMoGoPolyline != null) {
             mMoGoPolyline.remove();
             mMoGoPolyline = null;
-            mPolylinePointList.clear();
             mPolylineColors.clear();
         }
     }
@@ -114,57 +136,91 @@ public class RouteOverlayDrawer {
         MogoMarkerManager.getInstance(mContext).removeMarkers(markerType);
     }
 
+    private class RenderTask implements Runnable {
+        private volatile LinkedList<MogoLatLng> routeList;
+        private volatile MogoLocation location;
 
-    public void drawTrajectoryList(List<MogoLatLng> routeList) {
+        public void setData(MogoLocation location, LinkedList<MogoLatLng> routeList) {
-//        clearMogoRouteOverlay();
+            this.location = location;
-        mPolylinePointList.clear();
+            this.routeList = routeList;
-        if (routeList != null && routeList.size() > 0) {
+        }
-            for (MogoLatLng latLng : routeList) {
+
-                mPolylinePointList.add(latLng);
+        @Override
+        public void run() {
+            try {
+                long drawStart = SystemClock.elapsedRealtime();
+                double lon = CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84Lon();
+                double lat = CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84Lat();
+                if (lon == 0.0 || lat == 0.0) {
+                    return;
+                }
+                LinkedList<MogoLatLng> points = this.routeList;
+                MogoLocation location = this.location;
+                if (location != null && points != null && points.size() > 0) {
+                    int i = 0;
+                    int max = Math.min(20, points.size() / 2);
+                    while (i++ < max) {
+                        MogoLatLng first = points.peek();
+                        if (first == null) {
+                            continue;
+                        }
+                        if (!isPointOnCarFront(lon, lat, location.getBearing(), first.lon, first.lat)) {
+                            points.poll();
+                        }
+                    }
+                    points.addFirst(new MogoLatLng(lat, lon));
+                    if (mMoGoPolyline == null || mMoGoPolyline.isDestroyed()) {
+                        mPolylineOptions.points(points);
+                        mMoGoPolyline = mogoOverlayManager.addPolyline(mPolylineOptions);
+                    } else {
+                        mPolylineOptions.points(points);
+                        mMoGoPolyline.setOption(mPolylineOptions);
+                    }
+                    if (mMoGoPolyline != null && !mMoGoPolyline.isDestroyed() && !mMoGoPolyline.isVisible()) {
+                        mMoGoPolyline.setVisible(true);
+                    }
+                } else  {
+                    if(mMoGoPolyline != null) {
+                        mMoGoPolyline.setVisible(false);
+                    }
+                }
+                long drawEnd = SystemClock.elapsedRealtime();
+                CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "drawTrajectoryList cost : " + (drawEnd - drawStart));
+            } catch (Throwable t) {
+                CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "drawTrajectoryList error : " + t);
             }
-            mPolylineColors.clear();
+        }
-//            mPolylineColors.addAll(ColorUtils.gradientAlpha_("#FF2AAFFD", "#7b2965ED", "#002965ED", mPolylinePointList.size()));
-            List<Integer> list = new ArrayList<>();
-//            list =  ColorUtils.gradientAlpha("#FF2AAFFD", "#002965ED", mPolylinePointList.size());
-            int[] startColor = ColorUtils.hexToArgb("#CC64C3EA");
-            int[] endColor = ColorUtils.hexToArgb("#0064C3EA");
-            list.add(Color.argb(startColor[0], startColor[1], startColor[2], startColor[3]));
-            list.add(Color.argb(endColor[0], endColor[1], endColor[2], endColor[3]));
 
-            mPolylineColors.addAll(list);
+        private boolean isPointOnCarFront(double car_lon, double car_lat, double car_head, double lon, double lat) {
-            // 线条粗细，渐变，渐变色值
+            long start = SystemClock.elapsedRealtime();
-            mPolylineOptions.width(20).useGradient(true).colorValues(mPolylineColors);
+            try {
-            if (mMoGoPolyline == null || mMoGoPolyline.isDestroyed()) {
+                // 计算车辆与点之间的夹角
-                mPolylineOptions.points(mPolylinePointList);
+                long diffAngle = DrivingDirectionUtils.getDegreeOfCar2Poi(
-                mMoGoPolyline = mogoOverlayManager.addPolyline(mPolylineOptions);
+                        car_lon, car_lat, lon, lat, car_head);
-            } else {
+                CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "isPointOnCarFront: angle->" + diffAngle);
-                mPolylineOptions.points(mPolylinePointList);
+                return diffAngle < 90;
-                mMoGoPolyline.setOption(mPolylineOptions);
+            } finally {
+                CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "isPointOnCarFront cost:" + (SystemClock.elapsedRealtime() - start) + "ms");
             }
         }
     }
 
-    public void initDraw() {
+    private volatile RenderTask mRenderTask;
-        mPolylinePointList.clear();
-        MogoLatLng latLng = new MogoLatLng(
-                CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84Lat(),
-                CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84Lon());
-        mPolylinePointList.add(latLng);
-        mPolylinePointList.add(latLng);
-        mPolylineColors.clear();
-        long start = System.currentTimeMillis();
-        List<Integer> list = new ArrayList<>();
-        list = ColorUtils.gradientAlpha("#FF2AAFFD", "#002965ED", mPolylinePointList.size());
 
-        mPolylineColors.addAll(list);
+    public void drawTrajectoryList(MogoLocation carLoc, LinkedList<MogoLatLng> routeList) {
-        // 线条粗细，渐变，渐变色值
+        if (mRenderTask == null) {
-        mPolylineOptions.width(12).useGradient(true).colorValues(mPolylineColors);
+            mRenderTask = new RenderTask();
-        if (mMoGoPolyline == null || mMoGoPolyline.isDestroyed()) {
+        }
-            mPolylineOptions.points(mPolylinePointList);
+        mRenderTask.setData(carLoc, routeList);
-            mMoGoPolyline = mogoOverlayManager.addPolyline(mPolylineOptions);
+        if (mRenderHandler != null) {
-        } else {
+            mRenderHandler.removeCallbacks(mRenderTask);
-            mPolylineOptions.points(mPolylinePointList);
+            mRenderHandler.post(mRenderTask);
-            mMoGoPolyline.setOption(mPolylineOptions);
+        }
+    }
+
+    public void setVisible(boolean isVisible) {
+        if (mMoGoPolyline != null && !mMoGoPolyline.isDestroyed()) {
+            mMoGoPolyline.setVisible(isVisible);
         }
     }
 }