角度修正。

fctom1215

2020-02-23 9f309437fcafaebcc33224abd951a6b6fdea8e41

 lib/src/main/cpp/driver_test.cpp

@@ -977,25 +977,31 @@
    carModel->basePoint = main_ant;

    for (int i = 0; i < carModel->pointNum; ++i) {
        double tx = carModel->carDesc[i].distance*sin(toRadians(azimuth));
        double ty = carModel->carDesc[i].distance*cos(toRadians(azimuth));
        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 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 projDis = sqrt(pow(qrx, 2) + pow(pry, 2));
//        double projAng;
//
//        if (fabs(qry) <= EPSILON) {
//            projAng = 90;
//        } else {
//            projAng = toDegree(atan(qrx / qry));
//        }
        double projectDistance = sqrt(pow(qrx, 2) + pow(qry, 2));
        double projectAngle = toDegree(acos(qry / projectDistance));

        carModel->carXY[i].X = (tx)*cos(toRadians(carModel->carDesc[i].angle)) -
                                   (ty)*sin(toRadians(carModel->carDesc[i].angle)) + main_ant.X;
        carModel->carXY[i].Y = (tx)*sin(toRadians(carModel->carDesc[i].angle)) +
                                   (ty)*cos(toRadians(carModel->carDesc[i].angle)) + main_ant.Y;
        if (carModel->carDesc[i].angle > 180) {
            projectAngle = 360 - projectAngle;
        }

        double tx = projectDistance*sin(toRadians(azimuth));
        double ty = projectDistance*cos(toRadians(azimuth));

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

//        double tx = carModel->carDesc[i].distance*sin(toRadians(azimuth));
//        double ty = carModel->carDesc[i].distance*cos(toRadians(azimuth));
//
//        carModel->carXY[i].X = (tx)*cos(toRadians(carModel->carDesc[i].angle)) -
//                                   (ty)*sin(toRadians(carModel->carDesc[i].angle)) + main_ant.X;
//        carModel->carXY[i].Y = (tx)*sin(toRadians(carModel->carDesc[i].angle)) +
//                                   (ty)*cos(toRadians(carModel->carDesc[i].angle)) + main_ant.Y;
    }
}