S型加减速曲线算法----matlab

function x = Solve3Polynomial(a, b, c, d) % 范盛金. 一元三次方程的新求根公式与新判别法[J]. 海南师范学院学报,1989,2(2):91-98. A = b*b - 3*a*c; if abs(A) < 1e-14; A = 0; end B = b*c - 9*a*d; if abs(B) < 1e-14; B = 0; end C = c*c - 3*b*d; if abs(C) < 1e-14; C = 0; end DET = B*B - 4*A*C; if abs(DET) < 1e-14; DET = 0; end if (A == 0) && (B == 0) x1 = -c/b; x2 = x1 ; x3 = x1; end if DET > 0 Y1 = A*b + 1.5*a*(-B + sqrt(DET)); Y2 = A*b + 1.5*a*(-B - sqrt(DET)); y1 = nthroot(Y1,3); y2 = nthroot(Y2,3); x1 = (-b-y1-y2)/(3*a); vec1 = (-b + 0.5*(y1 + y2))/(3*a); vec2 = 0.5*sqrt(3)*(y1 - y2)/(3*a); x2 = complex(vec1, vec2); x3 = complex(vec1, -vec2); clear Y1 Y2 y1 y2 vec1 vec2; end if DET == 0 && (A ~= 0) && (B ~= 0) K = (b*c-9*a*d)/(b*b - 3*a*c); K = round(K,14); x1 = -b/a + K; x2 = -0.5*K; x3 = x2; end if DET < 0 sqA = sqrt(A); T = (A*b - 1.5*a*B)/(A*sqA); theta = acos(T); csth = cos(theta/3); sn3th = sqrt(3)*sin(theta/3); x1 = (-b - 2*sqA*csth)/(3*a); x2 = (-b + sqA*(csth + sn3th))/(3*a); x3 = (-b + sqA*(csth - sn3th))/(3*a); clear sqA T theta csth sn3th; end x = [x1; x2; x3]; end