- /* the center of the circle is at (d_mm, d_mm) when we have to
- * start the clothoid */
- d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) *
- sqrt(M_PI) / 2.;
+ /* L and A are the parameters of the clothoid, xm and ym are
+ * the relative coords (starting from the beginning of
+ * clothoid) of the crossing point between the clothoid and
+ * the circle. */
+ L = Vd_mm_s * t;
+ A = R_mm * sqrt(fabs(alpha_rad - beta_rad));
+ xm =
+ L
+ - (pow(L, 5) / (40. * pow(A, 4)))
+ + (pow(L, 9) / (3456. * pow(A, 8)))
+ - (pow(L, 13) / (599040. * pow(A, 12)));
+ ym =
+ (pow(L, 3) / (6. * pow(A, 2)))
+ - (pow(L, 7) / (336. * pow(A, 6)))
+ + (pow(L, 11) / (42240. * pow(A, 10)))
+ - (pow(L, 15) / (9676800. * pow(A, 14)));
+ DEBUG(E_TRAJECTORY, "relative xm,ym = (%2.2f, %2.2f)",
+ xm, ym);
+
+ /* the center of the circle is at d_mm when we have to start
+ * the clothoid */
+ tau = (alpha_rad - beta_rad) / 2.;
+ d_mm = ym + (R_mm * cos(tau));