- double line1_a_rad, line1_a_deg, line2_a_rad;
- double diff_a_deg, diff_a_deg_abs, beta_deg;
- double radius;
- struct line_2pts l1, l2;
- line_t ll1, ll2;
- point_t p;
-
- /* convert to 2 points */
- num2line(&l1, dir1, num1);
- num2line(&l2, dir2, num2);
-
- printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
- l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
- printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
- l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
-
- /* convert to line eq and find intersection */
- pts2line(&l1.p1, &l1.p2, &ll1);
- pts2line(&l2.p1, &l2.p2, &ll2);
- intersect_line(&ll1, &ll2, &p);
-
- line1_a_rad = atan2(l1.p2.y - l1.p1.y,
- l1.p2.x - l1.p1.x);
- line1_a_deg = DEG(line1_a_rad);
- line2_a_rad = atan2(l2.p2.y - l2.p1.y,
- l2.p2.x - l2.p1.x);
- diff_a_deg = DEG(line2_a_rad - line1_a_rad);
- diff_a_deg_abs = fabs(diff_a_deg);
-
- if (diff_a_deg_abs < 70.) {
- radius = 200;
- if (diff_a_deg > 0)
- beta_deg = 40;
- else
- beta_deg = -40;
- }
- else if (diff_a_deg_abs < 100.) {
- radius = 100;
- if (diff_a_deg > 0)
- beta_deg = 40;
- else
- beta_deg = -40;
- }
- else {
- radius = 120;
- if (diff_a_deg > 0)
- beta_deg = 60;
- else
- beta_deg = -60;
- }
- trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
- line1_a_deg, 150., diff_a_deg, beta_deg,
- radius, xy_norm(l1.p1.x, l1.p1.y,
- p.x, p.y));
- wait_traj_end(0xFF);