private static void HandleTopSegment(ref List <Vector2> visibility, LineSegment seg, Ray2D ray) { // dont handle colinear lines if (seg == null || Line.Colinear(ray.origin, seg.Point1, seg.Point2)) { return; } var intersect = seg.Intersect(ray); if (!intersect.HasValue) { // robustness issues if ray near endpoint var lineIntersect = seg.Line.Intersect(ray); if (lineIntersect.HasValue) { intersect = seg.ClosestPoint(lineIntersect.Value); } } if (intersect.HasValue && !MathUtil.EqualsEps(Vector2.zero, intersect.Value) && (visibility.Count == 0 || !MathUtil.EqualsEps(intersect.Value, visibility.Last()))) { visibility.Add(intersect.Value); } }
private void ComputeMinDistanceLinePoint(ILineString line, IPoint point, bool flip) { var lineCoord = line.Coordinates; var coord = point.Coordinate; // brute force approach! for (int i = 0; i < lineCoord.Length - 1; i++) { var dist = CGAlgorithms3D.DistancePointSegment(coord, lineCoord[i], lineCoord[i + 1]); if (dist < _minDistance) { var seg = new LineSegment(lineCoord[i], lineCoord[i + 1]); var segClosestPoint = seg.ClosestPoint(coord); UpdateDistance(dist, new GeometryLocation(line, i, segClosestPoint), new GeometryLocation(point, 0, coord), flip); } if (_isDone) { return; } } }
private void UpdatePts(Coordinate p, Coordinate seg0, Coordinate seg1) { _minPts[0] = p; var seg = new LineSegment(seg0, seg1); _minPts[1] = new Coordinate(seg.ClosestPoint(p)); }
/// <summary> /// /// </summary> /// <param name="line"></param> /// <param name="pt"></param> /// <param name="locGeom"></param> private void ComputeMinDistance(ILineString line, IPoint pt, GeometryLocation[] locGeom) { if (line.EnvelopeInternal.Distance(pt.EnvelopeInternal) > _minDistance) { return; } var coord0 = line.Coordinates; var coord = pt.Coordinate; // brute force approach! for (int i = 0; i < coord0.Length - 1; i++) { double dist = DistanceComputer.PointToSegment(coord, coord0[i], coord0[i + 1]); if (dist < _minDistance) { _minDistance = dist; var seg = new LineSegment(coord0[i], coord0[i + 1]); var segClosestPoint = seg.ClosestPoint(coord); locGeom[0] = new GeometryLocation(line, i, segClosestPoint); locGeom[1] = new GeometryLocation(pt, 0, coord); } if (_minDistance <= _terminateDistance) { return; } } }
/// <summary> /// /// </summary> /// <param name="line"></param> /// <param name="pt"></param> /// <param name="locGeom"></param> private void ComputeMinDistance(ILineString line, IPoint pt, GeometryLocation[] locGeom) { if (line.EnvelopeInternal.Distance(pt.EnvelopeInternal) > minDistance) { return; } ICoordinate[] coord0 = line.Coordinates; ICoordinate coord = pt.Coordinate; // brute force approach! for (int i = 0; i < coord0.Length - 1; i++) { double dist = CGAlgorithms.DistancePointLine(coord, coord0[i], coord0[i + 1]); if (dist < minDistance) { minDistance = dist; LineSegment seg = new LineSegment(coord0[i], coord0[i + 1]); ICoordinate segClosestPoint = seg.ClosestPoint(coord); locGeom[0] = new GeometryLocation(line, i, segClosestPoint); locGeom[1] = new GeometryLocation(pt, 0, coord); } if (minDistance <= terminateDistance) { return; } } }
/// <summary> /// /// </summary> /// <param name="line"></param> /// <param name="pt"></param> /// <param name="locGeom"></param> private void ComputeMinDistance(ILineString line, Point pt, GeometryLocation[] locGeom) { if (line.EnvelopeInternal.Distance(pt.EnvelopeInternal) > _minDistance) { return; } IList <Coordinate> coord0 = line.Coordinates; Coordinate coord = pt.Coordinate; // brute force approach! for (int i = 0; i < coord0.Count - 1; i++) { double dist = CgAlgorithms.DistancePointLine(coord, coord0[i], coord0[i + 1]); if (dist < _minDistance) { _minDistance = dist; LineSegment seg = new LineSegment(coord0[i], coord0[i + 1]); Coordinate segClosestPoint = new Coordinate(seg.ClosestPoint(coord)); locGeom[0] = new GeometryLocation(line, i, segClosestPoint); locGeom[1] = new GeometryLocation(pt, 0, coord); } if (_minDistance <= _terminateDistance) { return; } } }
public void Update(IMyEntity missile) { LineSegment line = new LineSegment(entity.GetPosition(), entity.GetPosition() + entity.WorldMatrix.Forward * 1e6f); position = line.ClosestPoint(missile.GetPosition()) + missile.Physics.LinearVelocity; linearVelocity = entity.WorldMatrix.Forward * missile.Physics.LinearVelocity.Length(); }
private void ComputeMinDistance(LineString line, Point pt, DistanceLocation[] locGeom) { if (line.Bounds.Distance(pt.Bounds) > minDistance) { return; } ICoordinateList coord0 = line.Coordinates; Coordinate coord = pt.Coordinate; // brute force approach! int nCount0 = coord0.Count; for (int i = 0; i < nCount0 - 1; i++) { double dist = CGAlgorithms.DistancePointLine(coord, coord0[i], coord0[i + 1]); if (dist < minDistance) { minDistance = dist; LineSegment seg = new LineSegment(m_objFactory, coord0[i], coord0[i + 1]); Coordinate segClosestPoint = seg.ClosestPoint(coord); locGeom[0] = new DistanceLocation(line, i, segClosestPoint); locGeom[1] = new DistanceLocation(pt, 0, coord); } if (minDistance <= terminateDistance) { return; } } }
private void OnDrawGizmos() { var pos = point.position; var line0 = new LineSegment() { start = line0_start.position, end = line0_end.position }; var line1 = new LineSegment() { start = line1_start.position, end = line1_end.position }; // line0 Gizmos.color = Color.red; Gizmos.DrawLine(line0.start, line0.end); // line1 Gizmos.color = Color.blue; Gizmos.DrawLine(line1.start, line1.end); // intersection line0,line1 var intersection = LineSegment.CalcIntersection(line0, line1); if (intersection != null) { Gizmos.color = Color.yellow; Gizmos.DrawWireSphere(intersection.Value, 0.3f); } // closet point Gizmos.color = Color.gray; var cpos0 = line0.ClosestPoint(pos); var cpos1 = line1.ClosestPoint(pos); Gizmos.DrawLine(cpos0, pos); Gizmos.DrawLine(cpos1, pos); // normal from pos Gizmos.color = Color.green; var normal0 = line0.CalcNormal(pos); var normal1 = line1.CalcNormal(pos); Gizmos.DrawRay(pos, -normal0); Gizmos.DrawRay(pos, -normal1); #if UNITY_EDITOR var dist0 = line0.Distance(pos); var dist1 = line1.Distance(pos); UnityEditor.Handles.Label(pos, $"Distance: {dist0},{dist1}"); var left0 = line0.IsLeft(pos); var left1 = line1.IsLeft(pos); UnityEditor.Handles.Label(pos - Vector3.up * 0.2f, $"IsLeft: {left0},{left1}"); #endif }
// Update is called once per frame void Update() { LineSegment l1 = new LineSegment(a.position, b.position); Vector3 closest = l1.ClosestPoint(c.position); closest.y = a.position.y; intersection.position = closest; }
private void UpdateNearestLocationsPointLine(Coordinate pt, FacetSequence facetSeq, int i, Coordinate q0, Coordinate q1, GeometryLocation[] locs) { locs[0] = new GeometryLocation(_geom, _start, pt.Copy()); var seg = new LineSegment(q0, q1); var segClosestPoint = seg.ClosestPoint(pt); locs[1] = new GeometryLocation(facetSeq._geom, i, segClosestPoint.Copy()); }
private void UpdateClearance(double candidateValue, Coordinate p, Coordinate seg0, Coordinate seg1) { if (candidateValue < _minClearance) { _minClearance = candidateValue; _minClearancePts[0] = p.Copy(); var seg = new LineSegment(seg0, seg1); _minClearancePts[1] = seg.ClosestPoint(p).Copy(); } }
public static void ComputeDistance(ILineString line, Coordinate pt, PointPairDistance ptDist) { var coords = line.Coordinates; for (var i = 0; i < coords.Length - 1; i++) { TempSegment.SetCoordinates(coords[i], coords[i + 1]); // this is somewhat inefficient - could do better var closestPt = TempSegment.ClosestPoint(pt); ptDist.SetMinimum(closestPt, pt); } }
// removed because Geometry.GeometryChangedAction() is not virtual // public void GeometryChangedAction() // { // DetermineMonotone(); // } /** * @param co * input coordinate in the neighbourhood of the MLineString * @param tolerance * max. distance that co may be from this MLineString * @return an MCoordinate on this MLineString with appropriate M-value */ public MCoordinate GetClosestPoint(Coordinate co, double tolerance) { if (!this.IsMonotone(false)) { throw new ApplicationException("MGeometryException.OPERATION_REQUIRES_MONOTONE"); } if (!this.IsEmpty) { LineSegment seg = new LineSegment(); Coordinate[] coAr = this.Coordinates; seg.P0 = coAr[0]; double d = 0.0; double projfact = 0.0; double minDist = Double.PositiveInfinity; MCoordinate mincp = null; for (int i = 1; i < coAr.Length; i++) { seg.P1 = coAr[i]; Coordinate cp = seg.ClosestPoint(co); d = cp.Distance(co); if (d <= tolerance && d <= minDist) { MCoordinate testcp = new MCoordinate(cp); projfact = seg.ProjectionFactor(cp); testcp.M = ((MCoordinate)coAr[i - 1]).M + projfact * (((MCoordinate)coAr[i]).M - ((MCoordinate)coAr[i - 1]).M); if (d < minDist || testcp.M < mincp.M) { mincp = testcp; minDist = d; } } seg.P0 = seg.P1; } if (minDist > tolerance) { return(null); } else { return(mincp); } } else { return(null); } }
/// <summary> /// Either intersects two line segments defined by the given four polar points /// or if endpoint is null, a line segment (a,b) and ray from point orig in the direction /// of its adjacent polygon segment. /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <param name="orig"></param> /// <param name="endpoint"></param> /// <returns> A vertex-displacement pair associated with the intersection. </returns> public static VertDispl IntersectWithWindow(VertDispl a, VertDispl b, VertDispl orig, VertDispl endpoint) { if (a == null || b == null || orig == null) { return(null); } var s1 = new LineSegment(a.p, b.p); // extra checks related to robustness issues if (s1.IsOnSegment(orig.p.Cartesian)) { return(orig); } Vector2?res; if (endpoint != null) { var s2 = new LineSegment(orig.p, endpoint.p); // check for parallel slopes if (s1.IsParallel(s2)) { res = s1.ClosestPoint(orig.p.Cartesian); if (res.HasValue && !s2.IsOnSegment(res.Value)) { res = null; } } else { res = s1.Intersect(s2); } } else { var ray = new Ray2D(orig.p.Cartesian, orig.Direction); res = s1.Intersect(ray); } if (!res.HasValue) { return(null); } return(DisplacementInBetween(new PolarPoint2D(res.Value), a, b)); }
private bool CheckFinalExit() { // rotate the polygon into the current relative frame CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp; RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp); LineSegment finalLine = finalOrientation.Transform(relTransform); double finalAngle = finalLine.UnitVector.ArcTan; if (Math.Abs(finalAngle) < 15 * Math.PI / 180.0 && finalLine.ClosestPoint(Coordinates.Zero).Length < 2) { // we can just execute a stay in lane return(true); } return(false); }
/// <summary> /// Given the specified test point, this checks each segment, and will /// return the closest point on the specified segment. /// </summary> /// <param name="self">The ILineString, whose point is returned.</param> /// <param name="testPoint">The point to test.</param> public static Coordinate ClosestPoint(this ILineString self, Coordinate testPoint) { Coordinate closest = self.GetCoordinateN(0); double dist = double.MaxValue; for (int i = 0; i < self.NumPoints - 1; i++) { LineSegment s = new LineSegment(self.GetCoordinateN(i), self.GetCoordinateN(i + 1)); Coordinate temp = s.ClosestPoint(testPoint); double tempDist = testPoint.Distance(temp); if (tempDist < dist) { dist = tempDist; closest = temp; } } return(closest); }
private static double GetObstacleClearanceLine(LineSegment segment, IList <Polygon> obstacles) { double minDist = double.MaxValue; foreach (Polygon obs in obstacles) { foreach (Coordinates pt in obs) { Coordinates closestPt = segment.ClosestPoint(pt); double dist = closestPt.DistanceTo(pt); if (dist < minDist) { minDist = dist; } } } return(minDist); }
/// <summary> /// Computes the Euclidean distance (L2 metric) from a <see cref="Coordinate"/> to a <see cref="LineSegment"/>. /// </summary> /// <param name="segment">The <c>LineSegment</c></param> /// <param name="pt">The Point</param> /// <param name="ptDist">The <c>PointPairDistance</c></param> public static void ComputeDistance(LineSegment segment, Coordinate pt, PointPairDistance ptDist) { var closestPt = segment.ClosestPoint(pt); ptDist.SetMinimum(closestPt, pt); }
/// <summary> /// /// </summary> /// <param name="line"></param> /// <param name="pt"></param> /// <param name="locGeom"></param> private void ComputeMinDistance(ILineString line, Point pt, GeometryLocation[] locGeom) { if (line.EnvelopeInternal.Distance(pt.EnvelopeInternal) > _minDistance) return; IList<Coordinate> coord0 = line.Coordinates; Coordinate coord = pt.Coordinate; // brute force approach! for (int i = 0; i < coord0.Count - 1; i++) { double dist = CgAlgorithms.DistancePointLine(coord, coord0[i], coord0[i + 1]); if (dist < _minDistance) { _minDistance = dist; LineSegment seg = new LineSegment(coord0[i], coord0[i + 1]); Coordinate segClosestPoint = new Coordinate(seg.ClosestPoint(coord)); locGeom[0] = new GeometryLocation(line, i, segClosestPoint); locGeom[1] = new GeometryLocation(pt, 0, coord); } if (_minDistance <= _terminateDistance) return; } }
/// <summary> /// Pops all vertices from the stack that have become invisible after the addition /// of a new vertex. /// Calls appropriate method (advance, scan, retard) after being done based on next vertex. /// </summary> /// <param name="v"></param> /// <param name="sOld"></param> /// <param name="iprev"></param> /// <returns></returns> public static NextCall Retard(ref VsRep v, ref Stack <VertDispl> s, ref int i, ref VertDispl w, ref bool ccw) { // LocateSj will pop vertices from the stack // until appropriated s_j is found // see paper var sjNext = LocateSj(v.Get(i), v.Get(i + 1), s); if (s.Count == 0) { return(NextCall.STOP); } var sj = s.Peek(); if (sj.alpha < v.Get(i + 1).alpha) { i++; var vi = v.Get(i); var p = (new LineSegment(sj.p.Cartesian, sjNext.p.Cartesian)).Intersect(vi.p.Ray); // fallback method, return point closest to intersection with segment line if (p == null) { var line = new Line(vi.p.Ray.origin, vi.p.Ray.origin + vi.p.Ray.direction); p = (new LineSegment(sj.p.Cartesian, sjNext.p.Cartesian)).Line.Intersect(line); p = Vector2.Distance(p.Value, sj.p.Cartesian) < Vector2.Distance(p.Value, sjNext.p.Cartesian) ? sj.p.Cartesian : sjNext.p.Cartesian; } var st1 = DisplacementInBetween(new PolarPoint2D(p.Value), sj, sjNext); if (st1 != null) { s.Push(st1); } s.Push(vi); // paper does i == v.n if (i == v.n - 1) { // TODO order of returned list correct? (check stack to list conversion) return(NextCall.STOP); } else if (MathUtil.GEQEps(v.Get(i + 1).alpha, vi.alpha) && MathUtil.Orient2D(v.Get(i - 1).p.Cartesian, vi.p.Cartesian, v.Get(i + 1).p.Cartesian) <= 0) { // -1 is RighTurn return(NextCall.ADVANCE); } else if (MathUtil.GreaterEps(v.Get(i + 1).alpha, vi.alpha) && MathUtil.Orient2D(v.Get(i - 1).p.Cartesian, vi.p.Cartesian, v.Get(i + 1).p.Cartesian) > 0) { // 1 is LeftTurn s.Pop(); w = vi; ccw = false; return(NextCall.SCAN); } else { s.Pop(); return(NextCall.RETARD); } } else { if (MathUtil.EqualsEps(v.Get(i + 1).alpha, sj.alpha) && MathUtil.GreaterEps(v.Get(i + 2).alpha, v.Get(i + 1).alpha) && MathUtil.Orient2D(v.Get(i).p.Cartesian, v.Get(i + 1).p.Cartesian, v.Get(i + 2).p.Cartesian) <= 0) { // -1 is RightTurn s.Push(v.Get(i + 1)); return(NextCall.ADVANCE); } else { w = IntersectWithWindow(v.Get(i), v.Get(i + 1), sj, sjNext); ccw = true; if (w == null) { var seg = new LineSegment(v.Get(i).p, v.Get(i + 1).p); var res = seg.ClosestPoint(sj.p.Cartesian); w = DisplacementInBetween(new PolarPoint2D(res), v.Get(i), v.Get(i + 1)); } return(NextCall.SCAN); } } }
private bool DoSegmentsCollide(IPathSegment seg1, IPathSegment seg2, out double collideDist1, out double collideDist2) { bool doesIntersect = false; Vector2 intersectPoint; Vector2[] p = new Vector2[4]; collideDist1 = Double.MaxValue; collideDist2 = Double.MaxValue; LineSegment ls1 = new LineSegment(seg1.Start, seg1.End); LineSegment ls2 = new LineSegment(seg2.Start, seg2.End); doesIntersect = ls1.Intersect(ls2, out intersectPoint); p[0] = ls1.ClosestPoint(seg2.Start); p[1] = ls1.ClosestPoint(seg2.End); p[2] = ls2.ClosestPoint(seg1.Start); p[3] = ls2.ClosestPoint(seg1.End); if (doesIntersect) { collideDist1 = seg1.Start.DistanceTo(intersectPoint); collideDist2 = seg2.Start.DistanceTo(intersectPoint); } if ((p[0] - seg2.Start).Length < collideThreshold) { if (collideDist1 > seg1.Start.DistanceTo(p[0])) { collideDist1 = seg1.Start.DistanceTo(p[0]); collideDist2 = 0; doesIntersect = true; } } if ((p[1] - seg2.End).Length < collideThreshold) { if (collideDist1 > seg1.Start.DistanceTo(p[1])) { collideDist1 = seg1.Start.DistanceTo(p[1]); collideDist2 = seg2.Length; doesIntersect = true; } } if ((p[2] - seg1.Start).Length < collideThreshold) { collideDist1 = 0; collideDist2 = seg2.Start.DistanceTo(p[2]); } if ((p[3] - seg1.End).Length < collideThreshold) { if (collideDist1 > seg1.Length) { collideDist1 = seg1.Length; collideDist2 = seg2.Start.DistanceTo(p[3]); } } return(doesIntersect); }