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