public void ContainsPointWithinDistanceTolerance()
{
var segment = new Segment2D(Inches, -1.894, 3.319, -2.46239065260586, 2.8249055437922);
var point = new Point2D(Inches, -1.8772648424708, 3.3335476504953);
segment.Contains(point).Should().BeTrue();
}
public static Either <Null, Point2D, Segment2D> IntersectWithDisk(this Segment2D segment, Circle2D diskBoundary)
{
var lineIntersectionsWithBoundary = diskBoundary.IntersectWithLine(segment.AsLine()).ToList();
if (lineIntersectionsWithBoundary.Count == 0)
{
return(Null.Instance);
}
if (lineIntersectionsWithBoundary.Count == 1)
{
var point = lineIntersectionsWithBoundary.Single();
if (segment.Contains(point))
{
return(point);
}
return(Null.Instance);
}
var segmentDirection = segment.GetDirection();
var(baseSideIntersection, endSideIntersection) =
(lineIntersectionsWithBoundary[0], lineIntersectionsWithBoundary[1]).ReverseIf(
(lineIntersectionsWithBoundary[0].DirectionTo(lineIntersectionsWithBoundary[1])
.Value
.DotProduct(segmentDirection)
) < 0);
var clipBase =
(baseSideIntersection - segment.BasePoint).GetComponentInDirection(segmentDirection)
> Unit.ZeroDistance;
var clipEnd =
(endSideIntersection - segment.EndPoint).GetComponentInDirection(segmentDirection) < Unit.ZeroDistance;
var clippedBase = clipBase ? baseSideIntersection : segment.BasePoint;
var clippedEnd = clipEnd ? endSideIntersection : segment.EndPoint;
if (clippedBase == clippedEnd)
{
return(clipBase ? segment.EndPoint : segment.BasePoint);
}
var signedDistance = (clippedEnd - clippedBase).GetComponentInDirection(segmentDirection);
if (signedDistance > Unit.ZeroDistance)
{
return(new Segment2D(clippedBase, clippedEnd));
}
return(Null.Instance);
}
protected virtual Vector2 PathCollisionOffset(Polygon pastSelf, Polygon obstacle, Vector2 p_vertex)
{
Segment2D p_vertexPath = new Segment2D(pastSelf.Vertices[GetVertexIndex(p_vertex)], p_vertex);
List <Segment2D> o_edges = obstacle.IntersectingEdges(p_vertexPath);
if (o_edges == null || o_edges.Count <= 0)
{
throw new ImpossibleCollisionException();
}
Segment2D o_edge = o_edges[0];
Vector2 p_PPoint = o_edge.ToLine().PerpendicularPoint(p_vertex);
if (o_edge.Contains(p_PPoint))
{
Vector2 offset = p_PPoint - p_vertex;
return(offset);
}
return(Vector2.zero);
}
public override float GetParamAtPoint(Point2D pt)
{
for (int i = 1; i < Points.Count; i++)
{
float dist = (pt - Points[i - 1]).Length;
Segment2D seg = new Segment2D(Points[i - 1], Points[i]);
bool onSegment = seg.Contains(pt, PointOnLineTolerance, out float t);
if (!onSegment && t < 0 && i - 1 == 0)
{
return(StartParam);
}
else if (!onSegment && t > 1 && i == Points.Count - 1)
{
return(EndParam);
}
else if (!onSegment)
{
continue;
}
float segmentLength = seg.Length;
if (MathF.IsZero(dist))
{
return(i - 1);
}
else if (MathF.IsEqual(dist, segmentLength))
{
return(i);
}
else if (segmentLength > dist)
{
return(dist / segmentLength + (i - 1));
}
}
return(EndParam);
}
void DrawDebugCollision(Polygon fake, Polygon obstacle)
{
Gizmos.color = Color.gray;
((RegularPolygon)fake).ReduceCollision(obstacle, debugMovement);
fake.DrawEdges();
List <Vector2> p_vertices = fake.VerticesInside(obstacle);
List <Vector2> o_vertices = obstacle.VerticesInside(fake);
Vector2 p_vertex = Vector2.zero;
if (p_vertices.Count > 0)
{
p_vertex = p_vertices[0];
Gizmos.DrawWireSphere(p_vertex, gizmosSize);
}
Vector2 o_vertex = Vector2.zero;
if (o_vertices.Count > 0)
{
o_vertex = o_vertices[0];
Gizmos.DrawWireSphere(o_vertex, gizmosSize);
}
//----------------------------------------------------------------
if (p_vertex == Vector2.zero || o_vertex == Vector2.zero)
{
return;
}
Segment2D p_vertexPath = new Segment2D(polygon.Vertices[fake.GetVertexIndex(p_vertex)], p_vertex);
p_vertexPath.DrawGizmos(false);
List <Segment2D> o_edges = obstacle.IntersectingEdges(p_vertexPath);
if (o_edges == null || o_edges.Count <= 0)
{
return;
}
if (fake.ContainsInEdge(o_vertex))
{
Gizmos.color = Color.magenta;
List <Vector2> intPoints = obstacle.IntersectionPoints(fake);
Vector2 pPoint = new Segment2D(intPoints[0], intPoints[1]).PerpendicularPoint(p_vertex);
Gizmos.DrawLine(pPoint, p_vertex);
Gizmos.DrawWireSphere(p_vertex, gizmosSize);
Gizmos.DrawWireSphere(pPoint, gizmosSize);
return;
}
Gizmos.color = Color.cyan;
Segment2D o_edge = o_edges[0];
o_edge.DrawGizmos(true, gizmosSize);
Vector2 p_PPoint = o_edge.ToLine().PerpendicularPoint(p_vertex);
Gizmos.DrawWireSphere(p_PPoint, gizmosSize / 2);
if (o_edge.Contains(p_PPoint))
{
Vector2 offset = p_PPoint - p_vertex;
Gizmos.DrawLine(p_PPoint, p_vertex);
}
Gizmos.color = Color.yellow;
foreach (Vector2 p in fake.IntersectionPoints(obstacle))
{
Gizmos.DrawWireSphere(p, gizmosSize);
}
Gizmos.color = Color.blue;
List <Segment2D> p_edges = fake.EdgesInside(obstacle, true);
foreach (Segment2D s in p_edges)
{
s.DrawGizmos(true, gizmosSize);
}
List <Vector2> o_PPoints = new List <Vector2>();
List <Segment2D> intersectingEdges = fake.EdgesInside(obstacle, true);
for (int i = 0; i < intersectingEdges.Count; i++)
{
Line2D line = new Line2D(intersectingEdges[i]);
Vector2 pPoint = line.PerpendicularPoint(o_vertex);
if (intersectingEdges[i].Contains(pPoint))
{
o_PPoints.Add(pPoint);
}
}
Gizmos.color = Color.magenta;
for (int i = 0; i < o_PPoints.Count; i++)
{
Gizmos.DrawWireSphere(o_PPoints[i], gizmosSize + (i * gizmosSize / 2));
Gizmos.DrawLine(o_vertex, o_PPoints[i]);
}
Gizmos.color = Color.white;
Gizmos.DrawLine(p_vertex, fake.Center);
Gizmos.DrawWireSphere(fake.Center, gizmosSize);
}
