public bool DoesIntersect(LinePath path)
{
foreach (LineSegment ls in path.GetSegmentEnumerator())
{
if (DoesIntersect(ls))
{
return(true);
}
}
return(false);
}
private LinePath FindBoundary(double offset)
{
// create a list of shift line segments
List <LineSegment> segs = new List <LineSegment>();
foreach (LineSegment ls in basePath.GetSegmentEnumerator())
{
segs.Add(ls.ShiftLateral(offset));
}
// find the intersection points between all of the segments
LinePath boundPoints = new LinePath();
// add the first point
boundPoints.Add(segs[0].P0);
// loop through the stuff
for (int i = 0; i < segs.Count - 1; i++)
{
// find the intersection
Line l0 = (Line)segs[i];
Line l1 = (Line)segs[i + 1];
Coordinates pt;
if (l0.Intersect(l1, out pt))
{
// figure out the angle of the intersection
double angle = Math.Acos(l0.UnitVector.Dot(l1.UnitVector));
// calculate the length factor
// 90 deg, 3x width
// 0 deg, 2x width
double widthFactor = Math.Pow(angle / (Math.PI / 2.0), 2) * 2 + 1;
// get the line formed by pt and the corresponding path point
Coordinates boundLine = pt - basePath[i + 1];
boundPoints.Add(widthFactor * Math.Abs(offset) * boundLine.Normalize() + basePath[i + 1]);
}
else
{
boundPoints.Add(segs[i].P1);
}
}
// add the last point
boundPoints.Add(segs[segs.Count - 1].P1);
return(boundPoints);
}
public override void Process(object param)
{
if (!base.BeginProcess())
{
return;
}
if (param is ZoneTravelingBehavior)
{
HandleBaseBehavior((ZoneTravelingBehavior)param);
}
extraObstacles = GetPerimeterObstacles();
if (reverseGear)
{
ProcessReverse();
return;
}
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
// get the vehicle relative path
LinePath relRecommendedPath = recommendedPath.Transform(absTransform);
LinePath.PointOnPath zeroPoint = relRecommendedPath.ZeroPoint;
// get the distance to the end point
double distToEnd = relRecommendedPath.DistanceBetween(zeroPoint, relRecommendedPath.EndPoint);
// get the planning distance
double planningDist = GetPlanningDistance();
planningDist = Math.Max(planningDist, 20);
planningDist -= zeroPoint.Location.Length;
if (planningDist < 2.5)
{
planningDist = 2.5;
}
if (distToEnd < planningDist)
{
// make the speed command at stop speed command
behaviorTimestamp = curTimestamp;
speedCommand = new StopAtDistSpeedCommand(distToEnd - TahoeParams.FL);
planningDist = distToEnd;
approachSpeed = recommendedSpeed.Speed;
settings.endingHeading = relRecommendedPath.EndSegment.UnitVector.ArcTan;
settings.endingPositionFixed = true;
settings.endingPositionMax = 2;
settings.endingPositionMin = -2;
}
else
{
speedCommand = new ScalarSpeedCommand(recommendedSpeed.Speed);
}
// get the distance of the path segment we care about
LinePath pathSegment = relRecommendedPath.SubPath(zeroPoint, planningDist);
double avoidanceDist = planningDist + 5;
avoidanceBasePath = relRecommendedPath.SubPath(zeroPoint, ref avoidanceDist);
if (avoidanceDist > 0)
{
avoidanceBasePath.Add(avoidanceBasePath.EndPoint.Location + avoidanceBasePath.EndSegment.Vector.Normalize(avoidanceDist));
}
// test if we should clear out of arc mode
if (arcMode)
{
if (TestNormalModeClear(relRecommendedPath, zeroPoint))
{
prevCurvature = double.NaN;
arcMode = false;
}
}
if (Math.Abs(zeroPoint.AlongtrackDistance(Coordinates.Zero)) > 1)
{
pathSegment.Insert(0, Coordinates.Zero);
}
else
{
if (pathSegment[0].DistanceTo(pathSegment[1]) < 1)
{
pathSegment.RemoveAt(0);
}
pathSegment[0] = Coordinates.Zero;
}
if (arcMode)
{
Coordinates relativeGoalPoint = relRecommendedPath.EndPoint.Location;
ArcVoteZone(relativeGoalPoint, extraObstacles);
return;
}
double pathLength = pathSegment.PathLength;
if (pathLength < 6)
{
double additionalDist = 6.25 - pathLength;
pathSegment.Add(pathSegment.EndPoint.Location + pathSegment.EndSegment.Vector.Normalize(additionalDist));
}
// determine if polygons are to the left or right of the path
for (int i = 0; i < zoneBadRegions.Length; i++)
{
Polygon poly = zoneBadRegions[i].Transform(absTransform);
int numLeft = 0;
int numRight = 0;
foreach (LineSegment ls in pathSegment.GetSegmentEnumerator())
{
for (int j = 0; j < poly.Count; j++)
{
if (ls.IsToLeft(poly[j]))
{
numLeft++;
}
else
{
numRight++;
}
}
}
if (numLeft > numRight)
{
// we'll consider this polygon on the left of the path
//additionalLeftBounds.Add(new Boundary(poly, 0.1, 0.1, 0));
}
else
{
//additionalRightBounds.Add(new Boundary(poly, 0.1, 0.1, 0));
}
}
// TODO: add zone perimeter
disablePathAngleCheck = false;
laneWidthAtPathEnd = 7;
settings.Options.w_diff = 3;
smootherBasePath = new LinePath();
smootherBasePath.Add(Coordinates.Zero);
smootherBasePath.Add(pathSegment.EndPoint.Location);
AddTargetPath(pathSegment, 0.005);
settings.maxSpeed = recommendedSpeed.Speed;
useAvoidancePath = false;
SmoothAndTrack(command_label, true);
}
public static LinePath FindBoundary(double offset, LinePath path)
{
// create a list of shift line segments
List<LineSegment> segs = new List<LineSegment>();
foreach (LineSegment ls in path.GetSegmentEnumerator()) {
segs.Add(ls.ShiftLateral(offset));
}
// find the intersection points between all of the segments
LinePath boundPoints = new LinePath();
// add the first point
boundPoints.Add(segs[0].P0);
// loop through the stuff
for (int i = 0; i < segs.Count-1; i++) {
// find the intersection
Line l0 = (Line)segs[i];
Line l1 = (Line)segs[i+1];
Coordinates pt;
if (l0.Intersect(l1, out pt)) {
// figure out the angle of the intersection
double angle = Math.Acos(l0.UnitVector.Dot(l1.UnitVector));
double angle2 = Math.Sin(l0.UnitVector.Cross(l1.UnitVector));
// determine if it's a left or right turn
bool leftTurn = angle2 > 0;
double f = 2.5;
if ((leftTurn && offset > 0) || (!leftTurn && offset < 0)) {
// left turn and looking for left bound or right turn and looking for right bound
f = 0.5;
}
// calculate the width expansion factor
// 90 deg, 3x width
// 45 deg, 1.5x width
// 0 deg, 1x width
double widthFactor = Math.Pow(angle/(Math.PI/2.0),1.25)*f + 1;
// get the line formed by pt and the corresponding path point
Coordinates boundLine = pt - path[i+1];
boundPoints.Add(widthFactor*Math.Abs(offset)*boundLine.Normalize() + path[i+1]);
}
else {
boundPoints.Add(segs[i].P1);
}
}
// add the last point
boundPoints.Add(segs[segs.Count-1].P1);
return boundPoints;
}
public static LinePath FindBoundary(double offset, LinePath path)
{
// create a list of shift line segments
List <LineSegment> segs = new List <LineSegment>();
foreach (LineSegment ls in path.GetSegmentEnumerator())
{
segs.Add(ls.ShiftLateral(offset));
}
// find the intersection points between all of the segments
LinePath boundPoints = new LinePath();
// add the first point
boundPoints.Add(segs[0].P0);
// loop through the stuff
for (int i = 0; i < segs.Count - 1; i++)
{
// find the intersection
Line l0 = (Line)segs[i];
Line l1 = (Line)segs[i + 1];
Coordinates pt;
if (l0.Intersect(l1, out pt))
{
// figure out the angle of the intersection
double angle = Math.Acos(l0.UnitVector.Dot(l1.UnitVector));
double angle2 = Math.Sin(l0.UnitVector.Cross(l1.UnitVector));
// determine if it's a left or right turn
bool leftTurn = angle2 > 0;
double f = 2.5;
if ((leftTurn && offset > 0) || (!leftTurn && offset < 0))
{
// left turn and looking for left bound or right turn and looking for right bound
f = 0.5;
}
// calculate the width expansion factor
// 90 deg, 3x width
// 45 deg, 1.5x width
// 0 deg, 1x width
double widthFactor = Math.Pow(angle / (Math.PI / 2.0), 1.25) * f + 1;
// get the line formed by pt and the corresponding path point
Coordinates boundLine = pt - path[i + 1];
boundPoints.Add(widthFactor * Math.Abs(offset) * boundLine.Normalize() + path[i + 1]);
}
else
{
boundPoints.Add(segs[i].P1);
}
}
// add the last point
boundPoints.Add(segs[segs.Count - 1].P1);
return(boundPoints);
}