26 if (psi<0) psi+=2*
M_PI;
37 if (fabs(psi-(t2+t0)/2)<
eA || fabs(psi-(t2+t0)/2+
M_PI)<
eA || fabs(psi-(t2+t0)/2-
M_PI)<
eA){
38 if (fabs(fmod(psi-t0,2*
M_PI))<
eA){
45 k1 = -4*sin((3*t0+t2)/4 - psi)*cos((t2-t0)/4)/L;
62 k1 = -4*sin((3*t0+t2)/4 - psi)*cos((t2-t0)/4)/L;
63 k2 = 4*sin((t0+3*t2)/4 - psi)*cos((t2-t0)/4)/L;
67 L1 = L*sin((t2+t0)/2-psi)/sin((t2-t0)/2);
73 L2 = L*sin((t2+t0)/2-psi)/sin((t0-t2)/2);
88 k3 = 4*cos((3*t0+t2)/4 - psi)*sin((t2-t0)/4)/L;
89 k4 = 4*cos((t0+3*t2)/4 - psi)*sin((t2-t0)/4)/L;
93 if ( (fabs(k3)>
eK || fabs(k4)>
eK) && fabs(k4-k3)>
eK){
96 L3 = L*sin((t2+t0)/2-psi)/sin((t2-t0)/2);
102 L4 = L*sin((t2+t0)/2-psi)/sin((t0-t2)/2);
117 if ((L1>0 && L2>0) && ((L3<0 || L4<0) || (L1+L2)<(L3+L4))){
135 throw std::runtime_error(
"Could not compute biarc parameters");
183 double sin_join_theta = (k1*k2*(x2-x0)+k2*sin(t0)-k1*sin(t2))/(k2-k1);
184 double cos_join_theta = (-k1*k2*(y2-y0)+k2*cos(t0)-k1*cos(t2))/(k2-k1);
186 double join_theta = atan2(sin_join_theta, cos_join_theta);
187 if (join_theta<0) join_theta += 2*
M_PI;
194 double num = (t1-t0);
196 if (k<0 && (t1-t0)>0)
198 else if (k>0 && (t1-t0)<0)
void compute_other_stuff()
double euc_distance(Point2D< point_type1 > pt1, Point2D< point_type2 > pt2)
Point2D< double > start_pt
void compute_biarc_params()
double compute_arclength(double theta0, double theta2, double k)
double compute_join_theta(double k1, double k2)
Point2D< double > center1
Point2D< double > center2