private IntersectionDetails FindNearestIntersection(Vector2 start, Vector2 end)
{
IntersectionDetails nearestIntersection = IntersectionDetails.CreateEmpty();
var segmentToEnd = Segment.Create(start, end);
for (var i = 0; i < _obstacles.Count; i++)
{
var obstacle = _obstacles[i];
IntersectionDetails thisObstactleIntersection;
if (!obstacle.Inersects(segmentToEnd, out thisObstactleIntersection))
{
continue;
}
if (nearestIntersection.Obstacle == null || thisObstactleIntersection.SquaredDistanceFromStart < nearestIntersection.SquaredDistanceFromStart)
{
nearestIntersection = thisObstactleIntersection;
}
}
return(nearestIntersection);
}
private void FindNextNodes(Node currentNode, Vector2 end)
{
//тут надо найти ближайший обстакл, который пересекается с путём до финиша
//если такого не найдено - вернуть новую ноду, которая будет совпадать с финишем
//если найдено - то найти на нём 2 ближайшие к точке пересечения вершины (в случае, если точка пересечения лежит на вершине они будут одинаковые)
//а потом найти все вершины этого обстакла, до которых можем добраться из текущей ноды и добавить в наш список
//это тормозит, но должно работать
IntersectionDetails nearestIntersection = FindNearestIntersection(currentNode.Point, end);
if (nearestIntersection.Obstacle == null)
{
AddNode(currentNode.CreateChild(end, end));
return;
}
var foundObstacle = nearestIntersection.Obstacle;
RecursivelyAddReachableVerticesFromObstacle(currentNode, end, foundObstacle);
}
public bool Inersects(Segment segment, out IntersectionDetails intersectionDetails)
{
intersectionDetails = IntersectionDetails.CreateEmpty();
intersectionDetails.Obstacle = null;
intersectionDetails.SquaredDistanceFromStart = 0;
// int firstVerticeIndex = -1;
// int secondVerticeIndex = -1;
// for (var i = 0; i < _vertices.Length; i++)
// {
// if (_vertices[i].Equals(segment.Start))
// {
// firstVerticeIndex = i;
// }
// if (_vertices[i].Equals(segment.End))
// {
// secondVerticeIndex = i;
// }
// }
// if (firstVerticeIndex != -1 && secondVerticeIndex != -1)
// {
// int delta = Math.Abs(firstVerticeIndex - secondVerticeIndex);
// if (delta >= _vertices.Length)
// {
// delta -= _vertices.Length;
// }
// if (delta == 1)
// {
// return false;
// }
// }
for (var i = 0; i < _vertices.Length; i++)
{
var secondVertice = i == _vertices.Length - 1 ? 0 : i + 1;
Segment obstacleSegment = Segment.Create(_vertices[i], _vertices[secondVertice]);
Vector2 intersectionPoint = Vector2.zero;
if (Vector2.SegmentToSegmentIntersection(segment.Start, segment.End, obstacleSegment.Start, obstacleSegment.End, ref intersectionPoint))
{
float squaredDistance = Vector2.SqrDistance(segment.Start, intersectionPoint);
if (intersectionDetails.Obstacle == null || squaredDistance < intersectionDetails.SquaredDistanceFromStart)
{
intersectionDetails.SquaredDistanceFromStart = squaredDistance;
}
intersectionDetails.Obstacle = this;
}
}
if (_vertices.Any(vector2 => vector2.Equals(segment.Start)) && _vertices.Any(vector2 => vector2.Equals(segment.End)))
{
//TODO: всё это говнохак, направленный на то, чтобы определить, что мы проходим внутри обстакла, когда наши точки находятся на вершинах обстакла
//это должно быть удалено
for (var i = 0; i < _vertices.Length; i++)
{
for (var j = i + 1; j < _vertices.Length; j++)
{
var innerSegment = Segment.Create(_vertices[i], _vertices[j]);
innerSegment = Segment.Extend(innerSegment);
Vector2 temp = Vector2.down;
if (Vector2.SegmentToSegmentIntersection(segment.Start, segment.End, innerSegment.Start, innerSegment.End, ref temp))
{
intersectionDetails.Obstacle = this;
intersectionDetails.SquaredDistanceFromStart = 0;
break;
}
}
}
if (intersectionDetails.Obstacle == null)
{
var shrinkedSegment = Segment.Shrink(segment);
if (Contains(shrinkedSegment.Start) || Contains(shrinkedSegment.End))
{
intersectionDetails.Obstacle = this;
intersectionDetails.SquaredDistanceFromStart = 0;
}
}
}
return(intersectionDetails.Obstacle != null);
}