坐标

yy1717

2024-02-28 27fc91fbe8f88b6885356e68828cfe1ce1db7601

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
//
// Created by YY on 2023/4/20.
//
 
#include <tuple>
#include "train.h"
#include "../driver_test.h"
#include "../jni_log.h"
 
#define DEBUG(fmt, args...)     LOGD("<train> <%s>: " fmt, __func__, ##args)
 
// 得到点后，相应场地的坐标变换
// coordinate transformation
void train(prime_t &prime)
{
    park_button_map_t az;
 
    az.points.resize(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points.size());
 
    // 以2号点(左侧车库入口角)为原点
    double dx = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].X;
    double dy = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].Y;
 
    for (int i = 0; i < std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points.size(); ++i) {
        az.points[i] = {.X = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[i].X - dx,
                        .Y = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[i].Y - dy};
    }
 
    // 以2---->3线为-Y轴，旋转
    double angle = YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3]);
 
    if (angle > 180) {
        angle = angle - 180;
    } else {
        angle = angle + 180;
    }
 
    for (int i = 0; i < az.points.size(); ++i) {
        if (i != 2) {
            az.points[i] = rotatePoint(az.points[i], az.points[2], angle);
        }
    }
 
    // 记录车辆位置经过坐标变换后的值
    PointF car = {.X = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_REAR]].X - dx,
                  .Y = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_REAR]].Y - dy};
 
    car = rotatePoint(car, az.points[2], angle);
 
    DEBUG("坐标变换后的 %f,%f", car.X, car.Y);
}