+/*** CLOTHOID */
+
+/**
+ * process clitoid parameters
+ *
+ * - alpha: total angle
+ * - beta: circular part of angle (lower than alpha)
+ * - R: the radius of the circle (must be != 0)
+ * - Vd: linear speed to use (in imp per cs period)
+ * - Amax: maximum angular acceleration
+ * - d_inter: distance in mm until the intersection of the
+ * 2 lines
+ *
+ * return 0 on success: in this case these parameters are filled:
+ * - Aa_out: the angular acceleration to configure in quadramp
+ * - Va_out: the angular speed to configure in quadramp
+ * - remain_d_mm_out: remaining distance before start to turn
+ */
+static int8_t calc_clitoid(struct trajectory *traj,
+ double x, double y, double a_rad,
+ double alpha_deg, double beta_deg, double R_mm,
+ double Vd, double Amax, double d_inter_mm,
+ double *Aa_out, double *Va_out, double *remain_d_mm_out)
+{
+ double Vd_mm_s;
+ double Va, Va_rd_s;
+ double t, tau, d_mm, alpha_rad, beta_rad;
+ double remain_d_mm;
+ double Aa, Aa_rd_s2;
+ line_t line1, line2;
+ line_t line1_int, line2_int;
+ point_t robot, intersect, pt2, center, proj, M;
+ vect_t v;
+ double xm, ym, L, A;
+
+ /* param check */
+ if (fabs(alpha_deg) <= fabs(beta_deg)) {
+ DEBUG(E_TRAJECTORY, "alpha is smaller than beta");
+ return -1;
+ }
+
+ /* get angular speed Va */
+ Vd_mm_s = speed_imp2mm(traj, Vd);
+ DEBUG(E_TRAJECTORY, "Vd_mm_s=%2.2f", Vd_mm_s);
+ Va_rd_s = Vd_mm_s / R_mm;
+ Va = speed_rd2imp(traj, Va_rd_s);
+ DEBUG(E_TRAJECTORY, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va);
+
+ /* process 't', the time in seconds that we will take to do
+ * the first clothoid */
+ alpha_rad = RAD(alpha_deg);
+ beta_rad = RAD(beta_deg);
+ t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s);
+ DEBUG(E_TRAJECTORY, "R_mm=%2.2f a_rad=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f",
+ R_mm, a_rad, alpha_rad, beta_rad, t);
+
+ /* process the angular acceleration */
+ Aa_rd_s2 = Va_rd_s / t;
+ Aa = acc_rd2imp(traj, Aa_rd_s2);
+ DEBUG(E_TRAJECTORY, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa);
+
+ /* exit if the robot cannot physically do it */
+ if (Aa > Amax) {
+ DEBUG(E_TRAJECTORY, "greater than max acceleration");
+ return -1;
+ }
+
+ /* define line1 and line2 */
+ robot.x = x;
+ robot.y = y;
+ intersect.x = x + cos(a_rad) * d_inter_mm;
+ intersect.y = y + sin(a_rad) * d_inter_mm;
+ pts2line(&robot, &intersect, &line1);
+ pt2.x = intersect.x + cos(a_rad + alpha_rad);
+ pt2.y = intersect.y + sin(a_rad + alpha_rad);
+ pts2line(&intersect, &pt2, &line2);
+ DEBUG(E_TRAJECTORY, "intersect=(%2.2f, %2.2f)",
+ intersect.x, intersect.y);
+
+ /* 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));
+ DEBUG(E_TRAJECTORY, "d_mm=%2.2f", d_mm);
+
+ /* translate line1 */
+ memcpy(&line1_int, &line1, sizeof(line1_int));
+ memcpy(&line2_int, &line2, sizeof(line2_int));
+ v.x = intersect.x - robot.x;
+ v.y = intersect.y - robot.y;
+ if (alpha_rad > 0)
+ vect_rot_trigo(&v);
+ else
+ vect_rot_retro(&v);
+ vect_resize(&v, d_mm);
+ line_translate(&line1_int, &v);
+ DEBUG(E_TRAJECTORY, "translate line1 by %2.2f,%2.2f", v.x, v.y);
+
+ /* translate line2_int */
+ v.x = intersect.x - pt2.x;
+ v.y = intersect.y - pt2.y;
+ if (alpha_rad < 0)
+ vect_rot_trigo(&v);
+ else
+ vect_rot_retro(&v);
+ vect_resize(&v, d_mm);
+ line_translate(&line2_int, &v);
+ DEBUG(E_TRAJECTORY, "translate line2 by %2.2f,%2.2f", v.x, v.y);
+
+ /* find the center of the circle, at the intersection of the
+ * new translated lines */
+ if (intersect_line(&line1_int, &line2_int, ¢er) != 1) {
+ DEBUG(E_TRAJECTORY, "cannot find circle center");
+ return -1;
+ }
+ DEBUG(E_TRAJECTORY, "center=(%2.2f,%2.2f)", center.x, center.y);
+
+ /* M is the same point than xm, ym but in absolute coords */
+ if (alpha_rad < 0) {
+ M.x = center.x + cos(a_rad + M_PI/2 + tau) * R_mm;
+ M.y = center.y + sin(a_rad + M_PI/2 + tau) * R_mm;
+ }
+ else {
+ M.x = center.x + cos(a_rad - M_PI/2 + tau) * R_mm;
+ M.y = center.y + sin(a_rad - M_PI/2 + tau) * R_mm;
+ }
+ DEBUG(E_TRAJECTORY, "absolute M = (%2.2f, %2.2f)", M.x, M.y);
+
+ /* project M on line 1 */
+ proj_pt_line(&M, &line1, &proj);
+ DEBUG(E_TRAJECTORY, "proj M = (%2.2f, %2.2f)", proj.x, proj.y);
+
+ /* process remaining distance before start turning */
+ remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + xm);
+ DEBUG(E_TRAJECTORY, "remain_d=%2.2f", remain_d_mm);
+ if (remain_d_mm < 0) {
+ DEBUG(E_TRAJECTORY, "too late, cannot turn");
+ return -1;
+ }
+
+ /* return result */
+ *Aa_out = Aa;
+ *Va_out = Va;
+ *remain_d_mm_out = remain_d_mm;
+ return 0;
+}
+
+/* after the line, start the clothoid */
+static void start_clitoid(struct trajectory *traj)
+{
+ double Aa = traj->target.line.Aa;
+ double Va = traj->target.line.Va;
+ double a_rad = traj->target.line.alpha;
+ double R_mm = traj->target.line.R;
+ double d;
+
+ DEBUG(E_TRAJECTORY, "%s() Va=%2.2f Aa=%2.2f",
+ __FUNCTION__, Va, Aa);
+ delete_event(traj);
+ d = fabs(R_mm * a_rad);
+ d *= 3.; /* margin to avoid deceleration */
+ trajectory_d_a_rel(traj, d, DEG(a_rad));
+ set_quadramp_acc(traj, traj->d_acc, Aa);
+ set_quadramp_speed(traj, traj->d_speed, Va);
+ traj->state = RUNNING_CLITOID_CURVE;
+}
+
+
+/**
+ * do a superb curve joining line1 to line2 which is composed of:
+ * - a clothoid starting from line1
+ * - a circle
+ * - another clothoid up to line2
+ * this curve is called a clitoid (hehe)
+ *
+ * the function assumes that the initial linear speed is Vd and
+ * angular speed is 0.
+ *
+ * - x,y,a_deg: starting position
+ * - advance: parameter for line following
+ * - alpha: total angle
+ * - beta: circular part of angle (lower than alpha)
+ * - R: the radius of the circle (must be != 0)
+ * - Vd: linear speed to use (in imp per cs period)
+ * - Amax: maximum angular acceleration
+ * - d_inter: distance in mm until the intersection of the
+ * 2 lines
+ *
+ * return 0 if trajectory can be loaded, then it is processed in
+ * background.
+ */
+int8_t trajectory_clitoid(struct trajectory *traj,
+ double x, double y, double a_deg, double advance,
+ double alpha_deg, double beta_deg, double R_mm,
+ double d_inter_mm)
+{
+ double remain = 0, Aa = 0, Va = 0, Vd;
+ double turnx, turny;
+ double a_rad = RAD(a_deg);
+
+ Vd = traj->d_speed;
+ if (calc_clitoid(traj, x, y, a_rad, alpha_deg, beta_deg, R_mm,
+ Vd, traj->a_acc, d_inter_mm,
+ &Aa, &Va, &remain) < 0) {
+ DEBUG(E_TRAJECTORY, "%s() calc_clitoid returned an error",
+ __FUNCTION__);
+ return -1;
+ }
+
+ delete_event(traj);
+ turnx = x + cos(a_rad) * remain;
+ turny = y + sin(a_rad) * remain;
+ traj->target.line.Aa = Aa;
+ traj->target.line.Va = Va;
+ traj->target.line.alpha = RAD(alpha_deg);
+ traj->target.line.R = R_mm;
+ traj->target.line.turn_pt.x = turnx;
+ traj->target.line.turn_pt.y = turny;
+ DEBUG(E_TRAJECTORY, "%s() turn_pt=%2.2f,%2.2f",
+ __FUNCTION__, turnx, turny);
+
+ __trajectory_line_abs(traj, x, y, turnx, turny,
+ advance);
+ traj->state = RUNNING_CLITOID_LINE;
+ trajectory_manager_event(traj);
+ schedule_event(traj);
+ return 0;
+}