public double DistanceSquared(Vector2D p, out int iNearSeg, out double fNearSegT)
{
iNearSeg = -1;
fNearSegT = double.MaxValue;
double dist = double.MaxValue;
int N = vertices.Count;
for (int vi = 0; vi < N; ++vi)
{
Segment2d seg = new Segment2d(vertices[vi], vertices[(vi + 1) % N]);
double t = (p - seg.Center).Dot(seg.Direction);
double d = double.MaxValue;
if (t >= seg.Extent)
{
d = seg.P1.DistanceSquared(p);
}
else if (t <= -seg.Extent)
{
d = seg.P0.DistanceSquared(p);
}
else
{
d = (seg.PointAt(t) - p).LengthSquared;
}
if (d < dist)
{
dist = d;
iNearSeg = vi;
fNearSegT = t;
}
}
return(dist);
}
public void TrimSegment2d_SegmentCollinear()
{
// Checks that when the segment is coincident with segments of the polygon,
// segments that lie on polygon edges are not included, as per the intended
// behavior.
// Arrange
var square = new GeneralPolygon2d(new Polygon2d(new double[] {
0, 0,
3, 0,
3, 1,
2, 1,
2, 1.3,
2, 1.6,
2, 2,
3, 2,
3, 3,
0, 3,
}));
var seg = new Segment2d(new Vector2d(2, 0.5), new Vector2d(2, 2.5));
// Act
var result = square.TrimSegment2d(seg);
// Assert
Assert.AreEqual(2, result.Count);
AssertExtensions.AreEqual(new Vector2d(2, 0.5), result[0].P0);
AssertExtensions.AreEqual(new Vector2d(2, 1.0), result[0].P1);
AssertExtensions.AreEqual(new Vector2d(2, 2.0), result[1].P0);
AssertExtensions.AreEqual(new Vector2d(2, 2.5), result[1].P1);
}
public static void Restore(out IParametricCurve2d curve, BinaryReader reader)
{
curve = null;
int nType = reader.ReadInt32();
if (nType == 1)
{
Segment2d segment = new Segment2d();
Restore(ref segment, reader);
curve = segment;
}
else if (nType == 2)
{
Circle2d circle = new Circle2d(Vector2D.Zero, 1.0);
Restore(ref circle, reader);
curve = circle;
}
else if (nType == 3)
{
Arc2d arc = new Arc2d(Vector2D.Zero, 1.0, 0, 1);
Restore(ref arc, reader);
curve = arc;
}
else if (nType == 100)
{
ParametricCurveSequence2 seq = new ParametricCurveSequence2();
Restore(ref seq, reader);
curve = seq;
}
else
{
throw new Exception("gSerialization.Restore: IParametricCurve2D : unknown curve type " + nType.ToString());
}
}
void append_path(LinearToolpath3 <PrintVertex> path, double cellSize)
{
double threshSqr = 4 * cellSize * cellSize;
int NV = path.VertexCount - 1;
for (int i = 0; i < NV; ++i)
{
Vector2d a = path[i].Position.xy;
Vector2d b = path[i + 1].Position.xy;
double dist_sqr = a.DistanceSquared(b);
if (dist_sqr < threshSqr)
{
Segment2d seg = new Segment2d(a, b);
append_segment(ref seg);
}
else
{
int n = (int)(dist_sqr / threshSqr) + 1;
Vector2d prev = a;
for (int k = 1; k <= n; ++k)
{
double t = ((double)k / (double)n);
Vector2d next = Vector2d.Lerp(a, b, t);
Segment2d seg = new Segment2d(prev, next);
append_segment(ref seg);
prev = next;
}
}
}
}
public static bool IsOutside(this GeneralPolygon2d poly, Segment2d seg)
{
bool isOutside = true;
if (poly.Outer.IsMember(seg, out isOutside))
{
if (isOutside)
{
return(true);
}
else
{
return(false);
}
}
foreach (Polygon2d hole in poly.Holes)
{
if (hole.IsMember(seg, out isOutside))
{
if (isOutside)
{
return(true);
}
else
{
return(false);
}
}
}
return(false);
}
static void Main(string[] args)
{
var seg1 = new Segment2d(new Vector2d(-2, -1), new Vector2d(2, 3));
var seg2 = new Segment2d(new Vector2d(2, 3), new Vector2d(3, -2));
var seg3 = new Segment2d(new Vector2d(3, -2), new Vector2d(-2, -1));
var arr = new[] { seg1, seg2, seg3 };
var result1 = Vec.SegmentIntersection(arr);
var list = new List <string>();
string input = null;
while (!string.IsNullOrEmpty(input = Console.In.ReadLine()))
{
list.Add(input);
}
if (!list.Any())
{
Console.Error.WriteLine("CMD LINE ERROR!!!");
}
IProcessTask task1_1 = new PolygonsIntersection();
var result = task1_1.ProcessTask(list.ToArray());
Output(result);
}
void append_segment(ref Segment2d seg)
{
int idx = segments.size;
segments.push_back(seg);
grid.InsertSegment(idx, seg.Center, seg.Extent);
}
public double FindClosestElementToPoint(Vector2d point, out ElementLocation location)
{
location = new ElementLocation(int.MinValue, 0);
double closestDistanceSquared = double.MaxValue;
int currentElementIndex = 0;
foreach (var element in elements)
{
// Update results if current element is closer
Segment2d seg = element.GetSegment2d();
double currentSegmentClosestDistanceSquared = seg.DistanceSquared(point);
// Update results if current element is closer
if (currentSegmentClosestDistanceSquared < closestDistanceSquared)
{
closestDistanceSquared = currentSegmentClosestDistanceSquared;
location.Index = currentElementIndex;
location.ParameterizedDistance = GetParameterizedDistance(point, seg);
}
currentElementIndex++;
}
// For consistency, if the closest point is on a vertex,
// give the index of the element after the vertex
if (MathUtil.EpsilonEqual(location.ParameterizedDistance, 1, 1e-6))
{
location.ParameterizedDistance = 0;
location.Index = (location.Index + 1) % elements.Count;
}
return(Math.Sqrt(closestDistanceSquared));
}
public static bool Circle2dWithOrientedRectangle2d(Circle2d circle, OrientedRectangle2d rectangle)
{
Vector3L forward = RotateHelper.GetForward(rectangle.m_rotation);
Vector3L right = Vector3L.Cross(Vector3L.up, forward);
Vector3L pos = new Vector3L(rectangle.m_pos.x, 0, rectangle.m_pos.y);
Vector3L a = pos + forward * rectangle.m_length * 0.5f + -right * rectangle.m_width * 0.5f;
Vector3L b = pos + forward * rectangle.m_length * 0.5f + right * rectangle.m_width * 0.5f;
Vector3L c = pos + -forward * rectangle.m_length * 0.5f + right * rectangle.m_width * 0.5f;
Vector3L d = pos + -forward * rectangle.m_length * 0.5f + -right * rectangle.m_width * 0.5f;
List <Vector2L> lineList = new List <Vector2L>();
lineList.Add(new Vector2L(a.x, a.z));
lineList.Add(new Vector2L(b.x, b.z));
lineList.Add(new Vector2L(c.x, c.z));
lineList.Add(new Vector2L(d.x, d.z));
for (int i = 0; i < lineList.Count; ++i)
{
Segment2d segment = new Segment2d();
segment.m_point1 = lineList[i];
segment.m_point2 = lineList[(i + 1) % lineList.Count];
if (Segment2dWithCircle2d(segment, circle))
{
return(true);
}
}
return(false);
}
public static Segment3d OutFrame(this Segment2d _seg2d, Frame3f _frame)
{
var seg3f = new Segment3f
(_frame.FromPlaneUV((Vector2f)_seg2d.P0, 2),
_frame.FromPlaneUV((Vector2f)_seg2d.P1, 2));
return(new Segment3d(seg3f.P0, seg3f.P1));
}
public static void Restore(ref Segment2d segment, BinaryReader reader)
{
segment.Center.x = reader.ReadDouble();
segment.Center.y = reader.ReadDouble();
segment.Direction.x = reader.ReadDouble();
segment.Direction.y = reader.ReadDouble();
segment.Extent = reader.ReadDouble();
}
public static void Store(Segment2d segment, BinaryWriter writer)
{
writer.Write(segment.Center.x);
writer.Write(segment.Center.y);
writer.Write(segment.Direction.x);
writer.Write(segment.Direction.y);
writer.Write(segment.Extent);
}
private void validate_path_caches()
{
if (path_cache_valid)
{
return;
}
double maxLen = 2.5f;
double maxLenSqr = maxLen * maxLen;
Segment2d invalid = new Segment2d(Vector2d.MaxValue, Vector2d.MaxValue);
below_grid = new SegmentHashGrid2d <Segment2d>(3 * maxLen, invalid);
current_grid = new SegmentHashGrid2d <Segment2d>(3 * maxLen, invalid);
Action <LinearToolpath3 <PrintVertex> > pathFuncF = (polyPath) => {
if (polyPath.Type != ToolpathTypes.Deposition)
{
return;
}
Vector3d v0 = polyPath.Start.Position;
byte layer_alpha = LayerFilterF(v0);
if (layer_alpha == 0)
{
return;
}
bool is_below = (layer_alpha < 255);
var grid = (is_below) ? below_grid : current_grid;
int N = polyPath.VertexCount;
for (int k = 0; k < N - 1; ++k)
{
Vector2d a = polyPath[k].Position.xy;
Vector2d b = polyPath[k + 1].Position.xy;
double d2 = a.DistanceSquared(b);
if (d2 < maxLenSqr)
{
Segment2d s = new Segment2d(a, b);
grid.InsertSegment(s, s.Center, s.Extent);
continue;
}
int subdivs = (int)(d2 / maxLenSqr);
Vector2d prev = a;
for (int i = 1; i <= subdivs; ++i)
{
double t = (double)i / (double)subdivs;
Vector2d next = Vector2d.Lerp(a, b, t);
Segment2d s = new Segment2d(prev, next);
grid.InsertSegment(s, s.Center, s.Extent);
prev = next;
}
}
};
ProcessLinearPaths(Paths, pathFuncF);
path_cache_valid = true;
}
public Vector2D GetNormal(int iSeg, double segT)
{
Segment2d seg = new Segment2d(vertices[iSeg], vertices[(iSeg + 1) % vertices.Count]);
double t = ((segT / seg.Extent) + 1.0) / 2.0;
Vector2D n0 = GetNormal(iSeg);
Vector2D n1 = GetNormal((iSeg + 1) % vertices.Count);
return((1.0 - t) * n0 + t * n1);
}
public static bool Segment2dWithCircle2d(Segment2d segment, Circle2d circle)
{
Segment3d segment3d = new Segment3d(
new Vector3L(segment.m_point1.x, 0, segment.m_point1.y),
new Vector3L(segment.m_point2.x, 0, segment.m_point2.y));
Vector3L closestPoint = Distance3d.ClosestPointOfPoint3dWithSegment3d(new Vector3L(circle.m_pos.x, 0, circle.m_pos.y), segment3d);
Vector2L distance = circle.m_pos - new Vector2L(closestPoint.x, closestPoint.z);
return(distance.magnitude <= circle.m_radius);
}
public static bool BiContains(this Polygon2d poly, Segment2d seg)
{
foreach (Segment2d thisSeg in poly.SegmentItr())
{
if (thisSeg.BiEquals(seg))
{
return(true);
}
}
return(false);
}
public static bool IsIn(this Segment2d segment2d01, List <Segment2d> segment2ds)
{
foreach (Segment2d segment2d in segment2ds)
{
if (segment2d01.IsSmae(segment2d))
{
return(true);
}
}
return(false);
}
static int Next(List <StatusPoint> arr1, Segment2d target)
{
if (!arr1.Any())
{
return(0);
}
var segComparer = new SegmentsOriginXComparer();
var idx = arr1.FindIndex(s => segComparer.Compare(s.Segment, target) == 1);
return(idx == -1 ? arr1.Count() : idx);
}
public virtual void ObserveGcodeLine(GCodeLine line)
{
if (line.Type != LineType.GCode)
{
return;
}
double x = VertexPrevious.Position.x;
double y = VertexPrevious.Position.y;
GCodeUtil.TryFindParamNum(line.Parameters, "X", ref x);
GCodeUtil.TryFindParamNum(line.Parameters, "Y", ref y);
VertexCurrent.Position = new Vector3d(x, y, 0);
double f = GCodeUtil.UnspecifiedValue;
if (GCodeUtil.TryFindParamNum(line.Parameters, "F", ref f))
{
VertexCurrent.FeedRate = f;
}
double extrusionAmount = GCodeUtil.UnspecifiedValue;
bool featureActive = GCodeUtil.TryFindParamNum(line.Parameters, "E", ref extrusionAmount) &&
extrusionAmount > VertexPrevious.Extrusion.x &&
currentFeatureInfo != null;
foreach (var s in endFeatureComments)
{
if (!string.IsNullOrWhiteSpace(line.Comment) && line.Comment.ToLower().Contains(s))
{
featureActive = false;
}
}
if (featureActive)
{
Vector2d average = new Segment2d(VertexCurrent.Position.xy, VertexPrevious.Position.xy).Center;
double distance = VertexCurrent.Position.Distance(VertexPrevious.Position);
double extrusion = extrusionAmount - VertexPrevious.Extrusion.x;
currentFeatureInfo.Extrusion += extrusion;
currentFeatureInfo.Distance += distance;
currentFeatureInfo.BoundingBox.Contain(VertexPrevious.Position.xy);
currentFeatureInfo.BoundingBox.Contain(VertexCurrent.Position.xy);
currentFeatureInfo.UnweightedCenterOfMass += average * extrusion;
currentFeatureInfo.Duration += distance / VertexCurrent.FeedRate;
VertexCurrent.Extrusion = new Vector3d(extrusionAmount, 0, 0);
}
VertexPrevious = new PrintVertex(VertexCurrent);
}
public static bool Segment2dWithSegment2d(Segment2d segment1, Segment2d segment2, ref Vector3L result)
{
FloatL resultX = 0;
FloatL resultY = 0;
bool ret = get_line_intersection(segment1.m_point1.x, segment1.m_point1.y, segment1.m_point2.x, segment1.m_point2.y,
segment2.m_point1.x, segment2.m_point1.y, segment2.m_point2.x, segment2.m_point2.y,
ref resultX, ref resultY);
result.x = resultX;
result.z = resultY;
return(ret);
}
public static void test_svg()
{
Polygon2d poly = Polygon2d.MakeCircle(100.0f, 10);
PolyLine2d pline = new PolyLine2d();
pline.AppendVertex(Vector2d.Zero);
pline.AppendVertex(200 * Vector2d.AxisX);
pline.AppendVertex(200 * Vector2d.One);
Circle2d circ = new Circle2d(33 * Vector2d.One, 25);
Segment2d seg = new Segment2d(Vector2d.Zero, -50 * Vector2d.AxisY);
SVGWriter writer = new SVGWriter();
writer.AddPolygon(poly, SVGWriter.Style.Filled("lime", "black", 0.25f));
writer.AddPolyline(pline, SVGWriter.Style.Outline("orange", 2.0f));
writer.AddCircle(circ, SVGWriter.Style.Filled("yellow", "red", 5.0f));
writer.AddLine(seg, SVGWriter.Style.Outline("blue", 10.0f));
int astep = 29;
Vector2d c = new Vector2d(-200, 100);
for (int k = 1; k <= 12; ++k)
{
Arc2d arc = new Arc2d(c + k * 45 * Vector2d.AxisX, 20, 0, k * astep);
writer.AddArc(arc);
writer.AddBox(arc.Bounds, SVGWriter.Style.Outline("red", 0.5f));
}
c.y += 50;
for (int k = 1; k <= 12; ++k)
{
Arc2d arc = new Arc2d(c + k * 45 * Vector2d.AxisX, 20, k * astep, (k + 5) * astep);
writer.AddArc(arc);
writer.AddBox(arc.Bounds, SVGWriter.Style.Outline("red", 0.5f));
}
c.y += 50;
for (int k = 1; k <= 12; ++k)
{
Arc2d arc = new Arc2d(c + k * 45 * Vector2d.AxisX, 20, k * astep, (k + 10) * astep);
writer.AddArc(arc);
writer.AddBox(arc.Bounds, SVGWriter.Style.Outline("red", 0.5f));
}
c.y += 50;
for (int k = 1; k <= 12; ++k)
{
Arc2d arc = new Arc2d(c + k * 45 * Vector2d.AxisX, 20, k * astep, (k + 10) * astep);
arc.Reverse();
writer.AddArc(arc);
writer.AddBox(arc.Bounds, SVGWriter.Style.Outline("red", 0.5f));
}
writer.Write(TestUtil.GetTestOutputPath("test.svg"));
}
public static bool IsMember(this Polygon2d poly, Segment2d seg, out bool IsOutside)
{
IsOutside = true;
if (poly.Vertices.Contains(seg.P0) && poly.Vertices.Contains(seg.P1))
{
if (poly.BiContains(seg))
{
IsOutside = false;
}
return(true);
}
return(false);
}
bool self_intersects(PolyLine2d poly)
{
var seg = new Segment2d(poly.Start, poly.End);
int NS = poly.VertexCount - 2;
for (int i = 1; i < NS; ++i)
{
if (poly.Segment(i).Intersects(ref seg))
{
return(true);
}
}
return(false);
}
public void TrimSegment2d_SegmentEndsPartial()
{
// Arrange
var square = new GeneralPolygon2d(Polygon2d.MakeRectangle(new Vector2d(-1, 0), new Vector2d(1, 1)));
var seg = new Segment2d(new Vector2d(0, 0.1), new Vector2d(0, 2));
// Act
var result = square.TrimSegment2d(seg);
// Assert
Assert.AreEqual(1, result.Count);
Assert.IsTrue(new Vector2d(0, 0.1).EpsilonEqual(result[0].P0, Epsilon));
Assert.IsTrue(new Vector2d(0, 1).EpsilonEqual(result[0].P1, Epsilon));
}
public void TrimSegment2d_SegmentEndsOutside()
{
// Arrange
var square = new GeneralPolygon2d(Polygon2d.MakeRectangle(new Vector2d(-1, 0), new Vector2d(1, 1)));
var seg = new Segment2d(new Vector2d(0, -1), new Vector2d(0, 2));
// Act
var result = square.TrimSegment2d(seg);
// Assert
Assert.AreEqual(1, result.Count);
AssertExtensions.AreEqual(new Vector2d(0, 0), result[0].P0);
AssertExtensions.AreEqual(new Vector2d(0, 1), result[0].P1);
}
/// <summary>
/// Find any segment in set that intersects input segment.
/// Returns intersection test, and index of segment
/// </summary>
public IntrSegment2Segment2 FindAnyIntersection(Segment2d seg, out int iSegment)
{
int N = Segments.Count;
for (iSegment = 0; iSegment < N; ++iSegment)
{
IntrSegment2Segment2 intr = new IntrSegment2Segment2(seg, Segments[iSegment]);
if (intr.Find())
{
return(intr);
}
}
return(null);
}
public double DistanceSquared(Vector2D point)
{
double fNearestSqr = Double.MaxValue;
for (int i = 0; i < vertices.Count - 1; ++i)
{
Segment2d seg = new Segment2d(vertices[i], vertices[i + 1]);
double d = seg.DistanceSquared(point);
if (d < fNearestSqr)
{
fNearestSqr = d;
}
}
return(fNearestSqr);
}
public void ObserveGcodeLine(GCodeLine line)
{
if (line.type != GCodeLine.LType.GCode)
{
return;
}
double x = VertexPrevious.Position.x;
double y = VertexPrevious.Position.y;
bool found_x = GCodeUtil.TryFindParamNum(line.parameters, "X", ref x);
bool found_y = GCodeUtil.TryFindParamNum(line.parameters, "Y", ref y);
if (!found_x || !found_y)
{
return;
}
VertexCurrent.Position = new Vector3d(x, y, 0);
double f = GCodeUtil.UnspecifiedValue;
if (GCodeUtil.TryFindParamNum(line.parameters, "F", ref f))
{
VertexCurrent.FeedRate = f;
}
double extrusionAmount = GCodeUtil.UnspecifiedValue;
if (GCodeUtil.TryFindParamNum(line.parameters, "E", ref extrusionAmount) &&
extrusionAmount >= VertexPrevious.Extrusion.x && currentFeatureInfo != null)
{
Vector2d average = new Segment2d(VertexCurrent.Position.xy, VertexPrevious.Position.xy).Center;
double distance = VertexCurrent.Position.Distance(VertexPrevious.Position);
double extrusion = extrusionAmount - VertexPrevious.Extrusion.x;
currentFeatureInfo.Extrusion += extrusion;
currentFeatureInfo.Distance += distance;
currentFeatureInfo.BoundingBox.Contain(VertexCurrent.Position.xy);
currentFeatureInfo.UnweightedCenterOfMass += average * extrusion;
currentFeatureInfo.Duration += distance / VertexCurrent.FeedRate;
VertexCurrent.Extrusion = new Vector3d(extrusionAmount, 0, 0);
}
VertexPrevious = new PrintVertex(VertexCurrent);
}
public static Segment2d Project(this Segment2d _host, Segment2d _seg)
{
//project each end point
double t0 = _host.Project(_seg.P0);
double t1 = _host.Project(_seg.P1);
t0 = MathUtil.Clamp(t0, -1.0 * _host.Extent, _host.Extent);
t1 = MathUtil.Clamp(t1, -1.0 * _host.Extent, _host.Extent);
if (t0.IsAlmostEqualByDifference(t1))
{
return(new Segment2d(Vector2d.Zero, Vector2d.Zero));
}
var p0 = _host.PointAt(t0);
var p1 = _host.PointAt(t1);
return(new Segment2d(p0, p1));
}
public static bool IsSmae(this Segment2d segment2d01, Segment2d segment2d02)
{
if (!segment2d01.Center.Distance(segment2d02.Center).EqualZreo())
{
return(false);
}
if (!Math.Abs(segment2d01.Direction.Dot(segment2d02.Direction)).EqualPrecision(1))
{
return(false);
}
if (!segment2d01.Extent.EqualPrecision(segment2d02.Extent))
{
return(false);
}
return(true);
}
