pantor / ruckig

Motion Generation for Robots and Machines. Real-time. Jerk-constrained. Time-optimal.
https://ruckig.com
MIT License
635 stars 155 forks source link

Errors in solving cubic equation of one variable #180

Closed songhongxiang closed 5 months ago

songhongxiang commented 5 months ago

In the roots.hpp/solve_cubic(), when yy > 0, www should be assigned a positive value instead of taking the number with the largest absolute value. Otherwise, real roots cannot be obtained.

        if (yy > DBL_EPSILON) {
            // Sqrt is positive: one real solution
            const double y = std::sqrt(yy);
            const double uuu = -halfq + y;
            const double vvv = -halfq - y;
            const double www = std::abs(uuu) > std::abs(vvv) ? uuu : vvv;
            const double w = std::cbrt(www);
            roots.insert(w - p / (3 * w) - bover3a);

should be changed to

        if (yy > DBL_EPSILON) {
            // Sqrt is positive: one real solution
            const double y = std::sqrt(yy);
            const double uuu = -halfq + y;
            const double vvv = -halfq - y;
            const double www = uuu > vvv ? uuu : vvv;
            if(www > 0){
                const double w = std::cbrt(www);
                roots.insert(w - p / (3 * w) - bover3a);
            }