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); }