private static double get_ted(double prev_r, double this_r, double tool_r)
{
double prev_wall_r = prev_r + tool_r;
double this_wall_r = this_r + tool_r;
if (this_r - prev_wall_r >= tool_r)
{
return(double.MaxValue);
}
if (prev_wall_r - this_r >= tool_r)
{
return(double.MinValue);
}
// get intersection of two cicles = cutter circle and prev wall
Circle2F prev_wall = new Circle2F(new Point2F(0, 0), prev_wall_r);
Circle2F tool_circle = new Circle2F(new Point2F(this_r, 0), tool_r);
Line2F insects = prev_wall.CircleIntersect(tool_circle);
if (insects.p1.IsUndefined || insects.p2.IsUndefined)
{
return(0);
}
double ted = this_wall_r - insects.p1.X;
return(ted);
}
public bool Is_line_inside_region(Line2F line, bool should_analize_inner_intersections, double tolerance)
{
if (!_outline.PointInPolyline(line.p1, tolerance))
{
return(false); // p1 is outside of outer curve boundary
}
if (!_outline.PointInPolyline(line.p2, tolerance))
{
return(false); // p2 is outside of outer curve boundary
}
if (should_analize_inner_intersections && _outline.LineIntersections(line, tolerance).Length != 0)
{
return(false); // both endpoints are inside, but there are intersections, outer curve must be concave
}
foreach (Polyline island in _islands)
{
if (island.PointInPolyline(line.p1, tolerance))
{
return(false); // p1 is inside hole
}
if (island.PointInPolyline(line.p2, tolerance))
{
return(false); // p2 is inside hole
}
if (should_analize_inner_intersections && island.LineIntersections(line, tolerance).Length != 0)
{
return(false); // p1, p2 are outside hole, but there are intersections
}
}
return(true);
}
private double calc_ted(Point2F tool_center, double tool_r)
{
Circle2F parent_wall = new Circle2F(_parent.Center, _parent.Radius + tool_r);
// find the points tool touching parent wall
Circle2F cut_circle = new Circle2F(tool_center, tool_r);
Line2F insects = cut_circle.CircleIntersect(parent_wall);
if (insects.p1.IsUndefined || insects.p2.IsUndefined)
{
Logger.err("no wall intersections #0");
return(0);
}
// we're interested in tool head point, so choose more advanced point in mill direction
Vector2d v1 = new Vector2d(_parent.Center, insects.p1);
Vector2d v_parent_to_tool_center = new Vector2d(_parent.Center, tool_center);
Point2F cut_head = v_parent_to_tool_center.Det(v1) * (int)this.Dir > 0 ? insects.p1 : insects.p2;
// project headpoint to the center find TED
Vector2d v_me_to_tool_center = new Vector2d(this.Center, tool_center);
Vector2d v_me_to_cut_head = new Vector2d(this.Center, cut_head);
double ted = this.Radius + tool_r - v_me_to_cut_head * v_me_to_tool_center.Unit();
return(ted > 0 ? ted : 0);
}
private void calc_teds(double tool_r)
{
// expand both slices to form walls
Circle2F parent_wall = new Circle2F(_parent.Center, _parent.Radius + tool_r);
Circle2F this_wall = new Circle2F(this.Center, this.Radius + tool_r);
// find the point of cut start (intersection of walls)
Line2F wall_insects = parent_wall.CircleIntersect(this_wall);
if (wall_insects.p1.IsUndefined || wall_insects.p2.IsUndefined)
{
Logger.err("no wall intersections #1");
return;
}
Vector2d v_move = new Vector2d(_parent.Center, this.Center);
Vector2d v1 = new Vector2d(_parent.Center, wall_insects.p1);
Point2F cut_tail = v_move.Det(v1) * (int)this.Dir < 0 ? wall_insects.p1 : wall_insects.p2;
Vector2d v_tail = new Vector2d(this.Center, cut_tail);
Point2F entry_tool_center = this.Center + (v_tail.Unit() * this.Radius).Point;
_entry_ted = calc_ted(entry_tool_center, tool_r);
Point2F mid_tool_center = this.Center + (v_move.Unit() * this.Radius).Point;
_mid_ted = calc_ted(mid_tool_center, tool_r);
}
private void insert_in_t4(Polyline p)
{
for (int i = 0; i < p.NumSegments; i++)
{
object seg = p.GetSegment(i);
T4_rect rect;
if (seg is Line2F)
{
Line2F line = ((Line2F)seg);
rect = new T4_rect(Math.Min(line.p1.X, line.p2.X),
Math.Min(line.p1.Y, line.p2.Y),
Math.Max(line.p1.X, line.p2.X),
Math.Max(line.p1.Y, line.p2.Y));
}
else if (seg is Arc2F)
{
Point2F min = Point2F.Undefined;
Point2F max = Point2F.Undefined;
((Arc2F)seg).GetExtrema(ref min, ref max);
rect = new T4_rect(min.X, min.Y, max.X, max.Y);
}
else
{
throw new Exception("unknown segment type");
}
_t4.Add(rect, seg);
}
}
private void remove_seg(ulong h, Line2F seg)
{
if (_pool.ContainsKey(h))
{
_pool[h].Remove(seg);
}
}
// add line hashed by the beginning or end point
public void Add(Line2F seg, bool reverse)
{
ulong[] h = hash(reverse ? seg.p2 : seg.p1);
insert_seg(h[0], seg);
insert_seg(h[1], seg);
insert_seg(h[2], seg);
insert_seg(h[3], seg);
}
private void insert_seg(ulong h, Line2F seg)
{
if (!_pool.ContainsKey(h))
{
_pool[h] = new List <Line2F>();
}
if (!_pool[h].Contains(seg))
{
_pool[h].Add(seg);
}
}
public void Remove(Line2F seg)
{
ulong[] h1 = hash(seg.p1);
remove_seg(h1[0], seg);
remove_seg(h1[1], seg);
remove_seg(h1[2], seg);
remove_seg(h1[3], seg);
ulong[] h2 = hash(seg.p2);
remove_seg(h2[0], seg);
remove_seg(h2[1], seg);
remove_seg(h2[2], seg);
remove_seg(h2[3], seg);
}
private List <Circle2F> find_intersecting_balls(Line2F line)
{
T4_rect rect = new T4_rect(Math.Min(line.p1.X, line.p2.X),
Math.Min(line.p1.Y, line.p2.Y),
Math.Max(line.p1.X, line.p2.X),
Math.Max(line.p1.Y, line.p2.Y));
List <Circle2F> balls = new List <Circle2F>();
// since objects in t4 are generic, convert convert rects backs to balls
foreach (T4_rect ballrect in _t4.Get_colliding_obj_rects(rect))
{
balls.Add(new Circle2F(new Point2F(ballrect.Xc, ballrect.Yc), ballrect.W / 2));
}
return(balls);
}
private Vector2F calc_start_tangent(Polyline poly)
{
object obj = poly.GetSegment(0);
Vector2F tangent = new Vector2F();
if (obj is Line2F)
{
Line2F line = (Line2F)obj;
tangent = new Vector2F(line.p1, line.p2);
}
else if (obj is Arc2F)
{
Arc2F arc = (Arc2F)obj;
tangent = new Vector2F(arc.Center, arc.P1).Normal();
if (arc.Direction == RotationDirection.CW)
{
tangent.Invert();
}
}
return(tangent);
}
private Sliced_path generate_path(Slice_sequence sequence)
{
Sliced_path_generator gen;
if (!_should_smooth_chords)
{
gen = new Sliced_path_generator(_general_tolerance);
}
else
{
gen = new Sliced_path_smooth_generator(_general_tolerance, 0.1 * _tool_r);
}
// NOTE: this manipulations are to make the beginning of spiral tangent to the polyline
object obj = _poly.GetSegment(0);
Vector2d tangent = new Vector2d();
if (obj is Line2F)
{
Line2F line = (Line2F)obj;
tangent = new Vector2d(line.p1, line.p2);
}
else if (obj is Arc2F)
{
Arc2F arc = (Arc2F)obj;
tangent = new Vector2d(arc.Center, arc.P1).Normal();
if (arc.Direction == RotationDirection.CW)
{
tangent = tangent.Inverted();
}
}
gen.Append_spiral(sequence.Root_slice.Center, sequence.Root_slice.End, tangent, _max_ted, _tool_r, _dir == RotationDirection.Unknown ? RotationDirection.CCW : _dir);
gen.Append_slice_sequence(sequence);
return(gen.Path);
}
private static object[] calc_arcs(Point2F p1, Vector2d t1, Point2F p2, Vector2d t2, Point2F pm)
{
object[] segs = new object[2];
Vector2d n1 = t1.Normal();
Vector2d pm_minus_p1 = new Vector2d(pm - p1);
double c1_denom = 2 * (n1 * pm_minus_p1);
if (c1_denom == 0) // c1 is at infinity, replace arc with line
{
segs[0] = new Line2F(p1, pm);
}
else
{
Point2F c1 = p1 + (pm_minus_p1 * pm_minus_p1 / c1_denom * n1).Point;
RotationDirection dir1 = new Vector2d(p1 - c1) * n1 > 0 ? RotationDirection.CW : RotationDirection.CCW;
segs[0] = new Arc2F(c1, p1, pm, dir1);
}
Vector2d n2 = t2.Normal();
Vector2d pm_minus_p2 = new Vector2d(pm - p2);
double c2_denom = 2 * (n2 * pm_minus_p2);
if (c2_denom == 0) // c2 is at infinity, replace arc with line
{
segs[1] = new Line2F(pm, p2);
}
else
{
Point2F c2 = p2 + (pm_minus_p2 * pm_minus_p2 / c2_denom * n2).Point;
RotationDirection dir2 = new Vector2d(p2 - c2) * n2 > 0 ? RotationDirection.CW : RotationDirection.CCW;
segs[1] = new Arc2F(c2, pm, p2, dir2);
}
return(segs);
}
private static List <Line2F> get_medial_axis_segments(Topographer topo, List <Point2F> samples, double general_tolerance)
{
double[] xs = new double[samples.Count + 1];
double[] ys = new double[samples.Count + 1];
double min_x = double.MaxValue;
double max_x = double.MinValue;
double min_y = double.MaxValue;
double max_y = double.MinValue;
// HACK
// There is a bug in Voronoi generator implementation. Sometimes it produces a completely crazy partitioning.
// Looks like its overly sensitive to the first processed points, their count and location. If first stages
// go ok, then everything comes nice. Beeing a Steven Fortune's algorithm, it process points by a moving sweep line.
// Looks like the line is moving from the bottom to the top, thus sensitive points are the most bottom ones.
// We try to cheat and add one more point so it would be the single most bottom point.
// Then generator initially will see just one point, do a right magic and continue with a sane result :-)
// We place this initial point under the lefmost bottom point at the sufficient distance,
// then these two points will form a separate Voronoi cite not influencing the remaining partition.
// Sufficient distance is defined as region width / 2 for now.
int lb_idx = 0;
for (int i = 0; i < samples.Count; i++)
{
xs[i] = samples[i].X;
ys[i] = samples[i].Y;
if (xs[i] < min_x)
{
min_x = xs[i];
}
if (xs[i] > max_x)
{
max_x = xs[i];
}
if (ys[i] > max_y)
{
max_y = ys[i];
}
if (ys[i] <= min_y)
{
if (ys[i] < min_y)
{
min_y = ys[i];
lb_idx = i; // stricly less, it's a new leftmost bottom for sure
}
else
{
if (xs[i] < xs[lb_idx]) // it's a new leftmost bottom if more lefty
{
lb_idx = i;
}
}
}
}
double width = max_x - min_x;
xs[samples.Count] = xs[lb_idx];
ys[samples.Count] = ys[lb_idx] - width / 2;
min_x -= VORONOI_MARGIN;
max_x += VORONOI_MARGIN;
min_y -= VORONOI_MARGIN + width / 2;
max_y += VORONOI_MARGIN;
List <GraphEdge> edges = new Voronoi(general_tolerance).generateVoronoi(xs, ys, min_x, max_x, min_y, max_y);
Logger.log("voronoi partitioning completed. got {0} edges", edges.Count);
List <Line2F> inner_segments = new List <Line2F>();
foreach (GraphEdge e in edges)
{
Line2F seg = new Line2F(e.x1, e.y1, e.x2, e.y2);
if (seg.Length() < double.Epsilon)
{
continue; // extra small segment, discard
}
if (!topo.Is_line_inside_region(seg, ANALIZE_INNER_INTERSECTIONS, general_tolerance))
{
continue;
}
inner_segments.Add(seg);
}
return(inner_segments);
}
// 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);
}
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);
}
public bool Is_line_inside_region(Line2F line, double tolerance)
{
return(Is_line_inside_region(line, true, tolerance));
}
public Slice(Slice parent, Point2F center, double radius, RotationDirection dir, double tool_r, Point2F magnet)
{
_parent = parent;
_ball = new Circle2F(center, radius);
double dist = Point2F.Distance(center, parent.Center);
double delta_r = this.Radius - parent.Radius;
double coarse_ted = dist + delta_r;
// 1) one ball is inside other, no intersections
// 2) balls are spaced too far and do not intersect, TED is 0, distance is positive
// 3) balls are intersect, TED is valid
if (dist <= Math.Abs(delta_r))
{
_placement = Slice_placement.INSIDE_ANOTHER;
return;
}
if (dist >= this.Radius + parent.Radius)
{
_placement = Slice_placement.TOO_FAR;
return;
}
// TED can't be more >= tool diameter, this check prevents fails during the calculation of real angle-based TED
if (coarse_ted > tool_r * 1.999)
{
_placement = Slice_placement.TOO_FAR;
return;
}
Line2F insects = _parent.Ball.CircleIntersect(this._ball);
if (insects.p1.IsUndefined || insects.p2.IsUndefined)
{
// try to return meaningful result even if CB routine had failed (unlikely)
Logger.err("no intersections found there intersections should be (_parent.Ball.CircleIntersect(_ball))");
_placement = (dist <= this.Radius || dist <= parent.Radius) ? Slice_placement.INSIDE_ANOTHER : Slice_placement.TOO_FAR;
return;
}
_placement = Slice_placement.NORMAL;
Vector2d v_move = new Vector2d(_parent.Center, this.Center);
Vector2d v1 = new Vector2d(_parent.Center, insects.p1);
RotationDirection default_dir = v_move.Det(v1) > 0 ? RotationDirection.CW : RotationDirection.CCW;
if (dir == RotationDirection.Unknown)
{
if (magnet.IsUndefined)
{
dir = RotationDirection.CCW; // just something
}
else
{
if (insects.p1.DistanceTo(magnet) < insects.p2.DistanceTo(magnet)) // match, use existing dir
{
dir = default_dir;
}
else
{
dir = (default_dir == RotationDirection.CCW) ? RotationDirection.CW : RotationDirection.CCW; // flip
}
}
}
Arc2F arc;
if (default_dir == dir)
{
arc = new Arc2F(this.Center, insects.p1, insects.p2, dir);
}
else
{
arc = new Arc2F(this.Center, insects.p2, insects.p1, dir);
}
_segments.Add(arc);
calc_teds(tool_r);
if (_mid_ted <= 0)
{
Logger.warn("forced to patch mid_ted");
_mid_ted = coarse_ted; // shouldn't be, but ok
}
_max_ted = Math.Max(_mid_ted, _entry_ted);
}
//-----------------------------------------------------------------------------
// Vector graphics
//-----------------------------------------------------------------------------
// Draws the specified line.
public void DrawLine(Line2F line, float thickness, Color color, float depth = 0.0f)
{
DrawImage(white1x1, line.Center + new Vector2F(0.5f, 0.5f), new Vector2F(0.5f, 0.5f),
new Vector2F((line.Length + thickness), thickness),
line.Direction, color, SpriteEffects.None, depth);
}