private static void HandleTopSegment(ref List <Vector2> visibility, LineSegment seg, Ray2D ray)
{
// dont handle colinear lines
if (seg == null || Line.Colinear(ray.origin, seg.Point1, seg.Point2))
{
return;
}
var intersect = seg.Intersect(ray);
if (!intersect.HasValue)
{
// robustness issues if ray near endpoint
var lineIntersect = seg.Line.Intersect(ray);
if (lineIntersect.HasValue)
{
intersect = seg.ClosestPoint(lineIntersect.Value);
}
}
if (intersect.HasValue &&
!MathUtil.EqualsEps(Vector2.zero, intersect.Value) &&
(visibility.Count == 0 || !MathUtil.EqualsEps(intersect.Value, visibility.Last())))
{
visibility.Add(intersect.Value);
}
}
private void ComputeMinDistanceLinePoint(ILineString line, IPoint point,
bool flip)
{
var lineCoord = line.Coordinates;
var coord = point.Coordinate;
// brute force approach!
for (int i = 0; i < lineCoord.Length - 1; i++)
{
var dist = CGAlgorithms3D.DistancePointSegment(coord, lineCoord[i],
lineCoord[i + 1]);
if (dist < _minDistance)
{
var seg = new LineSegment(lineCoord[i], lineCoord[i + 1]);
var segClosestPoint = seg.ClosestPoint(coord);
UpdateDistance(dist,
new GeometryLocation(line, i, segClosestPoint),
new GeometryLocation(point, 0, coord),
flip);
}
if (_isDone)
{
return;
}
}
}
private void UpdatePts(Coordinate p, Coordinate seg0, Coordinate seg1)
{
_minPts[0] = p;
var seg = new LineSegment(seg0, seg1);
_minPts[1] = new Coordinate(seg.ClosestPoint(p));
}
/// <summary>
///
/// </summary>
/// <param name="line"></param>
/// <param name="pt"></param>
/// <param name="locGeom"></param>
private void ComputeMinDistance(ILineString line, IPoint pt, GeometryLocation[] locGeom)
{
if (line.EnvelopeInternal.Distance(pt.EnvelopeInternal) > _minDistance)
{
return;
}
var coord0 = line.Coordinates;
var coord = pt.Coordinate;
// brute force approach!
for (int i = 0; i < coord0.Length - 1; i++)
{
double dist = DistanceComputer.PointToSegment(coord, coord0[i], coord0[i + 1]);
if (dist < _minDistance)
{
_minDistance = dist;
var seg = new LineSegment(coord0[i], coord0[i + 1]);
var segClosestPoint = seg.ClosestPoint(coord);
locGeom[0] = new GeometryLocation(line, i, segClosestPoint);
locGeom[1] = new GeometryLocation(pt, 0, coord);
}
if (_minDistance <= _terminateDistance)
{
return;
}
}
}
/// <summary>
///
/// </summary>
/// <param name="line"></param>
/// <param name="pt"></param>
/// <param name="locGeom"></param>
private void ComputeMinDistance(ILineString line, IPoint pt, GeometryLocation[] locGeom)
{
if (line.EnvelopeInternal.Distance(pt.EnvelopeInternal) > minDistance)
{
return;
}
ICoordinate[] coord0 = line.Coordinates;
ICoordinate coord = pt.Coordinate;
// brute force approach!
for (int i = 0; i < coord0.Length - 1; i++)
{
double dist = CGAlgorithms.DistancePointLine(coord, coord0[i], coord0[i + 1]);
if (dist < minDistance)
{
minDistance = dist;
LineSegment seg = new LineSegment(coord0[i], coord0[i + 1]);
ICoordinate segClosestPoint = seg.ClosestPoint(coord);
locGeom[0] = new GeometryLocation(line, i, segClosestPoint);
locGeom[1] = new GeometryLocation(pt, 0, coord);
}
if (minDistance <= terminateDistance)
{
return;
}
}
}
/// <summary>
///
/// </summary>
/// <param name="line"></param>
/// <param name="pt"></param>
/// <param name="locGeom"></param>
private void ComputeMinDistance(ILineString line, Point pt, GeometryLocation[] locGeom)
{
if (line.EnvelopeInternal.Distance(pt.EnvelopeInternal) > _minDistance)
{
return;
}
IList <Coordinate> coord0 = line.Coordinates;
Coordinate coord = pt.Coordinate;
// brute force approach!
for (int i = 0; i < coord0.Count - 1; i++)
{
double dist = CgAlgorithms.DistancePointLine(coord, coord0[i], coord0[i + 1]);
if (dist < _minDistance)
{
_minDistance = dist;
LineSegment seg = new LineSegment(coord0[i], coord0[i + 1]);
Coordinate segClosestPoint = new Coordinate(seg.ClosestPoint(coord));
locGeom[0] = new GeometryLocation(line, i, segClosestPoint);
locGeom[1] = new GeometryLocation(pt, 0, coord);
}
if (_minDistance <= _terminateDistance)
{
return;
}
}
}
public void Update(IMyEntity missile)
{
LineSegment line = new LineSegment(entity.GetPosition(), entity.GetPosition() + entity.WorldMatrix.Forward * 1e6f);
position = line.ClosestPoint(missile.GetPosition()) + missile.Physics.LinearVelocity;
linearVelocity = entity.WorldMatrix.Forward * missile.Physics.LinearVelocity.Length();
}
private void ComputeMinDistance(LineString line, Point pt,
DistanceLocation[] locGeom)
{
if (line.Bounds.Distance(pt.Bounds) > minDistance)
{
return;
}
ICoordinateList coord0 = line.Coordinates;
Coordinate coord = pt.Coordinate;
// brute force approach!
int nCount0 = coord0.Count;
for (int i = 0; i < nCount0 - 1; i++)
{
double dist = CGAlgorithms.DistancePointLine(coord, coord0[i], coord0[i + 1]);
if (dist < minDistance)
{
minDistance = dist;
LineSegment seg =
new LineSegment(m_objFactory, coord0[i], coord0[i + 1]);
Coordinate segClosestPoint = seg.ClosestPoint(coord);
locGeom[0] = new DistanceLocation(line, i, segClosestPoint);
locGeom[1] = new DistanceLocation(pt, 0, coord);
}
if (minDistance <= terminateDistance)
{
return;
}
}
}
private void OnDrawGizmos()
{
var pos = point.position;
var line0 = new LineSegment()
{
start = line0_start.position, end = line0_end.position
};
var line1 = new LineSegment()
{
start = line1_start.position, end = line1_end.position
};
// line0
Gizmos.color = Color.red;
Gizmos.DrawLine(line0.start, line0.end);
// line1
Gizmos.color = Color.blue;
Gizmos.DrawLine(line1.start, line1.end);
// intersection line0,line1
var intersection = LineSegment.CalcIntersection(line0, line1);
if (intersection != null)
{
Gizmos.color = Color.yellow;
Gizmos.DrawWireSphere(intersection.Value, 0.3f);
}
// closet point
Gizmos.color = Color.gray;
var cpos0 = line0.ClosestPoint(pos);
var cpos1 = line1.ClosestPoint(pos);
Gizmos.DrawLine(cpos0, pos);
Gizmos.DrawLine(cpos1, pos);
// normal from pos
Gizmos.color = Color.green;
var normal0 = line0.CalcNormal(pos);
var normal1 = line1.CalcNormal(pos);
Gizmos.DrawRay(pos, -normal0);
Gizmos.DrawRay(pos, -normal1);
#if UNITY_EDITOR
var dist0 = line0.Distance(pos);
var dist1 = line1.Distance(pos);
UnityEditor.Handles.Label(pos, $"Distance: {dist0},{dist1}");
var left0 = line0.IsLeft(pos);
var left1 = line1.IsLeft(pos);
UnityEditor.Handles.Label(pos - Vector3.up * 0.2f, $"IsLeft: {left0},{left1}");
#endif
}
// Update is called once per frame
void Update()
{
LineSegment l1 = new LineSegment(a.position, b.position);
Vector3 closest = l1.ClosestPoint(c.position);
closest.y = a.position.y;
intersection.position = closest;
}
private void UpdateNearestLocationsPointLine(Coordinate pt,
FacetSequence facetSeq, int i, Coordinate q0, Coordinate q1,
GeometryLocation[] locs)
{
locs[0] = new GeometryLocation(_geom, _start, pt.Copy());
var seg = new LineSegment(q0, q1);
var segClosestPoint = seg.ClosestPoint(pt);
locs[1] = new GeometryLocation(facetSeq._geom, i, segClosestPoint.Copy());
}
private void UpdateClearance(double candidateValue, Coordinate p,
Coordinate seg0, Coordinate seg1)
{
if (candidateValue < _minClearance)
{
_minClearance = candidateValue;
_minClearancePts[0] = p.Copy();
var seg = new LineSegment(seg0, seg1);
_minClearancePts[1] = seg.ClosestPoint(p).Copy();
}
}
public static void ComputeDistance(ILineString line, Coordinate pt, PointPairDistance ptDist)
{
var coords = line.Coordinates;
for (var i = 0; i < coords.Length - 1; i++)
{
TempSegment.SetCoordinates(coords[i], coords[i + 1]);
// this is somewhat inefficient - could do better
var closestPt = TempSegment.ClosestPoint(pt);
ptDist.SetMinimum(closestPt, pt);
}
}
// removed because Geometry.GeometryChangedAction() is not virtual
// public void GeometryChangedAction()
// {
// DetermineMonotone();
// }
/**
* @param co
* input coordinate in the neighbourhood of the MLineString
* @param tolerance
* max. distance that co may be from this MLineString
* @return an MCoordinate on this MLineString with appropriate M-value
*/
public MCoordinate GetClosestPoint(Coordinate co, double tolerance)
{
if (!this.IsMonotone(false))
{
throw new ApplicationException("MGeometryException.OPERATION_REQUIRES_MONOTONE");
}
if (!this.IsEmpty)
{
LineSegment seg = new LineSegment();
Coordinate[] coAr = this.Coordinates;
seg.P0 = coAr[0];
double d = 0.0;
double projfact = 0.0;
double minDist = Double.PositiveInfinity;
MCoordinate mincp = null;
for (int i = 1; i < coAr.Length; i++)
{
seg.P1 = coAr[i];
Coordinate cp = seg.ClosestPoint(co);
d = cp.Distance(co);
if (d <= tolerance && d <= minDist)
{
MCoordinate testcp = new MCoordinate(cp);
projfact = seg.ProjectionFactor(cp);
testcp.M = ((MCoordinate)coAr[i - 1]).M
+ projfact
* (((MCoordinate)coAr[i]).M - ((MCoordinate)coAr[i - 1]).M);
if (d < minDist || testcp.M < mincp.M)
{
mincp = testcp;
minDist = d;
}
}
seg.P0 = seg.P1;
}
if (minDist > tolerance)
{
return(null);
}
else
{
return(mincp);
}
}
else
{
return(null);
}
}
/// <summary>
/// Either intersects two line segments defined by the given four polar points
/// or if endpoint is null, a line segment (a,b) and ray from point orig in the direction
/// of its adjacent polygon segment.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="orig"></param>
/// <param name="endpoint"></param>
/// <returns> A vertex-displacement pair associated with the intersection. </returns>
public static VertDispl IntersectWithWindow(VertDispl a, VertDispl b, VertDispl orig, VertDispl endpoint)
{
if (a == null || b == null || orig == null)
{
return(null);
}
var s1 = new LineSegment(a.p, b.p);
// extra checks related to robustness issues
if (s1.IsOnSegment(orig.p.Cartesian))
{
return(orig);
}
Vector2?res;
if (endpoint != null)
{
var s2 = new LineSegment(orig.p, endpoint.p);
// check for parallel slopes
if (s1.IsParallel(s2))
{
res = s1.ClosestPoint(orig.p.Cartesian);
if (res.HasValue && !s2.IsOnSegment(res.Value))
{
res = null;
}
}
else
{
res = s1.Intersect(s2);
}
}
else
{
var ray = new Ray2D(orig.p.Cartesian, orig.Direction);
res = s1.Intersect(ray);
}
if (!res.HasValue)
{
return(null);
}
return(DisplacementInBetween(new PolarPoint2D(res.Value), a, b));
}
private bool CheckFinalExit()
{
// rotate the polygon into the current relative frame
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp);
LineSegment finalLine = finalOrientation.Transform(relTransform);
double finalAngle = finalLine.UnitVector.ArcTan;
if (Math.Abs(finalAngle) < 15 * Math.PI / 180.0 && finalLine.ClosestPoint(Coordinates.Zero).Length < 2)
{
// we can just execute a stay in lane
return(true);
}
return(false);
}
/// <summary>
/// Given the specified test point, this checks each segment, and will
/// return the closest point on the specified segment.
/// </summary>
/// <param name="self">The ILineString, whose point is returned.</param>
/// <param name="testPoint">The point to test.</param>
public static Coordinate ClosestPoint(this ILineString self, Coordinate testPoint)
{
Coordinate closest = self.GetCoordinateN(0);
double dist = double.MaxValue;
for (int i = 0; i < self.NumPoints - 1; i++)
{
LineSegment s = new LineSegment(self.GetCoordinateN(i), self.GetCoordinateN(i + 1));
Coordinate temp = s.ClosestPoint(testPoint);
double tempDist = testPoint.Distance(temp);
if (tempDist < dist)
{
dist = tempDist;
closest = temp;
}
}
return(closest);
}
private static double GetObstacleClearanceLine(LineSegment segment, IList <Polygon> obstacles)
{
double minDist = double.MaxValue;
foreach (Polygon obs in obstacles)
{
foreach (Coordinates pt in obs)
{
Coordinates closestPt = segment.ClosestPoint(pt);
double dist = closestPt.DistanceTo(pt);
if (dist < minDist)
{
minDist = dist;
}
}
}
return(minDist);
}
/// <summary>
/// Computes the Euclidean distance (L2 metric) from a <see cref="Coordinate"/> to a <see cref="LineSegment"/>.
/// </summary>
/// <param name="segment">The <c>LineSegment</c></param>
/// <param name="pt">The Point</param>
/// <param name="ptDist">The <c>PointPairDistance</c></param>
public static void ComputeDistance(LineSegment segment, Coordinate pt, PointPairDistance ptDist)
{
var closestPt = segment.ClosestPoint(pt);
ptDist.SetMinimum(closestPt, pt);
}
/// <summary>
///
/// </summary>
/// <param name="line"></param>
/// <param name="pt"></param>
/// <param name="locGeom"></param>
private void ComputeMinDistance(ILineString line, Point pt, GeometryLocation[] locGeom)
{
if (line.EnvelopeInternal.Distance(pt.EnvelopeInternal) > _minDistance) return;
IList<Coordinate> coord0 = line.Coordinates;
Coordinate coord = pt.Coordinate;
// brute force approach!
for (int i = 0; i < coord0.Count - 1; i++)
{
double dist = CgAlgorithms.DistancePointLine(coord, coord0[i], coord0[i + 1]);
if (dist < _minDistance)
{
_minDistance = dist;
LineSegment seg = new LineSegment(coord0[i], coord0[i + 1]);
Coordinate segClosestPoint = new Coordinate(seg.ClosestPoint(coord));
locGeom[0] = new GeometryLocation(line, i, segClosestPoint);
locGeom[1] = new GeometryLocation(pt, 0, coord);
}
if (_minDistance <= _terminateDistance) return;
}
}
public void Update(IMyEntity missile)
{
LineSegment line = new LineSegment(entity.GetPosition(), entity.GetPosition() + entity.WorldMatrix.Forward * 1e6f);
position = line.ClosestPoint(missile.GetPosition()) + missile.Physics.LinearVelocity;
linearVelocity = entity.WorldMatrix.Forward * missile.Physics.LinearVelocity.Length();
}
/// <summary>
/// Pops all vertices from the stack that have become invisible after the addition
/// of a new vertex.
/// Calls appropriate method (advance, scan, retard) after being done based on next vertex.
/// </summary>
/// <param name="v"></param>
/// <param name="sOld"></param>
/// <param name="iprev"></param>
/// <returns></returns>
public static NextCall Retard(ref VsRep v, ref Stack <VertDispl> s, ref int i, ref VertDispl w, ref bool ccw)
{
// LocateSj will pop vertices from the stack
// until appropriated s_j is found
// see paper
var sjNext = LocateSj(v.Get(i), v.Get(i + 1), s);
if (s.Count == 0)
{
return(NextCall.STOP);
}
var sj = s.Peek();
if (sj.alpha < v.Get(i + 1).alpha)
{
i++;
var vi = v.Get(i);
var p = (new LineSegment(sj.p.Cartesian, sjNext.p.Cartesian)).Intersect(vi.p.Ray);
// fallback method, return point closest to intersection with segment line
if (p == null)
{
var line = new Line(vi.p.Ray.origin, vi.p.Ray.origin + vi.p.Ray.direction);
p = (new LineSegment(sj.p.Cartesian, sjNext.p.Cartesian)).Line.Intersect(line);
p = Vector2.Distance(p.Value, sj.p.Cartesian) < Vector2.Distance(p.Value, sjNext.p.Cartesian)
? sj.p.Cartesian : sjNext.p.Cartesian;
}
var st1 = DisplacementInBetween(new PolarPoint2D(p.Value), sj, sjNext);
if (st1 != null)
{
s.Push(st1);
}
s.Push(vi);
// paper does i == v.n
if (i == v.n - 1)
{
// TODO order of returned list correct? (check stack to list conversion)
return(NextCall.STOP);
}
else if (MathUtil.GEQEps(v.Get(i + 1).alpha, vi.alpha) &&
MathUtil.Orient2D(v.Get(i - 1).p.Cartesian, vi.p.Cartesian, v.Get(i + 1).p.Cartesian) <= 0)
{ // -1 is RighTurn
return(NextCall.ADVANCE);
}
else if (MathUtil.GreaterEps(v.Get(i + 1).alpha, vi.alpha) &&
MathUtil.Orient2D(v.Get(i - 1).p.Cartesian, vi.p.Cartesian, v.Get(i + 1).p.Cartesian) > 0)
{ // 1 is LeftTurn
s.Pop();
w = vi;
ccw = false;
return(NextCall.SCAN);
}
else
{
s.Pop();
return(NextCall.RETARD);
}
}
else
{
if (MathUtil.EqualsEps(v.Get(i + 1).alpha, sj.alpha) &&
MathUtil.GreaterEps(v.Get(i + 2).alpha, v.Get(i + 1).alpha) &&
MathUtil.Orient2D(v.Get(i).p.Cartesian, v.Get(i + 1).p.Cartesian, v.Get(i + 2).p.Cartesian) <= 0)
{ // -1 is RightTurn
s.Push(v.Get(i + 1));
return(NextCall.ADVANCE);
}
else
{
w = IntersectWithWindow(v.Get(i), v.Get(i + 1), sj, sjNext);
ccw = true;
if (w == null)
{
var seg = new LineSegment(v.Get(i).p, v.Get(i + 1).p);
var res = seg.ClosestPoint(sj.p.Cartesian);
w = DisplacementInBetween(new PolarPoint2D(res), v.Get(i), v.Get(i + 1));
}
return(NextCall.SCAN);
}
}
}
private bool DoSegmentsCollide(IPathSegment seg1, IPathSegment seg2, out double collideDist1, out double collideDist2)
{
bool doesIntersect = false;
Vector2 intersectPoint;
Vector2[] p = new Vector2[4];
collideDist1 = Double.MaxValue;
collideDist2 = Double.MaxValue;
LineSegment ls1 = new LineSegment(seg1.Start, seg1.End);
LineSegment ls2 = new LineSegment(seg2.Start, seg2.End);
doesIntersect = ls1.Intersect(ls2, out intersectPoint);
p[0] = ls1.ClosestPoint(seg2.Start);
p[1] = ls1.ClosestPoint(seg2.End);
p[2] = ls2.ClosestPoint(seg1.Start);
p[3] = ls2.ClosestPoint(seg1.End);
if (doesIntersect)
{
collideDist1 = seg1.Start.DistanceTo(intersectPoint);
collideDist2 = seg2.Start.DistanceTo(intersectPoint);
}
if ((p[0] - seg2.Start).Length < collideThreshold)
{
if (collideDist1 > seg1.Start.DistanceTo(p[0]))
{
collideDist1 = seg1.Start.DistanceTo(p[0]);
collideDist2 = 0;
doesIntersect = true;
}
}
if ((p[1] - seg2.End).Length < collideThreshold)
{
if (collideDist1 > seg1.Start.DistanceTo(p[1]))
{
collideDist1 = seg1.Start.DistanceTo(p[1]);
collideDist2 = seg2.Length;
doesIntersect = true;
}
}
if ((p[2] - seg1.Start).Length < collideThreshold)
{
collideDist1 = 0;
collideDist2 = seg2.Start.DistanceTo(p[2]);
}
if ((p[3] - seg1.End).Length < collideThreshold)
{
if (collideDist1 > seg1.Length)
{
collideDist1 = seg1.Length;
collideDist2 = seg2.Start.DistanceTo(p[3]);
}
}
return(doesIntersect);
}
