/// <summary> /// Returns the closest segment and point that is on or past the segment specified by <c>prev</c>. /// </summary> /// <param name="pt">Target point</param> /// <param name="prev">PointOnPath to start search from</param> /// <returns>Closest segment and point past <c>prev</c></returns> /// <exception cref="ArgumentOutOfRangeException"> /// Thrown when the segment specified by <c>prev</c> is not a member of the path's segments collection. /// </exception> /// <remarks> /// Starting with the segment specified by <c>prev</c>, will search forward until a segment's closest point /// is not it's end point, i.e. the target point is not past the end of the segment. Will return /// <c>EndPoint</c> if none of the path segments satisify this condition, indicating that the point is past /// the end of the path. /// </remarks> public PointOnPath GetClosest(Vector2 pt, PointOnPath prev) { int i = segments.IndexOf(prev.segment); IPathSegment seg; PointOnPath pop; if (i == -1) { throw new ArgumentOutOfRangeException(); } for (; i < segments.Count; i++) { seg = segments[i]; if (!achieved.Contains(seg)) { pop = seg.ClosestPoint(pt); if (pop != seg.EndPoint) { return(pop); } } } return(EndPoint); }
public LinePath RemoveBetween(PointOnPath start, PointOnPath end) { // remove all the points between start and end (non-inclusive) // nothing to remove if they are the same index if (start.Index == end.Index) { return(new LinePath(this)); } int endIndex = end.DistFraction == 1 ? end.Index + 2 : end.Index + 1; LinePath ret = new LinePath(); for (int i = 0; i <= start.Index; i++) { ret.Add(this[i]); } // if the start is not at a point, add it if (start.DistFraction != 0) { ret.Add(start.Location); } // add the end point ret.Add(end.Location); for (int i = endIndex; i < this.Count; i++) { ret.Add(this[i]); } return(ret); }
public PointOnPath AdvancePoint(PointOnPath pt, ref double dist) { // assert that we're looking at the same segment Debug.Assert(Equals(pt.segment)); if (pt.dist + dist <= 0) { // handle the case of negative distance going before start point dist += pt.dist; return(StartPoint); } else if (pt.dist + dist >= cb.ArcLength) { // handle the case of positive distance going after end point dist -= cb.ArcLength - pt.dist; return(EndPoint); } else { // we're in the range that we can achieve double targetDist = pt.dist + dist; double tValue = cb.FindT(targetDist); double actDist = cb.PartialArcLength(tValue); dist = 0; return(new PointOnPath(this, targetDist, cb.Bt(tValue))); } }
public double DistanceToGo(PointOnPath pt) { // assert that we're looking at the same segment Debug.Assert(Equals(pt.segment)); // calculate as the distance remaining return(cb.ArcLength - pt.dist); }
public Vector2 GetPoint(PointOnPath pt) { // check the first point specially Vector2 s = this[pt.Index]; Vector2 e = this[pt.Index + 1]; Vector2 v = e - s; return(s + pt.DistFraction * v); }
public Vector2 Tangent(PointOnPath pt) { double angle = (pt.pt - center).ArcTan; if (ccw) { return(new Vector2(1, 0).Rotate(angle + Math.PI / 2)); } else { return(new Vector2(1, 0).Rotate(angle - Math.PI / 2)); } }
public LinePath Resample(PointOnPath start, PointOnPath end, double spacing) { LinePath ret = new LinePath(); PointOnPath pt = start; while (pt < end) { ret.Add(GetPoint(pt)); pt = AdvancePoint(pt, spacing); } ret.Add(GetPoint(end)); return(ret); }
public LinePath Resample(double spacing) { LinePath ret = new LinePath(); PointOnPath pt = StartPoint; while (pt != EndPoint) { ret.Add(GetPoint(pt)); pt = AdvancePoint(pt, spacing); } ret.Add(GetPoint(EndPoint)); return(ret); }
public LinePath SubPath(PointOnPath start, PointOnPath end) { Debug.Assert(start.Valid); Debug.Assert(end.Valid); LinePath ret = new LinePath(); if (start.Index < end.Index || ((start.Index == end.Index) && (start.Dist < end.Dist))) { if (start.DistFraction < 0.999999999) { ret.Add(GetPoint(start)); } // iterate through and add all the end point for (int i = start.Index + 1; i <= end.Index; i++) { ret.Add(this[i]); } if (end.DistFraction > 0) { ret.Add(GetPoint(end)); } return(ret); } else { // end is first if (start.DistFraction > 0) { ret.Add(GetPoint(start)); } for (int i = start.Index; i > end.Index; i--) { ret.Add(this[i]); } if (end.DistFraction < 1) { ret.Add(GetPoint(end)); } return(ret); } }
/// <summary> /// Returns a line path with all points before pt removed /// </summary> public LinePath RemoveBefore(PointOnPath pt) { LinePath ret = new LinePath(Count - pt.Index); if (pt.DistFraction < 1) { ret.Add(pt.Location); } for (int i = pt.Index + 1; i < Count; i++) { ret.Add(this[i]); } return(ret); }
public LinePath RemoveAfter(PointOnPath pt) { LinePath ret = new LinePath(pt.Index + 2); for (int i = 0; i < pt.Index; i++) { ret.Add(this[i]); } if (pt.DistFraction > 0) { ret.Add(pt.Location); } return(ret); }
public IEnumerable <Vector2> GetSubpathEnumerator(PointOnPath start, PointOnPath end) { Debug.Assert(start.Valid); Debug.Assert(end.Valid); LinePath ret = new LinePath(); if (start.Index < end.Index || ((start.Index == end.Index) && (start.Dist < end.Dist))) { if (start.DistFraction < 1) { yield return(start.Location); } // iterate through and add all the end point for (int i = start.Index + 1; i <= end.Index; i++) { yield return(this[i]); } if (end.DistFraction > 0) { yield return(end.Location); } } else { // end is first if (start.DistFraction > 0) { yield return(start.Location); } for (int i = start.Index; i > end.Index; i--) { yield return(this[i]); } if (end.DistFraction < 1) { yield return(end.Location); } } }
public PointOnPath GetClosestPoint(Vector2 loc) { double minDist = double.MaxValue; PointOnPath pointMin = new PointOnPath(); // iterate throug each line segment and find the closest point for (int i = 0; i < Count - 1; i++) { Vector2 s = this[i]; Vector2 v = this[i + 1] - s; double len = v.Length; v /= len; double t = v * (loc - s); Vector2 pt; if (t < 0) { pt = s; t = 0; } else if (t > len) { pt = this[i + 1]; t = len; } else { pt = s + t * v; } // figure out the distance double dist = (pt - loc).Length; if (dist < minDist) { minDist = dist; pointMin = new PointOnPath(i, t, t / len, this); } } return(pointMin); }
public virtual PointOnPath AdvancePoint(PointOnPath pt, ref double dist) { Debug.Assert(Equals(pt.segment)); if (dist >= 0) { double d = Math.Min(DistanceToGo(pt), dist); Vector2 tan = end - start; tan = tan / tan.VectorLength; dist -= d; return(new PointOnPath(this, pt.dist + d, pt.pt + tan * d)); } else { double d = Math.Max(-pt.pt.DistanceTo(this.start), dist); Vector2 tan = end - start; tan = tan / tan.VectorLength; dist -= d; return(new PointOnPath(this, pt.dist + d, pt.pt + tan * d)); } }
public double DistanceBetween(PointOnPath start, PointOnPath end) { if (start.Index == end.Index) { return(end.Dist - start.Dist); } else if (start.Index < end.Index) { double dist = SegmentLength(start.Index) - start.Dist; for (int i = start.Index + 1; i < end.Index; i++) { dist += SegmentLength(i); } dist += end.Dist; return(dist); } else { // start.Index > end.Index double dist = start.Dist; for (int i = end.Index + 1; i < start.Index; i++) { dist += SegmentLength(i); } dist += (SegmentLength(end.Index) - end.Dist); return(-dist); } // compute the total distance to start, end //double startDist = start.Dist; //for (int i = 0; i < start.Index; i++) { // startDist += SegmentLength(i); //} //double endDist = end.Dist; //for (int i = 0; i < end.Index; i++) { // endDist += SegmentLength(i); //} //return endDist - startDist; }
public double Curvature(PointOnPath pt) { // assert that we're looking at the same segment Debug.Assert(Equals(pt.segment)); // short circuit for near the start/end point if (pt.pt.ApproxEquals(cb.P0, 0.001)) { return(cb.Curvature(0)); } else if (pt.pt.ApproxEquals(cb.P3, 0.001)) { return(cb.Curvature(1)); } else { // find the t-value in the general case double tvalue = cb.FindT(pt.dist); return(cb.Curvature(tvalue)); } }
public Vector2 Tangent(PointOnPath pt) { // assert that we're looking at the same segment Debug.Assert(Equals(pt.segment)); // short circuit for near the start/end point if (pt.pt.ApproxEquals(cb.P0, 0.001)) { return(cb.dBdt(0).Normalize()); } else if (pt.pt.ApproxEquals(cb.P3, 0.001)) { return(cb.dBdt(1).Normalize()); } else { // find the t-value in the general case double tvalue = cb.FindT(pt.dist); return(cb.dBdt(tvalue).Normalize()); } }
/// <summary> /// Returns the closest point on the path in terms of Euclidean distance. /// </summary> /// <param name="pt">Point to test</param> /// <returns> /// Closest point on the path. If there are no path segments, will return <c>PointOnPath.Empty</c>. /// </returns> /// <remarks> /// This enumerates over all path segments and class <c>IPathSegment.ClosestPoint(pt)</c> on each. The /// segment and associated point that has the smallest distance to the target point will be returned. /// </remarks> public PointOnPath GetClosest(Vector2 pt) { double minDist = 1e100; PointOnPath minpop = PointOnPath.Empty; for (int i = 0; i < segments.Count; i++) { IPathSegment seg = segments[i]; PointOnPath pop = seg.ClosestPoint(pt); //if (pop != seg.StartPoint && pop != seg.EndPoint) { double dist = pop.pt.DistanceTo(pt); if (dist < minDist) { minDist = dist; minpop = pop; } //} } return(minpop); }
/// <summary> /// Calculates the distance between two points, integrated along the path. /// </summary> /// <param name="ptStart">Starting point</param> /// <param name="ptEnd">Ending point</param> /// <returns> /// Distance between the two path points integrated along the path. If <c>startPt</c> is before /// <c>endPt</c>, this will be positive. If <c>startPt</c> is after <c>endPt</c>, this will be negative. /// </returns> /// <exception cref="ArgumentOutOfRangeException"> /// Thrown when the segment specified by either <c>startPt</c> or <c>endPt</c> does not exist in the segments collection. /// </exception> public double DistanceBetween(PointOnPath ptStart, PointOnPath ptEnd) { int startIndex = segments.IndexOf(ptStart.segment); int endIndex = segments.IndexOf(ptEnd.segment); if (startIndex == -1 || endIndex == -1) { throw new ArgumentOutOfRangeException(); } if (startIndex == endIndex) { return(ptEnd.dist - ptStart.dist); } else if (startIndex < endIndex) { double dist = 0; dist += ptStart.segment.DistanceToGo(ptStart); while (++startIndex < endIndex) { dist += segments[startIndex].Length; } dist += ptEnd.dist; return(dist); } else { // startIndex > endIndex double dist = 0; dist -= ptStart.dist; while (--startIndex > endIndex) { dist -= segments[startIndex].Length; } dist -= ptEnd.segment.DistanceToGo(ptEnd); return(dist); } }
public virtual double DistanceToGo(PointOnPath pt) { Debug.Assert(Equals(pt.segment)); return(Length - pt.dist); }
public double Curvature(PointOnPath pt) { return(1 / radius); }
public LinePath SubPath(PointOnPath start, ref double dist) { PointOnPath end = AdvancePoint(start, ref dist); return(SubPath(start, end)); }
public double DistanceToGo(PointOnPath pt) { return(DistanceToGo(pt.pt)); }
public Vector2 Tangent(PointOnPath pt) { return((end - start).Normalize()); }
public LinePath SubPath(PointOnPath start, double dist) { return(SubPath(start, ref dist)); }
public virtual double DistanceOffPath(Vector2 pt) { PointOnPath pop = ClosestPoint(pt); return(pop.pt.DistanceTo(pt)); }
public PointOnPath AdvancePoint(PointOnPath startPoint, ref double dist) { Debug.Assert(startPoint.Valid); if (dist > 0) { // check if we can satisfy fith the first point double len = SegmentLength(startPoint.Index); if (startPoint.Dist + dist < len) { double totDist = startPoint.Dist + dist; PointOnPath ret = new PointOnPath(startPoint.Index, totDist, totDist / len, this); dist = 0; return(ret); } else { // we need to use multiple segments dist -= (len - startPoint.Dist); for (int i = startPoint.Index + 1; i < Count - 1; i++) { len = SegmentLength(i); if (dist < len) { // this segment will satisfy the remaining distance PointOnPath ret = new PointOnPath(i, dist, dist / len, this); dist = 0; return(ret); } else { dist -= len; } } // we couldn't satisfy the distance, return the end point return(EndPoint); } } else { // distance < 0, go backwards if (startPoint.Dist + dist >= 0) { double len = SegmentLength(startPoint.Index); double totDist = startPoint.Dist + dist; PointOnPath ret = new PointOnPath(startPoint.Index, totDist, totDist / len, this); dist = 0; return(ret); } else { dist += startPoint.Dist; for (int i = startPoint.Index - 1; i >= 0; i--) { double len = SegmentLength(i); if (len + dist >= 0) { // this segment will satisfy the remaining distance PointOnPath ret = new PointOnPath(i, len + dist, (len + dist) / len, this); dist = 0; return(ret); } else { dist += len; } } return(StartPoint); } } }
public PointOnPath AdvancePoint(PointOnPath startPoint, double dist) { return(AdvancePoint(startPoint, ref dist)); }
public PointOnPath GetReverseClosestPoint(PointOnPath startPoint, Vector2 loc) { double minDist = double.MaxValue; PointOnPath minPoint = new PointOnPath(); Debug.Assert(startPoint.Valid); // check the first point specially Vector2 s = this[startPoint.Index]; Vector2 e = this[startPoint.Index + 1]; Vector2 v = e - s; double len = v.Length; v /= len; double t = v * (loc - s); if (t > startPoint.Dist) { Vector2 pt = s + v * startPoint.Dist; minDist = (loc - pt).Length; minPoint = startPoint; } else if (t <= 0) { minDist = (loc - s).Length; minPoint = new PointOnPath(startPoint.Index, 0, 0, this); } else { Vector2 pt = s + v * t; minDist = (loc - pt).Length; minPoint = new PointOnPath(startPoint.Index, t, t / len, this); } for (int i = startPoint.Index - 1; i >= 0; i--) { s = this[i]; e = this[i + 1]; v = e - s; len = v.Length; v /= len; t = v * (loc - s); Vector2 pt; if (t < 0) { pt = s; t = 0; } else if (t > len) { pt = e; t = len; } else { pt = s + t * v; } // figure out the distance double dist = (pt - loc).Length; if (dist < minDist) { minDist = dist; minPoint = new PointOnPath(i, t, t / len, this); } } return(minPoint); }
public double Curvature(PointOnPath pt) { return(0); }