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