// with the manual startpoint, which is a hint
private static Point2F prepare_segments_w_hinted_starpoint(Topographer topo, List <Line2F> segments, Segpool pool, double min_dist_to_wall, double general_tolerance, Point2F startpoint)
{
// same as automatic, but seek the segment with the closest end to startpoint
double min_dist = double.MaxValue;
Point2F tree_start = Point2F.Undefined;
foreach (Line2F seg in segments)
{
double r1 = topo.Get_dist_to_wall(seg.p1);
double r2 = topo.Get_dist_to_wall(seg.p2);
if (r1 >= min_dist_to_wall)
{
pool.Add(seg, false);
double dist = startpoint.DistanceTo(seg.p1);
if (dist < min_dist)
{
min_dist = dist;
tree_start = seg.p1;
}
}
if (r2 >= min_dist_to_wall)
{
pool.Add(seg, true);
double dist = startpoint.DistanceTo(seg.p2);
if (dist < min_dist)
{
min_dist = dist;
tree_start = seg.p2;
}
}
}
return(tree_start);
}
private bool should_flip_opened_startpoint(Polyline poly, Point2F startpoint)
{
Point2F start = (Point2F)poly.FirstPoint;
Point2F end = (Point2F)poly.LastPoint;
return(startpoint.DistanceTo(end) < startpoint.DistanceTo(start));
}
public bool Contains_point(Point2F pt)
{
ulong[] h = hash(pt);
for (int i = 0; i < 4; i++)
{
if (!_pool.ContainsKey(h[i]))
{
continue;
}
foreach (Line2F seg in _pool[h[i]])
{
if (pt.DistanceTo(seg.p1) < _tolerance)
{
return(true);
}
if (pt.DistanceTo(seg.p2) < _tolerance)
{
return(true);
}
}
}
return(false);
}
public Point2F Get_parametric_pt(double u)
{
if (_points.Count < 2)
{
return(_points[0]);
}
double offset = u * _len;
int seg = _seg_offsets.BinarySearch(offset);
// if there is no exact element found, binary search returns 1's complemented index of the
// first larger element, so we use it to find last smaller element
if (seg < 0)
{
seg = ~seg - 1;
}
offset -= _seg_offsets[seg];
Point2F p1 = _points[seg];
Point2F p2 = _points[seg + 1];
double dist = p2.DistanceTo(p1);
double x = p1.X + offset / dist * (p2.X - p1.X);
double y = p1.Y + offset / dist * (p2.Y - p1.Y);
return(new Point2F(x, y));
}
public List <Point2F> Pull_follow_points(Point2F join_pt)
{
List <Point2F> followers = new List <Point2F>();
List <Line2F> processed = new List <Line2F>();
ulong[] h = hash(join_pt);
for (int i = 0; i < 4; i++)
{
if (!_pool.ContainsKey(h[i]))
{
continue;
}
foreach (Line2F seg in _pool[h[i]])
{
if (processed.Contains(seg))
{
continue; // already got it
}
double d1 = join_pt.DistanceTo(seg.p1);
double d2 = join_pt.DistanceTo(seg.p2);
if (d1 > _tolerance && d2 > _tolerance)
{
continue;
}
if (d1 < d2)
{
followers.Add(seg.p2);
}
else
{
followers.Add(seg.p1);
}
processed.Add(seg);
}
}
foreach (Line2F seg in processed)
{
Remove(seg);
}
return(followers);
}
// with the manual startpoint
private static Point2F prepare_segments_w_manual_starpoint(Topographer topo, List <Line2F> segments, Segpool pool, double min_dist_to_wall, double general_tolerance, Point2F startpoint)
{
// same as automatic, but seek the segment with the closest end to startpoint
if (!topo.Is_line_inside_region(new Line2F(startpoint, startpoint), general_tolerance))
{
Logger.warn("startpoint is outside the pocket");
return(Point2F.Undefined);
}
if (topo.Get_dist_to_wall(startpoint) < min_dist_to_wall)
{
Logger.warn("startpoint radius < tool radius");
return(Point2F.Undefined);
}
double min_dist = double.MaxValue;
Point2F tree_start = Point2F.Undefined;
foreach (Line2F seg in segments)
{
double r1 = topo.Get_dist_to_wall(seg.p1);
double r2 = topo.Get_dist_to_wall(seg.p2);
if (r1 >= min_dist_to_wall)
{
pool.Add(seg, false);
double dist = startpoint.DistanceTo(seg.p1);
if (dist < min_dist && topo.Is_line_inside_region(new Line2F(startpoint, seg.p1), general_tolerance))
{
min_dist = dist;
tree_start = seg.p1;
}
}
if (r2 >= min_dist_to_wall)
{
pool.Add(seg, true);
double dist = startpoint.DistanceTo(seg.p2);
if (dist < min_dist && topo.Is_line_inside_region(new Line2F(startpoint, seg.p2), general_tolerance))
{
min_dist = dist;
tree_start = seg.p2;
}
}
}
return(tree_start);
}
public void Change_startpoint(Point2F p1)
{
if (_parent != null)
{
throw new Exception("startpoint may be changed for the root slice only");
}
if (Math.Abs((p1.DistanceTo(this.Center) - this.Radius) / this.Radius) > 0.001)
{
throw new Exception("new startpoint is outside the initial circle");
}
create_arc_circle(p1, Dir);
}
public void Append_spiral(Point2F start, Point2F end, Vector2d start_tangent, double ted, double tool_r, RotationDirection dir)
{
Sliced_path_item spiral = new Sliced_path_item(Sliced_path_item_type.SPIRAL);
double spacing = Spiral_generator.Calc_reverse_spacing(end.DistanceTo(start), tool_r, ted, _general_tolerance);
foreach (Biarc2d biarc in Spiral_generator.Gen_archimedean_spiral(start, end, start_tangent, spacing, dir))
{
spiral.Add(biarc, _general_tolerance);
}
Path.Add(spiral);
}
public List <Point2F> sample_curve_exact(Polyline p, double step)
{
List <Point2F> points = new List <Point2F>();
foreach (Point3F pt in PointListUtils.CreatePointlistFromPolylineStep(p, step).Points)
{
points.Add((Point2F)pt);
}
Point2F last_sample = points[points.Count - 1];
Point2F poly_end = (Point2F)p.LastPoint;
if (last_sample.DistanceTo(poly_end) > step * 0.001)
{
points.Add(poly_end);
}
return(points);
}
public void Bisect(Branch_visitor visitor, ref double t, double stop_distance)
{
if (t < 0.0 || t > 1.0)
{
throw new Exception("branch bisector was called with a wrong range");
}
double left = t;
double right = 1.0;
double mid;
while (true)
{
mid = (left + right) / 2;
Point2F pt = _curve.Get_parametric_pt(mid);
int result = visitor(pt);
if (result == 0)
{
break;
}
if (result < 0)
{
right = mid;
}
else if (result > 0)
{
left = mid;
}
Point2F other = _curve.Get_parametric_pt(left == mid ? right : left);
if (pt.DistanceTo(other) < stop_distance) // range has shrinked, stop search
{
break;
}
}
t = mid;
}
// we are collecting all the intersections and tracking the list of balls we're inside
// at any given point. If list becomes empty, we can't shortcut
public bool Is_line_inside_region(Line2F line, double tolerance)
{
Point2F a = line.p1;
Point2F b = line.p2;
SortedList <double, List <Circle2F> > intersections = new SortedList <double, List <Circle2F> >();
List <Circle2F> running_balls = new List <Circle2F>();
foreach (Circle2F ball in find_intersecting_balls(line))
{
Line2F insects = ball.LineIntersect(line, tolerance);
if (insects.p1.IsUndefined && insects.p2.IsUndefined)
{
// no intersections: check if whole path lay inside the circle
if (a.DistanceTo(ball.Center) < ball.Radius + tolerance &&
b.DistanceTo(ball.Center) < ball.Radius + tolerance)
{
return(true);
}
}
else if (insects.p1.IsUndefined || insects.p2.IsUndefined)
{
// single intersection. one of the path ends must be inside the circle, otherwise it is a tangent case
// and should be ignored
if (a.DistanceTo(ball.Center) < ball.Radius + tolerance)
{
running_balls.Add(ball);
}
else if (b.DistanceTo(ball.Center) < ball.Radius + tolerance)
{
;
}
else
{
continue;
}
Point2F c = insects.p1.IsUndefined ? insects.p2 : insects.p1;
double d = c.DistanceTo(a);
if (!intersections.ContainsKey(d))
{
intersections.Add(d, new List <Circle2F>());
}
intersections[d].Add(ball);
}
else
{
// double intersection
double d = insects.p1.DistanceTo(a);
if (!intersections.ContainsKey(d))
{
intersections.Add(d, new List <Circle2F>());
}
intersections[d].Add(ball);
d = insects.p2.DistanceTo(a);
if (!intersections.ContainsKey(d))
{
intersections.Add(d, new List <Circle2F>());
}
intersections[d].Add(ball);
}
}
if (running_balls.Count == 0)
{
return(false);
}
foreach (var ins in intersections)
{
foreach (Circle2F s in ins.Value)
{
if (running_balls.Contains(s))
{
running_balls.Remove(s);
}
else
{
running_balls.Add(s);
}
}
if (running_balls.Count == 0 && (ins.Key + tolerance < a.DistanceTo(b)))
{
return(false);
}
}
return(true);
}
// simple Archimedean spiral:
// r = a * theta + c
//
// in cartesian coordinates:
// x = center_x + (a * theta + c) * cos(theta)
// y = center_y + (a * theta + c) * sin(theta)
//
// tangent vector (derivate relative to theta):
// x' = a * cos(theta) - sin(theta) * r
// y' = a * sin(theta) + cos(theta) * r
// we output biarcs to approximate spiral, 6 biarcs (12 arcs) per loop.
// real number of biarcs would be more, since in general case there would be incomplete loops
// last biarc is exact to the endpoint
// if start tangent is defined, spiral will start from it (spacing will be reduced a little to make a fit).
// otherwise start tangent is choosed automatically and spacing is exact.
public static List <Biarc2d> Gen_archimedean_spiral(Point2F center, Point2F end, Vector2d start_tangent, double spacing, RotationDirection dir)
{
double r_max = center.DistanceTo(end);
Vector2d v_end = new Vector2d(center, end);
double theta_end = v_end.Ccw_angle;
double theta_step = 2 * Math.PI / 6;
double a = spacing / (2 * Math.PI);
double theta_start;
if (dir == RotationDirection.CW)
{
a = -a;
theta_step = -theta_step;
}
if (double.IsNaN(start_tangent.X))
{
theta_start = theta_end - r_max / a;
}
else
{
theta_start = start_tangent.Ccw_angle;
double candidate_a;
if (dir == RotationDirection.CCW)
{
while (theta_start >= theta_end)
{
theta_start -= 2 * Math.PI;
}
while (true)
{
candidate_a = r_max / (theta_end - theta_start);
if (candidate_a <= a)
{
break;
}
theta_start -= 2 * Math.PI;
}
}
else
{
while (theta_start <= theta_end)
{
theta_start += 2 * Math.PI;
}
while (true)
{
candidate_a = r_max / (theta_end - theta_start);
if (candidate_a >= a)
{
break;
}
theta_start += 2 * Math.PI;
}
}
a = candidate_a;
}
int nsegments = (int)Math.Floor((theta_end - theta_start) / theta_step);
double c = -theta_start * a;
Point2F p1 = new Point2F();
Vector2d t1 = new Vector2d();
List <Biarc2d> spiral = new List <Biarc2d>();
for (int segidx = 0; segidx < nsegments; segidx++)
{
double theta = theta_start + segidx * theta_step;
double r = a * theta + c;
double sin = Math.Sin(theta);
double cos = Math.Cos(theta);
Point2F p2 = center + new Point2F(r * cos, r * sin);
Vector2d t2 = new Vector2d(a * cos - r * sin, a * sin + r * cos).Unit();
if (dir == RotationDirection.CW)
{
t2 = t2.Inverted();
}
if (segidx != 0)
{
spiral.Add(new Biarc2d(p1, t1, p2, t2));
}
p1 = p2;
t1 = t2;
}
Vector2d t_end = v_end.Normal().Unit();
if (dir == RotationDirection.CW)
{
t_end = t_end.Inverted();
}
spiral.Add(new Biarc2d(p1, t1, end, t_end));
return(spiral);
}
public void Refine(List <Slice> colliding_slices, double end_clearance, double tool_r)
{
double clearance = end_clearance;
// check if arc is small. refining is worthless in this case
// criterion for smallness: there should be at least 4 segments with chord = clearance, plus
// one segment to space ends far enough. A pentagon with a 5 segments with edge length = clearance
// will define the min radius of circumscribed circle. clearance = 2 * R * sin (Pi / 5),
// R = clearance / 2 / sin (Pi / 5)
if (_segments.Count != 1)
{
throw new Exception("attempt to refine slice with n segments != 1");
}
Arc2F arc = _segments[0];
double r_min = clearance / 2 / Math.Sin(Math.PI / 5.0);
if (arc.Radius <= r_min)
{
return;
}
if (colliding_slices.Contains(this))
{
throw new Exception("attempt to collide slice with itself");
}
// now apply the colliding slices. to keep things simple and robust, we apply just one slice - the one who trims
// us most (removed length of arc is greatest).
// end clearance adjustment:
// to guarantee the tool will never hit the unmilled area while rapiding between segments,
// arc will always have original ends, trimming will happen in the middle only.
// to prevent the tool from milling extra small end segments and minimize numeric errors at small tangents,
// original ends would always stick at least for a clearance (chordal) length.
// i.e. if the point of intersection of arc and colliding circle is closer than clearance to the end,
// it is moved to clearance distance.
// there is a two cases of intersecting circles: with single intersection and with a double intersection.
// double intersections splits arc to three pieces (length of the middle part is the measure),
// single intesections splits arc in two (the part inside the circle is removed, its length is the measure).
// in both cases the intersection points are subject to "end clearance adjustment".
// single intersections are transformed to the double intersections, second point being one of the end clearances.
// TODO: calculate clearance point the right way, with math :-)
Line2F c1_insects = arc.CircleIntersect(new Circle2F(arc.P1, clearance));
Line2F c2_insects = arc.CircleIntersect(new Circle2F(arc.P2, clearance));
Point2F c1 = c1_insects.p1.IsUndefined ? c1_insects.p2 : c1_insects.p1;
Point2F c2 = c2_insects.p1.IsUndefined ? c2_insects.p2 : c2_insects.p1;
Line2F max_secant = new Line2F();
double max_sweep = 0;
foreach (Slice s in colliding_slices)
{
if (s == _parent)
{
continue; // no reason to process it
}
Line2F secant = arc.CircleIntersect(s.Ball);
if (secant.p1.IsUndefined && secant.p2.IsUndefined)
{
continue;
}
if (secant.p1.IsUndefined || secant.p2.IsUndefined)
{
// single intersection
Point2F splitpt = secant.p1.IsUndefined ? secant.p2 : secant.p1;
if (arc.P1.DistanceTo(s.Center) < arc.P2.DistanceTo(s.Center))
{
if (splitpt.DistanceTo(arc.P1) < clearance)
{
continue; // nothing to remove
}
else if (splitpt.DistanceTo(arc.P2) < clearance)
{
secant = new Line2F(c1, c2);
}
else
{
secant = new Line2F(c1, splitpt);
}
}
else
{
// remove second segment
if (splitpt.DistanceTo(arc.P2) < clearance)
{
continue;
}
else if (splitpt.DistanceTo(arc.P1) < clearance)
{
secant = new Line2F(c1, c2);
}
else
{
secant = new Line2F(splitpt, c2);
}
}
}
else
{
// double intersection
if (secant.p1.DistanceTo(arc.P1) < clearance)
{
secant.p1 = c1;
}
else if (secant.p1.DistanceTo(arc.P2) < clearance)
{
secant.p1 = c2;
}
if (secant.p2.DistanceTo(arc.P1) < clearance)
{
secant.p2 = c1;
}
else if (secant.p2.DistanceTo(arc.P2) < clearance)
{
secant.p2 = c2;
}
}
if (secant.p1.DistanceTo(secant.p2) < clearance * 2) // segment is too short, ignore it
{
continue;
}
// sort insects by sweep (already sorted for single, may be unsorted for the double)
Vector2d v_p1 = new Vector2d(arc.Center, arc.P1);
Vector2d v_ins1 = new Vector2d(arc.Center, secant.p1);
Vector2d v_ins2 = new Vector2d(arc.Center, secant.p2);
double sweep = angle_between_vectors(v_ins1, v_ins2, arc.Direction);
if (angle_between_vectors(v_p1, v_ins1, arc.Direction) > angle_between_vectors(v_p1, v_ins2, arc.Direction))
{
secant = new Line2F(secant.p2, secant.p1);
sweep = 2.0 * Math.PI - sweep;
}
if (sweep > max_sweep)
{
// ok, a last check - removed arc midpoint should be inside the colliding circle
Arc2F check_arc = new Arc2F(arc.Center, secant.p1, secant.p2, arc.Direction);
if (check_arc.Midpoint.DistanceTo(s.Center) < s.Radius)
{
max_sweep = sweep;
max_secant = secant;
}
}
}
if (max_sweep == 0)
{
return;
}
Arc2F head = new Arc2F(arc.Center, arc.P1, max_secant.p1, arc.Direction);
Arc2F tail = new Arc2F(arc.Center, max_secant.p2, arc.P2, arc.Direction);
_segments.Clear();
_segments.Add(head);
_segments.Add(tail);
// recalculate max TED accounting for the new endpoints.
// this TED is 'virtual' and averaged with previous max_ted to make arcs look more pretty
// if ends of removed segment are at the same side of direction vector,
// midpoint is still present, _max_ted is valid and is maximum for sure
Vector2d v_move = new Vector2d(_parent.Center, this.Center);
Vector2d v_removed_p1 = new Vector2d(_parent.Center, max_secant.p1);
Vector2d v_removed_p2 = new Vector2d(_parent.Center, max_secant.p2);
if (v_move.Det(v_removed_p1) * v_move.Det(v_removed_p2) > 0)
{
return;
}
double ted_head_end = calc_ted(max_secant.p1, tool_r);
double ted_tail_start = calc_ted(max_secant.p2, tool_r);
double ted = Math.Max(ted_head_end, ted_tail_start);
if (ted <= 0)
{
Logger.err("max TED vanished after refining the slice !");
return;
}
ted = (ted + _mid_ted) / 2;
_max_ted = Math.Max(ted, _entry_ted);
}
