private void HandleBehavior(UTurnBehavior cb) { // get the absolute transform AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp); this.polygonTimestamp = absTransform.Timestamp; this.polygon = cb.Boundary.Transform(absTransform); this.finalLane = cb.EndingLane; this.finalSpeedCommand = cb.EndingSpeedCommand; this.stopOnLine = cb.StopOnEndingPath; this.stayOutPolygons = cb.StayOutPolygons; // run constant checking if we're supposed to stop on the final line if (stopOnLine) { checkMode = true; } else { checkMode = false; } // transform the path LinePath.PointOnPath closestPoint = cb.EndingPath.GetClosestPoint(originalPoint); // relativize the path LinePath relFinalPath = cb.EndingPath.Transform(absTransform); // get the ending orientation finalOrientation = new LineSegment(relFinalPath[closestPoint.Index], relFinalPath[closestPoint.Index+1]); Services.UIService.PushLineList(cb.EndingPath, cb.TimeStamp, "original path1", false); }
private static double GetObstacleClearanceLine(LineSegment segment, IList<Polygon> obstacles) { double minDist = double.MaxValue; foreach (Polygon obs in obstacles) { foreach (Coordinates pt in obs) { Coordinates closestPt = segment.ClosestPoint(pt); double dist = closestPt.DistanceTo(pt); if (dist < minDist) minDist = dist; } } return minDist; }
private ITrackingCommand BuildBackwardPass() { BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "UTurn Behavior: Initialize Backward Pass"); // rotate the polygon into the current relative frame CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp; RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp); relTransform.TransformPointsInPlace(polygon); finalOrientation = finalOrientation.Transform(relTransform); polygonTimestamp = curTimestamp; // retrieve the vehicle state Coordinates headingVec = new Coordinates(1, 0); Coordinates headingVec180 = headingVec.Rotate180(); Coordinates headingVec90 = headingVec.Rotate90(); // figure out center point of turn Circle rearAxleCircle = Circle.FromPointSlopeRadius(new Coordinates(0,0), headingVec180, minRadius); Coordinates center = rearAxleCircle.center; // calculate the points of the wheels Coordinates rearLeftPoint = headingVec90*TahoeParams.T/2; Coordinates rearRightPoint = -headingVec90*TahoeParams.T/2; Coordinates frontLeftPoint = headingVec*TahoeParams.L + headingVec90*TahoeParams.T/2; Coordinates frontRightPoint = headingVec*TahoeParams.L - headingVec90*TahoeParams.T/2; double minHit = Math.PI/2.1; GetMinHitAngle(rearLeftPoint, center, ref minHit); GetMinHitAngle(rearRightPoint, center, ref minHit); //GetMinHitAngle(frontLeftPoint, center, false, ref minHit); //GetMinHitAngle(frontRightPoint, center, false, ref minHit); frontLeftPoint = headingVec*TahoeParams.FL + headingVec90*TahoeParams.T/2; frontRightPoint = headingVec*TahoeParams.FL - headingVec90*TahoeParams.T/2.0; rearRightPoint = -headingVec*TahoeParams.RL - headingVec90*TahoeParams.T/2.0; rearLeftPoint = -headingVec*TahoeParams.RL + headingVec90*TahoeParams.T/2.0; List<Polygon> obstacles = GetObstacles(curTimestamp); GetObstacleHitAngle(frontLeftPoint, center, obstacles, ref minHit); GetObstacleHitAngle(rearLeftPoint, center, obstacles, ref minHit); GetObstacleHitAngle(rearRightPoint, center, obstacles, ref minHit); // trim some off the hit for safety' minHit -= (0.3/minRadius); // move at least 0.6 meters minHit = Math.Max(minHit, 0.6 / minRadius); // calculate the exit stopping point // shift the line by the minimum turning radius Coordinates u = finalOrientation.P1 - finalOrientation.P0; u = u.Normalize().Rotate90(); Line offsetLine = new Line(); offsetLine.P0 = finalOrientation.P0 + u*(minRadius+2); offsetLine.P1 = finalOrientation.P1 + u*(minRadius+2); // final the intersection of the current turn circle with a radius of twice the min turn radius and the offset line Circle twoTurn = new Circle(2*minRadius + 2, center); Coordinates[] intersections; double startAngle = (-center).ArcTan; if (twoTurn.Intersect(offsetLine, out intersections)) { // figure out where there were hits for (int i = 0; i < intersections.Length; i++) { // get the angle of the hit double angle = (intersections[i] - center).ArcTan; if (angle < startAngle) angle += 2*Math.PI; angle -= startAngle; if (angle < minHit) minHit = angle; } } minHit = Math.Max(minHit, 0.6 / minRadius); // set the stopping point at the min hit point Coordinates stopPoint = rearAxleCircle.GetPoint(startAngle+minHit); // calculate the stop distance stopDistance = rearAxleCircle.r*minHit; stopTimestamp = curTimestamp; curvature = 1/minRadius; // calculate the required steering angle double steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(-1/minRadius, uturnSpeed); ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(TransmissionGear.Reverse); ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true); ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), uturnSpeed)); ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false); ChainedTrackingCommand cmd = new ChainedTrackingCommand( new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true), new TrackingCommand(passSpeedCommand, passSteeringCommand, false)); cmd.Label = backwardLabel; Services.UIService.PushCircle(new Circle(minRadius, center), curTimestamp, "uturn circle", true); Services.UIService.PushPoint(stopPoint, curTimestamp, "uturn stop point", true); Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true); return cmd; }
private ITrackingCommand BuildForwardPass(out bool finalPass) { BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "UTurn Behavior: Determine Forward Pass"); // rotate the polygon into the current relative frame CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp; RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp); relTransform.TransformPointsInPlace(polygon); finalOrientation = finalOrientation.Transform(relTransform); polygonTimestamp = curTimestamp; // retrieve the vehicle state Coordinates headingVec = new Coordinates(1, 0); Coordinates headingVec90 = headingVec.Rotate90(); // check if we can make it out now Line curLine = new Line(new Coordinates(0,0), headingVec); Coordinates intersectionPoint; LineSegment finalLine = finalOrientation; Circle outCircle = Circle.FromLines(curLine, (Line)finalLine, out intersectionPoint); double steeringCommand; if (!outCircle.Equals(Circle.Infinite) && outCircle.r > minRadius) { // we found an out circle BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "found final pass output"); // build the circle segment double hitAngle = (intersectionPoint - outCircle.center).ArcTan; double startAngle = (-outCircle.center).ArcTan; if (hitAngle < startAngle) hitAngle += 2*Math.PI; hitAngle -= startAngle; if (stopOnLine) { // get the angle of the end point of the line segment double endPointAngle = (finalLine.P1 - outCircle.center).ArcTan; if (endPointAngle < startAngle) endPointAngle += 2*Math.PI; endPointAngle -= startAngle; if (endPointAngle < hitAngle) { hitAngle = endPointAngle; } } // get the obstacles Coordinates frontLeftPoint = headingVec * TahoeParams.FL + headingVec90 * TahoeParams.T / 2; Coordinates frontRightPoint = headingVec * TahoeParams.FL - headingVec90 * TahoeParams.T / 2.0; Coordinates rearRightPoint = -headingVec * TahoeParams.RL - headingVec90 * TahoeParams.T / 2.0; List<Polygon> obstacles = GetObstacles(curTimestamp); GetObstacleHitAngle(frontLeftPoint, outCircle.center, obstacles, ref hitAngle); GetObstacleHitAngle(frontRightPoint, outCircle.center, obstacles, ref hitAngle); GetObstacleHitAngle(rearRightPoint, outCircle.center, obstacles, ref hitAngle); // calculate stopping distance stopDistance = outCircle.r*hitAngle; stopTimestamp = curTimestamp; curvature = 1/outCircle.r; intersectionPoint = outCircle.center + Coordinates.FromAngle(startAngle + hitAngle)*outCircle.r; // calculate steering angle steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1/outCircle.r, uturnSpeed); // mark that this will be the final pass finalPass = true; Services.UIService.PushCircle(outCircle, curTimestamp, "uturn circle", true); Services.UIService.PushPoint(intersectionPoint, curTimestamp, "uturn stop point", true); Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true); } else { finalPass = false; // draw out 3 circles // - front right wheel // - front left wheel // - rear left wheel // figure out center point of turn Circle rearAxleCircle = Circle.FromPointSlopeRadius(Coordinates.Zero, headingVec, minRadius); Coordinates center = rearAxleCircle.center; // calculate the points of the wheels Coordinates rearLeftPoint = headingVec90*TahoeParams.T/2; Coordinates rearRightPoint = -headingVec90*TahoeParams.T/2; Coordinates frontLeftPoint = headingVec*TahoeParams.L + headingVec90*TahoeParams.T/2; Coordinates frontRightPoint = headingVec*TahoeParams.L - headingVec90*TahoeParams.T/2; // initialize min hit angle to slightly less than 90 degrees double minHit = Math.PI/2.1; //GetMinHitAngle(rearLeftPoint, center, true, ref minHit); //GetMinHitAngle(rearRightPoint, center, true, ref minHit); GetMinHitAngle(frontLeftPoint, center, ref minHit); GetMinHitAngle(frontRightPoint, center, ref minHit); // get the obstacles List<Polygon> obstacles = GetObstacles(curTimestamp); frontLeftPoint = headingVec*TahoeParams.FL + headingVec90*TahoeParams.T/2; frontRightPoint = headingVec*TahoeParams.FL - headingVec90*TahoeParams.T/2.0; rearRightPoint = -headingVec*TahoeParams.RL - headingVec90*TahoeParams.T/2.0; GetObstacleHitAngle(frontLeftPoint, center, obstacles, ref minHit); GetObstacleHitAngle(frontRightPoint, center, obstacles, ref minHit); GetObstacleHitAngle(rearRightPoint, center, obstacles, ref minHit); // trim some off the hit for safety //if (minHit > 0.5/minRadius) minHit -= (0.5/minRadius); minHit = Math.Max(minHit, 0.6 / minRadius); double startAngle = (Coordinates.Zero-center).ArcTan; double hitAngle = startAngle + minHit; // set the stopping point at the min hit point Coordinates stopPoint = rearAxleCircle.GetPoint(hitAngle); // calculate the stop distance stopDistance = minRadius*minHit; // calculate the required steering angle steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1/minRadius, uturnSpeed); Services.UIService.PushCircle(new Circle(minRadius, center), curTimestamp, "uturn circle", true); Services.UIService.PushPoint(stopPoint, curTimestamp, "uturn stop point", true); Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true); } // build the command ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(TransmissionGear.First); ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true); ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), uturnSpeed)); ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false); ChainedTrackingCommand cmd = new ChainedTrackingCommand( new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true), new TrackingCommand(passSpeedCommand, passSteeringCommand, false)); cmd.Label = forwardLabel; return cmd; }
private Obstacle CreatePerimeterObstacle(LineSegment ls) { // perimeter points go clockwise, so shift this segment left to make it go outward LineSegment shifted = ls.ShiftLateral(1); // create a polygon for the obstacle Polygon obstaclePoly = new Polygon(); obstaclePoly.Add(ls.P0); obstaclePoly.Add(ls.P1); obstaclePoly.Add(shifted.P1); obstaclePoly.Add(shifted.P0); Obstacle obs = new Obstacle(); obs.obstaclePolygon = obstaclePoly; Circle tahoeCircle = new Circle(TahoeParams.T/2.0+0.1, Coordinates.Zero); Polygon tahoePoly = tahoeCircle.ToPolygon(24); obs.cspacePolygon = Polygon.ConvexMinkowskiConvolution(tahoePoly, obstaclePoly); obs.obstacleClass = ObstacleClass.StaticLarge; obs.desSpacing = 0.5; obs.minSpacing = 0.1; obs.age = 1; obs.trackID = -1; return obs; }
private List<Obstacle> GetPerimeterObstacles() { // transform the polygon to relative coordinates AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(); Polygon relPerimeter = zonePerimeter.Transform(absTransform); LinePath relRecommendedPath = recommendedPath.Transform(absTransform); if (relPerimeter.IsCounterClockwise) { relPerimeter = relPerimeter.Reverse(); } // create a polygon for ourselves and see if we intersect any perimeters Polygon vehiclePoly = new Polygon(); vehiclePoly.Add(new Coordinates(-TahoeParams.RL, -(TahoeParams.T/2.0))); vehiclePoly.Add(new Coordinates(TahoeParams.FL, -(TahoeParams.T/2.0))); vehiclePoly.Add(new Coordinates(TahoeParams.FL, TahoeParams.T/2.0)); vehiclePoly.Add(new Coordinates(-TahoeParams.RL, TahoeParams.T/2.0)); // inflate by about 2 m vehiclePoly = vehiclePoly.Inflate(2); // test if we intersect any of the perimeter points List<Obstacle> perimeterObstacles = new List<Obstacle>(); List<OperationalObstacle> operationalObstacles = new List<OperationalObstacle>(); List<LineSegment> segments = new List<LineSegment>(); foreach (LineSegment ls1 in relPerimeter.GetSegmentEnumerator()) { segments.Clear(); if (ls1.Length > 15) { // split into multiple segment double targetLength = 10; int numSegments = (int)Math.Round(ls1.Length/targetLength); double splitLength = ls1.Length/numSegments; Coordinates pt = ls1.P0; for (int i = 0; i < numSegments; i++) { Coordinates endPoint = pt + ls1.Vector.Normalize(splitLength); LineSegment seg = new LineSegment(pt, endPoint); segments.Add(seg); pt = endPoint; } } else { segments.Add(ls1); } foreach (LineSegment ls in segments) { bool pathTooClose = false; foreach (Coordinates pt in relRecommendedPath) { Coordinates closest = ls.ClosestPoint(pt); if (closest.DistanceTo(pt) < 1) pathTooClose = true; } if (!vehiclePoly.DoesIntersect(ls) && !pathTooClose) { Obstacle obs = CreatePerimeterObstacle(ls); perimeterObstacles.Add(obs); OperationalObstacle uiobs = new OperationalObstacle(); uiobs.age = obs.age; uiobs.heading = 0; uiobs.headingValid = false; uiobs.ignored = false; uiobs.obstacleClass = obs.obstacleClass; uiobs.poly = obs.obstaclePolygon; operationalObstacles.Add(uiobs); } } } Services.UIService.PushObstacles(operationalObstacles.ToArray(), curTimestamp, "perimeter obstacles", true); return perimeterObstacles; }
private void HandleBehavior(ZoneParkingBehavior b) { base.HandleBaseBehavior(b); this.parkingSpotLine = new LineSegment(b.ParkingSpotPath[0], b.ParkingSpotPath[1]); this.parkingSpotExtraDist = b.ExtraDistance; this.parkingSpotLeftBound = b.SpotLeftBound; this.parkingSpotRightBound = b.SpotRightBound; if (b is ZoneParkingPullOutBehavior) { ZoneParkingPullOutBehavior pb = (ZoneParkingPullOutBehavior)b; this.endingLine = new Line(pb.RecommendedPullOutPath[3], pb.RecommendedPullOutPath[2]); Coordinates parkPoint = parkingSpotLine.P1 + parkingSpotLine.UnitVector.Normalize(parkingSpotExtraDist); this.pulloutPoint = parkPoint + endingLine.UnitVector*10; this.pulloutMode = true; } else { this.pulloutMode = false; } Services.UIService.PushLineList(b.ParkingSpotPath, b.TimeStamp, "original path1", false); Services.UIService.PushLineList(parkingSpotLeftBound, b.TimeStamp, "left bound", false); Services.UIService.PushLineList(parkingSpotRightBound, b.TimeStamp, "right bound", false); }
public static void FindParkingInterval(IList<Obstacle> obstacles, LineSegment projectionLine, double offDistMax, double offDistMin, double maxBottomDist, double maxTopDist, ref Coordinates topPoint, ref Coordinates bottomPoint) { List<double> projectedDists = new List<double>(1000); Coordinates vec = projectionLine.UnitVector; Coordinates startPt = projectionLine.P0; double len = projectionLine.Length; foreach (Obstacle obs in obstacles) { foreach (Coordinates pt in obs.AvoidancePolygon) { double projDist = vec.Dot(pt - startPt); double offDist = vec.Cross(pt - startPt); if (projDist < 0 || projDist > len) continue; if (offDist < offDistMin || offDist > offDistMax) continue; // add to the list projectedDists.Add(projDist); } } // sort the list projectedDists.Sort(); // find the max dist between two adjacent projected distance double bestDist = -1; double bestProjBottom = 0; double bestProjTop = maxTopDist; for (int i = 0; i < projectedDists.Count-1; i++) { if (projectedDists[i] > maxBottomDist) { if (bestDist < 0) { bestDist = bestProjTop = projectedDists[i]; } break; } double dist = projectedDists[i+1]-projectedDists[i]; if (dist > bestDist) { bestDist = dist; bestProjBottom = projectedDists[i]; bestProjTop = projectedDists[i+1]; } } bottomPoint = projectionLine.P0 + vec*bestProjBottom; topPoint = projectionLine.P0 + vec*bestProjTop; }
public bool Equals(LineSegment other) { return P0.Equals(other.P0) && P1.Equals(other.P1); }
public bool Intersect(LineSegment l, out Coordinates pt) { Coordinates K; return Intersect(l, out pt, out K); }
public bool Intersect(LineSegment l, out Coordinates pt, out Coordinates K) { Coordinates P = P1 - P0; Coordinates S = l.P1 - l.P0; Matrix2 A = new Matrix2(P.X, -S.X, P.Y, -S.Y); if (Math.Abs(A.Determinant()) < 1e-10) { pt = default(Coordinates); K = default(Coordinates); return false; } K = A.Inverse()*(l.P0 - P0); if (K.X >= 0 && K.X <= 1 && K.Y >= 0 && K.Y <= 1) { pt = P0 + P*K.X; return true; } else { pt = default(Coordinates); return false; } }
// obstacles - list of obstacles to avoid near the parking spot // projectionLine - P1 is the place where the front bumper of the vehicle should end up; P0 is where the rear bumper of the vehicle should end up // zonePerimiter - list of points specifying the perimiter of the parking zone // vehiclePosition - current position of the vehicle // vehicleHeading - heading of the car, in radians // corner1 and corner2 are always set to the corners of the rectangle, however if the function returns FALSE then // the rectangle is the minimum 8x8 box containing the vehicle's back axis whether or not the box contains obstacles or crosses the zone perimiter public static bool FindParkingInterval(IList<Obstacle> obstacles, LineSegment projectionLine, Polygon zonePerimeter, Coordinates vehiclePosition, double vehicleHeading, ref Coordinates[] corners) { const double expansion_step = .25; bool ret = true; // transform everything into "parking coordinates": // 0,0 is where the center of the rear bumper should end up when parking is finished // the vehicle should be parallel to (and on top of) the Y axis when parking has finished (positive Y) List<Polygon> pcObstacles = new List<Polygon>(obstacles.Count); for(int i=0;i<obstacles.Count;i++){ Polygon p = obstacles[i].AvoidancePolygon.Inflate(Math.Max(obstacles[i].minSpacing,.5)); for(int j=0;j<p.points.Count;j++){ p.points[j] -= projectionLine.P0; p.points[j] = p.points[j].Rotate(-projectionLine.UnitVector.RotateM90().ArcTan); } pcObstacles.Add(p); } Polygon pcPerimeter = new Polygon(zonePerimeter); for (int i = 0; i < pcPerimeter.points.Count; i++) { pcPerimeter.points[i] -= projectionLine.P0; pcPerimeter.points[i] = pcPerimeter.points[i].Rotate(-projectionLine.UnitVector.RotateM90().ArcTan); } Coordinates pcVehPos = vehiclePosition; pcVehPos -= projectionLine.P0; pcVehPos = pcVehPos.Rotate(-projectionLine.UnitVector.RotateM90().ArcTan); double leftBound, rightBound; double lowBound; // highbound is always 0 const double highBound = 0; leftBound = Math.Min(-TahoeParams.T / 2.0 - 1, pcVehPos.X - TahoeParams.T / 2.0 * 1.2); rightBound = Math.Max(TahoeParams.T / 2.0 + 1, pcVehPos.X + TahoeParams.T / 2.0 * 1.2); lowBound = Math.Min(0, pcVehPos.Y - TahoeParams.T / 2.0 * 1.2); bool canExpandDown = true; bool canExpandLeft = true; bool canExpandRight = true; // initial expansion... lower boundary while (lowBound > -8 && canExpandDown) { LineSegment l = new LineSegment(new Coordinates(leftBound, lowBound), new Coordinates(rightBound, lowBound)); if (ExpansionViolatesConstraints(l,pcPerimeter,pcObstacles)) { canExpandDown = false; break; } lowBound -= expansion_step; } // initial expansion... left & right boundary while ((rightBound-leftBound) < 8 && (canExpandRight || canExpandLeft)) { LineSegment ls; ls = new LineSegment(new Coordinates(leftBound, lowBound), new Coordinates(leftBound, highBound)); if (ExpansionViolatesConstraints(ls, pcPerimeter, pcObstacles)) { canExpandLeft = false; } else { leftBound -= expansion_step; } ls = new LineSegment(new Coordinates(rightBound, lowBound), new Coordinates(rightBound, highBound)); if (ExpansionViolatesConstraints(ls, pcPerimeter, pcObstacles)) { canExpandRight = false; } else { rightBound += expansion_step; } } // move boundaries out to make at least an 8x8 m box, even if this violates the perimiter/includes obstacles if(rightBound-leftBound < 8){ double tmp = (rightBound+leftBound)/2; leftBound = tmp - 4; rightBound = tmp + 4; ret = false; } if (lowBound > -8) { ret = false; lowBound = -8; } bool lastExpandLeft = false; while((canExpandDown || canExpandLeft || canExpandRight) && (lowBound>-30) && (rightBound-leftBound)<30){ if(!(canExpandLeft || canExpandRight) || (-lowBound < (rightBound-leftBound) && canExpandDown)){ LineSegment l = new LineSegment(new Coordinates(leftBound, lowBound - .5), new Coordinates(rightBound, lowBound - .5)); if (ExpansionViolatesConstraints(l, pcPerimeter, pcObstacles)) { canExpandDown = false; }else{ lowBound -= expansion_step; } }else if(!(canExpandDown || canExpandRight) || (canExpandLeft && !(lastExpandLeft && canExpandRight))){ LineSegment l = new LineSegment(new Coordinates(leftBound - .5, lowBound), new Coordinates(leftBound - .5, highBound)); if (ExpansionViolatesConstraints(l, pcPerimeter, pcObstacles)) { canExpandLeft = false; }else{ leftBound -= expansion_step; } lastExpandLeft = true; }else{ // canExpandRight LineSegment l = new LineSegment(new Coordinates(rightBound + .5, lowBound), new Coordinates(rightBound + .5, highBound)); if (ExpansionViolatesConstraints(l, pcPerimeter, pcObstacles)) { canExpandRight = false; } else{ rightBound += expansion_step; } lastExpandLeft = false; } } corners = new Coordinates[4]; corners[0] = new Coordinates(leftBound, lowBound); corners[1] = new Coordinates(leftBound, highBound); corners[2] = new Coordinates(rightBound, highBound); corners[3] = new Coordinates(rightBound, lowBound); Random r = new Random(); for (int i = 0; i < 4; i++) { corners[i] = corners[i].Rotate(projectionLine.UnitVector.RotateM90().ArcTan); corners[i] += projectionLine.P0; } return ret; /*Line pll = new Line(projectionLine.P0,projectionLine.P1); Coordinates offVec = projectionLine.UnitVector.Rotate90(); // find the leftmost boundary (looking into the parking spot) for (int i = 0; i < 20; i++) { Line tl = new Line(pll); tl.P0 = tl.P0 + offVec * i; tl.P1 = tl.P1 + offVec * i; Coordinates[] ipts; zonePerimeter.Intersect(tl, out ipts); obstacles[0].AvoidancePolygon if (ipts.Length < 2) break; // the test line is completely outside of the zone perimiter if (ipts.Length == 2) { } }*/ }
// returns true if the line segment is not completely inside of the perimeter or if it touches one of the obstacles private static bool ExpansionViolatesConstraints(LineSegment ls, Polygon pcPerimeter, List<Polygon> pcObstacles) { if (!pcPerimeter.IsInside(ls)) return true; for (int i = 0; i < pcObstacles.Count; i++) { if (pcObstacles[i].DoesIntersect(ls)) { return true; } } return false; }
private static void TestObstacleCollision(double curvature, double dist, IList<Polygon> obstacles, out double collisionDist, out double clearanceDist) { if (Math.Abs(curvature) < 1e-10) { // process as a straight line // determine end point of circle -- s = rθ, θ = sk (s = arc len, r = radius, θ = angle, k = curvature (1/r) // this will always be very very near straight, so just process as straight ahead LineSegment rearAxleSegment = new LineSegment(new Coordinates(0, 0), new Coordinates(dist, 0)); LineSegment frontAxleSegment = new LineSegment(new Coordinates(TahoeParams.FL, 0), new Coordinates(dist+TahoeParams.FL,0)); collisionDist = Math.Min(TestObstacleCollisionStraight(rearAxleSegment, obstacles), TestObstacleCollisionStraight(frontAxleSegment, obstacles)); clearanceDist = Math.Min(GetObstacleClearanceLine(rearAxleSegment, obstacles), GetObstacleClearanceLine(frontAxleSegment, obstacles)); } else { // build out the circle formed by the rear and front axle bool leftTurn = curvature > 0; double radius = Math.Abs(1/curvature); double frontRadius = Math.Sqrt(TahoeParams.FL*TahoeParams.FL + radius*radius); CircleSegment rearSegment, frontSegment; if (leftTurn) { Coordinates center = new Coordinates(0, radius); rearSegment = new CircleSegment(radius, center, Coordinates.Zero, dist, true); frontSegment = new CircleSegment(frontRadius, center, new Coordinates(TahoeParams.FL, 0), dist, true); } else { Coordinates center = new Coordinates(0, -radius); rearSegment = new CircleSegment(radius, center, Coordinates.Zero, dist, false); frontSegment = new CircleSegment(frontRadius, center, new Coordinates(TahoeParams.FL, 0), dist, false); } collisionDist = Math.Min(TestObstacleCollisionCircle(rearSegment, obstacles), TestObstacleCollisionCircle(frontSegment, obstacles)); clearanceDist = Math.Min(GetObstacleClearanceCircle(rearSegment, obstacles), GetObstacleClearanceCircle(frontSegment, obstacles)); } }
/// <summary> /// Get lanes that overlap with hte interconnect /// </summary> /// <param name="lanes"></param> /// <param name="ai"></param> /// <returns></returns> private List<IntersectionInvolved> laneOverlaps(List<ArbiterLane> lanes, ArbiterInterconnect ai, IEnumerable<ITraversableWaypoint> exits) { List<IntersectionInvolved> overlaps = new List<IntersectionInvolved>(); LineSegment aiSegment = new LineSegment(ai.InitialGeneric.Position, ai.FinalGeneric.Position); if (ai.FinalGeneric is ArbiterWaypoint) { ArbiterWaypoint fin = (ArbiterWaypoint)ai.FinalGeneric; if (fin.PreviousPartition != null && !this.FoundStop(fin.PreviousPartition.Initial, exits, fin.Lane)) { ArbiterWaypoint foundExit = null; foreach (ITraversableWaypoint itw in exits) { if (itw is ArbiterWaypoint && ((ArbiterWaypoint)itw).Lane.Equals(fin.Lane) && (foundExit == null || itw.Position.DistanceTo(fin.Position) < foundExit.Position.DistanceTo(fin.Position))) foundExit = (ArbiterWaypoint)itw; } if (foundExit != null) overlaps.Add(new IntersectionInvolved(foundExit, fin.Lane, ArbiterTurnDirection.Straight)); else overlaps.Add(new IntersectionInvolved(fin.Lane)); } } foreach (ArbiterLane al in lanes) { if (!(ai.InitialGeneric is ArbiterWaypoint) || !((ArbiterWaypoint)ai.InitialGeneric).Lane.Equals(al)) { foreach (LineSegment ls in al.LanePath().GetSegmentEnumerator()) { Coordinates interCoord; bool intersect = ls.Intersect(aiSegment, out interCoord); /*if (intersect) { Console.WriteLine(""); }*/ if (intersect && ai.IsInside(interCoord) && !overlaps.Contains(new IntersectionInvolved(al)) && ai.InterconnectPath.GetClosestPoint(interCoord).Location.DistanceTo(interCoord) < 0.00001 && al.IsInside(interCoord)) { // get closest partition ArbiterLanePartition alp = al.GetClosestPartition(interCoord); if (!this.FoundStop(alp.Initial, exits, al)) { ArbiterWaypoint foundExit = null; foreach (ITraversableWaypoint itw in exits) { if (itw is ArbiterWaypoint && ((ArbiterWaypoint)itw).Lane.Equals(alp.Lane) && (foundExit == null || itw.Position.DistanceTo(interCoord) < foundExit.Position.DistanceTo(interCoord))) foundExit = (ArbiterWaypoint)itw; } if (foundExit != null) overlaps.Add(new IntersectionInvolved(foundExit, alp.Lane, ArbiterTurnDirection.Straight)); else overlaps.Add(new IntersectionInvolved(al)); } } } } } return overlaps; }
private static double TestObstacleCollisionStraight(LineSegment segment, IList<Polygon> obstacles) { double minDist = double.MaxValue; foreach (Polygon obs in obstacles) { Coordinates[] pts; double[] K; if (obs.Intersect(segment, out pts, out K)) { // this is a potentially closest intersection for (int i = 0; i < K.Length; i++) { double dist = K[i]*segment.Length; if (dist < minDist) { minDist = dist; } } } } return minDist; }
public bool Intersect(LineSegment line, out Coordinates[] pts) { double[] K; return(Intersect(line, out pts, out K)); }