private void GetPathInner(Vector2 start, Vector2 end, List <Vector2> result)
{
result.Add(start);
ShapeIntersection nearestIntersection = ShapeIntersection.EMPTY;
foreach (Shape obstacle in obstacles)
{
ShapeIntersection intersecion = obstacle.FindNearestIntersection(start, end);
if (intersecion.isEmpty())
{
continue;
}
if (nearestIntersection.isEmpty() ||
Vector2.SqrDistance(start, nearestIntersection.IntersectionPoint) > Vector2.SqrDistance(start, intersecion.IntersectionPoint))
{
nearestIntersection = intersecion;
}
}
if (nearestIntersection.isEmpty())
{
result.Add(end);
}
else
{
result.Add(nearestIntersection.IntersectionPoint);
GetPathInner(nearestIntersection.GetShapeRidLeftPoint(), end, result);
}
}
public ShapeIntersection FindNearestIntersection(Vector2 fromPoint, Vector2 toPoint)
{
ShapeIntersection result = ShapeIntersection.EMPTY;
Vector2 intersection = new Vector2(0, 0);
int fromPointAsVrtexIdx = -1;
for (int i = 0; i < points.Length; i++)
{
// отлавливаем ситуацию, когда fromPoint - вершина многоугольника
if (fromPointAsVrtexIdx < 0 && fromPoint.Equals(points[i]))
{
fromPointAsVrtexIdx = i;
}
Vector2 nextRidPoint = GetRidNextPoint(i);
if (Vector2.SegmentToSegmentIntersection(points[i], nextRidPoint, fromPoint, toPoint, ref intersection))
{
if (result.isEmpty() ||
Vector2.SqrDistance(fromPoint, intersection) < Vector2.SqrDistance(fromPoint, result.IntersectionPoint))
{
result = new ShapeIntersection(this, intersection, i);
intersection = new Vector2(0, 0);
}
}
}
// отлавливаем, что мы не попадаем внутрь многоугольника через его вершину
if (!result.isEmpty() && fromPointAsVrtexIdx >= 0)
{
int nextRidIdx = GetNextRidIdx(fromPointAsVrtexIdx);
Vector2 ridVector = points[nextRidIdx] - fromPoint;
Vector2 directionVector = result.IntersectionPoint - fromPoint;
float angle = Vector2.SignedAngle(ridVector, directionVector);
if (angle < 0)
{
// заменяем на пересечение со следующим ребром многоугольника
result = new ShapeIntersection(this, points[nextRidIdx], nextRidIdx);
}
//Console.WriteLine("ridVector angle: " + Vector2.SignedAngle(ridVector, new Vector2(1, 0)));
//Console.WriteLine("directionVector angle: " + Vector2.SignedAngle(directionVector, new Vector2(1, 0)));
//Console.WriteLine("Point=" + fromPoint + ", angle=" + angle);
}
return(result);
}
