//
|
// 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 <vector>
|
#include <cstdlib>
|
|
using namespace std;
|
|
#define DEBUG(fmt, args...) LOGD("<driving_curve> <%s>: " fmt, __func__, ##args)
|
|
enum {
|
DRIVING_ON_CURVE
|
};
|
|
const uint32_t STOP_CAR_TIME = D_SEC(2);
|
|
static bool testing = false;
|
static uint32_t stopTimepoint = 0;
|
|
static bool reportStopCarTimeout;
|
static int prevMoveDirect;
|
static bool crashRedLine;
|
static struct {
|
int leftStart;
|
int leftEnd;
|
int rightStart;
|
int rightEnd;
|
} calcZone;
|
|
static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car);
|
static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car);
|
|
void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
|
{
|
DEBUG("进入曲线行驶场地");
|
|
testing = true;
|
|
prevMoveDirect = moveDirect;
|
if (moveDirect == 0) {
|
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
}
|
reportStopCarTimeout = false;
|
crashRedLine = false;
|
|
calcZone.leftStart = calcZone.leftEnd = calcZone.rightStart = calcZone.rightEnd = -1;
|
}
|
|
int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
Line start, end;
|
Polygon zone;
|
|
if (calcZone.leftStart == -1) {
|
calcZone.leftStart = 0;
|
}
|
if (calcZone.rightStart == -1) {
|
calcZone.rightStart = 0;
|
}
|
|
start.X1 = map->point[calcZone.leftStart].X;
|
start.Y1 = map->point[calcZone.leftStart].Y;
|
|
start.X2 = map->point[calcZone.rightStart].X;
|
start.Y2 = map->point[calcZone.rightStart].Y;
|
|
zone.num = 4;
|
zone.point = (PointF *) malloc(zone.num * sizeof(PointF));
|
zone.point[0] = car->carXY[car->left_front_tire[TIRE_OUTSIDE]];
|
zone.point[1] = car->carXY[car->right_front_tire[TIRE_OUTSIDE]];
|
zone.point[2] = car->carXY[car->right_rear_tire[TIRE_OUTSIDE]];
|
zone.point[3] = car->carXY[car->left_rear_tire[TIRE_OUTSIDE]];
|
|
if (IntersectionOf(start, &zone) == GM_None) {
|
|
}
|
|
free(zone.point);
|
|
return testing ? 1 : 0;
|
}
|
|
// 车轮是否压边线
|
static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
|
{
|
bool ret = false;
|
|
car_model_cache_t *prev_car = GetCarModelCache(1);
|
if (prev_car == NULL)
|
return false;
|
|
// 按4个轮子的外侧计算
|
Line front_left_tire_track, front_right_tire_track, rear_left_tire_track, rear_right_tire_track;
|
MakeLine(&front_left_tire_track, &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], &prev_car->points[car->desc->front_left_tire[TIRE_OUTSIDE]]);
|
MakeLine(&front_right_tire_track, &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], &prev_car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
|
MakeLine(&rear_left_tire_track, &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]], &prev_car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]]);
|
MakeLine(&rear_right_tire_track, &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]], &prev_car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);
|
|
Line line;
|
|
line.X1 = left->point[0].X;
|
line.Y1 = left->point[0].Y;
|
for (int i = 1; i < left->num; ++i) {
|
line.X2 = left->point[i].X;
|
line.Y2 = left->point[i].Y;
|
|
if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
|
IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
|
IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
|
IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
|
ret = true;
|
break;
|
}
|
line.X1 = line.X2;
|
line.Y1 = line.Y2;
|
}
|
|
line.X1 = right->point[0].X;
|
line.Y1 = right->point[0].Y;
|
for (int i = 1; !ret && i < right->num; ++i) {
|
line.X2 = right->point[i].X;
|
line.Y2 = right->point[i].Y;
|
|
if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
|
IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
|
IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
|
IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
|
ret = true;
|
break;
|
}
|
line.X1 = line.X2;
|
line.Y1 = line.Y2;
|
}
|
|
return ret;
|
}
|
|
// 整个车辆都要驶离该测试区域
|
static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
|
{
|
for (int i = 0; i < car->point_num; ++i) {
|
if (IntersectionOfLine(left->point[0], right->point[0], car->points[i]) != 1)
|
return false;
|
}
|
return true;
|
}
|
