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);
}
private static void RoadMapTest()
{
var map = new MockTrapezoidalMap();
var forbidden = new HashSet<ITrapezoid>();
foreach (var trapezoid in map.Trapezoids)
{
var lefts = trapezoid.LeftEdges;
var rights = trapezoid.RightEdges;
if (lefts != null && lefts.Count() == 0 || rights != null && rights.Count() == 0)
forbidden.Add(trapezoid);
}
var roadMap = new RoadMap(map, forbidden);
var start = new Vector2(0.5, 0.5);
var goal = new Vector2(3.5, 3.5);
var path = roadMap.CalculatePath(start, goal);
string result = "";
foreach (var point in path)
result += point.ToString() + " ";
string correct = "(0.5, 0.5) (0.5, 2) (1, 0.5) (2, 0.5) (3, 0.5) (3.5, 2) (3.5, 3.5) ";
Debug.Assert(result == correct);
}
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);
}
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);
}