24 if (psi<0) psi+=2*
M_PI;
35 if (fabs(psi-(t2+t0)/2)<
eA || fabs(psi-(t2+t0)/2+
M_PI)<
eA || fabs(psi-(t2+t0)/2-
M_PI)<
eA){
36 if (fabs(fmod(psi-t0,2*
M_PI))<
eA){
43 k1 = -4*sin((3*t0+t2)/4 - psi)*cos((t2-t0)/4)/L;
60 k1 = -4*sin((3*t0+t2)/4 - psi)*cos((t2-t0)/4)/L;
61 k2 = 4*sin((t0+3*t2)/4 - psi)*cos((t2-t0)/4)/L;
65 L1 = L*sin((t2+t0)/2-psi)/sin((t2-t0)/2);
71 L2 = L*sin((t2+t0)/2-psi)/sin((t0-t2)/2);
86 k3 = 4*cos((3*t0+t2)/4 - psi)*sin((t2-t0)/4)/L;
87 k4 = 4*cos((t0+3*t2)/4 - psi)*sin((t2-t0)/4)/L;
91 if ( (fabs(k3)>
eK || fabs(k4)>
eK) && fabs(k4-k3)>
eK){
94 L3 = L*sin((t2+t0)/2-psi)/sin((t2-t0)/2);
100 L4 = L*sin((t2+t0)/2-psi)/sin((t0-t2)/2);
115 if ((L1>0 && L2>0) && ((L3<0 || L4<0) || (L1+L2)<(L3+L4))){
181 double sin_join_theta = (k1*k2*(x2-x0)+k2*sin(t0)-k1*sin(t2))/(k2-k1);
182 double cos_join_theta = (-k1*k2*(y2-y0)+k2*cos(t0)-k1*cos(t2))/(k2-k1);
184 double join_theta = atan2(sin_join_theta, cos_join_theta);
185 if (join_theta<0) join_theta += 2*
M_PI;
192 double num = (t1-t0);
194 if (k<0 && (t1-t0)>0)
196 else if (k>0 && (t1-t0)<0)