/// <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);
}
/// <summary>
/// Checks if the path contains the specified segment.
/// </summary>
/// <param name="item">Segment to search for.</param>
/// <returns><c>true</c> if the item is found in the path, <c>false</c> otherwise.</returns>
public bool Contains(IPathSegment item)
{
PartitionPathSegment laneSegment = item as PartitionPathSegment;
if (laneSegment == null)
{
return(false);
}
return(cw.UserPartitions.Contains(laneSegment.UserPartition));
}
/// <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();
}
}
/// <summary>
/// Returns the index of the specified segment.
/// </summary>
/// <param name="item">Segment to look for.</param>
/// <returns>
/// Zero-based index of segment if found, -1 if not found.
/// </returns>
public int IndexOf(IPathSegment item)
{
PartitionPathSegment laneSeg = item as PartitionPathSegment;
if (laneSeg == null)
{
return(-1);
}
UserPartition partition = laneSeg.UserPartition;
return(partition.ParentPartition.UserPartitions.IndexOf(partition));
}
/// <summary>
/// Checks if the path contains the specified segment.
/// </summary>
/// <param name="item">Segment to search for.</param>
/// <returns><c>true</c> if the item is found in the path, <c>false</c> otherwise.</returns>
public bool Contains(IPathSegment item)
{
PartitionPathSegment laneSegment = item as PartitionPathSegment;
if (laneSegment == null)
{
return(false);
}
LanePartition lanePartition = laneSegment.UserPartition.ParentPartition as LanePartition;
if (lanePartition == null)
{
return(false);
}
return(lanePartition.Lane.Equals(lane));
}
/// <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);
}
/// <summary>
/// Returns the <see cref="PointOnPath"/> associated with the supplied <see cref="RndfWayPoint"/>.
/// </summary>
/// <param name="waypoint"><see cref="RndfWayPoint"/> to find on the path.</param>
/// <returns><see cref="PointOnPath"/> instance of <paramref name="waypoint"/>.</returns>
/// <remarks>
/// The methods does not do any searching but returns the <see cref="PointOnPath"/> object directly. It will put
/// the point at the end of a <see cref="PartitionPathSegment"/> if possible or at the beginning if the waypoint
/// is the first in the lane.
/// </remarks>
/// <exception cref="ArgumentException">
/// Throw when the waypoint does not belong to the <see cref="Lane"/> associated with the <see cref="LanePath"/>.
/// </exception>
public PointOnPath GetWaypointPoint(RndfWayPoint waypoint)
{
if (!lane.Equals(waypoint.Lane))
{
throw new ArgumentException("Waypoint does not belong to the current lane");
}
// get the user partition for that stuff
if (waypoint.PreviousLanePartition != null)
{
LanePartition laneParition = waypoint.PreviousLanePartition;
PartitionPathSegment pathSeg = GetPathSegment(laneParition.UserPartitions[laneParition.UserPartitions.Count - 1]);
return(pathSeg.EndPoint);
}
else
{
LanePartition laneParition = waypoint.NextLanePartition;
PartitionPathSegment pathSeg = GetPathSegment(laneParition.UserPartitions[0]);
return(pathSeg.StartPoint);
}
}
/// <summary>
/// Returns the index of the specified segment.
/// </summary>
/// <param name="item">Segment to look for.</param>
/// <returns>
/// Zero-based index of segment if found, -1 if not found.
/// </returns>
public int IndexOf(IPathSegment item)
{
PartitionPathSegment laneSeg = item as PartitionPathSegment;
if (laneSeg == null)
{
return(-1);
}
UserPartition partition = laneSeg.UserPartition;
LanePartition lanePartition = partition.ParentPartition as LanePartition;
Debug.Assert(lanePartition.Lane.Equals(lane));
int index = partition.ParentPartition.UserPartitions.IndexOf(partition);
int laneIndex = lanePartition.Lane.LanePartitions.IndexOf(lanePartition);
while (--laneIndex > 0)
{
index += lane.LanePartitions[laneIndex].UserPartitions.Count;
}
return(index);
}
