地图修正

yy1717

2020-04-01 8dc7432cae8008c1932d4bcb3571d8b82bc6712a

地图修正

 app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentaa.java

@@ -45,7 +45,6 @@
import safeluck.drive.evaluation.util.CThreadPoolExecutor;
import safeluck.drive.evaluation.util.FileUtil;


public class RoadDriveMapFragmentaa extends SupportFragment {

    private static final int ALL_MAP = 100;
@@ -195,16 +194,90 @@
        });
    }

    public void DrawMap(List<Double> mainAnt, final double[][] map, final double[][] car,
    class PointF {
        double x;
        double y;

        public PointF(double x, double y) {
            this.x = x;
            this.y = y;
        }

        double getX() {
            return x;
        }

        double getY() {
            return y;
        }

        void setX(double x) {
            this.x = x;
        }

        void setY(double y) {
            this.y = y;
        }
    }

    public PointF rotatePoint(PointF oldPoint, PointF centre, double degree) {
        PointF newPoint = new PointF(0, 0);

        newPoint.setX( (oldPoint.getX()-centre.getX())*Math.cos(Math.toRadians(degree)) - (oldPoint.getY()-centre.getY())*Math.sin(Math.toRadians(degree)) + centre.getX() );
        newPoint.setY ( (oldPoint.getX()-centre.getX())*Math.sin(Math.toRadians(degree)) + (oldPoint.getY()-centre.getY())*Math.cos(Math.toRadians(degree)) + centre.getY() );

        return newPoint;
    }

    public void DrawMap(double yaw, List<Double> mainAnt, double[][] map, double[][] car,
                        List<Integer>body, List<Integer> tire, List<RoadExamMap.MapsBean> maps) {
        if (canvas2 == null || bmp == null) {
            return;
        }
        double base_x = 300, base_y = 20;
        double max_x = 0, min_x = 0, max_y = 0, min_y = 0;
        long scale_x, scale_y;

        Log.d(TAG, "DrawMap map size " + map.length + " car size " + car.length);

        PointF mainPoint = new PointF(mainAnt.get(0), mainAnt.get(1));

        Log.d(TAG, String.format("主天线 X = %f Y = %f 旋转角度 %f", mainPoint.getX(), mainPoint.getY(), yaw));



        for (int x = 0; x < map.length; x++) {
            PointF oldPoint = new PointF(map[x][0], map[x][1]);
            PointF newPoint = rotatePoint(oldPoint, mainPoint, yaw );
            map[x][0] = newPoint.getX();
            map[x][1] = newPoint.getY();

            map[x][0] = map[x][0] - mainPoint.getX();
            map[x][1] = map[x][1] - mainPoint.getY();
            map[x][1] = -map[x][1];
        }

        for (int i = 0; i < car.length; i++) {
            PointF oldPoint = new PointF(car[i][0], car[i][1]);
            PointF newPoint = rotatePoint(oldPoint, mainPoint, yaw );
            car[i][0] = newPoint.getX();
            car[i][1] = newPoint.getY();

            car[i][0] = car[i][0] - mainPoint.getX();
            car[i][1] = car[i][1] - mainPoint.getY();
            car[i][1] = -car[i][1];
        }

        scale_x = Math.round(Math.abs(80 / Math.sqrt(Math.pow(car[0][0], 2) + Math.pow(car[0][1], 2)) ));
        scale_y = scale_x;

        base_x = bmp.getWidth() / 2;
        base_y = bmp.getHeight() * 7 / 10;

        Log.d(TAG, String.format("DrawMapAll scale_x = %d 车头据 = %f", scale_x, Math.sqrt(Math.pow(car[0][0], 2) + Math.pow(car[0][1], 2))));


/*
        for (int i = 0; i < map.length; i++) {
            if (i == 0) {
                max_x = map[0][0];
@@ -250,6 +323,8 @@

        Log.i(TAG,String.format("screen_width - base_x - 10=%f,screen_height - base_y - 10=%f,max_x - min_x=%f,max_y - min_y=%f",screen_width - base_x - 10,screen_height - base_y - 10
        ,max_x - min_x,max_y - min_y));


        long scale_x = Math.round((screen_width - base_x - 10) / (max_x - min_x));
        long scale_y = Math.round((screen_height - base_y - 10) / (max_y - min_y));

@@ -262,7 +337,7 @@
        }

        Log.d(TAG, "DrawMap scale_x " + scale_x + " scale_y " + scale_y);

*/
        canvas2.drawColor(Color.WHITE);

        if (paint == null || canvas2==null){
@@ -271,7 +346,7 @@
        paint.setStyle(Paint.Style.FILL_AND_STROKE);
        paint.setColor(Color.BLUE);

        canvas2.drawCircle((float) (base_x + (mainAnt.get(0) - min_x) * scale_x), (float) (base_y + (0 - mainAnt.get(1) - min_y) * scale_y), 2, paint);
        canvas2.drawCircle((float) (base_x), (float) (base_y), 2, paint);

        paint.setColor(Color.RED);
        canvas2.drawCircle((float) (base_x + (car[tire.get(0)][0] - min_x) * scale_x), (float) (base_y + (car[tire.get(0)][1] - min_y) * scale_y), 2.5f, paint);
@@ -524,7 +599,7 @@
                        if ((i % 2) == 0) {
                            car[line][0] = points.get(i);
                        } else {
                            double value = 0 - points.get(i);
                            double value = points.get(i);
                            car[line][1] = value;
                            line++;
                        }
@@ -541,6 +616,8 @@
                    List<Integer> body = timeCarPos.getBody();

                    List<Integer> tire = new ArrayList<>();

                    double yaw = timeCarPos.getHeading();

                    tire.add(tire1.get(0));
                    tire.add(tire2.get(0));
@@ -582,7 +659,7 @@
                                    map[map_line][0] = points.get(i);

                                } else {
                                    double value = 0 - points.get(i);
                                    double value = points.get(i);
                                    map[map_line][1] = value;
                                    map_line++;
                                }
@@ -590,7 +667,7 @@
                        }
                    }

                    DrawMap(mainAnt,map, car, body, tire,  examMaps.getMaps());
                    DrawMap(yaw, mainAnt,map, car, body, tire,  examMaps.getMaps());

                }