坐标

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
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
//
// Created by YY on 2019/11/4.
//
 
#include "driving_curve.h"
#include "../jni_log.h"
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"
#include "../master/comm_if.h"
#include "../native-lib.h"
#include "area_exam.h"
 
#include <vector>
#include <cstdlib>
#include <tuple>
 
using namespace std;
 
#define DEBUG(fmt, args...)     LOGD("<driving_curve> <%s>: " fmt, __func__, ##args)
 
static bool crashRedLine;
 
static bool CrashRedLine(prime_t &prime);
 
void StartDrivingCurve(prime_t &prime)
{
    DEBUG("进入曲线行驶场地");
 
    crashRedLine = false;
 
    MA_EnterMap(prime.examing_area.idx, MAP_TYPE_CURVE, 1);
}
 
void TestDrivingCurve(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_CURVE)
        return;
 
    if (CrashRedLine(prime)) {
        if (!crashRedLine) {
            // 车轮压边线，不合格
            DEBUG("车轮压边线");
            AddExamFault(20601);
        }
        crashRedLine = true;
    } else {
        crashRedLine = false;
    }
}
 
void StopDrivingCurve(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_CURVE)
        return;
    DEBUG("离开曲行驶是场地");
 
    prime.examing_area.type = MAP_TYPE_NONE;
}
 
 
// 曲线场地关键点
// 入口左右两点，出口左右两点，前半部大小圆圆心和直径，后半部大小圆圆心和直径，实际测量大小圆圆心偏差在20cm内
static bool CrashRedLine(prime_t &prime)
{
    // 判断是否在范围内，4个车轮只要满足在小圆外，大圆内，就说明没有压线
    PointF tires[] = {prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
                          prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
                          prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
                          prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]};
 
    for (int i = 0; i < 4; i++) {
        if (IntersectionOfLine(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_centre,
                               std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_centre, tires[i]) == REL_POS_RIGHT) {
            // 前半部分
            if (DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_centre, tires[i]) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_radius
                && DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_centre, tires[i]) < std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_radius) {
 
            } else {
                return true;
            }
        } else {
 
            if (DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_centre, tires[i]) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_radius
                && DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_centre, tires[i]) < std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_radius) {
 
            } else {
                return true;
            }
        }
    }
 
    return false;
}