坐标

yy1717

2020-08-24 fb7b0660a319a9c54ff35c3944548348fae11b60

 lib/src/main/cpp/test_items2/through_something.cpp

@@ -66,10 +66,12 @@
    TargetReduceRec2.clear();
}

void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
double ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    vector<double> nearbyTarget;        // 学校、公交站、人行道区域的距离

    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return;
        return 100000;
    // 路口
    for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) {
        PointF point;
@@ -106,12 +108,19 @@
            continue;

        if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
            double distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
            double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
            double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge);

            int key =  i;
            int rec = GetTargetReduceRec(TargetReduceRec2, key);

            if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
            if (distance1 < -1e-3 && distance2 > 1e-3) {
                nearbyTarget.push_back(0);
            } else if (distance1 > 1e-3 && distance2 > 1e-3) {
                nearbyTarget.push_back(distance1);
            }

            if (distance1 > 1e-3 && distance1 < LASTEST_BREAK_POINT) {
                if (rec == NOT_ENTER) {
                    SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
                }
@@ -119,30 +128,45 @@
                if (dobreak && !(rec & REDUCE_SPEED)) {
                    SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED);
                }
            } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) {
            } else if (distance1 > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) {
                RemoveTargetReduceRec(TargetReduceRec2, key);
            }
        } else if (RoadMap.specialAreas[i].type == SCHOOL_AREA) {
            double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
            double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge);

            int key =  i;
            int rec = GetTargetReduceRec(TargetReduceRec2, key);

            if (distance1 < -1e-3 && distance2 > 1e-3) {
                nearbyTarget.push_back(0);
                if (rec == NOT_ENTER) {
                    SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
                }
                if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED && !(rec & OVER_SPEED)) {
                    SetTargetReduceRec(TargetReduceRec2, key, rec | OVER_SPEED);

                    DEBUG("通过学校区域超速");
                    DEBUG("通过学校区域超速 %f kmh", ConvertMs2KMh(speed));
                    AddExamFault(49, rtkTime);
                }
            } else if (rec != NOT_ENTER) {
                RemoveTargetReduceRec(TargetReduceRec2, key);
            } else if (distance1 < -1e-3 && distance2 < -1e-3) {
                if (rec != NOT_ENTER) {
                    RemoveTargetReduceRec(TargetReduceRec2, key);
                }
            } else {
                nearbyTarget.push_back(distance1);
                if (rec != NOT_ENTER) {
                    RemoveTargetReduceRec(TargetReduceRec2, key);
                }
            }
        }
    }

    if (nearbyTarget.size() > 0) {
        sort(nearbyTarget.begin(), nearbyTarget.end());
        return nearbyTarget[0];
    }
    return 100000;
}

void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime)