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))); } }
/// <summary> /// Advances down the path by <paramref name="dist"/> units. /// </summary> /// <param name="pt">Starting point on path</param> /// <param name="dist"> /// Distance to advance. On exit, will be the distance that could not be satisfied or 0 if the request did not go past /// the end of the path. /// </param> /// <returns> /// New advanced <see cref="PointOnPath"/> or <see cref="EndPoint"/> if past the end of the path. /// </returns> /// <exception cref="ArgumentOutOfRangeException"> /// Thrown when the segment specified by <paramref name="pt"/> is not in the segments collection. /// </exception> /// <remarks> /// Advances the point starting with the segment specified in <paramref name="pt"/> and moving forward along the path. /// At each segment, the <see cref="IPathSegment.AdvancePoint(UrbanChallenge.Common.Path.PointOnPath, ref double)"/> method /// is called, then moving to the next segment until dist is exhausted. /// </remarks> /// <example> /// This shows an example advancing down the first thirty meters of a path and checking if the result is past the end. /// <code> /// PointOnPath start = path.StartPoint; /// double dist = 30; /// PointOnPath end = path.Advance(start, ref dist); /// if (dist > 0) { /// // the ending point should be the end point of the path /// Debug.Assert(end == path.EndPoint); /// Debug.WriteLine("Past the end of the path!"); /// } /// else { /// // the ending point should not be the end point of the path /// Debug.Assert(end != path.EndPoint); /// Debug.WriteLine("Not past the end!"); /// } /// </code> /// </example> public PointOnPath AdvancePoint(PointOnPath pt, ref double dist) { if (!Contains(pt.segment)) { throw new ArgumentException(); } PartitionPathSegment laneSegment = pt.segment as PartitionPathSegment; // handle the first segment pt = laneSegment.AdvancePoint(pt, ref dist); if (dist == 0) { return(pt); } // enumerate over the remaining segments foreach (IPathSegment seg in GetEnumeratorAfter(laneSegment.UserPartition)) { // set the point on path to the beginning point of the next segment pt = seg.StartPoint; // advance the point down the segment pt = seg.AdvancePoint(pt, ref dist); // check if we've depleted our target distance if (dist == 0) { return(pt); } } // this will end up being the same as EndPoint if we've gone past the end return(pt); }
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 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 Coordinates GetPoint(PointOnPath pt) { // check the first point specially Coordinates s = this[pt.Index]; Coordinates e = this[pt.Index + 1]; Coordinates v = e - s; return(s + pt.DistFraction * v); }
/// <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 <paramref name="ptStart"/> is before /// <paramref name="ptEnd"/>, this will be positive. If <paramref name="ptStart"/> is after <paramref name="ptEnd"/>, this will be negative. /// </returns> /// <exception cref="ArgumentOutOfRangeException"> /// Thrown when the segment specified by either <paramref name="ptStart"/> or <paramref name="ptEnd"/> does not exist in the segments collection. /// </exception> public double DistanceBetween(PointOnPath ptStart, PointOnPath ptEnd) { // check if they're on different segments, which comes first, etc int startIndex = IndexOf(ptStart.segment); int endIndex = IndexOf(ptEnd.segment); if (startIndex == -1 || endIndex == -1) { throw new ArgumentException(); } bool swapped = false; // if the order is reversed, swap if (startIndex > endIndex) { PointOnPath temp = ptStart; ptStart = ptEnd; ptEnd = temp; swapped = true; } PartitionPathSegment startLaneSegment = ptStart.segment as PartitionPathSegment; if (startIndex == endIndex) { return(ptEnd.dist - ptStart.dist); } else { double dist = ptStart.segment.DistanceToGo(ptStart); foreach (IPathSegment seg in GetEnumeratorAfter(startLaneSegment.UserPartition)) { if (seg.Equals(ptEnd.segment)) { dist += ptEnd.dist; if (swapped) { return(-dist); } else { return(dist); } } else { dist += seg.Length; } } throw new InvalidOperationException(); } }
public Coordinates Tangent(PointOnPath pt) { double angle = (pt.pt - center).ArcTan; if (ccw) { return(new Coordinates(1, 0).Rotate(angle + Math.PI / 2)); } else { return(new Coordinates(1, 0).Rotate(angle - Math.PI / 2)); } }
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 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 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); } }
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); }
/// <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); }
/// <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 <see cref="PointOnPath.Empty"/>. /// </returns> /// <remarks> /// This enumerates over all path segments and calls <see cref="IPathSegment.ClosestPoint(Coordinates)"/> on each. The /// segment and associated point that has the smallest distance to the target point will be returned. /// </remarks> public PointOnPath GetClosest(Coordinates pt) { double minDist = 1e100; PointOnPath minpop = new PointOnPath(); foreach (IPathSegment seg in this) { PointOnPath pop = seg.ClosestPoint(pt); double dist = pop.pt.DistanceTo(pt); if (dist < minDist) { minDist = dist; minpop = pop; } } return(minpop); }
public IEnumerable <Coordinates> 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(Coordinates 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++) { Coordinates s = this[i]; Coordinates v = this[i + 1] - s; double len = v.Length; v /= len; double t = v * (loc - s); Coordinates 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); Coordinates 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); Coordinates tan = end - start; tan = tan / tan.VectorLength; dist -= d; return(new PointOnPath(this, pt.dist + d, pt.pt + tan * d)); } }
/// <summary> /// Returns the closest segment and point that is on or past the segment specified by <paramref name="prev"/>. /// </summary> /// <param name="pt">Target point.</param> /// <param name="prev"><see cref="PointOnPath"/> to start search from.</param> /// <returns>Closest segment and point past <paramref name="prev"/>.</returns> /// <exception cref="ArgumentOutOfRangeException"> /// Thrown when the segment specified by <paramref name="prev"/> is not a member of the path's segments collection. /// </exception> /// <remarks> /// Starting with the segment specified by <paramref name="prev"/>, 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 /// <see cref="EndPoint"/> if none of the path segments satisify this condition, indicating that the point is past /// the end of the path. /// </remarks> public PointOnPath GetClosest(Coordinates pt, PointOnPath prev) { if (!Contains(prev.segment)) { throw new ArgumentException(); } PartitionPathSegment laneSegment = prev.segment as PartitionPathSegment; foreach (IPathSegment seg in GetEnumeratorFrom(laneSegment.UserPartition)) { PointOnPath pop = seg.ClosestPoint(pt); if (pop != seg.EndPoint) { return(pop); } } return(EndPoint); }
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)); } }
/// <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(Coordinates pt, PointOnPath prev) { int i = segments.IndexOf(prev.segment); if (i == -1) { throw new ArgumentOutOfRangeException(); } for (; i < segments.Count; i++) { IPathSegment seg = segments[i]; PointOnPath pop = seg.ClosestPoint(pt); if (pop != seg.EndPoint) { return(pop); } } return(EndPoint); }
/// <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(Coordinates 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); }
public Coordinates 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> /// 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 double DistanceToGo(PointOnPath pt) { return(DistanceToGo(pt.pt)); }
public double Curvature(PointOnPath pt) { return(1 / radius); }
public PointOnPath AdvancePoint(PointOnPath pt, ref double dist) { // get the angle of the point double angle = (pt.pt - center).ArcTan; // figure out where this sits in the scheme of things if (ccw) { // get it larger than start angle if (angle < startAngle) { angle += 2 * Math.PI; } // figure out if it's less than the end angle if (angle > endAngle) { throw new InvalidOperationException(); } // it's in the bounds, so calculate distance to end angle double distToGo = radius * (endAngle - angle); if (dist < distToGo) { double newAngle = angle + dist / radius; Coordinates pt2 = new Coordinates(center.X + radius * Math.Cos(newAngle), center.Y + radius * Math.Sin(newAngle)); double dist2 = radius * (newAngle - startAngle); dist = 0; return(new PointOnPath(this, dist2, pt2)); } else { dist -= distToGo; return(EndPoint); } } else { // get it smaller than the start angle if (angle > startAngle) { angle -= 2 * Math.PI; } // figure out oif it's smaller than the end angle if (angle < endAngle) { throw new InvalidOperationException(); } // it's in the bounds, so calculate distance to end angle double distToGo = radius * (angle - endAngle); if (dist < distToGo) { double newAngle = angle - dist / radius; Coordinates pt2 = new Coordinates(center.X + radius * Math.Cos(newAngle), center.Y + radius * Math.Sin(newAngle)); double dist2 = radius * (startAngle - newAngle); dist = 0; return(new PointOnPath(this, dist2, pt2)); } else { dist -= distToGo; return(EndPoint); } } }
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 LinePath SubPath(PointOnPath start, double dist) { return(SubPath(start, ref dist)); }
/// <summary> /// Advances down the path by <c>dist</c> units. /// </summary> /// <param name="pt">Starting point on path</param> /// <param name="dist"> /// Distance to advance. On exit, will be the distance that could not be satisfied or 0 if the request did not go past /// the end of the path. /// </param> /// <returns> /// New advanced <c>PointOnPath</c> or <c>EndPoint</c> if past the end of the path. /// </returns> /// <exception cref="ArgumentOutOfRangeException"> /// Thrown when the segment specified by <c>pt</c> is not in the segments collection. /// </exception> /// <remarks> /// Advances the point starting with the segment specified in <c>pt</c> and moving forward along the path. At each segment, /// the IPathSegment.Advance method is called, then moving to the next segment until dist is exhausted. /// </remarks> /// <example> /// This shows an example advancing down the first thirty meters of a path and checking if the result is past the end. /// <code> /// PointOnPath start = path.StartPoint; /// double dist = 30; /// PointOnPath thirtyMeters = path.Advance(start, ref dist); /// if (dist > 0) { /// Debug.WriteLine("Past the end of the path!"); /// } /// else { /// Debug.WriteLine("Not past the end!"); /// } /// </code> /// </example> public PointOnPath AdvancePoint(PointOnPath pt, ref double dist) { int ind = segments.IndexOf(pt.segment); if (ind == -1) { throw new ArgumentOutOfRangeException(); } if (dist > 0) { while (dist > 0) { IPathSegment seg = pt.segment; pt = seg.AdvancePoint(pt, ref dist); if (dist > 0) { // increment the segment index ind++; // check if we're at the end if (ind == segments.Count) { return(pt); } else { // otherwise, move the PointOnPath to the beginning of the next segment pt = segments[ind].StartPoint; } } } } else { while (dist < 0) { IPathSegment seg = pt.segment; pt = seg.AdvancePoint(pt, ref dist); if (dist < 0) { // increment the segment index ind--; // check if we're at the end if (ind == -1) { return(pt); } else { // otherwise, move the PointOnPath to the beginning of the next segment pt = segments[ind].EndPoint; } } } } return(pt); }
public LinePath SubPath(PointOnPath start, ref double dist) { PointOnPath end = AdvancePoint(start, ref dist); return(SubPath(start, end)); }