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