计算边距

yy1717

2020-04-13 46f56f26bfcc6ce26ffd8132ee11bf019eef3289

 lib/src/main/cpp/test_items/driving_curve.cpp

@@ -9,6 +9,7 @@
#include "../utils/xconvert.h"
#include "../master/comm_if.h"
#include "../native-lib.h"
#include "area_exam.h"

#include <vector>
#include <cstdlib>
@@ -60,6 +61,10 @@
{
    Polygon tireRect;
    int who = 0;
    vector<double> dtox;
    vector<Line> line_set;
    int s;

    MakePolygon(&tireRect, {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
                            car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
                            car->carXY[car->right_rear_tire[TIRE_OUTSIDE]],
@@ -75,6 +80,27 @@
    // 更新车尾扫描线
    UpdateEndLine(false, &scanWindow, map, map2, &tireRect);

    // 计算边距
    s = scanWindow.leftStart;
    for (int e = scanWindow.leftStart - 1; e >= scanWindow.leftEnd; --e) {
        Line redLine;
        MakeLine(&redLine, &map->point[s], &map->point[e]);
        line_set.push_back(redLine);
        s = e;
    }

    s = scanWindow.rightStart;
    for (int e = scanWindow.rightStart - 1; e >= scanWindow.rightEnd; --e) {
        Line redLine;
        MakeLine(&redLine, &map2->point[s], &map2->point[e]);
        line_set.push_back(redLine);
        s = e;
    }

    DistanceOfTire2X(dtox, car, line_set);

    MA_SendDistance(dtox[0], dtox[1]);

    DEBUG("scanWindow leftStart %d leftEnd %d rightStart %d rightEnd %d", scanWindow.leftStart, scanWindow.leftEnd, scanWindow.rightStart, scanWindow.rightEnd);

    if (CrashRedLine(map, map2, car, &scanWindow, who)) {