更新地图

yy1717

2020-02-28 345dcbfaf5a716fdd8649885c32c34b50d361968

 lib/src/main/cpp/driver_test.cpp

@@ -997,7 +997,9 @@

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

        double projectDistance = sqrt(pow(qrx, 2) + pow(qry, 2));
        double projectAngle = toDegree(acos(qry / projectDistance));
@@ -1009,10 +1011,14 @@
//        double tx = projectDistance*sin(toRadians(azimuth));
//        double ty = projectDistance*cos(toRadians(azimuth));

        carModel->carXY[i].X = projectDistance*sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
                projectDistance*cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) + main_ant.X;
        carModel->carXY[i].Y = projectDistance*sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
                projectDistance*cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) + main_ant.Y;
        carModel->carXY[i].X =
                projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
                projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
                main_ant.X;
        carModel->carXY[i].Y =
                projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
                projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
                main_ant.Y;
    }
}