1013 lines
35 KiB
C#
1013 lines
35 KiB
C#
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using OpenNest.Math;
|
|
|
|
namespace OpenNest.Geometry
|
|
{
|
|
public static class SpatialQuery
|
|
{
|
|
/// <summary>
|
|
/// Finds the distance from a vertex to a line segment along a push axis.
|
|
/// Returns double.MaxValue if the ray does not hit the segment.
|
|
/// </summary>
|
|
private static double RayEdgeDistance(Vector vertex, Line edge, PushDirection direction)
|
|
{
|
|
return RayEdgeDistance(
|
|
vertex.X,
|
|
vertex.Y,
|
|
edge.pt1.X,
|
|
edge.pt1.Y,
|
|
edge.pt2.X,
|
|
edge.pt2.Y,
|
|
direction
|
|
);
|
|
}
|
|
|
|
[System.Runtime.CompilerServices.MethodImpl(
|
|
System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining
|
|
)]
|
|
private static double RayEdgeDistance(
|
|
double vx,
|
|
double vy,
|
|
double p1x,
|
|
double p1y,
|
|
double p2x,
|
|
double p2y,
|
|
PushDirection direction
|
|
)
|
|
{
|
|
switch (direction)
|
|
{
|
|
case PushDirection.Left:
|
|
case PushDirection.Right:
|
|
{
|
|
var dy = p2y - p1y;
|
|
if (System.Math.Abs(dy) < Tolerance.Epsilon)
|
|
return double.MaxValue;
|
|
|
|
var t = (vy - p1y) / dy;
|
|
if (t < -Tolerance.Epsilon || t > 1.0 + Tolerance.Epsilon)
|
|
return double.MaxValue;
|
|
|
|
var ix = p1x + t * (p2x - p1x);
|
|
var dist = direction == PushDirection.Left ? vx - ix : ix - vx;
|
|
|
|
if (dist > Tolerance.Epsilon)
|
|
return dist;
|
|
if (dist >= -Tolerance.Epsilon)
|
|
return 0;
|
|
return double.MaxValue;
|
|
}
|
|
|
|
case PushDirection.Down:
|
|
case PushDirection.Up:
|
|
{
|
|
var dx = p2x - p1x;
|
|
if (System.Math.Abs(dx) < Tolerance.Epsilon)
|
|
return double.MaxValue;
|
|
|
|
var t = (vx - p1x) / dx;
|
|
if (t < -Tolerance.Epsilon || t > 1.0 + Tolerance.Epsilon)
|
|
return double.MaxValue;
|
|
|
|
var iy = p1y + t * (p2y - p1y);
|
|
var dist = direction == PushDirection.Down ? vy - iy : iy - vy;
|
|
|
|
if (dist > Tolerance.Epsilon)
|
|
return dist;
|
|
if (dist >= -Tolerance.Epsilon)
|
|
return 0;
|
|
return double.MaxValue;
|
|
}
|
|
|
|
default:
|
|
return double.MaxValue;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Generalized ray-edge distance along an arbitrary unit direction vector.
|
|
/// Returns double.MaxValue if the ray does not hit the segment.
|
|
/// </summary>
|
|
[System.Runtime.CompilerServices.MethodImpl(
|
|
System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining
|
|
)]
|
|
public static double RayEdgeDistance(
|
|
double vx,
|
|
double vy,
|
|
double p1x,
|
|
double p1y,
|
|
double p2x,
|
|
double p2y,
|
|
double dirX,
|
|
double dirY
|
|
)
|
|
{
|
|
var ex = p2x - p1x;
|
|
var ey = p2y - p1y;
|
|
|
|
var det = ex * dirY - ey * dirX;
|
|
if (System.Math.Abs(det) < Tolerance.Epsilon)
|
|
return double.MaxValue;
|
|
|
|
var dvx = p1x - vx;
|
|
var dvy = p1y - vy;
|
|
|
|
var t = (ex * dvy - ey * dvx) / det;
|
|
if (t < -Tolerance.Epsilon)
|
|
return double.MaxValue;
|
|
|
|
var s = (dirX * dvy - dirY * dvx) / det;
|
|
if (s < -Tolerance.Epsilon || s > 1.0 + Tolerance.Epsilon)
|
|
return double.MaxValue;
|
|
|
|
if (t > Tolerance.Epsilon)
|
|
return t;
|
|
if (t >= -Tolerance.Epsilon)
|
|
return 0;
|
|
return double.MaxValue;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Solves ray-circle intersection, returning the two parametric t values.
|
|
/// Returns false if no real intersection exists.
|
|
/// </summary>
|
|
[System.Runtime.CompilerServices.MethodImpl(
|
|
System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining
|
|
)]
|
|
private static bool SolveRayCircle(
|
|
double vx,
|
|
double vy,
|
|
double cx,
|
|
double cy,
|
|
double r,
|
|
double dirX,
|
|
double dirY,
|
|
out double t1,
|
|
out double t2
|
|
)
|
|
{
|
|
var ox = vx - cx;
|
|
var oy = vy - cy;
|
|
|
|
var a = dirX * dirX + dirY * dirY;
|
|
var b = 2.0 * (ox * dirX + oy * dirY);
|
|
var c = ox * ox + oy * oy - r * r;
|
|
|
|
var discriminant = b * b - 4.0 * a * c;
|
|
if (discriminant < 0)
|
|
{
|
|
t1 = t2 = double.MaxValue;
|
|
return false;
|
|
}
|
|
|
|
var sqrtD = System.Math.Sqrt(discriminant);
|
|
var inv2a = 1.0 / (2.0 * a);
|
|
t1 = (-b - sqrtD) * inv2a;
|
|
t2 = (-b + sqrtD) * inv2a;
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes the distance from a point along a direction to an arc.
|
|
/// Solves ray-circle intersection, then constrains hits to the arc's
|
|
/// angular span. Returns double.MaxValue if no hit.
|
|
/// </summary>
|
|
[System.Runtime.CompilerServices.MethodImpl(
|
|
System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining
|
|
)]
|
|
public static double RayArcDistance(
|
|
double vx,
|
|
double vy,
|
|
double cx,
|
|
double cy,
|
|
double r,
|
|
double startAngle,
|
|
double endAngle,
|
|
bool reversed,
|
|
double dirX,
|
|
double dirY
|
|
)
|
|
{
|
|
if (!SolveRayCircle(vx, vy, cx, cy, r, dirX, dirY, out var t1, out var t2))
|
|
return double.MaxValue;
|
|
|
|
var best = double.MaxValue;
|
|
|
|
if (t1 > -Tolerance.Epsilon)
|
|
{
|
|
var hitAngle = Angle.NormalizeRad(
|
|
System.Math.Atan2(vy + t1 * dirY - cy, vx + t1 * dirX - cx)
|
|
);
|
|
if (Angle.IsBetweenRad(hitAngle, startAngle, endAngle, reversed))
|
|
best = t1 > Tolerance.Epsilon ? t1 : 0;
|
|
}
|
|
|
|
if (t2 > -Tolerance.Epsilon && t2 < best)
|
|
{
|
|
var hitAngle = Angle.NormalizeRad(
|
|
System.Math.Atan2(vy + t2 * dirY - cy, vx + t2 * dirX - cx)
|
|
);
|
|
if (Angle.IsBetweenRad(hitAngle, startAngle, endAngle, reversed))
|
|
best = t2 > Tolerance.Epsilon ? t2 : 0;
|
|
}
|
|
|
|
return best;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes the distance from a point along a direction to a full circle.
|
|
/// Returns double.MaxValue if no hit.
|
|
/// </summary>
|
|
[System.Runtime.CompilerServices.MethodImpl(
|
|
System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining
|
|
)]
|
|
public static double RayCircleDistance(
|
|
double vx,
|
|
double vy,
|
|
double cx,
|
|
double cy,
|
|
double r,
|
|
double dirX,
|
|
double dirY
|
|
)
|
|
{
|
|
if (!SolveRayCircle(vx, vy, cx, cy, r, dirX, dirY, out var t1, out var t2))
|
|
return double.MaxValue;
|
|
|
|
if (t1 > Tolerance.Epsilon)
|
|
return t1;
|
|
if (t1 >= -Tolerance.Epsilon)
|
|
return 0;
|
|
if (t2 > Tolerance.Epsilon)
|
|
return t2;
|
|
if (t2 >= -Tolerance.Epsilon)
|
|
return 0;
|
|
|
|
return double.MaxValue;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns the first external or internal tangency along a unit direction, or
|
|
/// double.MaxValue if none exists. Centers must be in the same world frame;
|
|
/// radii must be nonnegative. An optional arc constrains the contact angle
|
|
/// (null means a full circle); only its angular range is used.
|
|
/// Endpoint contacts and coincident equal-radius curves remain the caller's
|
|
/// vertex-to-entity responsibility, so this is not a complete collision test.
|
|
/// </summary>
|
|
public static double CurveTangencyDistance(
|
|
double movingCx,
|
|
double movingCy,
|
|
double movingRadius,
|
|
Arc movingArc,
|
|
double stationaryCx,
|
|
double stationaryCy,
|
|
double stationaryRadius,
|
|
Arc stationaryArc,
|
|
double dirX,
|
|
double dirY
|
|
)
|
|
{
|
|
var best = double.MaxValue;
|
|
for (var kind = 0; kind < 2; kind++)
|
|
{
|
|
var internalContact = kind == 1;
|
|
var radius = internalContact
|
|
? System.Math.Abs(movingRadius - stationaryRadius)
|
|
: movingRadius + stationaryRadius;
|
|
|
|
// Equal-radius internal contact has coincident centers, not a unique
|
|
// tangent point. Endpoints detect any overlap of those angular spans.
|
|
if (radius == 0)
|
|
continue;
|
|
|
|
if (!SolveRayCircle(
|
|
movingCx, movingCy, stationaryCx, stationaryCy, radius,
|
|
dirX, dirY, out var t1, out var t2))
|
|
continue;
|
|
|
|
// The nearer center-circle root can be outside an arc while the farther
|
|
// root is its first contact. Check the actual tangent point at BOTH roots.
|
|
for (var root = 0; root < 2; root++)
|
|
{
|
|
var t = root == 0 ? t1 : t2;
|
|
if (t < -Tolerance.Epsilon || t >= best)
|
|
continue;
|
|
|
|
var toX = stationaryCx - (movingCx + t * dirX);
|
|
var toY = stationaryCy - (movingCy + t * dirY);
|
|
var movingSign = internalContact && movingRadius < stationaryRadius ? -1 : 1;
|
|
var stationarySign = internalContact ? movingSign : -1;
|
|
if (!ContainsContactAngle(movingArc, movingRadius, movingSign * toX, movingSign * toY)
|
|
|| !ContainsContactAngle(stationaryArc, stationaryRadius, stationarySign * toX, stationarySign * toY))
|
|
continue;
|
|
|
|
best = t > Tolerance.Epsilon ? t : 0;
|
|
if (best == 0)
|
|
return 0;
|
|
}
|
|
}
|
|
return best;
|
|
}
|
|
|
|
private static bool ContainsContactAngle(Arc arc, double radius, double x, double y)
|
|
{
|
|
// A zero-radius curve is a point: its angular range has no geometric meaning.
|
|
if (arc == null || radius == 0)
|
|
return true;
|
|
var angle = Angle.NormalizeRad(System.Math.Atan2(y, x));
|
|
return Angle.IsBetweenRad(angle, arc.StartAngle, arc.EndAngle, arc.IsReversed);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes the translation distance along a push direction before any edge of
|
|
/// movingLines first blocks against an edge of stationaryLines. A contact that
|
|
/// the push slides along or leaves does not block (see <see cref="SlideContact"/>).
|
|
/// Returns double.MaxValue if no collision path exists.
|
|
/// </summary>
|
|
public static double DirectionalDistance(
|
|
List<Line> movingLines,
|
|
List<Line> stationaryLines,
|
|
PushDirection direction
|
|
)
|
|
{
|
|
return DirectionalDistance(movingLines, 0, 0, stationaryLines, direction);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes the directional distance with the moving lines translated
|
|
/// by (movingDx, movingDy) without creating new Line objects.
|
|
/// </summary>
|
|
public static double DirectionalDistance(
|
|
List<Line> movingLines,
|
|
double movingDx,
|
|
double movingDy,
|
|
List<Line> stationaryLines,
|
|
PushDirection direction
|
|
)
|
|
{
|
|
return DirectionalDistance(
|
|
movingLines,
|
|
movingDx,
|
|
movingDy,
|
|
stationaryLines,
|
|
direction,
|
|
SlideContactClassifier.FromLines(
|
|
movingLines,
|
|
new Vector(movingDx, movingDy),
|
|
stationaryLines,
|
|
Vector.Zero
|
|
)
|
|
);
|
|
}
|
|
|
|
/// <summary>
|
|
/// <see cref="DirectionalDistance(List{Line}, double, double, List{Line}, PushDirection)"/>
|
|
/// with caller-supplied contact topology, for inputs that are not complete closed
|
|
/// boundaries (for example direction-filtered edges).
|
|
/// </summary>
|
|
public static double DirectionalDistance(
|
|
List<Line> movingLines,
|
|
double movingDx,
|
|
double movingDy,
|
|
List<Line> stationaryLines,
|
|
PushDirection direction,
|
|
SlideContactClassifier contacts
|
|
)
|
|
{
|
|
var movingOffset = new Vector(movingDx, movingDy);
|
|
var movingVertices = CollectVertices(movingLines, movingOffset).ToArray();
|
|
|
|
var stationaryEdges = ToEdgeArray(stationaryLines);
|
|
SortEdgesForPruning(stationaryEdges, direction);
|
|
|
|
var stationaryVertices = CollectVertices(stationaryLines, Vector.Zero).ToArray();
|
|
|
|
var movingEdges = ToEdgeArray(movingLines);
|
|
SortEdgesForPruning(movingEdges, OppositeDirection(direction));
|
|
|
|
var source = new AxisSlideEvents(
|
|
movingEdges,
|
|
movingOffset,
|
|
movingVertices,
|
|
stationaryEdges,
|
|
Vector.Zero,
|
|
stationaryVertices,
|
|
direction
|
|
);
|
|
var unit = DirectionToOffset(direction, 1.0);
|
|
return SlideResolver.FirstBlocking(ref source, contacts, unit.X, unit.Y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Packs line segments into a flat double array [x1,y1,x2,y2, ...] for GPU transfer.
|
|
/// </summary>
|
|
public static double[] FlattenLines(List<Line> lines)
|
|
{
|
|
var result = new double[lines.Count * 4];
|
|
for (int i = 0; i < lines.Count; i++)
|
|
{
|
|
var line = lines[i];
|
|
result[i * 4] = line.pt1.X;
|
|
result[i * 4 + 1] = line.pt1.Y;
|
|
result[i * 4 + 2] = line.pt2.X;
|
|
result[i * 4 + 3] = line.pt2.Y;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes the blocking directional distance using raw edge arrays and location
|
|
/// offsets. Sorts both edge arrays in place for pruning.
|
|
/// </summary>
|
|
public static double DirectionalDistance(
|
|
(Vector start, Vector end)[] movingEdges,
|
|
Vector movingOffset,
|
|
(Vector start, Vector end)[] stationaryEdges,
|
|
Vector stationaryOffset,
|
|
PushDirection direction
|
|
)
|
|
{
|
|
return DirectionalDistance(
|
|
movingEdges,
|
|
movingOffset,
|
|
stationaryEdges,
|
|
stationaryOffset,
|
|
direction,
|
|
SlideContactClassifier.FromEdges(
|
|
movingEdges,
|
|
movingOffset,
|
|
stationaryEdges,
|
|
stationaryOffset
|
|
)
|
|
);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Edge-array overload with caller-supplied contact topology. The classifier's
|
|
/// origins must match <paramref name="movingOffset"/> and
|
|
/// <paramref name="stationaryOffset"/> in the frame of its boundaries.
|
|
/// </summary>
|
|
public static double DirectionalDistance(
|
|
(Vector start, Vector end)[] movingEdges,
|
|
Vector movingOffset,
|
|
(Vector start, Vector end)[] stationaryEdges,
|
|
Vector stationaryOffset,
|
|
PushDirection direction,
|
|
SlideContactClassifier contacts
|
|
)
|
|
{
|
|
SortEdgesForPruning(stationaryEdges, direction);
|
|
var movingVertices = CollectVertices(movingEdges, movingOffset).ToArray();
|
|
|
|
SortEdgesForPruning(movingEdges, OppositeDirection(direction));
|
|
var stationaryVertices = CollectVertices(stationaryEdges, stationaryOffset).ToArray();
|
|
|
|
var source = new AxisSlideEvents(
|
|
movingEdges,
|
|
movingOffset,
|
|
movingVertices,
|
|
stationaryEdges,
|
|
stationaryOffset,
|
|
stationaryVertices,
|
|
direction
|
|
);
|
|
var unit = DirectionToOffset(direction, 1.0);
|
|
return SlideResolver.FirstBlocking(ref source, contacts, unit.X, unit.Y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Nearest raw hit from one vertex along a push direction against edges sorted for
|
|
/// pruning. This is a first-touch primitive; it does not classify sliding contacts.
|
|
/// </summary>
|
|
public static double OneWayDistance(
|
|
Vector vertex,
|
|
(Vector start, Vector end)[] edges,
|
|
Vector edgeOffset,
|
|
PushDirection direction
|
|
)
|
|
{
|
|
var minDist = double.MaxValue;
|
|
var vx = vertex.X;
|
|
var vy = vertex.Y;
|
|
var horizontal = IsHorizontalDirection(direction);
|
|
|
|
// Pruning: edges are sorted by their perpendicular min-coordinate.
|
|
// For horizontal push, prune by Y range; for vertical push, prune by X range.
|
|
for (var i = 0; i < edges.Length; i++)
|
|
{
|
|
var e1 = edges[i].start + edgeOffset;
|
|
var e2 = edges[i].end + edgeOffset;
|
|
|
|
double perpValue,
|
|
edgeMin,
|
|
edgeMax;
|
|
if (horizontal)
|
|
{
|
|
perpValue = vy;
|
|
edgeMin = e1.Y < e2.Y ? e1.Y : e2.Y;
|
|
edgeMax = e1.Y > e2.Y ? e1.Y : e2.Y;
|
|
}
|
|
else
|
|
{
|
|
perpValue = vx;
|
|
edgeMin = e1.X < e2.X ? e1.X : e2.X;
|
|
edgeMax = e1.X > e2.X ? e1.X : e2.X;
|
|
}
|
|
|
|
// Since edges are sorted by edgeMin, if perpValue < edgeMin, all subsequent edges are also past.
|
|
if (perpValue < edgeMin - Tolerance.Epsilon)
|
|
break;
|
|
|
|
if (perpValue > edgeMax + Tolerance.Epsilon)
|
|
continue;
|
|
|
|
var d = RayEdgeDistance(vx, vy, e1.X, e1.Y, e2.X, e2.Y, direction);
|
|
if (d < minDist)
|
|
minDist = d;
|
|
}
|
|
|
|
return minDist;
|
|
}
|
|
|
|
public static PushDirection OppositeDirection(PushDirection direction)
|
|
{
|
|
switch (direction)
|
|
{
|
|
case PushDirection.Left:
|
|
return PushDirection.Right;
|
|
case PushDirection.Right:
|
|
return PushDirection.Left;
|
|
case PushDirection.Up:
|
|
return PushDirection.Down;
|
|
case PushDirection.Down:
|
|
return PushDirection.Up;
|
|
default:
|
|
return direction;
|
|
}
|
|
}
|
|
|
|
public static bool IsHorizontalDirection(PushDirection direction)
|
|
{
|
|
return direction is PushDirection.Left or PushDirection.Right;
|
|
}
|
|
|
|
public static double EdgeDistance(Box box, Box boundary, PushDirection direction)
|
|
{
|
|
switch (direction)
|
|
{
|
|
case PushDirection.Left:
|
|
return box.Left - boundary.Left;
|
|
case PushDirection.Right:
|
|
return boundary.Right - box.Right;
|
|
case PushDirection.Up:
|
|
return boundary.Top - box.Top;
|
|
case PushDirection.Down:
|
|
return box.Bottom - boundary.Bottom;
|
|
default:
|
|
return double.MaxValue;
|
|
}
|
|
}
|
|
|
|
public static Vector DirectionToOffset(PushDirection direction, double distance)
|
|
{
|
|
switch (direction)
|
|
{
|
|
case PushDirection.Left:
|
|
return new Vector(-distance, 0);
|
|
case PushDirection.Right:
|
|
return new Vector(distance, 0);
|
|
case PushDirection.Up:
|
|
return new Vector(0, distance);
|
|
case PushDirection.Down:
|
|
return new Vector(0, -distance);
|
|
default:
|
|
return new Vector();
|
|
}
|
|
}
|
|
|
|
public static double DirectionalGap(Box from, Box to, PushDirection direction)
|
|
{
|
|
switch (direction)
|
|
{
|
|
case PushDirection.Left:
|
|
return from.Left - to.Right;
|
|
case PushDirection.Right:
|
|
return to.Left - from.Right;
|
|
case PushDirection.Up:
|
|
return to.Bottom - from.Top;
|
|
case PushDirection.Down:
|
|
return from.Bottom - to.Top;
|
|
default:
|
|
return double.MaxValue;
|
|
}
|
|
}
|
|
|
|
#region Generalized direction (Vector) overloads
|
|
|
|
/// <summary>
|
|
/// Computes how far a box can travel along the given unit direction
|
|
/// before exiting the boundary box.
|
|
/// </summary>
|
|
public static double EdgeDistance(Box box, Box boundary, Vector direction)
|
|
{
|
|
var dist = double.MaxValue;
|
|
|
|
if (direction.X < -Tolerance.Epsilon)
|
|
{
|
|
var d = (box.Left - boundary.Left) / -direction.X;
|
|
if (d < dist)
|
|
dist = d;
|
|
}
|
|
else if (direction.X > Tolerance.Epsilon)
|
|
{
|
|
var d = (boundary.Right - box.Right) / direction.X;
|
|
if (d < dist)
|
|
dist = d;
|
|
}
|
|
|
|
if (direction.Y < -Tolerance.Epsilon)
|
|
{
|
|
var d = (box.Bottom - boundary.Bottom) / -direction.Y;
|
|
if (d < dist)
|
|
dist = d;
|
|
}
|
|
else if (direction.Y > Tolerance.Epsilon)
|
|
{
|
|
var d = (boundary.Top - box.Top) / direction.Y;
|
|
if (d < dist)
|
|
dist = d;
|
|
}
|
|
|
|
return dist < 0 ? 0 : dist;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes the directional gap between two boxes along an arbitrary unit direction.
|
|
/// Positive means 'to' is ahead of 'from' in the push direction.
|
|
/// </summary>
|
|
public static double DirectionalGap(Box from, Box to, Vector direction)
|
|
{
|
|
var fromMax = BoxProjectionMax(from, direction.X, direction.Y);
|
|
var toMin = BoxProjectionMin(to, direction.X, direction.Y);
|
|
return toMin - fromMax;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns true if two boxes overlap when projected onto the axis
|
|
/// perpendicular to the given unit direction.
|
|
/// </summary>
|
|
public static bool PerpendicularOverlap(Box a, Box b, Vector direction)
|
|
{
|
|
var px = -direction.Y;
|
|
var py = direction.X;
|
|
|
|
var aMin = BoxProjectionMin(a, px, py);
|
|
var aMax = BoxProjectionMax(a, px, py);
|
|
var bMin = BoxProjectionMin(b, px, py);
|
|
var bMax = BoxProjectionMax(b, px, py);
|
|
|
|
return aMin <= bMax + Tolerance.Epsilon && bMin <= aMax + Tolerance.Epsilon;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes the translation distance along an arbitrary unit direction before any
|
|
/// edge of movingLines first blocks against an edge of stationaryLines.
|
|
/// </summary>
|
|
public static double DirectionalDistance(
|
|
List<Line> movingLines,
|
|
List<Line> stationaryLines,
|
|
Vector direction
|
|
)
|
|
{
|
|
return DirectionalDistance(
|
|
movingLines,
|
|
stationaryLines,
|
|
direction,
|
|
SlideContactClassifier.FromLines(
|
|
movingLines,
|
|
Vector.Zero,
|
|
stationaryLines,
|
|
Vector.Zero
|
|
)
|
|
);
|
|
}
|
|
|
|
/// <summary>
|
|
/// <see cref="DirectionalDistance(List{Line}, List{Line}, Vector)"/> with
|
|
/// caller-supplied contact topology.
|
|
/// </summary>
|
|
public static double DirectionalDistance(
|
|
List<Line> movingLines,
|
|
List<Line> stationaryLines,
|
|
Vector direction,
|
|
SlideContactClassifier contacts
|
|
)
|
|
{
|
|
var source = new LineSlideEvents(
|
|
movingLines,
|
|
CollectVertices(movingLines, Vector.Zero).ToArray(),
|
|
0,
|
|
0,
|
|
stationaryLines,
|
|
CollectVertices(stationaryLines, Vector.Zero).ToArray(),
|
|
direction.X,
|
|
direction.Y
|
|
);
|
|
return SlideResolver.FirstBlocking(ref source, contacts, direction.X, direction.Y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes the minimum translation distance along a push direction
|
|
/// before any vertex/edge of movingEntities contacts any vertex/edge of
|
|
/// stationaryEntities. Delegates to the Vector-based overload.
|
|
/// </summary>
|
|
public static double DirectionalDistance(
|
|
List<Entity> movingEntities,
|
|
List<Entity> stationaryEntities,
|
|
PushDirection direction
|
|
)
|
|
{
|
|
return DirectionalDistance(
|
|
movingEntities,
|
|
stationaryEntities,
|
|
DirectionToOffset(direction, 1.0)
|
|
);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes the translation distance along an arbitrary unit direction before any
|
|
/// vertex/edge of movingEntities first blocks against stationaryEntities. Works with
|
|
/// native Line, Arc, and Circle entities without tessellation. A contact that the
|
|
/// push slides along or leaves does not block (see <see cref="SlideContact"/>).
|
|
/// </summary>
|
|
public static double DirectionalDistance(
|
|
List<Entity> movingEntities,
|
|
List<Entity> stationaryEntities,
|
|
Vector direction
|
|
)
|
|
{
|
|
return DirectionalDistance(
|
|
movingEntities,
|
|
stationaryEntities,
|
|
direction,
|
|
new SlideContactClassifier(movingEntities, stationaryEntities)
|
|
);
|
|
}
|
|
|
|
/// <summary>
|
|
/// <see cref="DirectionalDistance(List{Entity}, List{Entity}, Vector)"/> with
|
|
/// caller-supplied contact topology.
|
|
/// </summary>
|
|
public static double DirectionalDistance(
|
|
List<Entity> movingEntities,
|
|
List<Entity> stationaryEntities,
|
|
Vector direction,
|
|
SlideContactClassifier contacts
|
|
)
|
|
{
|
|
// Phases: vertex rays both ways, arc-to-line closest points (vertex sampling
|
|
// misses interior arc contact), then native curve tangency.
|
|
var source = new EntitySlideEvents(
|
|
movingEntities,
|
|
ExtractEntityVertices(movingEntities),
|
|
0,
|
|
0,
|
|
stationaryEntities,
|
|
ExtractEntityVertices(stationaryEntities),
|
|
direction.X,
|
|
direction.Y,
|
|
arcToLine: true
|
|
);
|
|
return SlideResolver.FirstBlocking(ref source, contacts, direction.X, direction.Y);
|
|
}
|
|
|
|
public static Vector[] ExtractEntityVertices(List<Entity> entities)
|
|
{
|
|
var vertices = new HashSet<Vector>();
|
|
|
|
for (var i = 0; i < entities.Count; i++)
|
|
{
|
|
var entity = entities[i];
|
|
|
|
if (entity is Line line)
|
|
{
|
|
vertices.Add(line.pt1);
|
|
vertices.Add(line.pt2);
|
|
}
|
|
else if (entity is Arc arc)
|
|
{
|
|
vertices.Add(arc.StartPoint());
|
|
vertices.Add(arc.EndPoint());
|
|
AddArcExtremeVertices(vertices, arc);
|
|
}
|
|
else if (entity is Circle circle)
|
|
{
|
|
vertices.Add(new Vector(circle.Center.X + circle.Radius, circle.Center.Y));
|
|
vertices.Add(new Vector(circle.Center.X - circle.Radius, circle.Center.Y));
|
|
vertices.Add(new Vector(circle.Center.X, circle.Center.Y + circle.Radius));
|
|
vertices.Add(new Vector(circle.Center.X, circle.Center.Y - circle.Radius));
|
|
}
|
|
}
|
|
|
|
return vertices.ToArray();
|
|
}
|
|
|
|
private static void AddArcExtremeVertices(HashSet<Vector> points, Arc arc)
|
|
{
|
|
var a1 = arc.StartAngle;
|
|
var a2 = arc.EndAngle;
|
|
|
|
if (arc.IsReversed)
|
|
Generic.Swap(ref a1, ref a2);
|
|
|
|
if (Angle.IsBetweenRad(Angle.TwoPI, a1, a2))
|
|
points.Add(new Vector(arc.Center.X + arc.Radius, arc.Center.Y));
|
|
if (Angle.IsBetweenRad(Angle.HalfPI, a1, a2))
|
|
points.Add(new Vector(arc.Center.X, arc.Center.Y + arc.Radius));
|
|
if (Angle.IsBetweenRad(System.Math.PI, a1, a2))
|
|
points.Add(new Vector(arc.Center.X - arc.Radius, arc.Center.Y));
|
|
if (Angle.IsBetweenRad(System.Math.PI * 1.5, a1, a2))
|
|
points.Add(new Vector(arc.Center.X, arc.Center.Y - arc.Radius));
|
|
}
|
|
|
|
private static HashSet<Vector> CollectVertices(List<Line> lines, Vector offset)
|
|
{
|
|
return CollectVertices(ToEdgeArray(lines), offset);
|
|
}
|
|
|
|
private static HashSet<Vector> CollectVertices(
|
|
(Vector start, Vector end)[] edges,
|
|
Vector offset
|
|
)
|
|
{
|
|
var vertices = new HashSet<Vector>();
|
|
for (var i = 0; i < edges.Length; i++)
|
|
{
|
|
vertices.Add(edges[i].start + offset);
|
|
vertices.Add(edges[i].end + offset);
|
|
}
|
|
return vertices;
|
|
}
|
|
|
|
private static (Vector start, Vector end)[] ToEdgeArray(List<Line> lines)
|
|
{
|
|
var edges = new (Vector start, Vector end)[lines.Count];
|
|
for (var i = 0; i < lines.Count; i++)
|
|
edges[i] = (lines[i].pt1, lines[i].pt2);
|
|
return edges;
|
|
}
|
|
|
|
private static void SortEdgesForPruning(
|
|
(Vector start, Vector end)[] edges,
|
|
PushDirection direction
|
|
)
|
|
{
|
|
if (direction == PushDirection.Left || direction == PushDirection.Right)
|
|
System.Array.Sort(
|
|
edges,
|
|
(a, b) =>
|
|
System
|
|
.Math.Min(a.start.Y, a.end.Y)
|
|
.CompareTo(System.Math.Min(b.start.Y, b.end.Y))
|
|
);
|
|
else
|
|
System.Array.Sort(
|
|
edges,
|
|
(a, b) =>
|
|
System
|
|
.Math.Min(a.start.X, a.end.X)
|
|
.CompareTo(System.Math.Min(b.start.X, b.end.X))
|
|
);
|
|
}
|
|
|
|
private static double BoxProjectionMin(Box box, double dx, double dy)
|
|
{
|
|
var x = dx >= 0 ? box.Left : box.Right;
|
|
var y = dy >= 0 ? box.Bottom : box.Top;
|
|
return x * dx + y * dy;
|
|
}
|
|
|
|
private static double BoxProjectionMax(Box box, double dx, double dy)
|
|
{
|
|
var x = dx >= 0 ? box.Right : box.Left;
|
|
var y = dy >= 0 ? box.Top : box.Bottom;
|
|
return x * dx + y * dy;
|
|
}
|
|
|
|
#endregion
|
|
|
|
public static Box GetLargestBoxVertically(Vector pt, Box bounds, IEnumerable<Box> boxes)
|
|
{
|
|
var verticalBoxes = boxes.Where(b => !(b.Left > pt.X || b.Right < pt.X)).ToList();
|
|
|
|
if (!FindVerticalLimits(pt, bounds, verticalBoxes, out var top, out var btm))
|
|
return Box.Empty;
|
|
|
|
var horizontalBoxes = boxes.Where(b => !(b.Bottom >= top || b.Top <= btm)).ToList();
|
|
|
|
if (!FindHorizontalLimits(pt, bounds, horizontalBoxes, out var lft, out var rgt))
|
|
return Box.Empty;
|
|
|
|
return new Box(lft, btm, rgt - lft, top - btm);
|
|
}
|
|
|
|
public static Box GetLargestBoxHorizontally(Vector pt, Box bounds, IEnumerable<Box> boxes)
|
|
{
|
|
var horizontalBoxes = boxes.Where(b => !(b.Bottom > pt.Y || b.Top < pt.Y)).ToList();
|
|
|
|
if (!FindHorizontalLimits(pt, bounds, horizontalBoxes, out var lft, out var rgt))
|
|
return Box.Empty;
|
|
|
|
var verticalBoxes = boxes.Where(b => !(b.Left >= rgt || b.Right <= lft)).ToList();
|
|
|
|
if (!FindVerticalLimits(pt, bounds, verticalBoxes, out var top, out var btm))
|
|
return Box.Empty;
|
|
|
|
return new Box(lft, btm, rgt - lft, top - btm);
|
|
}
|
|
|
|
private static bool FindVerticalLimits(
|
|
Vector pt,
|
|
Box bounds,
|
|
List<Box> boxes,
|
|
out double top,
|
|
out double btm
|
|
)
|
|
{
|
|
top = double.MaxValue;
|
|
btm = double.MinValue;
|
|
|
|
foreach (var box in boxes)
|
|
{
|
|
var boxBtm = box.Bottom;
|
|
var boxTop = box.Top;
|
|
|
|
if (boxBtm > pt.Y && boxBtm < top)
|
|
top = boxBtm;
|
|
else if (box.Top < pt.Y && boxTop > btm)
|
|
btm = boxTop;
|
|
}
|
|
|
|
if (top == double.MaxValue)
|
|
{
|
|
if (bounds.Top > pt.Y)
|
|
top = bounds.Top;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
if (btm == double.MinValue)
|
|
{
|
|
if (bounds.Bottom < pt.Y)
|
|
btm = bounds.Bottom;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
private static bool FindHorizontalLimits(
|
|
Vector pt,
|
|
Box bounds,
|
|
List<Box> boxes,
|
|
out double lft,
|
|
out double rgt
|
|
)
|
|
{
|
|
lft = double.MinValue;
|
|
rgt = double.MaxValue;
|
|
|
|
foreach (var box in boxes)
|
|
{
|
|
var boxLft = box.Left;
|
|
var boxRgt = box.Right;
|
|
|
|
if (boxLft > pt.X && boxLft < rgt)
|
|
rgt = boxLft;
|
|
else if (boxRgt < pt.X && boxRgt > lft)
|
|
lft = boxRgt;
|
|
}
|
|
|
|
if (rgt == double.MaxValue)
|
|
{
|
|
if (bounds.Right > pt.X)
|
|
rgt = bounds.Right;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
if (lft == double.MinValue)
|
|
{
|
|
if (bounds.Left < pt.X)
|
|
lft = bounds.Left;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
}
|
|
}
|