//
|
// 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 calc_zone_t {
|
int leftStart;
|
int leftEnd;
|
int rightStart;
|
int rightEnd;
|
} calcZone;
|
|
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone);
|
static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *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 = 0;
|
}
|
|
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;
|
Line axial;
|
|
MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
|
|
// DEBUG("START 线 %d -- %d", calcZone.leftStart, calcZone.rightStart);
|
// DEBUG("END 线 %d -- %d", calcZone.leftEnd, calcZone.rightEnd);
|
|
MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
|
|
if (IntersectionOf(start, axial) == GM_None) {
|
// 向起点查找
|
int tempLeft = calcZone.leftStart, tempRight = calcZone.rightStart;
|
|
while (calcZone.leftStart > 0 || calcZone.rightStart > 0) {
|
if (calcZone.leftStart > 0)
|
tempLeft = calcZone.leftStart - 1;
|
if (calcZone.rightStart > 0)
|
tempRight = calcZone.rightStart - 1;
|
// DEBUG("START 向起点查找 %d -- %d", tempLeft, tempRight);
|
MakeLine(&start, &map->point[tempLeft], &map2->point[tempRight]);
|
|
if (IntersectionOf(start, axial) == GM_Intersection) {
|
// 保持之前的线
|
break;
|
} else {
|
calcZone.leftStart = tempLeft;
|
calcZone.rightStart = tempRight;
|
|
if (calcZone.leftStart == calcZone.leftEnd && calcZone.rightStart == calcZone.rightEnd) {
|
// 车辆丢失，重新搜索
|
calcZone.leftStart = calcZone.rightStart = 0;
|
calcZone.leftEnd = calcZone.rightEnd = 0;
|
|
DEBUG("车辆丢失，重新搜索");
|
|
while (calcZone.leftStart < map->num || calcZone.rightStart < map2->num) {
|
MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
|
|
if (IntersectionOf(start, axial) == GM_Intersection) {
|
while (calcZone.leftStart < map->num || calcZone.rightStart < map2->num) {
|
MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
|
if (IntersectionOf(start, axial) == GM_None) {
|
break;
|
}
|
if (calcZone.leftStart < map->num)
|
calcZone.leftStart++;
|
if (calcZone.rightStart < map2->num)
|
calcZone.rightStart++;
|
}
|
break;
|
}
|
|
calcZone.leftEnd = calcZone.leftStart;
|
calcZone.rightEnd = calcZone.rightStart;
|
|
if (calcZone.leftStart < map->num)
|
calcZone.leftStart++;
|
if (calcZone.rightStart < map2->num)
|
calcZone.rightStart++;
|
}
|
|
if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
|
// 离开场地
|
DEBUG("离开曲线场地");
|
testing = false;
|
goto TEST_END;
|
}
|
|
break;
|
}
|
}
|
}
|
} else {
|
// 向终点查找
|
do {
|
if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
|
break;
|
}
|
if (calcZone.leftStart < map->num)
|
calcZone.leftStart++;
|
if (calcZone.rightStart < map2->num)
|
calcZone.rightStart++;
|
// DEBUG("START 向终点查找 %d -- %d", calcZone.leftStart, calcZone.rightStart);
|
MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
|
} while (IntersectionOf(start, axial) == GM_Intersection);
|
}
|
|
MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]);
|
|
if (IntersectionOf(end, axial) == GM_None) {
|
// 向终点查找
|
int tempLeft = calcZone.leftEnd, tempRight = calcZone.rightEnd;
|
while (calcZone.leftEnd < map->num || calcZone.rightEnd < map2->num) {
|
if (calcZone.leftEnd >= map->num && calcZone.rightEnd >= map2->num) {
|
break;
|
}
|
if (calcZone.leftEnd < map->num)
|
tempLeft = calcZone.leftEnd + 1;
|
if (calcZone.rightEnd < map2->num)
|
tempRight = calcZone.rightEnd + 1;
|
// DEBUG("END 向终点查找 %d -- %d", tempLeft, tempRight);
|
MakeLine(&end, &map->point[tempLeft], &map2->point[tempRight]);
|
|
if (IntersectionOf(end, axial) == GM_Intersection) {
|
// 保持之前的线
|
break;
|
} else {
|
calcZone.leftEnd = tempLeft;
|
calcZone.rightEnd = tempRight;
|
}
|
}
|
} else {
|
// 向起点查找
|
do {
|
if (calcZone.leftEnd == 0 && calcZone.rightEnd == 0) {
|
break;
|
}
|
if (calcZone.leftEnd > 0)
|
calcZone.leftEnd--;
|
if (calcZone.rightEnd > 0)
|
calcZone.rightEnd--;
|
// DEBUG("END 向起点查找 %d -- %d", calcZone.leftEnd, calcZone.rightEnd);
|
MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]);
|
} while (IntersectionOf(end, axial) == GM_Intersection);
|
}
|
|
DEBUG("calcZone leftStart %d leftEnd %d rightStart %d rightEnd %d", calcZone.leftStart, calcZone.leftEnd, calcZone.rightStart, calcZone.rightEnd);
|
|
if (CrashRedLine(map, map2, car, &calcZone)) {
|
if (!crashRedLine) {
|
crashRedLine = true;
|
// 车轮压边线，不合格
|
AddExamFault(27, rtkTime);
|
DEBUG("车轮压边线");
|
}
|
} else {
|
crashRedLine = false;
|
}
|
|
if (moveDirect != prevMoveDirect) {
|
if (moveDirect == 0) {
|
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
reportStopCarTimeout = false;
|
|
DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
|
} else {
|
|
}
|
prevMoveDirect = moveDirect;
|
} else if (moveDirect == 0) {
|
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
|
if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarTimeout) {
|
// 停车超2秒，不合格
|
AddExamFault(28, rtkTime);
|
DEBUG("中途停车");
|
reportStopCarTimeout = true;
|
}
|
}
|
|
TEST_END:
|
return testing ? 1 : 0;
|
}
|
|
static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *car)
|
{
|
// 全车都需不在地图中
|
bool ret = false;
|
|
Polygon carBody;
|
Polygon bigMap;
|
|
bigMap.num = map->num + map2->num;
|
bigMap.point = (PointF *) malloc(bigMap.num * sizeof(PointF));
|
|
int i = 0;
|
for (; i < map->num; ++i) {
|
bigMap.point[i] = map->point[i];
|
}
|
for (int j = map2->num; j > 0; --j) {
|
bigMap.point[i++] = map2->point[j-1];
|
}
|
|
carBody.num = car->bodyNum;
|
carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
|
|
for (int i = 0; i < carBody.num; ++i) {
|
carBody.point[i] = car->carXY[car->body[i]];
|
}
|
|
if (IntersectionOf(&carBody, &bigMap) == GM_None) {
|
ret = true;
|
}
|
|
free(carBody.point);
|
free(bigMap.point);
|
|
return ret;
|
}
|
|
// 车轮是否压边线
|
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone)
|
{
|
bool ret = false;
|
|
Line frontTireAxial, rearTireAxial;
|
Line redLine;
|
|
MakeLine(&frontTireAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
|
MakeLine(&rearTireAxial, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
|
|
int s = zone->leftStart;
|
for (int e = zone->leftStart - 1; e >= zone->leftEnd; --e) {
|
MakeLine(&redLine, &map->point[s], &map->point[e]);
|
if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
|
return true;
|
}
|
if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
|
return true;
|
}
|
s = e;
|
}
|
|
s = zone->rightStart;
|
for (int e = zone->rightStart - 1; e >= zone->rightEnd; --e) {
|
MakeLine(&redLine, &map2->point[s], &map2->point[e]);
|
if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
|
return true;
|
}
|
if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
|
return true;
|
}
|
s = e;
|
}
|
|
return ret;
|
}
|
