private List <RectangularObstacle> segmentSplineByX(SplineObstacle obstacle, Polynom poly, int obstaclePassDistance)
{
RealCoordinate current = new RealCoordinate(obstacle.points[0].x, obstacle.points[0].y - obstaclePassDistance);
RealCoordinate end = new RealCoordinate(obstacle.points[obstacle.points.Count - 1].x,
obstacle.points[obstacle.points.Count - 1].y);
List <RectangularObstacle> obstacles = new List <RectangularObstacle>();
RectangularObstacle firstObstacle = new RectangularObstacle(2 * obstaclePassDistance, obstaclePassDistance,
new RealCoordinate(current.x - obstaclePassDistance, current.y - obstaclePassDistance));
obstacles.Add(firstObstacle);
do
{
RectangularObstacle rect = new RectangularObstacle(2 * obstaclePassDistance, 2,
new RealCoordinate(current.x, current.y));
obstacles.Add(rect);
if (current.x + 2 <= end.x)
{
current.x += 2;
current.y = poly.getPolynomValue(current.x) - obstaclePassDistance;
}
else
{
current.x = end.x;
current.y = end.y;
}
} while (RealCoordinate.getDistanceBetweenCoordinates(current, end) != 0);
RectangularObstacle lastObstacle = new RectangularObstacle(2 * obstaclePassDistance, obstaclePassDistance,
new RealCoordinate(current.x, current.y));
obstacles.Add(lastObstacle);
return(obstacles);
}
private BoolExpr handlePolygonalObstacle(PolygonalObstacle obstacle, Context ctx, IntExpr[] sourcesX, IntExpr[] sourcesY,
IntExpr[] destinationsX, IntExpr[] destinationsY)
{
List <LineSegment> segments = obstacle.getLines();
List <BoolExpr> avoidingLines = new List <BoolExpr>();
foreach (LineSegment segment in segments)
{
if (segment.isHorizontal)
{
int leftX = (segment.start.x < segment.end.x ? segment.start.x : segment.end.x) - obstaclePassDistance;
int leftY = segment.start.y - obstaclePassDistance;
RectangularObstacle rectangle = new RectangularObstacle(2 * obstaclePassDistance,
Coordinate.getXDistanceBetweenCoordinates(segment.start, segment.end) + 2 * obstaclePassDistance,
new Coordinate(leftX, leftY));
avoidingLines.Add(handleRectangularObstacle(rectangle, ctx, sourcesX, sourcesY, destinationsX, destinationsY));
}
else if (segment.isVertical)
{
int leftY = (segment.start.y < segment.end.y ? segment.start.y : segment.end.y) - obstaclePassDistance;
int leftX = segment.start.x - obstaclePassDistance;
RectangularObstacle rectangle = new RectangularObstacle(Coordinate.getYDistanceBetweenCoordinates(segment.start, segment.end)
+ 2 * obstaclePassDistance, 2 * obstaclePassDistance, new Coordinate(leftX, leftY));
avoidingLines.Add(handleRectangularObstacle(rectangle, ctx, sourcesX, sourcesY, destinationsX, destinationsY));
}
else
{
avoidingLines.Add(handleCurvedLine(segment, ctx, sourcesX, sourcesY, destinationsX, destinationsY));
}
}
return(ctx.MkAnd(ctx.MkAnd(avoidingLines.ToArray())));
}
private BoolExpr handleRealRectangularObstacle(RectangularObstacle obstacle, Context ctx, IntExpr[] sourcesX,
IntExpr[] sourcesY, IntExpr[] destinationsX, IntExpr[] destinationsY)
{
RealExpr obstacleLeftX = ctx.MkReal(obstacle.realLocation.x.ToString(new CultureInfo("en-US")));
RealExpr obstacleRightX = ctx.MkReal((obstacle.realLocation.x + obstacle.width).ToString(new CultureInfo("en-US")));
RealExpr obstacleLeftY = ctx.MkReal(obstacle.realLocation.y.ToString(new CultureInfo("en-US")));
RealExpr obstacleRightY = ctx.MkReal((obstacle.realLocation.y + obstacle.length).ToString(new CultureInfo("en-US")));
RealExpr passDistance = ctx.MkReal(obstaclePassDistance);
BoolExpr[] avoidingObstaclesX = new BoolExpr[pathSegments];
BoolExpr[] avoidingObstaclesY = new BoolExpr[pathSegments];
BoolExpr[] avoidingObstaclesCombined = new BoolExpr[pathSegments];
for (int i = 0; i < pathSegments; i++)
{
ArithExpr xl_minus_xs = ctx.MkSub(obstacleLeftX, sourcesX[i]);
ArithExpr xl_minus_xd = ctx.MkSub(obstacleLeftX, destinationsX[i]);
ArithExpr xs_minus_xh = ctx.MkSub(sourcesX[i], obstacleRightX);
ArithExpr xd_minus_xh = ctx.MkSub(destinationsX[i], obstacleRightX);
avoidingObstaclesX[i] = ctx.MkOr(ctx.MkAnd(ctx.MkGe(xl_minus_xs, passDistance), ctx.MkGe(xl_minus_xd, passDistance)),
ctx.MkAnd(ctx.MkGe(xs_minus_xh, passDistance), ctx.MkGe(xd_minus_xh, passDistance)));
ArithExpr yl_minus_ys = ctx.MkSub(obstacleLeftY, sourcesY[i]);
ArithExpr yl_minus_yd = ctx.MkSub(obstacleLeftY, destinationsY[i]);
ArithExpr ys_minus_yh = ctx.MkSub(sourcesY[i], obstacleRightY);
ArithExpr yd_minus_yh = ctx.MkSub(destinationsY[i], obstacleRightY);
avoidingObstaclesY[i] = ctx.MkOr(ctx.MkAnd(ctx.MkGe(yl_minus_ys, passDistance), ctx.MkGe(yl_minus_yd, passDistance)),
ctx.MkAnd(ctx.MkGe(ys_minus_yh, passDistance), ctx.MkGe(yd_minus_yh, passDistance)));
avoidingObstaclesCombined[i] = ctx.MkOr(avoidingObstaclesX[i], avoidingObstaclesY[i]);
}
return(ctx.MkAnd(avoidingObstaclesCombined));
}
private void drawRectangularObstacle(RectangularObstacle o)
{
Coordinate relLoc = calculateRelativeCanvasPosition(o.location);
Rectangle rectangle = new Rectangle(relLoc.x, relLoc.y, o.width, o.length);
graphicsObj.DrawRectangle(obstaclePen, rectangle);
graphicsObj.FillRectangle(brush, rectangle);
}
private BoolExpr handlePolygonEndPoints(PolygonalObstacle obstacle, Context ctx, IntExpr[] sourcesX, IntExpr[] sourcesY,
IntExpr[] destinationsX, IntExpr[] destinationsY)
{
List <BoolExpr> avoidingEndPoints = new List <BoolExpr>();
List <LineSegment> segments = obstacle.getLines();
foreach (LineSegment segment in segments)
{
RectangularObstacle first = new RectangularObstacle(obstaclePassDistance * 2, obstaclePassDistance * 2,
new Coordinate(segment.start.x - obstaclePassDistance, segment.start.y - obstaclePassDistance));
RectangularObstacle second = new RectangularObstacle(obstaclePassDistance * 2, obstaclePassDistance * 2,
new Coordinate(segment.end.x - obstaclePassDistance, segment.end.y - obstaclePassDistance));
avoidingEndPoints.Add(handleRectangularObstacle(first, ctx, destinationsX, destinationsY, sourcesX, sourcesY));
avoidingEndPoints.Add(handleRectangularObstacle(second, ctx, destinationsX, destinationsY, sourcesX, sourcesY));
}
return(ctx.MkAnd(avoidingEndPoints.ToArray()));
}
private BoolExpr handleEllipticalObstacle(EllipticalObstacle obstacle, Context ctx, IntExpr[] destinationsX,
IntExpr[] destinationsY, IntExpr[] sourcesX, IntExpr[] sourcesY)
{
if (obstacle.length != obstacle.width)
{
RectangularObstacle rectangle = new RectangularObstacle(obstacle.length, obstacle.width,
new Coordinate(obstacle.location.x - obstacle.width / 2, obstacle.location.y - obstacle.length / 2));
return(handleRectangularObstacle(rectangle, ctx, sourcesX, sourcesY, destinationsX, destinationsY));
}
else
{
BoolExpr[] avoidingCircle = new BoolExpr[pathSegments];
NumberFormatInfo dot = new NumberFormatInfo();
dot.NumberDecimalSeparator = ".";
for (int i = 0; i < pathSegments; i++)
{
double radius = obstacle.length / 2.0 + obstaclePassDistance;
int translateX = obstacle.location.x;
int translateY = obstacle.location.y;
IntExpr dist = ctx.MkInt((int)radius);
ArithExpr adjustedSourceX = ctx.MkSub(sourcesX[i], ctx.MkInt(translateX));
ArithExpr adjustedSourceY = ctx.MkSub(sourcesY[i], ctx.MkInt(translateY));
ArithExpr adjustedDestX = ctx.MkSub(destinationsX[i], ctx.MkInt(translateX));
ArithExpr adjustedDestY = ctx.MkSub(destinationsY[i], ctx.MkInt(translateY));
ArithExpr dx = ctx.MkSub(adjustedDestX, adjustedSourceX);
ArithExpr dy = ctx.MkSub(adjustedDestY, adjustedSourceY);
ArithExpr dr_2 = ctx.MkAdd(ctx.MkMul(dx, dx), ctx.MkMul(dy, dy));
ArithExpr D = ctx.MkSub(ctx.MkMul(adjustedSourceX, adjustedDestY), ctx.MkMul(adjustedDestX, adjustedSourceY));
ArithExpr incidence = ctx.MkSub(ctx.MkMul(ctx.MkMul(dist, dist), dr_2), ctx.MkMul(D, D));
BoolExpr final = ctx.MkLe(incidence, ctx.MkInt(0));
avoidingCircle[i] = final;
}
return(ctx.MkAnd(avoidingCircle));
}
}
