private List <Polygon> GetObstacles(CarTimestamp curTimestamp)
{
int total = 0;
ObstacleCollection obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, Services.BehaviorManager.SAUDILevel);
total += obstacles.obstacles.Count;
if (stayOutPolygons != null)
{
total += stayOutPolygons.Count;
}
List <Polygon> polys = new List <Polygon>(total);
foreach (Obstacle obs in obstacles.obstacles)
{
polys.Add(obs.AvoidancePolygon);
}
// transform the stay-out polygons
if (stayOutPolygons != null)
{
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
foreach (Polygon poly in stayOutPolygons)
{
polys.Add(poly.Transform(absTransform));
}
}
return(polys);
}
private void GetObstaclesPerLayer(int currentLayer, ObstacleCollection obstacles)
{
var rawObstacles = GetBlockObstacles(currentLayer);
var nodes = new LineNodeDictionary();
nodes.CreateLineNodes(rawObstacles);
while (nodes.Count > 0)
{
var currentFigure = new RawFigure(_map, currentLayer, nodes);
nodes.Purge(currentFigure.Lines);
currentFigure.Optimize();
currentFigure.Tokenize(obstacles);
}
nodes.Clear();
nodes.CreateLineNodes(rawObstacles, true);
while (nodes.Count > 0)
{
var currentFigure = new SlopeFigure(_map, currentLayer, nodes);
nodes.Purge(currentFigure.Lines);
currentFigure.Optimize();
currentFigure.Tokenize(obstacles);
}
}
protected List <Obstacle> GetObstacles(CarTimestamp curTimestamp)
{
ObstacleCollection col = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, SAUDILevel.None);
AbsoluteTransformer transform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp).Invert();
List <Obstacle> ret = new List <Obstacle>(col.obstacles.Count);
foreach (Obstacle obs in col.obstacles)
{
Obstacle newObs = obs.ShallowClone();
if (newObs.cspacePolygon != null)
{
newObs.cspacePolygon = newObs.cspacePolygon.Transform(transform);
}
if (newObs.extrudedPolygon != null)
{
newObs.extrudedPolygon = newObs.extrudedPolygon.Transform(transform);
}
if (newObs.obstaclePolygon != null)
{
newObs.obstaclePolygon = newObs.obstaclePolygon.Transform(transform);
}
if (newObs.predictedPolygon != null)
{
newObs.predictedPolygon = newObs.predictedPolygon.Transform(transform);
}
ret.Add(newObs);
}
return(ret);
}
public ObstacleCollection GetObstacles()
{
var obstacles = new ObstacleCollection();
for (var i = 7; i >= 0; i--)
GetObstaclesPerLayer(i, obstacles);
obstacles.RemoveUnnecessary();
return obstacles;
}
public bool CheckIfFilled(Map.Map map, int layer, ObstacleCollection obstacles)
{
Simplify();
var convexPolygons = BayazitDecomposer.ConvexPartition(this);
var blockPointsDictionary = new Dictionary<Block, List<Vector2>>();
var blocks = GetAssociatedBlocks(convexPolygons, map, layer, blockPointsDictionary);
var layerObstacles = CreateLayerObstacles(layer, obstacles); //ToDo: Don't call this method every time, the result does not change...
return CheckLid(blocks, map, layer, layerObstacles, blockPointsDictionary);
}
public override void Tokenize(ObstacleCollection obstacles)
{
/*
var polygons = SplitPolygon(Lines, obstacles);
var blocks = Polygon.GetAssociatedBlocks(polygons, Map, Layer).ToList();
var polygonEdges = new List<LineSegment>();
foreach (var polygon in polygons)
{
for (int i = 0, j = polygon.Count - 1; i < polygon.Count; j = i++)
polygonEdges.Add(GetLine(polygon[i], polygon[j], false, Lines));
}
var intersectionLines = polygonEdges.GroupBy(x => x).Where(group => group.Count() > 1).Select(group => group.Key).ToList(); //finds duplicated items
var blockLines = new Dictionary<Block, List<LineSegment>>(); //create a dictionary of block which intersection line they belong to. Because if the intersection line does not match,
//all intersecting lines of that block must be set invalid. Otherwise they engine would not not create a separate polygon.
var mismatchBlocks = new List<Block>();
foreach (var intersectionLine in intersectionLines)
{
Block blockX;
Block blockY;
Orientation orientation;
GetNeighborBlocks(intersectionLine, blocks, out blockX, out blockY, out orientation);
if (BlocksMatch(blockX, blockY, orientation))
{
Lines.Remove(intersectionLine);
}
else
{
if (blockX != null & !mismatchBlocks.Contains(blockX))
mismatchBlocks.Add(blockX);
if (blockY != null & !mismatchBlocks.Contains(blockY))
mismatchBlocks.Add(blockY);
}
AddBlockLines(blockLines, blockX, intersectionLine);
AddBlockLines(blockLines, blockY, intersectionLine);
}
foreach (var mismatchBlock in mismatchBlocks)
{
//Restore all lines of the mismatched blocks
List<LineSegment> blockLineSegments;
if (!blockLines.TryGetValue(mismatchBlock, out blockLineSegments))
continue;
foreach (var blockSegment in blockLineSegments.Where(blockSegment => !Lines.Contains(blockSegment)))
Lines.Add(blockSegment);
}
polygons.Clear();
polygons = SplitPolygon(Lines, obstacles);
foreach (var polygon in polygons)
{
var simplePolygon = SimplifyTools.CollinearSimplify(polygon);
AddSlopeObstacle(simplePolygon, VerticesEx.IsRectangle(simplePolygon), obstacles, Layer);
}
*/
}
private static Dictionary<int, List<IObstacle>> CreateLayerObstacles(int layer, ObstacleCollection obstacles)
{
var dict = new Dictionary<int, List<IObstacle>>();
for (var z = layer + 1; z < 8; z++)
{
var layerObstacles = obstacles.Where(obstacle => obstacle.Z == z && (obstacle is PolygonObstacle || obstacle is RectangleObstacle)).ToList();
dict.Add(z, layerObstacles);
}
return dict;
}
/// <summary>
/// Creates IObstacle objects of this figure.
/// </summary>
/// <returns></returns>
public virtual void Tokenize(ObstacleCollection obstacles)
{
if (Lines.Count == 0)
return;
GetForlorn(obstacles);
if (Lines.Count == 0)
return;
if (SwitchPoints.Count > 0)
{
var polygons = SplitPolygon(Lines, obstacles);
MergePolygons(polygons, obstacles);
}
else
{
var polygon = CreatePolygon(Lines);
if (polygon.CheckIfFilled(Map, Layer, obstacles))
AddPolygonObstacle(polygon, VerticesEx.IsRectangle(polygon), obstacles, Layer);
else
AddLineObstacles(polygon, obstacles, Layer);
}
}
private static double FindBestCurvature(double prevCurvature, Coordinates relativeGoalPoint, List <Polygon> perimeterPolygons)
{
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer();
// get a list of obstacles
ObstacleCollection obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, UrbanChallenge.Behaviors.SAUDILevel.None);
List <Polygon> obstaclePolygons = new List <Polygon>();
foreach (Obstacle obs in obstacles.obstacles)
{
obstaclePolygons.Add(obs.cspacePolygon);
}
obstaclePolygons.AddRange(perimeterPolygons);
List <ArcResults> arcs = new List <ArcResults>();
double maxUtility = double.MinValue;
ArcResults selectedArc = null;
// recalculate weights
double totalWeights = obstacle_weight + hysteresis_weight + straight_weight + goal_weight;
double obstacleWeight = obstacle_weight / totalWeights;
double hysteresisWeight = hysteresis_weight / totalWeights;
double straightWeight = straight_weight / totalWeights;
double goalWeight = goal_weight / totalWeights;
int start = num_arcs / 2;
double curvatureStep = max_curvature / start;
for (int i = -start; i <= start; i++)
{
double curvature = i * curvatureStep;
double collisionDist, clearanceDist, collisionUtility;
bool vetoed;
EvaluateObstacleUtility(curvature, 20, obstaclePolygons, out collisionDist, out clearanceDist, out collisionUtility, out vetoed);
double hystersisUtility = EvaluateHysteresisUtility(curvature, prevCurvature);
double straightUtility = EvaluateStraightUtility(curvature);
double goalUtility = EvaluateGoalUtility(curvature, relativeGoalPoint);
double totalUtility = collisionUtility * obstacleWeight + hystersisUtility * hysteresisWeight + straightUtility * straightWeight +
goalUtility * goalWeight;
ArcResults result = new ArcResults();
result.curvature = curvature;
result.vetoed = vetoed;
result.totalUtility = totalUtility;
result.obstacleHitDistance = collisionDist;
result.obstacleClearanceDistance = clearanceDist;
result.obstacleUtility = collisionUtility;
result.hysteresisUtility = hystersisUtility;
result.straightUtility = straightUtility;
result.goalUtility = goalUtility;
arcs.Add(result);
if (!vetoed && totalUtility > maxUtility)
{
maxUtility = totalUtility;
selectedArc = result;
}
}
ArcVotingResults results = new ArcVotingResults();
results.arcResults = arcs;
results.selectedArc = selectedArc;
Services.Dataset.ItemAs <ArcVotingResults>("arc voting results").Add(results, LocalCarTimeProvider.LocalNow);
if (selectedArc == null)
{
return(double.NaN);
}
else
{
return(selectedArc.curvature);
}
}
protected static void AddSlopeObstacle(Vertices polygonVertices, bool isRectangle, ObstacleCollection obstacles, int layer)
{
if (isRectangle)
{
var rectangle = new RectangleObstacle(polygonVertices, layer) {IsSlope = true};
obstacles.Add(rectangle);
}
else
{
var polygonObstacle = new PolygonObstacle(polygonVertices, layer) {IsSlope = true};
obstacles.Add(polygonObstacle);
}
}
/// <summary>
/// Splites a multi part figure into single parts.
/// See Polygon Split.png.
/// </summary>
/// <param name="sourceSegments"></param>
/// <param name="obstacles"></param>
protected virtual ICollection<Polygon> SplitPolygon(List<LineSegment> sourceSegments, ObstacleCollection obstacles)
{
var verticesCombinations = new HashSet<Polygon>();
/*
foreach (var switchPoint in SwitchPoints)
{
foreach (var endPoint in switchPoint.Value.EndPoints)
{
var startPoint = switchPoint.Key;
var polygon = new Polygon();
var remainingLines = new List<LineSegment>(sourceSegments);
var currentItem = startPoint;
var currentDirection = Direction.None;
do
{
if (currentItem == startPoint && polygon.Count > 0)
{
if (!verticesCombinations.Contains(polygon))
verticesCombinations.Add(polygon);
break;
}
if (polygon.Contains(currentItem))
break;
polygon.Add(currentItem);
LineSegment preferedLine;
if (currentItem == startPoint)
preferedLine = GetLine(currentItem, endPoint, true);
else
preferedLine = ChooseNextLine(currentItem, remainingLines, currentDirection);
if (preferedLine == null)
break;
currentItem = preferedLine.End;
currentDirection = preferedLine.Direction;
polygon.Lines.Add(preferedLine);
remainingLines.Remove(preferedLine);
} while (true);
}
}
RemoveUnnecessaryPolygons(verticesCombinations);
var forlornLines = GetPolygonForlornLines(sourceSegments, verticesCombinations);
obstacles.AddRange(forlornLines.Select(forlornLine => new LineObstacle(forlornLine.Start, forlornLine.End, Layer)));
*/
return verticesCombinations;
}
protected virtual void MergePolygons(IEnumerable<Polygon> polygons, ObstacleCollection obstacles)
{
/*
var filledPolygons = new List<Vertices>();
var mergedPolygons = new List<Vertices>();
foreach (var polygon in polygons)
{
if (polygon.CheckIfFilled(Map, Layer, obstacles))
filledPolygons.Add(polygon);
else
AddLineObstacles(polygon, obstacles, Layer);
}
while (filledPolygons.Count > 0)
{
var polygon = filledPolygons.First();
var changed = true;
while (changed)
{
changed = false;
var index = 0;
while (index < filledPolygons.Count)
{
var polygon2 = filledPolygons[index];
var error = PolyClipError.None;
var combinedPolygons = polygon == polygon2 ? new List<Vertices> { polygon } : YuPengClipper.Union(polygon2, polygon, out error);
if (combinedPolygons.Count == 0)
{
//sometimes a polygon is a subset of the other polygon. Try to check that...
Vertices biggerPolygon;
if (VerticesEx.IsPolygonSubsetOf(polygon, polygon2, out biggerPolygon))
{
combinedPolygons = new List<Vertices> { biggerPolygon };
error = PolyClipError.None; //we could recover it...
}
}
//if (combinedPolygons.Count > 2)
// //the polygons intersect at several points, we ignore them as this would lead to holes...
//if (error != PolyClipError.None)
// //some error occurred, so we igore this union process...
if (combinedPolygons.Count == 1) //they intersect
{
filledPolygons.Remove(polygon2);
changed = true;
polygon = SimplifyTools.CollinearSimplify(combinedPolygons[0]);
}
else //combinedPolygons.Count > 1 --> they don't intersect (or may do at several points, but we ignore it )
index++;
}
}
mergedPolygons.Add(polygon);
}
foreach (var mergedPolygon in mergedPolygons)
AddPolygonObstacle(mergedPolygon, VerticesEx.IsRectangle(mergedPolygon), obstacles, Layer);
*/
}
//Forlorn stuff
/// <summary>
/// Removes Forlorn out of the figure and converts them to LineObstacles.
/// </summary>
/// <param name="obstacles"></param>
protected virtual void GetForlorn(ObstacleCollection obstacles)
{
var forlornNodes = new Queue<Vector2>();
foreach (var forlornNodeStart in ForlornStartNodes)
forlornNodes.Enqueue(forlornNodeStart);
while (forlornNodes.Count > 0)
{
var currentItem = forlornNodes.Dequeue();
List<LineSegment> forlornLines;
Vector2 lastItemEndPoint; //this is needed for Switch Points below
var forlornRoot = GetforlornRoot(currentItem, out forlornLines, out lastItemEndPoint);
foreach (var line in forlornLines)
{
obstacles.Add(new LineObstacle(line.Start, line.End, Layer));
Lines.Remove(line);
}
if (SwitchPoints.Count == 0)
continue;
SwitchPoint switchPoint;
if (!SwitchPoints.TryGetValue(forlornRoot, out switchPoint))
continue;
if (switchPoint.EndPoints.Count > 0)
switchPoint.EndPoints.Remove(lastItemEndPoint);
if (switchPoint.EndPoints.Count == 1)
{
forlornNodes.Enqueue(forlornRoot);
SwitchPoints.Remove(forlornRoot);
}
}
ForlornStartNodes.Clear();
UpdateSwitchPoints();
}
