更新地图

yy1717

2020-03-05 7147f8046917a14e71f2330709e73aadb9844d30

 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;
    }