Optimize the PositionThirdOrderStep2::time_acc0 function

[Ydaos]

// UDUD
{
    const double h0a = -a0_a0 + af_af - 2*ad*aMax + 2*jMax*(aMax*tf - vd);
    const double h0b = a0_p3 + 2*af_p3 - 6*af_af*aMax - 3*a0_a0*(af - jMax*tf) - 3*a0*aMax*(aMax - 2*af + 2*jMax*tf) - 3*jMax*(jMax*(-2*pd + aMax*tf_tf + 2*tf*v0) + aMax*(aMax*tf - 2*vd)) + 3*af*(aMax*aMax + 2*aMax*jMax*tf - 2*jMax*vd);
    const double h0 = std::abs(jMax) * std::sqrt(4*h0b*h0b - 18*h0a*h0a*h0a);
    //const double h1 = 3*jMax*h0a;
    const double h1 = -(h0 + 2*jMax*h0b)/(6*jMax_jMax*h0a);   // t3

    profile.t[0] = (-a0 + aMax)/jMax;
    /*profile.t[1] = (-a0_p3 + af_p3 + af_af*(-6*aMax + 3*jMax*tf) + a0_a0*(-3*af + 6*aMax + 3*jMax*tf) + 6*af*(aMax*aMax - jMax*vd) + 3*a0*(af_af - 2*(aMax*aMax + jMax*vd)) - 6*jMax*(aMax*(aMax*tf - 2*vd) + jMax*g2))/h1;
    profile.t[2] = -(ad + h0/h1)/(2*jMax) + tf/2 - profile.t[1]/2;
    profile.t[3] = h0/(jMax*h1);*/

    profile.t[1] = tf - h1 - ad/jMax - h0a/(2*jMax_jMax*h1);
    profile.t[2] = h1;
    profile.t[3] = h0/(3*jMax_jMax*h0a);
    profile.t[4] = 0;
    profile.t[5] = 0;
    profile.t[6] = tf - (profile.t[0] + profile.t[1] + profile.t[2] + profile.t[3]);

    if (profile.check_with_timing<ControlSigns::UDDU, ReachedLimits::NONE>(tf, jMax, vMax, vMin, aMax, aMin)) {
        return true;
    }
}

In PositionThirdOrderStep2::time_acc1(), the value of 'h0' in case UDDU, I guess you forget std::abs(), because when jMax > 0, this case no solution.(But we will find solution in UDUD)
The true expression: 'h0 = -sqrt( jMax_jMax*(...) )/abs(jMax)' or 'h0 = -sqrt(...)'

//const double h0 = std::sqrt(jMax_jMax*(a0_p4 + af_p4 - 4*af_p3*jMax*tf + 6*af_af*jMax_jMax*tf_tf - 4*a0_p3*(af - jMax*tf) + 6*a0_a0*(af - jMax*tf)*(af - jMax*tf) + 24*af*jMax_jMax*g1 - 4*a0*(af_p3 - 3*af_af*jMax*tf + 6*jMax_jMax*(-pd + tf*vf)) - 12*jMax_jMax*(-vd_vd + jMax*tf*g2))/3)/jMax;
const double h0 = -std::sqrt((a0_p4 + af_p4 - 4*af_p3*jMax*tf + 6 * af_af*jMax_jMax*tf_tf - 4*a0_p3*(af - jMax*tf) + 6*a0_a0*(af - jMax*tf)*(af - jMax*tf) + 24*af*jMax_jMax*g1 - 4*a0*(af_p3 - 3*af_af*jMax*tf + 6*jMax_jMax*(-pd + tf*vf)) - 12*jMax_jMax*(-vd_vd + jMax*tf*g2))/3);