private RealCoordinate calculateRelativeCanvasPosition(RealCoordinate location)
{
double relativeX = location.x + 160;
double relativeY = location.y;
return(new RealCoordinate(relativeX, relativeY));
}
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 void drawPathSegment(RealCoordinate c1, RealCoordinate c2)
{
RealCoordinate relativePredecessorLocation = calculateRelativeCanvasPosition(c1);
RealCoordinate relativeSuccessorLocation = calculateRelativeCanvasPosition(c2);
graphicsObj.DrawLine(pathPen, (float)relativePredecessorLocation.x, (float)relativePredecessorLocation.y,
(float)relativeSuccessorLocation.x, (float)relativeSuccessorLocation.y);
}
public static double getYDistanceBetweenCoordinates(RealCoordinate c1, RealCoordinate c2)
{
return(Math.Abs(c1.y - c2.y));
}
public static double getDistanceBetweenCoordinates(RealCoordinate c1, RealCoordinate c2)
{
return(Math.Sqrt(Math.Pow(c1.x - c2.x, 2) + Math.Pow(c1.y - c2.y, 2)));
}
private double getBottomRightDistance(Coordinate start, Coordinate end)
{
List <RealCoordinate> intersects = new List <RealCoordinate>();
int sx = end.x - start.x;
int sy = end.y - start.y;
// 1. Check if path segment intersects with the left border of the sector
// Left sector line border equation: x = 2/3 * mapWidth
// Border points: [2/3*mapWidth,2/3*mapHeight],[2/3*mapWidth,mapHeight]
double x0 = 2.0 * mapWidth / 3.0;
double y0 = 2.0 * mapHeight / 3.0;
double vy = mapHeight / 3.0;
double t = (double)(x0 - start.x) / (double)sx;
double s = (start.y + sy * t - y0) / vy;
if (t <= 1 && t >= 0 && s <= 1 && s >= 0)
{
double intersectX = start.x + sx * t;
double intersectY = start.y + sy * t;
RealCoordinate intersect = new RealCoordinate(intersectX, intersectY);
intersects.Add(intersect);
}
// 2. Check if path segment intersects with the top border of the sector
// Top sector line border equation: y = 2/3 * mapHeight
// Border points: [2/3*mapWidth,2/3*mapHeight],[mapWidth,2/3*mapHeight]
x0 = 2.0 / 3.0 * mapWidth;
y0 = 2.0 / 3.0 * mapHeight;
double vx = 1.0 / 3.0 * mapWidth;
t = (double)(y0 - start.y) / (double)sy;
s = (start.x + sx * t - x0) / vx;
if (t <= 1 && t >= 0 && s <= 1 && s >= 0)
{
double intersectX = start.x + sx * t;
double intersectY = start.y + sy * t;
RealCoordinate intersect = new RealCoordinate(intersectX, intersectY);
intersects.Add(intersect);
}
// If 2 intersects were found, distance between them is measured:
if (intersects.Count == 2)
{
return(RealCoordinate.getDistanceBetweenCoordinates(intersects[0], intersects[1]));
}
// If 1 intersecting point, there are 2 possibilities:
// a) starting point is inside the sector
// b) end point is inside the sector
if (intersects.Count == 1)
{
if (start.x >= 2.0 * mapWidth / 3.0 && start.x <= mapWidth)
{
if (start.y >= 2.0 * mapHeight / 3.0 && start.y <= mapHeight)
{
return(RealCoordinate.getDistanceBetweenCoordinates(intersects[0], new RealCoordinate(start.x, start.y)));
}
}
if (end.x >= 2.0 * mapWidth / 3.0 && end.x <= mapWidth)
{
if (end.y >= 2.0 * mapHeight / 3.0 && end.y <= mapHeight)
{
return(RealCoordinate.getDistanceBetweenCoordinates(intersects[0], new RealCoordinate(end.x, end.y)));
}
}
}
// If no intersecting points were found, segment can still be fully inside the sector without
// intersecting its borders
if (intersects.Count == 0)
{
if (start.x >= 2.0 * mapWidth / 3.0 && start.x <= mapWidth && start.y >= 2.0 * mapHeight / 3.0 && start.y <= mapHeight)
{
if (end.x >= 2.0 * mapWidth / 3.0 && end.x <= mapWidth && end.y >= 2.0 * mapHeight / 3.0 && end.y <= mapHeight)
{
return(RealCoordinate.getDistanceBetweenCoordinates(new RealCoordinate(start.x, start.y), new RealCoordinate(end.x, end.y)));
}
}
}
return(0);
}
private void prettyPathDrawing(Coordinate c)
{
Coordinate predecessor = path[path.IndexOf(c) - 1];
Coordinate relativePredecessorLocation = calculateRelativeCanvasPosition(predecessor);
Coordinate relativeSuccessorLocation = calculateRelativeCanvasPosition(c);
int xDistance = Coordinate.getXDistanceBetweenCoordinates(c, predecessor);
int yDistance = Coordinate.getYDistanceBetweenCoordinates(c, predecessor);
if (xDistance != 0 && yDistance == 0)
{
int xChange = predecessor.x < c.x ? 1 : -1;
Coordinate[] segments = new Coordinate[xDistance + 1];
segments[0] = predecessor;
segments[xDistance - 1] = c;
for (int i = 1; i < xDistance; i++)
{
segments[i] = new Coordinate(new int[] { segments[i - 1].x + xChange, segments[i - 1].y });
drawPathSegment(segments[i - 1], segments[i]);
Thread.Sleep((int)Math.Ceiling((double)(1000 / xDistance)));
}
}
if (yDistance != 0 && xDistance == 0)
{
int yChange = predecessor.y < c.y ? 1 : -1;
Coordinate[] segments = new Coordinate[yDistance + 1];
segments[0] = predecessor;
segments[yDistance - 1] = c;
for (int i = 1; i < yDistance; i++)
{
segments[i] = new Coordinate(new int[] { segments[i - 1].x, segments[i - 1].y + yChange });
drawPathSegment(segments[i - 1], segments[i]);
Thread.Sleep((int)Math.Ceiling((double)(1000 / yDistance)));
}
}
if (yDistance != 0 && xDistance != 0)
{
double k = (double)(c.y - predecessor.y) / (double)(c.x - predecessor.x);
double q = predecessor.y - k * predecessor.x;
if (yDistance > xDistance)
{
int yChange = predecessor.y < c.y ? 1 : -1;
RealCoordinate[] segments = new RealCoordinate[yDistance + 1];
segments[0] = new RealCoordinate(predecessor.x, predecessor.y);
for (int i = 1; i < yDistance; i++)
{
float currentY = (float)segments[i - 1].y + yChange;
float currentX = (float)((currentY - q) / k);
segments[i] = new RealCoordinate(currentX, currentY);
drawPathSegment(segments[i - 1], segments[i]);
Thread.Sleep((int)Math.Ceiling((double)(1000 / yDistance)));
}
}
if (xDistance > yDistance)
{
int xChange = predecessor.x < c.x ? 1 : -1;
RealCoordinate[] segments = new RealCoordinate[xDistance + 1];
segments[0] = new RealCoordinate(predecessor.x, predecessor.y);
for (int i = 1; i < xDistance; i++)
{
float currentX = (float)segments[i - 1].x + xChange;
float currentY = (float)(k * currentX + q);
segments[i] = new RealCoordinate(currentX, currentY);
drawPathSegment(segments[i - 1], segments[i]);
Thread.Sleep((int)Math.Ceiling((double)(1000 / xDistance)));
}
}
}
}