diff --git a/OpenNest.Engine/RapidPlanning/DirectRapidPlanner.cs b/OpenNest.Engine/RapidPlanning/DirectRapidPlanner.cs new file mode 100644 index 0000000..154e525 --- /dev/null +++ b/OpenNest.Engine/RapidPlanning/DirectRapidPlanner.cs @@ -0,0 +1,44 @@ +using System.Collections.Generic; +using OpenNest.Geometry; + +namespace OpenNest.Engine.RapidPlanning +{ + public class DirectRapidPlanner : IRapidPlanner + { + public RapidPath Plan(Vector from, Vector to, IReadOnlyList cutAreas) + { + var travelLine = new Line(from, to); + + foreach (var cutArea in cutAreas) + { + if (TravelLineIntersectsShape(travelLine, cutArea)) + { + return new RapidPath + { + HeadUp = true, + Waypoints = new List() + }; + } + } + + return new RapidPath + { + HeadUp = false, + Waypoints = new List() + }; + } + + private static bool TravelLineIntersectsShape(Line travelLine, Shape shape) + { + foreach (var entity in shape.Entities) + { + if (entity is Line edge) + { + if (travelLine.Intersects(edge, out _)) + return true; + } + } + return false; + } + } +} diff --git a/OpenNest.Tests/RapidPlanning/DirectRapidPlannerTests.cs b/OpenNest.Tests/RapidPlanning/DirectRapidPlannerTests.cs new file mode 100644 index 0000000..ca6a00b --- /dev/null +++ b/OpenNest.Tests/RapidPlanning/DirectRapidPlannerTests.cs @@ -0,0 +1,56 @@ +using System.Collections.Generic; +using OpenNest.Engine.RapidPlanning; +using OpenNest.Geometry; +using Xunit; + +namespace OpenNest.Tests.RapidPlanning; + +public class DirectRapidPlannerTests +{ + [Fact] + public void NoCutAreas_ReturnsHeadDown() + { + var planner = new DirectRapidPlanner(); + var result = planner.Plan(new Vector(0, 0), new Vector(10, 10), new List()); + + Assert.False(result.HeadUp); + Assert.Empty(result.Waypoints); + } + + [Fact] + public void ClearPath_ReturnsHeadDown() + { + var planner = new DirectRapidPlanner(); + + var cutArea = new Shape(); + cutArea.Entities.Add(new Line(new Vector(50, 0), new Vector(50, 10))); + cutArea.Entities.Add(new Line(new Vector(50, 10), new Vector(60, 10))); + cutArea.Entities.Add(new Line(new Vector(60, 10), new Vector(60, 0))); + cutArea.Entities.Add(new Line(new Vector(60, 0), new Vector(50, 0))); + + var result = planner.Plan( + new Vector(0, 0), new Vector(10, 10), + new List { cutArea }); + + Assert.False(result.HeadUp); + } + + [Fact] + public void BlockedPath_ReturnsHeadUp() + { + var planner = new DirectRapidPlanner(); + + var cutArea = new Shape(); + cutArea.Entities.Add(new Line(new Vector(5, 0), new Vector(5, 20))); + cutArea.Entities.Add(new Line(new Vector(5, 20), new Vector(6, 20))); + cutArea.Entities.Add(new Line(new Vector(6, 20), new Vector(6, 0))); + cutArea.Entities.Add(new Line(new Vector(6, 0), new Vector(5, 0))); + + var result = planner.Plan( + new Vector(0, 10), new Vector(10, 10), + new List { cutArea }); + + Assert.True(result.HeadUp); + Assert.Empty(result.Waypoints); + } +}