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