角度修正。

fctom1215

2020-03-08 25bdbfbf6f4c9f1530be74fc194ad1dd0a13e8c9

 lib/src/main/cpp/driver_test.cpp

@@ -202,7 +202,7 @@

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                    int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                    int *body, int bodyNum,double (*point)[2], int pointNum)
                    int *body, int bodyNum,double (*point)[2], int pointNum, double antPitch)
{
    DEBUG("加入车辆信息 pointNum %d", pointNum);

@@ -264,6 +264,8 @@
            CarModel->body[i] = body[i];
        }
    }

    CarModel->antPitch = antPitch;

    CarModel->pointNum = pointNum;
    CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
@@ -962,23 +964,7 @@
            free(map.point);
    }
    if (mapList[index].type == MAP_TYPE_CURVE) {
        Line carAxial;
        Line scanLine;

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

        int startL = 0, startR = 0;
        while (startL < mapList[index].map.num || startR < mapList[index].map2.num) {
            MakeLine(&scanLine, &mapList[index].map.point[startL], &mapList[index].map.point[startR]);
            if (IntersectionOf(scanLine, carAxial) == GM_Intersection) {
                break;
            }
            if (startL < mapList[index].map.num)
                startL++;
            if (startR < mapList[index].map2.num)
                startR++;
        }
        ret = true;
        ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car);
    }

    return ret;
@@ -1032,6 +1018,8 @@
{
    carModel->basePoint = main_ant;

    pitch = pitch - carModel->antPitch;

    for (int i = 0; i < carModel->pointNum; ++i) {
        double qrx = carModel->carDesc[i].distance * sin(toRadians(carModel->carDesc[i].angle));
        double qry =