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));
}
