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 static ConvexPolygon MinkowskiSum(ConvexPolygon polygon1, ConvexPolygon polygon2) { var resultPoints = new List<Vector2>(); HalfEdge current1 = polygon1.firstEdge, current2 = polygon2.firstEdge; #if DEBUG int numberOfVertices1 = polygon1.Edges.Count(), numberOfVertices2 = polygon2.Edges.Count(), resultVerticesBound = numberOfVertices1 + numberOfVertices2; #endif do { var vertex1 = current1.Origin; var vertex2 = current2.Origin; var newPoint = vertex1.Position + vertex2.Position; resultPoints.Add(newPoint); Debug.Assert(resultPoints.Count <= resultVerticesBound); int compareResult = Vector2.CompareByAngleWithXAxis(current1.Vector, current2.Vector); if (compareResult >= 0) current1 = polygon1.GetNextEdge(current1); if (compareResult <= 0) current2 = polygon2.GetNextEdge(current2); } while (current1 != polygon1.firstEdge || current2 != polygon2.firstEdge); return new ConvexPolygon(resultPoints.ToArray()); }
public static ConvexPolygon MinkowskiSum(ConvexPolygon polygon1, ConvexPolygon polygon2) { var resultPoints = new List <Vector2>(); HalfEdge current1 = polygon1.firstEdge, current2 = polygon2.firstEdge; #if DEBUG int numberOfVertices1 = polygon1.Edges.Count(), numberOfVertices2 = polygon2.Edges.Count(), resultVerticesBound = numberOfVertices1 + numberOfVertices2; #endif do { var vertex1 = current1.Origin; var vertex2 = current2.Origin; var newPoint = vertex1.Position + vertex2.Position; resultPoints.Add(newPoint); Debug.Assert(resultPoints.Count <= resultVerticesBound); int compareResult = Vector2.CompareByAngleWithXAxis(current1.Vector, current2.Vector); if (compareResult >= 0) { current1 = polygon1.GetNextEdge(current1); } if (compareResult <= 0) { current2 = polygon2.GetNextEdge(current2); } }while (current1 != polygon1.firstEdge || current2 != polygon2.firstEdge); return(new ConvexPolygon(resultPoints.ToArray())); }
private static void MinkowskiRectangleSumTest() { Vector2 v1 = new Vector2(0, 0), v2 = new Vector2(1, 0), v3 = new Vector2(1, 1), v4 = new Vector2(0, 1), w1 = new Vector2(1, 1), w2 = new Vector2(2, 1), w3 = new Vector2(2, 2), w4 = new Vector2(1, 2), u1 = new Vector2(1, 1), u2 = new Vector2(3, 1), u3 = new Vector2(3, 3), u4 = new Vector2(1, 3); var polygon1 = new ConvexPolygon(new[] { v1, v2, v3, v4 }); var polygon2 = new ConvexPolygon(new[] { w1, w2, w3, w4 }); var polygon3 = new ConvexPolygon(new[] { u1, u2, u3, u4 }); var sum = ConvexPolygon.MinkowskiSum(polygon1, polygon2); Debug.Assert(sum.ToString() == polygon3.ToString()); }
private static void MinkowskiTriangleSumTest() { Vector2 v1 = new Vector2(0, -1), v2 = new Vector2(1, 0), v3 = new Vector2(0, 1), w1 = new Vector2(1, -1), w2 = new Vector2(1, 1), w3 = new Vector2(0, 0), u1 = new Vector2(1, -2), u2 = new Vector2(2, -1), u3 = new Vector2(2, 1), u4 = new Vector2(1, 2), u5 = new Vector2(0, 1), u6 = new Vector2(0, -1); var polygon1 = new ConvexPolygon(new[] { v1, v2, v3 }); var polygon2 = new ConvexPolygon(new[] { w1, w2, w3 }); var polygon3 = new ConvexPolygon(new[] { u1, u2, u3, u4, u5, u6 }); var sum = ConvexPolygon.MinkowskiSum(polygon1, polygon2); Debug.Assert(sum.ToString() == polygon3.ToString()); Debug.Assert(polygon1.ContainsPoint(new Vector2(0.5, 0.25))); Debug.Assert(!polygon1.ContainsPoint(new Vector2(2, 4))); Debug.Assert(!polygon1.ContainsPoint(v1)); Debug.Assert(!polygon1.ContainsPoint((v1 + v2)/2)); }
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); }
private ConvexPolygon[] Union(ConvexPolygon[] polygons) { if (polygons.Length < 1) return polygons; var output = new List<ConvexPolygon>(); var queue = new Queue<ConvexPolygon>(polygons); while (queue.Count != 0) { var current = queue.Dequeue(); bool isMerged = false; foreach (var polygon in output) { if (!current.Overlaps(polygon)) continue; current = (ConvexPolygon)current.UnionWith(polygon); isMerged = true; output.Remove(polygon); break; } if (!isMerged) output.Add(current); else queue.Enqueue(current); } return output.ToArray(); // NOT TESTED }
private static void UnionTest() { Vector2 v1 = new Vector2(0, -1), v2 = new Vector2(1, 0), v3 = new Vector2(0, 1), w1 = new Vector2(1, -1), w2 = new Vector2(1, 1), w3 = new Vector2(0, 0), u1 = new Vector2(1, -2), u2 = new Vector2(2, -1), u3 = new Vector2(2, 1), u4 = new Vector2(1, 2), u5 = new Vector2(0, 1), u6 = new Vector2(0, -1); var polygon1 = new ConvexPolygon(new[] { v1, v2, v3 }); var polygon2 = new ConvexPolygon(new[] { w1, w2, w3 }); var polygon3 = new ConvexPolygon(new[] { u1, u2, u3, u4, u5, u6 }); var sum = polygon1.UnionWith(polygon2); Console.WriteLine(sum.ToString()); }