更新地图

yy1717

2020-03-05 7147f8046917a14e71f2330709e73aadb9844d30

更新地图

 lib/src/main/cpp/driver_test.cpp

@@ -851,18 +851,18 @@
//        if (mapList[i].type == MAP_TYPE_CURVE) {
//
//        }
        if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1, enterLine2;

                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
                MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
                    DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
                    return i;
            }
        }
//        if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine1, enterLine2;
//
//                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//                MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
//                    DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
//                    return i;
//            }
//        }
//        if (mapList[i].type == MAP_TYPE_PART_EDGE) {
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine;
@@ -883,18 +883,18 @@
//                    return i;
//            }
//        }
//        if (mapList[i].type == MAP_TYPE_CURVE) {
//            Line startLine;
//            Line carAxial;
//
//            MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
//            MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
//
//            if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], startLine) > 0.1)
//                    return i;
//            }
//        }
        if (mapList[i].type == MAP_TYPE_CURVE) {
            Line startLine;
            Line carAxial;

            MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
            MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);

            if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], startLine) > 0.1)
                    return i;
            }
        }
    }
    return -1;
}

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

@@ -59,21 +59,50 @@

    MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);

    MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
    MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
    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) {
        // 向起点查找
        do {
            if (calcZone.leftStart == 0 && calcZone.rightStart == 0) {
                break;
            }
        int tempLeft = calcZone.leftStart, tempRight = calcZone.rightStart;

        while (calcZone.leftStart > 0 || calcZone.rightStart > 0) {
            if (calcZone.leftStart > 0)
                calcZone.leftStart--;
                tempLeft = calcZone.leftStart - 1;
            if (calcZone.rightStart > 0)
                calcZone.rightStart--;
            MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
        } while (IntersectionOf(start, axial) == GM_None);
                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) {
                    // 车辆丢失，重新搜索
                    Line cat;
                    int l = 0, r = 0;

                    while (l < map->num || r < map2->num) {
                        MakeLine(&cat, &map->point[l], &map2->point[r]);

                        if (IntersectionOf(cat, axial) == GM_Intersection) {

                        }

                        if (l < map->num)
                            l++;
                        if (r < map2->num)
                            r++;
                    }
                }
            }
        }
    } else {
        // 向终点查找
        do {
@@ -84,22 +113,35 @@
                calcZone.leftStart++;
            if (calcZone.rightStart < map2->num)
                calcZone.rightStart++;
            MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
        } while (IntersectionOf(start, axial) != GM_None);
            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) {
        // 向终点查找
        do {
        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)
                calcZone.leftEnd++;
                tempLeft = calcZone.leftEnd + 1;
            if (calcZone.rightEnd < map2->num)
                calcZone.rightEnd++;
            MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
        } while (IntersectionOf(end, axial) == GM_None);
                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 {
@@ -110,12 +152,16 @@
                calcZone.leftEnd--;
            if (calcZone.rightEnd > 0)
                calcZone.rightEnd--;
            MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
        } while (IntersectionOf(end, axial) == GM_None);
            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 (calcZone.leftStart <= calcZone.leftEnd || calcZone.rightStart <= calcZone.rightEnd) {
        // 离开场地
        DEBUG("离开曲线场地");
        testing = false;
    }