From 8dc7432cae8008c1932d4bcb3571d8b82bc6712a Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 01 四月 2020 15:07:27 +0800
Subject: [PATCH] 地图修正
---
app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentaa.java | 91 ++++++++++++++++++++++++++++++++++++++++++---
1 files changed, 84 insertions(+), 7 deletions(-)
diff --git a/app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentaa.java b/app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentaa.java
index 6e7ace1..4847c6c 100644
--- a/app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentaa.java
+++ b/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());
}
--
Gitblit v1.8.0