aj/OpenNest
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

fix(geometry): resolve blocking contacts across directional slide paths

Browse Source
This commit is contained in:
aj
2026-09-27 23:24:31 -04:00
parent a04460c57b
commit 2b78fb3a75
15 changed files with 2736 additions and 945 deletions
Download Patch File Download Diff File Expand all files Collapse all files
OpenNest.Core/Geometry/SlideContact.cs
+831
View File
@@ -0,0 +1,831 @@
using System.Collections.Generic;
using OpenNest.Math;
namespace OpenNest.Geometry
{
internal enum ContactSide
{
/// <summary>The boundary could not be decomposed into closed loops.</summary>
Unresolved,
/// <summary>The point is not on the boundary: a tolerance near-miss, not a contact.</summary>
Off,
/// <summary>Several boundary runs meet here, or the corner is a cusp or spike.</summary>
Ambiguous,
/// <summary>The material sector is known.</summary>
Sector,
}
/// <summary>
/// Closed boundary loops of one entity list, prepared so a directional slide can tell
/// which side of each boundary point is material. Immutable after
/// <see cref="Prepare"/>, so one instance may be shared by concurrent queries.
/// </summary>
/// <remarks>
/// Loops are recovered from contiguous runs whose end points chain back to their start
/// (the order produced by <see cref="ShapeBuilder"/> and the offset helpers). Nesting
/// depth decides holes: material is inside even-depth loops and outside odd-depth ones.
/// When the list cannot be decomposed that way, every contact query is unresolved.
/// </remarks>
public sealed class SlideContactGeometry
{
// Contact points are computed from unsnapped ray parameters, so a genuine contact is
// on both boundaries to floating-point accuracy. This also bounds the overlap sliver a
// tangential classification can admit, so keep it far below spacing tolerances.
internal const double IncidenceTolerance = 1e-7;
private readonly List<Entity> entities;
private readonly int[] loopOf;
private readonly int[] previous;
private readonly int[] following;
private readonly bool[] materialLeft;
private SlideContactGeometry(
List<Entity> entities,
int[] loopOf,
int[] previous,
int[] following,
bool[] materialLeft
)
{
this.entities = entities;
this.loopOf = loopOf;
this.previous = previous;
this.following = following;
this.materialLeft = materialLeft;
}
/// <summary>True when every entity belongs to a closed loop with a known material side.</summary>
public bool IsResolved => materialLeft != null;
public static SlideContactGeometry Prepare(List<Entity> entities)
{
var count = entities.Count;
var loopOf = new int[count];
var previous = new int[count];
var following = new int[count];
var loops = new List<(int First, int Last)>();
var i = 0;
while (i < count)
{
var first = i;
if (entities[i] is Circle)
{
i++;
}
else
{
if (!TryEndpoints(entities[i], out var start, out _))
return Unresolved(entities);
var closed = false;
while (i < count && TryEndpoints(entities[i], out _, out var end))
{
// A lone closed arc is a loop; a lone line cannot be, even when it
// has zero length and so ends where it starts.
if (Near(end, start) && (i > first || entities[i] is Arc))
{
closed = true;
i++;
break;
}
if (
i + 1 >= count
|| !TryEndpoints(entities[i + 1], out var nextStart, out _)
|| !Near(nextStart, end)
)
break;
i++;
}
if (!closed)
return Unresolved(entities);
}
var loop = loops.Count;
loops.Add((first, i - 1));
for (var k = first; k < i; k++)
{
loopOf[k] = loop;
previous[k] = k == first ? i - 1 : k - 1;
following[k] = k == i - 1 ? first : k + 1;
}
}
var materialLeft = new bool[loops.Count];
for (var loop = 0; loop < loops.Count; loop++)
{
var area = SignedArea(entities, loops[loop].First, loops[loop].Last);
if (System.Math.Abs(area) <= Tolerance.Epsilon)
return Unresolved(entities);
var depth = 0;
if (loops.Count > 1)
{
var sample = SamplePoint(entities[loops[loop].First]);
for (var other = 0; other < loops.Count; other++)
{
if (other == loop)
continue;
if (Contains(entities, loops[other].First, loops[other].Last, sample))
depth++;
}
}
materialLeft[loop] = (area > 0) == (depth % 2 == 0);
}
return new SlideContactGeometry(entities, loopOf, previous, following, materialLeft);
}
private static SlideContactGeometry Unresolved(List<Entity> entities) =>
new SlideContactGeometry(entities, null, null, null, null);
/// <summary>
/// Material directions at a boundary point: an angular sector starting at
/// <paramref name="start"/> and sweeping CCW by <paramref name="width"/>.
/// Concavity is recorded separately at each sector ray: only the supporting
/// curve, not an unrelated curve at that corner, can block a tangential slide.
/// Entities wholly inside the incidence tolerance are treated as part of the corner.
/// </summary>
internal ContactSide GetMaterialSector(
Vector point,
out double start,
out double width,
out bool startConcave,
out bool endConcave
)
{
start = width = 0;
startConcave = endConcave = false;
if (materialLeft == null)
return ContactSide.Unresolved;
var best = -1;
var bestDistance = double.MaxValue;
for (var i = 0; i < entities.Count; i++)
{
var distance = DistanceTo(entities[i], point);
if (distance < bestDistance)
{
bestDistance = distance;
best = i;
}
}
if (best < 0 || bestDistance > IncidenceTolerance)
return ContactSide.Off;
// Walk to the entities that enter and leave the tolerance disc.
var loopLength = LoopLength(best);
var incoming = best;
var steps = 0;
var smoothLoop = loopLength == 1 && (entities[best] is Circle
|| entities[best] is Arc fullArc && fullArc.IsFullCircle());
while (!smoothLoop && StartsNear(incoming, point))
{
incoming = previous[incoming];
if (++steps >= loopLength)
return ContactSide.Ambiguous;
}
var outgoing = best;
steps = 0;
while (!smoothLoop && EndsNear(outgoing, point))
{
outgoing = following[outgoing];
if (++steps >= loopLength)
return ContactSide.Ambiguous;
}
// Anything else touching this point (another loop, a spike, a self-crossing)
// makes the local material side ambiguous.
for (var i = 0; i < entities.Count; i++)
{
if (InRun(i, incoming, outgoing))
continue;
if (DistanceTo(entities[i], point) <= IncidenceTolerance)
return ContactSide.Ambiguous;
}
var interior = incoming == best && outgoing == best && !EndsNear(best, point);
var inTangent = interior ? TangentAt(entities[best], point) : EndTangent(entities[incoming]);
var outTangent = interior
? inTangent
: StartTangent(entities[outgoing]);
// A circle has no endpoints, so its point is always interior.
if (smoothLoop)
inTangent = outTangent = TangentAt(entities[best], point);
if (IsZero(inTangent) || IsZero(outTangent))
return ContactSide.Ambiguous;
var outAngle = System.Math.Atan2(outTangent.Y, outTangent.X);
var inAngle = System.Math.Atan2(-inTangent.Y, -inTangent.X);
var left = materialLeft[loopOf[best]];
start = left ? outAngle : inAngle;
width = Angle.NormalizeRad((left ? inAngle : outAngle) - start);
startConcave = IsConcave(entities[left ? outgoing : incoming], left);
endConcave = IsConcave(entities[left ? incoming : outgoing], left);
return
width > SlideContact.AngleTolerance
&& width < Angle.TwoPI - 2 * SlideContact.SplitOverlap
? ContactSide.Sector
: ContactSide.Ambiguous;
}
private int LoopLength(int index)
{
var length = 1;
for (var i = following[index]; i != index; i = following[i])
length++;
return length;
}
private bool StartsNear(int index, Vector point) =>
TryEndpoints(entities[index], out var start, out _)
&& start.DistanceTo(point) <= IncidenceTolerance;
private bool EndsNear(int index, Vector point) =>
TryEndpoints(entities[index], out _, out var end)
&& end.DistanceTo(point) <= IncidenceTolerance;
private bool InRun(int index, int first, int last)
{
for (var i = first; ; i = following[i])
{
if (i == index)
return true;
if (i == last)
return false;
}
}
private static bool IsZero(Vector v) => v.X == 0 && v.Y == 0;
private static bool IsConcave(Entity entity, bool materialLeft)
{
// A CCW curve has its center on its left; that center is on the free side
// (a concave boundary) exactly when material is on the right.
return entity switch
{
Arc arc => materialLeft == arc.IsReversed,
Circle circle => materialLeft == (circle.Rotation == RotationType.CW),
_ => false,
};
}
private static Vector StartTangent(Entity entity) =>
entity switch
{
Line line => Direction(line.pt1, line.pt2),
Arc arc => ArcTangent(arc.StartAngle, arc.IsReversed),
_ => new Vector(),
};
private static Vector EndTangent(Entity entity) =>
entity switch
{
Line line => Direction(line.pt1, line.pt2),
Arc arc => ArcTangent(arc.EndAngle, arc.IsReversed),
_ => new Vector(),
};
private static Vector TangentAt(Entity entity, Vector point) =>
entity switch
{
Line line => Direction(line.pt1, line.pt2),
Arc arc => ArcTangent(arc.Center.AngleTo(point), arc.IsReversed),
Circle circle => ArcTangent(
circle.Center.AngleTo(point),
circle.Rotation == RotationType.CW
),
_ => new Vector(),
};
private static Vector ArcTangent(double angle, bool clockwise)
{
var sign = clockwise ? -1.0 : 1.0;
return new Vector(-System.Math.Sin(angle) * sign, System.Math.Cos(angle) * sign);
}
private static Vector Direction(Vector from, Vector to)
{
var dx = to.X - from.X;
var dy = to.Y - from.Y;
var length = System.Math.Sqrt(dx * dx + dy * dy);
return length > 0 ? new Vector(dx / length, dy / length) : new Vector();
}
private static double DistanceTo(Entity entity, Vector point)
{
switch (entity)
{
case Line line:
return point.DistanceTo(line.ClosestPointTo(point));
case Arc arc:
{
var angle = arc.Center.AngleTo(point);
if (Angle.IsBetweenRad(angle, arc.StartAngle, arc.EndAngle, arc.IsReversed))
return System.Math.Abs(arc.Center.DistanceTo(point) - arc.Radius);
return System.Math.Min(
point.DistanceTo(arc.StartPoint()),
point.DistanceTo(arc.EndPoint())
);
}
case Circle circle:
return System.Math.Abs(circle.Center.DistanceTo(point) - circle.Radius);
default:
return double.MaxValue;
}
}
private static bool TryEndpoints(Entity entity, out Vector start, out Vector end)
{
switch (entity)
{
case Line line:
start = line.pt1;
end = line.pt2;
return true;
case Arc arc:
start = arc.StartPoint();
end = arc.EndPoint();
return true;
default:
start = end = new Vector();
return false;
}
}
private static bool Near(Vector a, Vector b) => a.DistanceTo(b) <= IncidenceTolerance;
private static double SignedArea(List<Entity> entities, int first, int last)
{
var area = 0.0;
for (var i = first; i <= last; i++)
{
switch (entities[i])
{
case Circle circle:
var sign = circle.Rotation == RotationType.CW ? -1 : 1;
area += sign * System.Math.PI * circle.Radius * circle.Radius;
break;
case Line line:
area += Cross(line.pt1, line.pt2) / 2;
break;
case Arc arc:
var sweep = arc.IsReversed ? -arc.SweepAngle() : arc.SweepAngle();
var r = arc.Radius;
area += Cross(arc.StartPoint(), arc.EndPoint()) / 2;
area += r * r / 2 * (sweep - System.Math.Sin(sweep));
break;
}
}
return area;
}
private static double Cross(Vector a, Vector b) => a.X * b.Y - b.X * a.Y;
private static Vector SamplePoint(Entity entity) =>
entity switch
{
Circle circle => new Vector(circle.Center.X + circle.Radius, circle.Center.Y),
Arc arc => arc.StartPoint(),
Line line => line.pt1,
_ => new Vector(),
};
// Exact horizontal-ray parity. Split arcs at Y extrema so every piece is
// monotone; the same half-open endpoint rule as lines avoids seam double counts.
// A coarse inscribed polygon can misclassify thin rings as solid material.
private static bool Contains(List<Entity> entities, int first, int last, Vector point)
{
var inside = false;
for (var i = first; i <= last; i++)
{
if (entities[i] is Circle circle)
return circle.Center.DistanceTo(point) < circle.Radius;
if (entities[i] is Line line)
{
var a = line.pt1;
var b = line.pt2;
if ((a.Y > point.Y) != (b.Y > point.Y)
&& point.X < (b.X - a.X) * (point.Y - a.Y) / (b.Y - a.Y) + a.X)
inside = !inside;
}
else if (entities[i] is Arc arc)
{
var sweep = arc.SweepAngle();
var sign = arc.IsReversed ? -1.0 : 1.0;
var cuts = new List<double> { 0, sweep };
foreach (var extreme in new[] { Angle.HalfPI, 3 * Angle.HalfPI })
{
var t = Angle.NormalizeRad(sign * (extreme - arc.StartAngle));
if (t > 0 && t < sweep)
cuts.Add(t);
}
cuts.Sort();
for (var k = 1; k < cuts.Count; k++)
{
var a = arc.StartAngle + sign * cuts[k - 1];
var b = arc.StartAngle + sign * cuts[k];
var y1 = arc.Center.Y + arc.Radius * System.Math.Sin(a);
var y2 = arc.Center.Y + arc.Radius * System.Math.Sin(b);
if ((y1 > point.Y) == (y2 > point.Y))
continue;
var dy = point.Y - arc.Center.Y;
var dx = System.Math.Sqrt(System.Math.Max(0, arc.Radius * arc.Radius - dy * dy));
var x = arc.Center.X + (System.Math.Cos((a + b) / 2) >= 0 ? dx : -dx);
if (point.X < x)
inside = !inside;
}
}
}
return inside;
}
}
/// <summary>
/// Contact classifier for one moving/stationary pair of boundaries. Geometry is prepared
/// on first use, so a slide whose nearest contact is never classified pays nothing; call
/// <see cref="Prepare"/> before sharing one instance across threads. Each boundary is
/// given in its own frame; the origins place those frames in the world coordinates used
/// by slide events.
/// </summary>
public sealed class SlideContactClassifier
{
private readonly System.Func<List<Entity>> movingSource;
private readonly System.Func<List<Entity>> stationarySource;
private SlideContactGeometry moving;
private SlideContactGeometry stationary;
public SlideContactClassifier(List<Entity> movingEntities, List<Entity> stationaryEntities)
: this(movingEntities, Vector.Zero, stationaryEntities, Vector.Zero) { }
public SlideContactClassifier(
List<Entity> movingEntities,
Vector movingOrigin,
List<Entity> stationaryEntities,
Vector stationaryOrigin
)
: this(() => movingEntities, movingOrigin, () => stationaryEntities, stationaryOrigin)
{ }
public SlideContactClassifier(
SlideContactGeometry moving,
Vector movingOrigin,
SlideContactGeometry stationary,
Vector stationaryOrigin
)
{
this.moving = moving;
this.stationary = stationary;
MovingOrigin = movingOrigin;
StationaryOrigin = stationaryOrigin;
}
private SlideContactClassifier(
System.Func<List<Entity>> movingSource,
Vector movingOrigin,
System.Func<List<Entity>> stationarySource,
Vector stationaryOrigin
)
{
this.movingSource = movingSource;
this.stationarySource = stationarySource;
MovingOrigin = movingOrigin;
StationaryOrigin = stationaryOrigin;
}
public Vector MovingOrigin { get; }
public Vector StationaryOrigin { get; }
public static SlideContactClassifier FromLines(
List<Line> movingLines,
Vector movingOrigin,
List<Line> stationaryLines,
Vector stationaryOrigin
) =>
new SlideContactClassifier(
() => new List<Entity>(movingLines),
movingOrigin,
() => new List<Entity>(stationaryLines),
stationaryOrigin
);
public static SlideContactClassifier FromEdges(
(Vector start, Vector end)[] movingEdges,
Vector movingOrigin,
(Vector start, Vector end)[] stationaryEdges,
Vector stationaryOrigin
)
{
// The kernel sorts edge arrays in place, so snapshot the chain order now.
var moving = ((Vector start, Vector end)[])movingEdges.Clone();
var stationary = ((Vector start, Vector end)[])stationaryEdges.Clone();
return new SlideContactClassifier(
() => ToLines(moving),
movingOrigin,
() => ToLines(stationary),
stationaryOrigin
);
}
private static List<Entity> ToLines((Vector start, Vector end)[] edges)
{
var lines = new List<Entity>(edges.Length);
foreach (var (start, end) in edges)
lines.Add(new Line(start, end));
// Public edge arrays are sorted in place by previous queries. Recover their
// chains on private line objects; never reverse or reorder caller geometry.
var ordered = new List<Entity>(lines.Count);
foreach (var shape in ShapeBuilder.GetShapes(lines))
ordered.AddRange(shape.Entities);
return ordered;
}
public SlideContactClassifier Prepare()
{
moving ??= SlideContactGeometry.Prepare(movingSource?.Invoke() ?? new List<Entity>());
stationary ??= SlideContactGeometry.Prepare(
stationarySource?.Invoke() ?? new List<Entity>()
);
return this;
}
/// <summary>The same prepared boundaries placed at other origins.</summary>
public SlideContactClassifier At(Vector movingOrigin, Vector stationaryOrigin)
{
Prepare();
return new SlideContactClassifier(moving, movingOrigin, stationary, stationaryOrigin);
}
/// <summary>
/// True when moving along (dirX, dirY) from this world-space contact would push
/// material into material, or the contact cannot be classified.
/// </summary>
public bool Blocks(Vector movingPoint, Vector stationaryPoint, double dirX, double dirY)
{
Prepare();
return SlideContact.Blocks(
moving,
movingPoint - MovingOrigin,
stationary,
stationaryPoint - StationaryOrigin,
dirX,
dirY
);
}
}
/// <summary>Receives candidate contact events from a directional slide query.</summary>
public interface ISlideEventSink
{
/// <summary>True once further events cannot change this sink's result.</summary>
bool IsDone { get; }
/// <param name="distance">Travel to the contact, snapped to zero within Tolerance.Epsilon.</param>
/// <param name="movingPoint">Contact on the moving boundary, at its start position.</param>
/// <param name="stationaryPoint">Contact on the stationary boundary.</param>
void Add(double distance, Vector movingPoint, Vector stationaryPoint);
}
/// <summary>
/// Enumerates every candidate contact of one slide. Must yield the same events each
/// time it is enumerated.
/// </summary>
public interface ISlideEventSource
{
void Enumerate<TSink>(ref TSink sink)
where TSink : struct, ISlideEventSink;
}
/// <summary>Keeps the nearest event; stops at a contact that is already touching.</summary>
public struct NearestSlideEvent : ISlideEventSink
{
public bool Found;
public double Distance;
public Vector MovingPoint;
public Vector StationaryPoint;
public bool IsDone => Found && Distance <= 0;
public void Add(double distance, Vector movingPoint, Vector stationaryPoint)
{
if (Found && distance >= Distance)
return;
Found = true;
Distance = distance;
MovingPoint = movingPoint;
StationaryPoint = stationaryPoint;
}
}
internal struct SlideEventList : ISlideEventSink
{
public List<(double Distance, Vector MovingPoint, Vector StationaryPoint)> Events;
public bool IsDone => false;
public void Add(double distance, Vector movingPoint, Vector stationaryPoint) =>
Events.Add((distance, movingPoint, stationaryPoint));
}
public static class SlideResolver
{
/// <summary>
/// Travel to the first contact that blocks the slide, or double.MaxValue. When the
/// nearest contact blocks (every contact, for unresolved boundaries), the result is
/// exactly the nearest event distance and the events are enumerated once.
/// </summary>
public static double FirstBlocking<TSource>(
ref TSource source,
SlideContactClassifier contacts,
double dirX,
double dirY
)
where TSource : struct, ISlideEventSource
{
var nearest = new NearestSlideEvent();
source.Enumerate(ref nearest);
if (!nearest.Found)
return double.MaxValue;
if (contacts.Blocks(nearest.MovingPoint, nearest.StationaryPoint, dirX, dirY))
return nearest.Distance;
var all = new SlideEventList
{
Events = new List<(double, Vector, Vector)>(),
};
source.Enumerate(ref all);
all.Events.Sort((a, b) => a.Distance.CompareTo(b.Distance));
foreach (var (distance, movingPoint, stationaryPoint) in all.Events)
{
if (contacts.Blocks(movingPoint, stationaryPoint, dirX, dirY))
return distance;
}
return double.MaxValue;
}
}
/// <summary>
/// Decides whether a first-contact event found by a directional slide stops the slide.
/// </summary>
/// <remarks>
/// Parts that already touch may slide along each other or apart. Only a direction that
/// would create positive-area overlap blocks: with S the stationary material sector and
/// M the moving one at the contact point, that is the open Minkowski cone S ⊕ −M.
/// A direction on that cone's boundary is a tangential slide; it blocks only when an
/// incident curve is concave, because the second-order bend then closes the gap.
/// Unresolved or ambiguous topology blocks, which is the previous behavior for every
/// contact.
/// </remarks>
public static class SlideContact
{
internal const double AngleTolerance = 1e-7;
// Reflex sectors are split into two overlapping convex halves; the overlap keeps
// the split ray in the interior of the union.
internal const double SplitOverlap = 1e-3;
/// <summary>
/// True when moving along (dirX, dirY) from this contact would push material into
/// material, or when the contact cannot be classified. False for a near-miss whose
/// point is not on both boundaries.
/// </summary>
/// <param name="movingPoint">Contact point in the moving entities' own frame.</param>
/// <param name="stationaryPoint">The same contact in the stationary frame.</param>
public static bool Blocks(
SlideContactGeometry moving,
Vector movingPoint,
SlideContactGeometry stationary,
Vector stationaryPoint,
double dirX,
double dirY
)
{
if (moving == null || stationary == null)
return true;
var stationarySide = stationary.GetMaterialSector(
stationaryPoint,
out var stationaryStart,
out var stationaryWidth,
out var stationaryStartConcave,
out var stationaryEndConcave
);
var movingSide = moving.GetMaterialSector(
movingPoint,
out var movingStart,
out var movingWidth,
out var movingStartConcave,
out var movingEndConcave
);
if (stationarySide == ContactSide.Unresolved || movingSide == ContactSide.Unresolved)
return true;
// Ray tolerances report hits slightly beyond an entity's end; such a point is
// not on the other boundary, so the parts pass without touching there.
if (stationarySide == ContactSide.Off || movingSide == ContactSide.Off)
return false;
if (stationarySide == ContactSide.Ambiguous || movingSide == ContactSide.Ambiguous)
return true;
var direction = System.Math.Atan2(dirY, dirX);
var stationaryPieces = Split(stationaryStart, stationaryWidth);
var movingPieces = Split(movingStart + System.Math.PI, movingWidth);
var onBoundary = false;
foreach (var s in stationaryPieces)
{
foreach (var m in movingPieces)
{
if (!TryHull(s, m, out var hullStart, out var hullWidth))
return true;
var offset = Angle.NormalizeRad(direction - hullStart);
if (offset > AngleTolerance && offset < hullWidth - AngleTolerance)
return true;
if (
offset <= AngleTolerance
|| offset >= Angle.TwoPI - AngleTolerance
|| System.Math.Abs(offset - hullWidth) <= AngleTolerance
)
onBoundary = true;
}
}
return onBoundary && (
stationaryStartConcave && SameRay(direction, stationaryStart)
|| stationaryEndConcave && SameRay(direction, stationaryStart + stationaryWidth)
|| movingStartConcave && SameRay(direction, movingStart + System.Math.PI)
|| movingEndConcave && SameRay(direction, movingStart + movingWidth + System.Math.PI));
}
private static bool SameRay(double a, double b)
{
var offset = Angle.NormalizeRad(a - b);
return offset <= AngleTolerance || offset >= Angle.TwoPI - AngleTolerance;
}
private static (double Start, double Width)[] Split(double start, double width)
{
if (width <= System.Math.PI + AngleTolerance)
return new[] { (start, width) };
var half = width / 2;
return new[]
{
(start, half + SplitOverlap),
(start + half - SplitOverlap, half + SplitOverlap),
};
}
/// <summary>
/// Convex cone generated by two convex sectors. False when it is the whole plane.
/// </summary>
private static bool TryHull(
(double Start, double Width) a,
(double Start, double Width) b,
out double start,
out double width
)
{
var fromA = System.Math.Max(a.Width, Angle.NormalizeRad(b.Start - a.Start) + b.Width);
var fromB = System.Math.Max(b.Width, Angle.NormalizeRad(a.Start - b.Start) + a.Width);
if (fromA <= fromB)
{
start = a.Start;
width = fromA;
}
else
{
start = b.Start;
width = fromB;
}
return width <= System.Math.PI + AngleTolerance;
}
}
}
OpenNest.Core/Geometry/SlideEvents.cs
+806
View File
@@ -0,0 +1,806 @@
using System.Collections.Generic;
using OpenNest.Math;
namespace OpenNest.Geometry
{
/// <summary>
/// Candidate contact events of directional slides. Each emitter reports every forward
/// hit its distance kernel considers, with the distance snapped exactly as that kernel
/// snaps it, so the nearest event equals the kernel's historical minimum.
/// </summary>
internal static class SlideEvents
{
private const double Eps = Tolerance.Epsilon;
private static double Snap(double t) => t > Eps ? t : 0;
/// <summary>
/// Ray from a vertex against one entity. When <paramref name="vertexMoves"/> is true
/// the vertex belongs to the moving boundary and the ray follows the push direction;
/// otherwise it is a stationary vertex and the ray runs opposite to the push.
/// </summary>
public static void Ray<TSink>(
ref TSink sink,
double vx,
double vy,
Entity entity,
double entityDx,
double entityDy,
double rayX,
double rayY,
bool vertexMoves
)
where TSink : struct, ISlideEventSink
{
switch (entity)
{
case Line line:
RayLine(
ref sink,
vx,
vy,
line.pt1.X + entityDx,
line.pt1.Y + entityDy,
line.pt2.X + entityDx,
line.pt2.Y + entityDy,
rayX,
rayY,
vertexMoves
);
break;
case Arc arc:
{
var cx = arc.Center.X + entityDx;
var cy = arc.Center.Y + entityDy;
if (!SolveRayCircle(vx, vy, cx, cy, arc.Radius, rayX, rayY, out var t1, out var t2))
return;
for (var k = 0; k < 2; k++)
{
var t = k == 0 ? t1 : t2;
if (t <= -Eps)
continue;
var hitAngle = Angle.NormalizeRad(
System.Math.Atan2(vy + t * rayY - cy, vx + t * rayX - cx)
);
if (!Angle.IsBetweenRad(hitAngle, arc.StartAngle, arc.EndAngle, arc.IsReversed))
continue;
Emit(ref sink, vx, vy, t, rayX, rayY, vertexMoves);
if (sink.IsDone)
return;
}
break;
}
case Circle circle:
{
if (
!SolveRayCircle(
vx,
vy,
circle.Center.X + entityDx,
circle.Center.Y + entityDy,
circle.Radius,
rayX,
rayY,
out var t1,
out var t2
)
)
return;
for (var k = 0; k < 2; k++)
{
var t = k == 0 ? t1 : t2;
if (t < -Eps)
continue;
Emit(ref sink, vx, vy, t, rayX, rayY, vertexMoves);
if (sink.IsDone)
return;
}
break;
}
}
}
/// <summary>Same hit rule as <see cref="SpatialQuery.RayEdgeDistance(double, double, double, double, double, double, double, double)"/>.</summary>
public static void RayLine<TSink>(
ref TSink sink,
double vx,
double vy,
double p1x,
double p1y,
double p2x,
double p2y,
double rayX,
double rayY,
bool vertexMoves
)
where TSink : struct, ISlideEventSink
{
var ex = p2x - p1x;
var ey = p2y - p1y;
var det = ex * rayY - ey * rayX;
if (System.Math.Abs(det) < Eps)
return;
var dvx = p1x - vx;
var dvy = p1y - vy;
var t = (ex * dvy - ey * dvx) / det;
if (t < -Eps)
return;
var s = (rayX * dvy - rayY * dvx) / det;
if (s < -Eps || s > 1.0 + Eps)
return;
Emit(ref sink, vx, vy, t, rayX, rayY, vertexMoves);
}
/// <summary>
/// Axis-aligned ray against a segment, with the same hit rule as the
/// <see cref="PushDirection"/> kernel.
/// </summary>
public static void AxisRayLine<TSink>(
ref TSink sink,
double vx,
double vy,
double p1x,
double p1y,
double p2x,
double p2y,
PushDirection rayDirection,
bool vertexMoves
)
where TSink : struct, ISlideEventSink
{
double dist,
hx,
hy;
switch (rayDirection)
{
case PushDirection.Left:
case PushDirection.Right:
{
var dy = p2y - p1y;
if (System.Math.Abs(dy) < Eps)
return;
var t = (vy - p1y) / dy;
if (t < -Eps || t > 1.0 + Eps)
return;
hx = p1x + t * (p2x - p1x);
hy = vy;
dist = rayDirection == PushDirection.Left ? vx - hx : hx - vx;
break;
}
case PushDirection.Down:
case PushDirection.Up:
{
var dx = p2x - p1x;
if (System.Math.Abs(dx) < Eps)
return;
var t = (vx - p1x) / dx;
if (t < -Eps || t > 1.0 + Eps)
return;
hx = vx;
hy = p1y + t * (p2y - p1y);
dist = rayDirection == PushDirection.Down ? vy - hy : hy - vy;
break;
}
default:
return;
}
if (dist < -Eps)
return;
var vertex = new Vector(vx, vy);
var hit = new Vector(hx, hy);
if (vertexMoves)
sink.Add(Snap(dist), vertex, hit);
else
sink.Add(Snap(dist), hit, vertex);
}
/// <summary>
/// Closest-approach points of arcs against lines, which vertex sampling can miss.
/// </summary>
public static void ArcToLine<TSink>(
ref TSink sink,
List<Entity> arcEntities,
double arcDx,
double arcDy,
List<Entity> lineEntities,
double lineDx,
double lineDy,
double rayX,
double rayY,
bool arcMoves
)
where TSink : struct, ISlideEventSink
{
for (var i = 0; i < arcEntities.Count; i++)
{
if (!TryGetCurve(arcEntities[i], out var localCx, out var localCy, out var r))
continue;
var arc = arcEntities[i] as Arc;
var cx = localCx + arcDx;
var cy = localCy + arcDy;
for (var j = 0; j < lineEntities.Count; j++)
{
if (lineEntities[j] is not Line line)
continue;
var p1x = line.pt1.X + lineDx;
var p1y = line.pt1.Y + lineDy;
var p2x = line.pt2.X + lineDx;
var p2y = line.pt2.Y + lineDy;
var ex = p2x - p1x;
var ey = p2y - p1y;
var det = ex * rayY - ey * rayX;
if (System.Math.Abs(det) < Eps)
continue;
// The directional distance from an arc point at angle θ to the
// line is t(θ) = [A + r·(ey·cosθ − ex·sinθ)] / det.
// dt/dθ = 0 at θ = atan2(−ex, ey) and θ + π.
var theta1 = Angle.NormalizeRad(System.Math.Atan2(-ex, ey));
var theta2 = Angle.NormalizeRad(theta1 + System.Math.PI);
for (var k = 0; k < 2; k++)
{
var theta = k == 0 ? theta1 : theta2;
if (arc != null && !Angle.IsBetweenRad(theta, arc.StartAngle, arc.EndAngle, arc.IsReversed))
continue;
var qx = cx + r * System.Math.Cos(theta);
var qy = cy + r * System.Math.Sin(theta);
RayLine(ref sink, qx, qy, p1x, p1y, p2x, p2y, rayX, rayY, arcMoves);
if (sink.IsDone)
return;
}
}
}
}
/// <summary>
/// External and internal tangencies of two curves along a unit direction. Radii must
/// be nonnegative; a null arc is a full circle.
/// </summary>
public static void CurveTangency<TSink>(
ref TSink sink,
double movingCx,
double movingCy,
double movingRadius,
Arc movingArc,
double stationaryCx,
double stationaryCy,
double stationaryRadius,
Arc stationaryArc,
double dirX,
double dirY
)
where TSink : struct, ISlideEventSink
{
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 < -Eps)
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;
var length = System.Math.Sqrt(toX * toX + toY * toY);
var ux = length > 0 ? toX / length : 0;
var uy = length > 0 ? toY / length : 0;
var movingPoint = new Vector(
movingCx + movingSign * movingRadius * ux,
movingCy + movingSign * movingRadius * uy
);
var stationaryPoint = new Vector(
stationaryCx + stationarySign * stationaryRadius * ux,
stationaryCy + stationarySign * stationaryRadius * uy
);
sink.Add(Snap(t), movingPoint, stationaryPoint);
if (sink.IsDone)
return;
}
}
}
public static bool TryGetCurve(Entity entity, out double cx, out double cy, out double r)
{
switch (entity)
{
case Circle circle:
cx = circle.Center.X;
cy = circle.Center.Y;
r = circle.Radius;
return true;
case Arc arc:
cx = arc.Center.X;
cy = arc.Center.Y;
r = arc.Radius;
return true;
default:
cx = cy = r = 0;
return false;
}
}
private static void Emit<TSink>(
ref TSink sink,
double vx,
double vy,
double t,
double rayX,
double rayY,
bool vertexMoves
)
where TSink : struct, ISlideEventSink
{
var vertex = new Vector(vx, vy);
var hit = new Vector(vx + t * rayX, vy + t * rayY);
if (vertexMoves)
sink.Add(Snap(t), vertex, hit);
else
sink.Add(Snap(t), hit, vertex);
}
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);
}
internal 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>
/// Slide events between native Line/Arc/Circle boundaries. The moving entities and
/// vertices are translated by (movingDx, movingDy); vertex arrays may be subsets.
/// </summary>
public struct EntitySlideEvents : ISlideEventSource
{
private readonly List<Entity> moving;
private readonly Vector[] movingVertices;
private readonly double movingDx;
private readonly double movingDy;
private readonly List<Entity> stationary;
private readonly Vector[] stationaryVertices;
private readonly double dirX;
private readonly double dirY;
private readonly bool arcToLine;
public EntitySlideEvents(
List<Entity> moving,
Vector[] movingVertices,
double movingDx,
double movingDy,
List<Entity> stationary,
Vector[] stationaryVertices,
double dirX,
double dirY,
bool arcToLine
)
{
this.moving = moving;
this.movingVertices = movingVertices;
this.movingDx = movingDx;
this.movingDy = movingDy;
this.stationary = stationary;
this.stationaryVertices = stationaryVertices;
this.dirX = dirX;
this.dirY = dirY;
this.arcToLine = arcToLine;
}
public void Enumerate<TSink>(ref TSink sink)
where TSink : struct, ISlideEventSink
{
// Phase 1: moving vertices along the push against stationary entities.
for (var v = 0; v < movingVertices.Length; v++)
{
var vx = movingVertices[v].X + movingDx;
var vy = movingVertices[v].Y + movingDy;
for (var j = 0; j < stationary.Count; j++)
{
SlideEvents.Ray(ref sink, vx, vy, stationary[j], 0, 0, dirX, dirY, true);
if (sink.IsDone)
return;
}
}
// Phase 2: stationary vertices against the push onto moving entities.
for (var v = 0; v < stationaryVertices.Length; v++)
{
var vx = stationaryVertices[v].X;
var vy = stationaryVertices[v].Y;
for (var j = 0; j < moving.Count; j++)
{
SlideEvents.Ray(
ref sink,
vx,
vy,
moving[j],
movingDx,
movingDy,
-dirX,
-dirY,
false
);
if (sink.IsDone)
return;
}
}
// Phase 3: arc-to-line closest points, which vertex sampling can miss.
if (arcToLine)
{
SlideEvents.ArcToLine(
ref sink,
moving,
movingDx,
movingDy,
stationary,
0,
0,
dirX,
dirY,
true
);
if (sink.IsDone)
return;
SlideEvents.ArcToLine(
ref sink,
stationary,
0,
0,
moving,
movingDx,
movingDy,
-dirX,
-dirY,
false
);
if (sink.IsDone)
return;
}
// Phase 4: native curve tangency, including a convex corner inside a concave arc.
for (var i = 0; i < moving.Count; i++)
{
if (!SlideEvents.TryGetCurve(moving[i], out var mcx, out var mcy, out var mr))
continue;
for (var j = 0; j < stationary.Count; j++)
{
if (!SlideEvents.TryGetCurve(stationary[j], out var scx, out var scy, out var sr))
continue;
SlideEvents.CurveTangency(
ref sink,
mcx + movingDx,
mcy + movingDy,
mr,
moving[i] as Arc,
scx,
scy,
sr,
stationary[j] as Arc,
dirX,
dirY
);
if (sink.IsDone)
return;
}
}
}
}
/// <summary>
/// Slide events between line boundaries along an arbitrary unit direction. The moving
/// lines and vertices are translated by (movingDx, movingDy); vertex arrays may be subsets.
/// </summary>
public struct LineSlideEvents : ISlideEventSource
{
private readonly List<Line> moving;
private readonly Vector[] movingVertices;
private readonly double movingDx;
private readonly double movingDy;
private readonly List<Line> stationary;
private readonly Vector[] stationaryVertices;
private readonly double dirX;
private readonly double dirY;
public LineSlideEvents(
List<Line> moving,
Vector[] movingVertices,
double movingDx,
double movingDy,
List<Line> stationary,
Vector[] stationaryVertices,
double dirX,
double dirY
)
{
this.moving = moving;
this.movingVertices = movingVertices;
this.movingDx = movingDx;
this.movingDy = movingDy;
this.stationary = stationary;
this.stationaryVertices = stationaryVertices;
this.dirX = dirX;
this.dirY = dirY;
}
public void Enumerate<TSink>(ref TSink sink)
where TSink : struct, ISlideEventSink
{
for (var v = 0; v < movingVertices.Length; v++)
{
var vx = movingVertices[v].X + movingDx;
var vy = movingVertices[v].Y + movingDy;
for (var j = 0; j < stationary.Count; j++)
{
var e = stationary[j];
SlideEvents.RayLine(
ref sink,
vx,
vy,
e.pt1.X,
e.pt1.Y,
e.pt2.X,
e.pt2.Y,
dirX,
dirY,
true
);
if (sink.IsDone)
return;
}
}
for (var v = 0; v < stationaryVertices.Length; v++)
{
var vx = stationaryVertices[v].X;
var vy = stationaryVertices[v].Y;
for (var j = 0; j < moving.Count; j++)
{
var e = moving[j];
SlideEvents.RayLine(
ref sink,
vx,
vy,
e.pt1.X + movingDx,
e.pt1.Y + movingDy,
e.pt2.X + movingDx,
e.pt2.Y + movingDy,
-dirX,
-dirY,
false
);
if (sink.IsDone)
return;
}
}
}
}
/// <summary>
/// Axis-aligned slide events between edge arrays sorted for pruning, as used by the
/// <see cref="PushDirection"/> kernel. Offsets translate each side into world space.
/// </summary>
public struct AxisSlideEvents : ISlideEventSource
{
private readonly (Vector start, Vector end)[] movingEdges;
private readonly Vector movingOffset;
private readonly Vector[] movingVertices;
private readonly (Vector start, Vector end)[] stationaryEdges;
private readonly Vector stationaryOffset;
private readonly Vector[] stationaryVertices;
private readonly PushDirection direction;
/// <param name="movingVertices">World-space moving vertices.</param>
/// <param name="stationaryVertices">World-space stationary vertices.</param>
public AxisSlideEvents(
(Vector start, Vector end)[] movingEdges,
Vector movingOffset,
Vector[] movingVertices,
(Vector start, Vector end)[] stationaryEdges,
Vector stationaryOffset,
Vector[] stationaryVertices,
PushDirection direction
)
{
this.movingEdges = movingEdges;
this.movingOffset = movingOffset;
this.movingVertices = movingVertices;
this.stationaryEdges = stationaryEdges;
this.stationaryOffset = stationaryOffset;
this.stationaryVertices = stationaryVertices;
this.direction = direction;
}
public void Enumerate<TSink>(ref TSink sink)
where TSink : struct, ISlideEventSink
{
for (var v = 0; v < movingVertices.Length; v++)
{
OneWay(ref sink, movingVertices[v], stationaryEdges, stationaryOffset, direction, true);
if (sink.IsDone)
return;
}
var opposite = SpatialQuery.OppositeDirection(direction);
for (var v = 0; v < stationaryVertices.Length; v++)
{
OneWay(ref sink, stationaryVertices[v], movingEdges, movingOffset, opposite, false);
if (sink.IsDone)
return;
}
}
private static void OneWay<TSink>(
ref TSink sink,
Vector vertex,
(Vector start, Vector end)[] edges,
Vector edgeOffset,
PushDirection rayDirection,
bool vertexMoves
)
where TSink : struct, ISlideEventSink
{
var vx = vertex.X;
var vy = vertex.Y;
var horizontal = SpatialQuery.IsHorizontalDirection(rayDirection);
// Edges are sorted by their perpendicular min-coordinate.
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;
}
if (perpValue < edgeMin - Tolerance.Epsilon)
break;
if (perpValue > edgeMax + Tolerance.Epsilon)
continue;
SlideEvents.AxisRayLine(
ref sink,
vx,
vy,
e1.X,
e1.Y,
e2.X,
e2.Y,
rayDirection,
vertexMoves
);
if (sink.IsDone)
return;
}
}
}
}
OpenNest.Core/Geometry/SpatialQuery.cs
+168 -353
View File
@@ -320,8 +320,9 @@ namespace OpenNest.Geometry
}
/// <summary>
/// Computes the minimum translation distance along a push direction before
/// any edge of movingLines contacts any edge of stationaryLines.
/// 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(
@@ -334,7 +335,7 @@ namespace OpenNest.Geometry
}
/// <summary>
/// Computes the minimum directional distance with the moving lines translated
/// Computes the directional distance with the moving lines translated
/// by (movingDx, movingDy) without creating new Line objects.
/// </summary>
public static double DirectionalDistance(
@@ -345,37 +346,57 @@ namespace OpenNest.Geometry
PushDirection direction
)
{
var minDist = double.MaxValue;
var movingOffset = new Vector(movingDx, movingDy);
return DirectionalDistance(
movingLines,
movingDx,
movingDy,
stationaryLines,
direction,
SlideContactClassifier.FromLines(
movingLines,
new Vector(movingDx, movingDy),
stationaryLines,
Vector.Zero
)
);
}
// Case 1: Each moving vertex -> each stationary edge
var movingVertices = CollectVertices(movingLines, movingOffset);
/// <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);
foreach (var mv in movingVertices)
{
var d = OneWayDistance(mv, stationaryEdges, Vector.Zero, direction);
if (d < minDist)
minDist = d;
}
// Case 2: Each stationary vertex -> each moving edge (opposite direction)
var opposite = OppositeDirection(direction);
var stationaryVertices = CollectVertices(stationaryLines, Vector.Zero);
var stationaryVertices = CollectVertices(stationaryLines, Vector.Zero).ToArray();
var movingEdges = ToEdgeArray(movingLines);
SortEdgesForPruning(movingEdges, opposite);
SortEdgesForPruning(movingEdges, OppositeDirection(direction));
foreach (var sv in stationaryVertices)
{
var d = OneWayDistance(sv, movingEdges, movingOffset, opposite);
if (d < minDist)
minDist = d;
}
return minDist;
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>
@@ -396,8 +417,8 @@ namespace OpenNest.Geometry
}
/// <summary>
/// Computes the minimum directional distance using raw edge arrays and location offsets
/// to avoid all intermediate object allocations.
/// 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,
@@ -407,36 +428,58 @@ namespace OpenNest.Geometry
PushDirection direction
)
{
var minDist = double.MaxValue;
SortEdgesForPruning(stationaryEdges, direction);
// Case 1: Each moving vertex -> each stationary edge
var movingVertices = CollectVertices(movingEdges, movingOffset);
foreach (var mv in movingVertices)
{
var d = OneWayDistance(mv, stationaryEdges, stationaryOffset, direction);
if (d < minDist)
minDist = d;
}
// Case 2: Each stationary vertex -> each moving edge (opposite direction)
var opposite = OppositeDirection(direction);
SortEdgesForPruning(movingEdges, opposite);
var stationaryVertices = CollectVertices(stationaryEdges, stationaryOffset);
foreach (var sv in stationaryVertices)
{
var d = OneWayDistance(sv, movingEdges, movingOffset, opposite);
if (d < minDist)
minDist = d;
}
return minDist;
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,
@@ -628,8 +671,8 @@ namespace OpenNest.Geometry
}
/// <summary>
/// Computes the minimum translation distance along an arbitrary unit direction
/// before any edge of movingLines contacts any edge of stationaryLines.
/// 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,
@@ -637,58 +680,41 @@ namespace OpenNest.Geometry
Vector direction
)
{
var minDist = double.MaxValue;
var dirX = direction.X;
var dirY = direction.Y;
return DirectionalDistance(
movingLines,
stationaryLines,
direction,
SlideContactClassifier.FromLines(
movingLines,
Vector.Zero,
stationaryLines,
Vector.Zero
)
);
}
var movingVertices = CollectVertices(movingLines, Vector.Zero);
foreach (var mv in movingVertices)
{
for (var i = 0; i < stationaryLines.Count; i++)
{
var e = stationaryLines[i];
var d = RayEdgeDistance(
mv.X,
mv.Y,
e.pt1.X,
e.pt1.Y,
e.pt2.X,
e.pt2.Y,
dirX,
dirY
);
if (d < minDist)
minDist = d;
}
}
var oppX = -dirX;
var oppY = -dirY;
var stationaryVertices = CollectVertices(stationaryLines, Vector.Zero);
foreach (var sv in stationaryVertices)
{
for (var i = 0; i < movingLines.Count; i++)
{
var e = movingLines[i];
var d = RayEdgeDistance(
sv.X,
sv.Y,
e.pt1.X,
e.pt1.Y,
e.pt2.X,
e.pt2.Y,
oppX,
oppY
);
if (d < minDist)
minDist = d;
}
}
return minDist;
/// <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>
@@ -710,10 +736,10 @@ namespace OpenNest.Geometry
}
/// <summary>
/// Computes the minimum translation distance along an arbitrary unit direction
/// before any vertex/edge of movingEntities contacts any vertex/edge of
/// stationaryEntities. Works with native Line, Arc, and Circle entities
/// without tessellation.
/// 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,
@@ -721,228 +747,42 @@ namespace OpenNest.Geometry
Vector direction
)
{
var minDist = double.MaxValue;
var dirX = direction.X;
var dirY = direction.Y;
var movingVertices = ExtractEntityVertices(movingEntities);
for (var v = 0; v < movingVertices.Length; v++)
{
var vx = movingVertices[v].X;
var vy = movingVertices[v].Y;
for (var j = 0; j < stationaryEntities.Count; j++)
{
var d = RayEntityDistance(vx, vy, stationaryEntities[j], dirX, dirY);
if (d < minDist)
{
minDist = d;
if (d <= 0)
return 0;
}
}
}
var oppX = -dirX;
var oppY = -dirY;
var stationaryVertices = ExtractEntityVertices(stationaryEntities);
for (var v = 0; v < stationaryVertices.Length; v++)
{
var vx = stationaryVertices[v].X;
var vy = stationaryVertices[v].Y;
for (var j = 0; j < movingEntities.Count; j++)
{
var d = RayEntityDistance(vx, vy, movingEntities[j], oppX, oppY);
if (d < minDist)
{
minDist = d;
if (d <= 0)
return 0;
}
}
}
// Phase 3: Arc-to-line closest-point check.
// Phases 1-2 sample arc endpoints and cardinal extremes, but the actual
// closest point on a small corner arc to a straight edge may lie between
// those samples. Use ClosestPointTo to find it and fire a ray from there.
minDist = ArcToLineClosestDistance(
return DirectionalDistance(
movingEntities,
stationaryEntities,
dirX,
dirY,
minDist
direction,
new SlideContactClassifier(movingEntities, stationaryEntities)
);
if (minDist <= 0)
return 0;
minDist = ArcToLineClosestDistance(
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,
oppX,
oppY,
minDist
ExtractEntityVertices(movingEntities),
0,
0,
stationaryEntities,
ExtractEntityVertices(stationaryEntities),
direction.X,
direction.Y,
arcToLine: true
);
if (minDist <= 0)
return 0;
// Phase 4: Native curve tangency, including a convex corner inside a concave arc.
for (var i = 0; i < movingEntities.Count; i++)
{
var me = movingEntities[i];
if (!TryGetCurveParams(me, out var mcx, out var mcy, out var mr))
continue;
for (var j = 0; j < stationaryEntities.Count; j++)
{
var se = stationaryEntities[j];
if (!TryGetCurveParams(se, out var scx, out var scy, out var sr))
continue;
var d = CurveTangencyDistance(
mcx, mcy, mr, me as Arc,
scx, scy, sr, se as Arc, dirX, dirY);
if (d >= minDist)
continue;
minDist = d;
if (d <= 0)
return 0;
}
}
return minDist;
return SlideResolver.FirstBlocking(ref source, contacts, direction.X, direction.Y);
}
private static double ArcToLineClosestDistance(
List<Entity> arcEntities,
List<Entity> lineEntities,
double dirX,
double dirY,
double minDist
)
{
for (var i = 0; i < arcEntities.Count; i++)
{
if (arcEntities[i] is not Arc arc)
continue;
var cx = arc.Center.X;
var cy = arc.Center.Y;
var r = arc.Radius;
for (var j = 0; j < lineEntities.Count; j++)
{
if (lineEntities[j] is not Line line)
continue;
var p1x = line.pt1.X;
var p1y = line.pt1.Y;
var ex = line.pt2.X - p1x;
var ey = line.pt2.Y - p1y;
var det = ex * dirY - ey * dirX;
if (System.Math.Abs(det) < Tolerance.Epsilon)
continue;
// The directional distance from an arc point at angle θ to the
// line is t(θ) = [A + r·(ey·cosθ − ex·sinθ)] / det.
// dt/dθ = 0 at θ = atan2(−ex, ey) and θ + π.
var theta1 = Angle.NormalizeRad(System.Math.Atan2(-ex, ey));
var theta2 = Angle.NormalizeRad(theta1 + System.Math.PI);
for (var k = 0; k < 2; k++)
{
var theta = k == 0 ? theta1 : theta2;
if (
!Angle.IsBetweenRad(theta, arc.StartAngle, arc.EndAngle, arc.IsReversed)
)
continue;
var qx = cx + r * System.Math.Cos(theta);
var qy = cy + r * System.Math.Sin(theta);
var d = RayEdgeDistance(
qx,
qy,
p1x,
p1y,
line.pt2.X,
line.pt2.Y,
dirX,
dirY
);
if (d < minDist)
{
minDist = d;
if (d <= 0)
return 0;
}
}
}
}
return minDist;
}
private static double RayEntityDistance(
double vx,
double vy,
Entity entity,
double dirX,
double dirY
)
{
if (entity is Line line)
{
return RayEdgeDistance(
vx,
vy,
line.pt1.X,
line.pt1.Y,
line.pt2.X,
line.pt2.Y,
dirX,
dirY
);
}
if (entity is Arc arc)
{
return RayArcDistance(
vx,
vy,
arc.Center.X,
arc.Center.Y,
arc.Radius,
arc.StartAngle,
arc.EndAngle,
arc.IsReversed,
dirX,
dirY
);
}
if (entity is Circle circle)
{
return RayCircleDistance(
vx,
vy,
circle.Center.X,
circle.Center.Y,
circle.Radius,
dirX,
dirY
);
}
return double.MaxValue;
}
private static Vector[] ExtractEntityVertices(List<Entity> entities)
public static Vector[] ExtractEntityVertices(List<Entity> entities)
{
var vertices = new HashSet<Vector>();
@@ -1041,31 +881,6 @@ namespace OpenNest.Geometry
);
}
private static bool TryGetCurveParams(
Entity entity,
out double cx,
out double cy,
out double r
)
{
if (entity is Circle circle)
{
cx = circle.Center.X;
cy = circle.Center.Y;
r = circle.Radius;
return true;
}
if (entity is Arc arc)
{
cx = arc.Center.X;
cy = arc.Center.Y;
r = arc.Radius;
return true;
}
cx = cy = r = 0;
return false;
}
private static double BoxProjectionMin(Box box, double dx, double dy)
{
var x = dx >= 0 ? box.Left : box.Right;