public IList <Vector2> GetPathAroundObstacle(SegmentIntersection from, Vector2 closestTo) { int indexOfSegment = _segments.IndexOf(from.Segment); var clockwise = new List <Vector2>(); bool finished = false; for (int i = indexOfSegment; i < _segments.Count; ++i) { clockwise.Add(_segments[i].Point2); SegmentIntersection si; if (!GetClosestIntersection(new Segment(_segments[i].Point2, closestTo), out si)) { finished = true; break; } } if (!finished) { for (int i = 0; i < indexOfSegment; ++i) { clockwise.Add(_segments[i].Point2); SegmentIntersection si; if (!GetClosestIntersection(new Segment(_segments[i].Point2, closestTo), out si)) { break; } } } var counterClockwise = new List <Vector2>(); finished = false; for (int i = indexOfSegment; i >= 0; --i) { counterClockwise.Add(_segments[i].Point1); SegmentIntersection si; if (!GetClosestIntersection(new Segment(_segments[i].Point1, closestTo), out si)) { finished = true; break; } } if (!finished) { for (int i = _segments.Count - 1; i > indexOfSegment; --i) { counterClockwise.Add(_segments[i].Point1); SegmentIntersection si; if (!GetClosestIntersection(new Segment(_segments[i].Point1, closestTo), out si)) { break; } } } float cwLen = GetLen(from.Point, clockwise); float ccwLen = GetLen(from.Point, counterClockwise); return((cwLen < ccwLen) ? clockwise : counterClockwise); }
public bool GetClosestIntersection(Segment ray, out SegmentIntersection intersection) { List <SegmentIntersection> intersections = new List <SegmentIntersection>(); foreach (var segment in _segments) { Vector2 i = new Vector2(); if (MathHelper.SegmentToSegmentIntersection(segment, ray, ref i)) { intersections.Add(new SegmentIntersection(segment, i)); } } if (intersections.Count == 0) { intersection = null; return(false); } intersection = intersections[0]; float dist = (intersection.Point - ray.Point1).sqrMagnitude; for (int i = 1; i < intersections.Count; ++i) { float tmp = (intersections[i].Point - ray.Point1).sqrMagnitude; if (tmp < dist) { intersection = intersections[i]; dist = tmp; } } return(true); }