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