计算边距

yy1717

2020-04-13 46f56f26bfcc6ce26ffd8132ee11bf019eef3289

 lib/src/main/cpp/test_items/area_exam.cpp

@@ -43,6 +43,61 @@
    ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime);
}

void DistanceOfTire2X(std::vector<double> &array, const car_model *car, std::vector<Line> line_set)
{
    double ld = 100, rd = 100;

    array.clear();

    for (auto line = line_set.begin(); line != line_set.end(); line++) {
        double ld_t = 100, rd_t = 100;
        double d;
        if (VerticalPointOnLine(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], *line)) {
            d = DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], *line);
            if (d < ld_t) {
                ld_t = d;
            }
        }
        if (VerticalPointOnLine(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], *line)) {
            d = DistanceOf(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], *line);
            if (d < ld_t) {
                ld_t = d;
            }
        }

        if (VerticalPointOnLine(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], *line)) {
            d = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], *line);
            if (d < rd_t) {
                rd_t = d;
            }
        }
        if (VerticalPointOnLine(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], *line)) {
            d = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], *line);
            if (d < rd_t) {
                rd_t = d;
            }
        }

        if (isEqual2(ld_t, rd_t)) {
            if (!isEqual(ld_t, 100)) {
                if (rd_t < rd)
                    rd = rd_t;
                if (ld_t < ld)
                    ld = ld_t;
            }
        } else if (ld_t > rd_t) {
            if (rd_t < rd)
                rd = rd_t;
        } else {
            if (ld_t < ld)
                ld = ld_t;
        }
    }

    array.push_back(ld);
    array.push_back(rd);
}

static void DetectEnterOrExitMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
{
    if (CurrExamMapIndex < 0) {