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