科目三草案实现

yy1717

2020-03-17 4ff195404b21f74ca11e26a69cbf0418eaa4595f

 lib/src/main/cpp/driver_test.cpp

@@ -14,6 +14,7 @@
#include <list>
#include <numeric>
#include <algorithm>
#include <string>

#include "driver_test.h"
#include "defs.h"
@@ -88,42 +89,9 @@
    Polygon map2;
} MapList[MAP_LIST_SIZE];

struct trigger_line {
    int mapId;
    Line tirgger;
};
static Polygon RoadMapPoints;

struct road_exam_parent_map {
    int id;
    int type;
    Polygon map;
    int redLineNum;
    Polygon *redLine;
    int greenLineNum;
    Polygon *greenLine;
    int redAreaNum;
    Polygon *redArea;
    int triggerLineNum;
    struct trigger_line *triggerLine;
};

static struct road_exam_parent_map * RoadParentMap;

struct road_exam_son_map {
    int id;
    int type;
    char tts[512];
    Line triggerLine;
    Line controlLine;
    Line targetLine;
    int stop_flag;

    int redLineNum;
    Polygon *redLine;
};

typedef list<struct road_exam_son_map *> LIST_ROAD_SON_MAP;
static LIST_ROAD_SON_MAP RoadSonMapList;
static LIST_ROAD_MAP RoadMapList;

static int MapNum = 0;
static int CurrExamMapIndex = -1;
@@ -136,7 +104,7 @@

typedef list<car_model *> LIST_CAR_MODEL;

static LIST_CAR_MODEL CarModelList;
static LIST_CAR_MODEL CarModelList;             // 一段时间的车辆轨迹集合

static struct dummy_light_exam *DummyLightContent;
static int DummyLightContentSize;
@@ -174,8 +142,10 @@
    CarModel = NULL;
    CarModelList.clear();

    RoadParentMap = NULL;
    RoadSonMapList.clear();
    RoadMapPoints.num = 0;
    RoadMapPoints.point = NULL;

    RoadMapList.clear();

    CarSensorInit();

@@ -252,42 +222,133 @@
    DEBUG("AddMap num %d", MapNum);
}

void AddRoadMapParent(int id, int type, const double (*points)[2], int pointNum, const int **redLine,
        int redLineNum, const int **greenLine, int greenLineNum)
void CleanRoadMap(void)
{
    int id;
    int type;
    Polygon map;
    int redLineNum;
    Polygon *redLine;
    int greenLineNum;
    Polygon *greenLine;
    int redAreaNum;
    Polygon *redArea;
    int triggerLineNum;
    struct trigger_line *triggerLine;
    if (ExamStart) return;

    RoadParentMap = (struct road_exam_parent_map *)malloc(sizeof(struct road_exam_parent_map));
    RoadParentMap->id = id;
    RoadParentMap->type = type;
    if (RoadMapPoints.point != NULL) {
        free(RoadMapPoints.point);
    }
    RoadMapPoints.num = 0;

    RoadParentMap->map.num = pointNum;
    if (pointNum > 0) {
        RoadParentMap->map.point = (PointF *)malloc(sizeof(PointF) * pointNum);
        for (int i = 0; i < pointNum; ++i) {
            RoadParentMap->map.point[i].X = points[i][0];
            RoadParentMap->map.point[i].Y = points[i][1];
    for (int i = 0; i < RoadMapList.size(); ++i) {
        struct road_exam_map map = RoadMapList[i];

        if (map.redLine != NULL) {
            for (int j = 0; j < map.redLineNum; ++j) {
                if (map.redLine[j].point != NULL)
                    free(map.redLine[j].point);
            }
            free(map.redLine);
        }

        if (map.greenLine != NULL) {
            for (int j = 0; j < map.greenLineNum; ++j) {
                if (map.greenLine[j].point != NULL)
                    free(map.greenLine[j].point);
            }
            free(map.greenLine);
        }

        if (map.redArea != NULL) {
            for (int j = 0; j < map.redAreaNum; ++j) {
                if (map.redArea[j].point != NULL)
                    free(map.redArea[j].point);
            }
            free(map.redArea);
        }

        if (map.triggerLine != NULL) {
            for (int j = 0; j < map.triggerLineNum; ++j) {
                if (map.triggerLine[j].point != NULL)
                    free(map.triggerLine[j].point);
            }
            free(map.triggerLine);
        }
    }

    RoadParentMap->redLineNum = redLineNum;
    RoadParentMap->redLine = (Polygon *) malloc(sizeof(Polygon *));
    RoadMapList.clear();
}

    for (int i = 0; i < redLineNum; ++i) {
        RoadParentMap->redLine[i].num =
void SetRoadMapPoints(vector<double> &mapPoints)
{
    DEBUG("加入路考地图点集合 num = %d", mapPoints.size()/2);

    RoadMapPoints.num = mapPoints.size()/2;

    if (RoadMapPoints.num > 0) {
        RoadMapPoints.point = (PointF *)malloc(sizeof(PointF) * RoadMapPoints.num);
        for (int i = 0; i < RoadMapPoints.num; ++i) {
            RoadMapPoints.point[i].X = mapPoints[i*2];
            RoadMapPoints.point[i].Y = mapPoints[i*2+1];
        }
    }
}

void AddRoadMapParent(int id, int type, string tts, vector<vector<int>> &redLines,
                      vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines)
{
    struct road_exam_map newMap;

    newMap.id = id;
    newMap.type = type;
    newMap.tts = tts;

    DEBUG("加入路考地图信息 id = %d type = %d", id, type);

    if ((newMap.redLineNum = redLines.size()) > 0) {
        newMap.redLine = (Polygon *) malloc(sizeof(Polygon) * newMap.redLineNum);

        for (int i = 0; i < newMap.redLineNum; ++i) {
            newMap.redLine[i].num = redLines[i].size();
            newMap.redLine[i].point = (PointF *) malloc(sizeof(PointF) * newMap.redLine[i].num);

            for (int j = 0; j < newMap.redLine[i].num; ++j) {
                newMap.redLine[i].point[j] = RoadMapPoints.point[redLines[i][j]];
            }
        }
    }

    if ((newMap.redAreaNum = redAreas.size()) > 0) {
        newMap.redArea = (Polygon *) malloc(sizeof(Polygon) * newMap.redAreaNum);

        for (int i = 0; i < newMap.redAreaNum; ++i) {
            newMap.redArea[i].num = redAreas[i].size();
            newMap.redArea[i].point = (PointF *) malloc(sizeof(PointF) * newMap.redLine[i].num);

            for (int j = 0; j < newMap.redArea[i].num; ++j) {
                newMap.redArea[i].point[j] = RoadMapPoints.point[redAreas[i][j]];
            }
        }
    }

    if ((newMap.greenLineNum = greenLines.size()) > 0) {
        newMap.greenLine = (Polygon *) malloc(sizeof(Polygon) * newMap.greenLineNum);

        for (int i = 0; i < newMap.greenLineNum; ++i) {
            newMap.greenLine[i].num = greenLines[i].size();
            newMap.greenLine[i].point = (PointF *) malloc(sizeof(PointF) * newMap.greenLine[i].num);

            for (int j = 0; j < newMap.greenLine[i].num; ++j) {
                newMap.greenLine[i].point[j] = RoadMapPoints.point[greenLines[i][j]];
            }
        }
    }

    if ((newMap.triggerLineNum = triggerLines.size()) > 0) {
        newMap.triggerLine = (Polygon *) malloc(sizeof(Polygon) * newMap.triggerLineNum);

        for (int i = 0; i < newMap.triggerLineNum; ++i) {
            newMap.triggerLine[i].num = triggerLines[i].size();
            newMap.triggerLine[i].point = (PointF *) malloc(sizeof(PointF) * newMap.triggerLine[i].num);

            for (int j = 0; j < newMap.triggerLine[i].num; ++j) {
                newMap.triggerLine[i].point[j] = RoadMapPoints.point[triggerLines[i][j]];
            }
        }
    }

    RoadMapList.push_back(newMap);
}

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,