解方程组¶

/*
* 计算偏航位置
* { ax + by = cos(85) * (a^2+b^2+c^2）^.5)
* { x^2 + y^2 = 1
*/

const double DEG_TO_RAD = 3.1415926 / 180.0;
const double COS_85 = std::cos(85.0 * DEG_TO_RAD);
const double EPSILON = 1e-4;

bool NTMath::CalculateOptimalYawPosition(double tx, double ty, double tz, double& x, double& y)
{
    double length = std::sqrt(tx * tx + ty * ty + tz * tz);
    double a = tx / length;
    double b = ty / length;
    double c = tz / length;

    if (std::abs(a) < EPSILON && std::abs(b) < EPSILON) {
    }
    else if (std::abs(a) < EPSILON) {
        y = COS_85 / b;
        if (std::abs(y) <= 1) {
            x = std::sqrt(1 - y * y);
            return true;
        }
    }
    else if (std::abs(b) < EPSILON) {
        x = COS_85 / a;
        if (std::abs(x) <= 1) {
            y = std::sqrt(1 - x * x);
            return true;
        }
    }
    else {
        double A = 1 + (a * a) / (b * b);
        double B = -2 * a * COS_85 / (b * b);
        double C = (COS_85 * COS_85) / (b * b) - 1;
        double Delta = B * B - 4 * A * C;

        if (Delta >= 0) {
            double sqrtDelta = std::sqrt(Delta);
            double x1 = (-B + sqrtDelta) / (2 * A);
            double x2 = (-B - sqrtDelta) / (2 * A);

            double y1 = std::sqrt(1 - x1 * x1);
            double y2 = std::sqrt(1 - x2 * x2);

            if (y1 >= 0 && std::abs(a * x1 + b * y1 - COS_85) < EPSILON) {
                x = x1;
                y = y1;
                return true;
            }
            else if (y2 >= 0 && std::abs(a * x2 + b * y2 - COS_85) < EPSILON) {
                x = x2;
                y = y2;
                return true;
            }
        }
    }

    return false;
}