//
|
// 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);
|
}
|