private bool FindTrasingData(Segment l, ref TracingData data)
{
var intersections = new Dictionary <Vector2, int>();
var inDistances = new Dictionary <float, Vector2>();
var outDistances = new Dictionary <float, Vector2>();
for (var i = 0; i < Segments.Length; i++)
{
var segment = Segments[i];
var intersectPoint = new Vector2();
var isIntersects = Vector2.SegmentToSegmentIntersection(l.Start, l.End, segment.Start, segment.End, ref intersectPoint);
if (!isIntersects)
{
continue;
}
intersections[intersectPoint] = i;
inDistances[Vector2.Distance(intersectPoint, l.Start)] = intersectPoint;
outDistances[Vector2.Distance(intersectPoint, l.End)] = intersectPoint;
}
if (intersections.Count == 0)
{
return(false);
}
data.InPoint = inDistances[inDistances.Keys.Min()];
data.OutPoint = outDistances[outDistances.Keys.Min()];
data.InSegmentIndex = intersections[data.InPoint];
data.OutSegmentIndex = intersections[data.OutPoint];
return(true);
}
protected override void OnMouseDown(MouseButtonEventArgs e)
{
base.OnMouseDown(e);
Vector3 point = new Vector3(0, 0, 0);
Glu.UnProject(new Vector3(e.X, e.Y, 0.0f), ref point);
Vector2 point2 = new Vector2(point.X, point.Y);
if (e.Button == MouseButton.Left)
{
if (Vector2.Distance(start, point2) < selectionRadius)
{
dragMode = DragMode.Start;
start = point2;
}
else if (Vector2.Distance(end, point2) < selectionRadius)
{
dragMode = DragMode.End;
end = point2;
}
}
else if (e.Button == MouseButton.Right)
{
pathSet = false;
OnResize();
}
}
private void OnResize()
{
GL.Viewport(ClientRectangle.X, ClientRectangle.Y, ClientRectangle.Width, ClientRectangle.Height);
InternalBox box = obstaclesCollection.Bounds;
settings.Size = new Size(Bounds.Width, Bounds.Height);
// теперь отскейлим box так, чтобы сохранить пропорции
// также добавим ещё процентов 20 по сторонам
float coeffW = (float)Width / Height;
float coeffM = box.W / box.H;
float halfW, halfH;
if (coeffW < coeffM)
{
halfW = box.W / 2;
halfH = box.W / coeffW / 2;
}
else
{
halfH = box.H / 2;
halfW = box.H * coeffW / 2;
}
Vector2 center = box.Center;
// если пути нет, проставим его ровно посереднине слева направо
if (!pathSet)
{
start = new Vector2(center.x - halfW * 1.1f, center.y);
end = new Vector2(center.x + halfW * 1.1f, center.y);
pathSet = true;
settings.StartAndEnd = new[] { start, end };
}
// И надбавим 20% чтобы было откуда и куда строить путь
halfW *= 1.2f;
halfH *= 1.2f;
Matrix4 projection = Matrix4.CreateOrthographicOffCenter(center.x - halfW, center.x + halfW,
center.y - halfH, center.y + halfH, -10, 10);
GL.MatrixMode(MatrixMode.Projection);
GL.LoadMatrix(ref projection);
Vector3 point = new Vector3(0, 0, 0);
Glu.UnProject(new Vector3(0, 0, 0), ref point);
Vector2 pointA = new Vector2(point.X, point.Y);
Glu.UnProject(new Vector3(20, 0, 0), ref point);
Vector2 pointB = new Vector2(point.X, point.Y);
selectionRadius = Vector2.Distance(pointA, pointB);
}
public PathDistance calcPathByStart(Vector2 obstacleEnd, List <Vector2> intersectPoints, Vector2 start, Vector2 end)
{
// Находим ближайшую точку пересечения и ее сегмент, запоминаем
Vector2 intersectNearestPoint = findNearestPoint(obstacleEnd, intersectPoints);
Segment nearestSegment = findNearestSegment(intersectNearestPoint);
// Проверяем можем ли с вершины обстакла дойти до финиша
if (findIntersectPoints2(nearestSegment.start, end, segments) == 0)
{
PathDistance path1 = new PathDistance();
path1.addPoint(intersectNearestPoint);
path1.accDistance(Vector2.Distance(start, intersectNearestPoint));
// Выбираем Начало отрезка и запоминаем
path1.addPoint(nearestSegment.start);
path1.accDistance(Vector2.Distance(intersectNearestPoint, nearestSegment.start));
path1.obstacleEnd = Vector2.zero;
return(path1);
}
PathDistance path = new PathDistance();
path.addPoint(intersectNearestPoint);
path.accDistance(Vector2.Distance(start, intersectNearestPoint));
// Выбираем Начало отрезка и запоминаем
path.addPoint(nearestSegment.start);
path.accDistance(Vector2.Distance(intersectNearestPoint, nearestSegment.start));
// Если не можем, ищем обстакл на котором располагается ближайший сегмент
Obstacle obstacle = findObstacleBySegment(nearestSegment);
// Идем по сегментам обстакла, пока не будем пересекать сами себя и не будем пересекать уже пройденные обстаклы,
// или не построем путь из вершина до финиша
while (!(findIntersectPoints2(nearestSegment.start, end, obstacle.segments) == 0 &&
findIntersectPoints2(nearestSegment.start, end, movedSegments.ToArray()) == 0))
{
nearestSegment = obstacle.getNextSegmentByCounterClockWise(nearestSegment);
// Нашли следующий сегмент обстакла, проверяем можем ли дойти до финиша без пересечений
if (findIntersectPoints2(nearestSegment.start, end, segments) == 0)
{
path.addPoint(nearestSegment.start);
path.accDistance(Vector2.Distance(nearestSegment.end, nearestSegment.start));
path.obstacleEnd = Vector2.zero;
return(path);
}
// Не можем - запоминаем конец обстакла
path.addPoint(nearestSegment.start);
path.accDistance(Vector2.Distance(nearestSegment.end, nearestSegment.start));
}
path.obstacleEnd = nearestSegment.start;
movedSegments.AddRange(obstacle.segments.ToList());
return(path);
}
private Segment findNearestSegment(Vector2 intersectNearestPoint)
{
var result = new Segment();
foreach (Segment segment in segments)
{
float diff = Vector2.Distance(segment.start, intersectNearestPoint)
+ Vector2.Distance(segment.end, intersectNearestPoint)
- Vector2.Distance(segment.start, segment.end);
if (diff < 0.01)
{
result = segment;
break;
}
}
return(result);
}
private static double Length(IEnumerable <Vector2> path)
{
double res = 0;
bool first = true;
Vector2 prev = Vector2.zero;
foreach (Vector2 vertex in path)
{
if (first)
{
first = false;
prev = vertex;
continue;
}
res += Vector2.Distance(prev, vertex);
prev = vertex;
}
return(res);
}
private Vector2 findNearestPeak(Vector2 nearestPoint, bool v)
{
Vector2 res = new Vector2();
for (int i = 0; i < obstacles.Length; i++)
{
for (int j = 0; j < obstacles[i].Length; j++)
{
Vector2 v1;
Vector2 v2;
if (j == obstacles[i].Length - 1)
{
v1 = new Vector2(obstacles[i][j].x, obstacles[i][j].y);
v2 = new Vector2(obstacles[i][0].x, obstacles[i][0].y);
}
else
{
v1 = new Vector2(obstacles[i][j].x, obstacles[i][j].y);
v2 = new Vector2(obstacles[i][j + 1].x, obstacles[i][j + 1].y);
}
Vector2 part1 = new Vector2();
Vector2 part2 = new Vector2();
float diff = Vector2.Distance(v1, nearestPoint) + Vector2.Distance(v2, nearestPoint) - Vector2.Distance(v1, v2);
Vector2 peak = v ? v1 : v2;
if (diff < 0.1)
{
res = peak;
break;
}
}
}
return(res);
}
public List <Vector2> WayAround(Segment l)
{
var td = new TracingData();
if (!FindTrasingData(l, ref td))
{
return(new List <Vector2>());
}
var toEndPath = new List <Vector2>();
var toEndPathLen = 0.0;
toEndPath.Add(td.InPoint);
for (var i = td.InSegmentIndex; i != td.OutSegmentIndex; i = (i + 1) % Segments.Length)
{
var newP = Segments[i].End;
toEndPathLen += Vector2.Distance(toEndPath.Last(), newP);
toEndPath.Add(newP);
}
toEndPathLen += Vector2.Distance(toEndPath.Last(), td.OutPoint);
toEndPath.Add(td.OutPoint);
var toStartPath = new List <Vector2>();
var toStartPathLen = 0.0;
toStartPath.Add(td.InPoint);
for (var i = td.InSegmentIndex; i != td.OutSegmentIndex; i = i == 0 ? Segments.Length - 1 : i - 1)
{
var newP = Segments[i].Start;
toStartPathLen += Vector2.Distance(toStartPath.Last(), newP);
toStartPath.Add(newP);
}
toStartPathLen += Vector2.Distance(toStartPath.Last(), td.OutPoint);
toStartPath.Add(td.OutPoint);
return(toStartPathLen < toEndPathLen ? toStartPath : toEndPath);
}
private Vector2 findNearestPoint(Vector2 startPath, IEnumerable <Vector2> intersectPoints)
{
Vector2 res = new Vector2();
float minDistance = 0;
foreach (Vector2 point in intersectPoints)
{
var distance = Vector2.Distance(startPath, point);
if (minDistance == 0)
{
minDistance = distance;
res = point;
}
if (distance < minDistance)
{
res = point;
}
}
// Console.WriteLine("nearestPoint = " + res);
return(res);
}
private Vector2 findNearestPoint(Vector2 start, IEnumerable <Vector2> intersectPoints)
{
Vector2 res = new Vector2();
float minDistance = 0;
foreach (Vector2 point in intersectPoints)
{
var distance = Vector2.Distance(start, point);
if (minDistance == 0)
{
minDistance = distance;
res = point;
}
if (distance < minDistance)
{
minDistance = distance;
res = point;
}
}
return(res);
}
