public MotionPlanner(IEnumerable<ConvexPolygon> obstacles, ConvexPolygon robot)
{
var reflection = robot.GetPointReflection(Vector2.Zero);
var cObstacles = from obstacle in obstacles
select ConvexPolygon.MinkowskiSum(reflection, obstacle);
var union = Union(cObstacles.ToArray());
var trapezoidalMap = GenerateTrapezoidalMap(union);
this.boundingBox = trapezoidalMap.BoundingBox;
var topRightOfObstacles = from obstacle in union
from point in obstacle.GetPointsAbove()
select point;
var forbiddenSpace = new HashSet<ITrapezoid>();
foreach (var trapezoid in trapezoidalMap.Trapezoids)
{
var top = trapezoid.TopSegment;
if (top == null)
continue;
var topRight = top.RightVertex.Position;
var trapezoidIsObstacle = topRightOfObstacles.Contains(topRight);
if (trapezoidIsObstacle)
forbiddenSpace.Add(trapezoid);
}
this.roadMap = new RoadMap(trapezoidalMap, forbiddenSpace);
}
public MotionPlanner(IEnumerable<SimplePolygon> obstacles)
{
var trapezoidalMap = GenerateTrapezoidalMap(obstacles);
this.boundingBox = trapezoidalMap.BoundingBox;
var topRightOfObstacles = from obstacle in obstacles
from point in obstacle.GetPointsAbove()
select point;
var forbiddenSpace = new HashSet<ITrapezoid>();
foreach (var trapezoid in trapezoidalMap.Trapezoids)
{
var top = trapezoid.TopSegment;
if (top == null)
continue;
var topRight = top.RightVertex.Position;
var trapezoidIsObstacle = topRightOfObstacles.Contains(topRight);
if (trapezoidIsObstacle)
forbiddenSpace.Add(trapezoid);
}
this.roadMap = new RoadMap(trapezoidalMap, forbiddenSpace);
}
/// <summary>
/// Construct rectangle boundary of a list of half - edges
/// </summary>
/// <param name="edges"></param>
/// <returns></returns>
Trapezoid RectangleBoundary(IEnumerable<Common.Segment> segments)
{
double left = 0;
double right = 0;
double top = 0;
double bottom = 0;
foreach(Common.Segment segment in segments)
{
left = left < segment.Vertex1.X ? left : segment.Vertex1.X;
left = left < segment.Vertex2.X ? left : segment.Vertex2.X;
right = right > segment.Vertex1.X ? right : segment.Vertex1.X;
right = right > segment.Vertex2.X ? right : segment.Vertex2.X;
top = top > segment.Vertex1.Y ? top : segment.Vertex1.Y;
top = top > segment.Vertex2.Y ? top : segment.Vertex2.Y;
bottom = bottom < segment.Vertex1.Y ? bottom : segment.Vertex1.Y;
bottom = bottom < segment.Vertex2.Y ? bottom : segment.Vertex2.Y;
}
--left;
++right;
++top;
--bottom;
this.boundingBox = new Rectangle(left, right, bottom, top);
Trapezoid R = new Trapezoid();
R.Leftp = new Vertex(left, top);
R.Rightp = new Vertex(right, bottom);
R.Top = new Segment(R.Leftp, new Vertex(right, top));
R.Bottom = new Segment(new Vertex(left, bottom), R.Rightp);
return R;
}