diff --git a/Sources/ModelingEvolution.Drawing.Tests/Epic128PrimitivesTests.cs b/Sources/ModelingEvolution.Drawing.Tests/Epic128PrimitivesTests.cs new file mode 100644 index 0000000..b0b248e --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Epic128PrimitivesTests.cs @@ -0,0 +1,108 @@ +using FluentAssertions; +using ModelingEvolution.Drawing.Units; +using Xunit; + +namespace ModelingEvolution.Drawing.Tests; + +/// +/// Epic 128 §1.2 additive primitives, rebased onto the epic-129 quaternion model: +/// (basis-from-directions for tool placement), , and the +/// mm/s→mm/min bridge. These are the shared geometry the ModelingEvolution.ToolPath +/// module builds on. (The old Rotation3.FromAxes / Pose3.FromFrame were dropped: the basis→rotation now lives +/// on , which this builds on.) +/// +public class Epic128PrimitivesTests +{ + private const double Eps = 1e-9; + + private static void BeClose(Vector3 actual, Vector3 expected) + { + actual.X.Should().BeApproximately(expected.X, Eps); + actual.Y.Should().BeApproximately(expected.Y, Eps); + actual.Z.Should().BeApproximately(expected.Z, Eps); + } + + [Fact] + public void FromAxes_aligned_hints_give_an_axis_aligned_frame() + { + // forward = +X, approach = +Z ⇒ local X=+X, Z=+Z, Y = Z×X = +Y (identity orientation). + var f = Frame3.FromAxes(new Point3(1, 2, 3), Vector3.EX, Vector3.EZ); + + f.Position.X.Should().BeApproximately(1, Eps); + f.Position.Y.Should().BeApproximately(2, Eps); + f.Position.Z.Should().BeApproximately(3, Eps); + BeClose(f.TransformVector(Vector3.EX), Vector3.EX); + BeClose(f.TransformVector(Vector3.EY), Vector3.EY); + BeClose(f.TransformVector(Vector3.EZ), Vector3.EZ); + } + + [Fact] + public void FromAxes_reorthogonalizes_forward_against_approach() + { + // forward is not orthogonal to approach — its approach-component must be projected out, so local X + // comes back as the clean +X while local Z stays +Z. + var f = Frame3.FromAxes(Point3.Zero, new Vector3(1, 0, 0.5), Vector3.EZ); + + BeClose(f.TransformVector(Vector3.EZ), Vector3.EZ); // local Z == approach + BeClose(f.TransformVector(Vector3.EX), Vector3.EX); // local X == forward, de-Z'd + } + + [Fact] + public void FromAxes_90deg_about_Z_maps_the_basis() + { + // forward +Y, approach +Z ⇒ local X=+Y, Z=+Z, Y = Z×X = -X (a +90° turn about Z). + var f = Frame3.FromAxes(Point3.Zero, Vector3.EY, Vector3.EZ); + + BeClose(f.TransformVector(Vector3.EX), Vector3.EY); + BeClose(f.TransformVector(Vector3.EY), new Vector3(-1, 0, 0)); + BeClose(f.TransformVector(Vector3.EZ), Vector3.EZ); + } + + [Fact] + public void FromAxes_throws_when_forward_parallel_to_approach() + { + var act = () => Frame3.FromAxes(Point3.Zero, Vector3.EZ, Vector3.EZ); + act.Should().Throw(); + } + + [Fact] + public void FromAxes_throws_when_approach_is_degenerate() + { + var act = () => Frame3.FromAxes(Point3.Zero, Vector3.EX, Vector3.Zero); + act.Should().Throw(); + } + + [Fact] + public void Arc_points_and_length_for_quarter_circle() + { + var arc = new Arc(new Point(0, 0), 1.0, 0.0, 90.0); // CCW quarter from +X + arc.StartPoint.X.Should().BeApproximately(1, Eps); + arc.StartPoint.Y.Should().BeApproximately(0, Eps); + arc.EndPoint.X.Should().BeApproximately(0, Eps); + arc.EndPoint.Y.Should().BeApproximately(1, Eps); + arc.Mid.X.Should().BeApproximately(System.Math.Sqrt(0.5), 1e-9); + arc.Mid.Y.Should().BeApproximately(System.Math.Sqrt(0.5), 1e-9); + arc.Length.Should().BeApproximately(System.Math.PI / 2, 1e-9); + } + + [Fact] + public void Arc_tangent_follows_sweep_sign() + { + var ccw = new Arc(new Point(0, 0), 1.0, 0.0, 90.0); + var cw = new Arc(new Point(0, 0), 1.0, 0.0, -90.0); + var tCcw = ccw.TangentAt(0.0); + var tCw = cw.TangentAt(0.0); + tCcw.X.Should().BeApproximately(0, Eps); + tCcw.Y.Should().BeApproximately(1, Eps); // CCW at +X travels +Y + tCw.X.Should().BeApproximately(0, Eps); + tCw.Y.Should().BeApproximately(-1, Eps); // CW reverses + } + + [Fact] + public void Speed_bridge_converts_mm_per_second_to_mm_per_minute() + { + var mmPerS = new Speed>(10); // 10 mm/s + var legacy = mmPerS.ToLegacyMmPerMin(); // expect 600 mm/min + ((double)legacy).Should().BeApproximately(600, 1e-9); + } +} diff --git a/Sources/ModelingEvolution.Drawing/Arc.cs b/Sources/ModelingEvolution.Drawing/Arc.cs new file mode 100644 index 0000000..e67c853 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing/Arc.cs @@ -0,0 +1,72 @@ +using System.Numerics; +using ProtoBuf; + +namespace ModelingEvolution.Drawing; + +/// +/// A circular arc in 2D space: a and , swept from +/// by a signed (CCW positive, CW negative). A full circle is +/// an arc with |Sweep| = 360°. Unlike it carries start/sweep, so it maps +/// directly to a CAD ARC entity and to a 3-point circular robot move (start / / end). +/// +/// The numeric type used for coordinates and angles. +[ProtoContract] +public readonly record struct Arc + where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, + IFloatingPointIeee754, IMinMaxValue +{ + private static readonly T Two = T.CreateTruncating(2); + private static readonly T DegToRad = T.Pi / T.CreateTruncating(180); + + /// The arc's centre. + [ProtoMember(1)] public Point Center { get; init; } + + /// The arc's radius. + [ProtoMember(2)] public T Radius { get; init; } + + /// The start angle, measured CCW from +X. + [ProtoMember(3)] public Degree Start { get; init; } + + /// The signed swept angle (CCW positive). A full circle is ±360°. + [ProtoMember(4)] public Degree Sweep { get; init; } + + public Arc(Point center, T radius, Degree start, Degree sweep) + { + Center = center; + Radius = radius; + Start = start; + Sweep = sweep; + } + + /// The end angle ( + ). + public Degree End => (Degree)((T)Start + (T)Sweep); + + /// The mid angle ( + /2). + public Degree MidAngle => (Degree)((T)Start + (T)Sweep / Two); + + /// The point on the arc at the given angle. + public Point PointAt(Degree angle) + => new(Center.X + Radius * Degree.Cos(angle), Center.Y + Radius * Degree.Sin(angle)); + + /// The start point of the arc. + public Point StartPoint => PointAt(Start); + + /// The end point of the arc. + public Point EndPoint => PointAt(End); + + /// The midpoint of the arc (at ) — the natural via-point for a circular move. + public Point Mid => PointAt(MidAngle); + + /// The arc length. + public T Length => Radius * T.Abs((T)Sweep) * DegToRad; + + /// + /// The unit travel tangent at the given angle, oriented in the sweep direction (CW arcs point the + /// opposite way to CCW arcs). + /// + public Vector TangentAt(Degree angle) + { + var sign = (T)Sweep >= T.Zero ? T.One : -T.One; + return new Vector(-Degree.Sin(angle) * sign, Degree.Cos(angle) * sign).Normalize(); + } +} diff --git a/Sources/ModelingEvolution.Drawing/Frame3.cs b/Sources/ModelingEvolution.Drawing/Frame3.cs index 788c89e..5e40aaa 100644 --- a/Sources/ModelingEvolution.Drawing/Frame3.cs +++ b/Sources/ModelingEvolution.Drawing/Frame3.cs @@ -80,6 +80,39 @@ public static Frame3 FromPose3(Pose3 p, T lengthScale) public Pose3 ToPose3(T lengthScale) => new(Position * lengthScale, Rotation3.FromQuaternion(Rotation)); + /// + /// Creates a frame from an origin and two direction hints: the forward (local +X) and approach (local +Z) + /// axes. Local Z is the normalized , local X is + /// re-orthogonalized against Z, and local Y = Z × X. The hints need not be unit or exactly orthogonal — they + /// are cleaned up here — but must not be (near-)parallel or degenerate. This is the basis-from-directions + /// factory for tool placement (a torch approach axis + travel tangent), complementing the point-based + /// . + /// + /// The frame origin. + /// Desired local +X (e.g. a travel tangent); its approach-component is projected out. + /// Desired local +Z (e.g. a tool approach axis). + /// Thrown when the axes are (near-)parallel or degenerate. + public static Frame3 FromAxes(Point3 origin, Vector3 forward, Vector3 approach) + { + var epsilon = T.CreateTruncating(1e-9); + + var zLen = approach.Length; + if (zLen < epsilon) + throw new ArgumentException("Approach axis is degenerate (near-zero length).", nameof(approach)); + var z = approach.Normalize(); + + // X = forward with its approach-component removed, then normalized. + var dot = Vector3.Dot(forward, z); + var xRaw = forward - z * dot; + var xLen = xRaw.Length; + if (xLen < epsilon) + throw new ArgumentException("Forward axis is parallel to the approach axis; cannot build a frame.", nameof(forward)); + var x = xRaw.Normalize(); + + var y = Vector3.Cross(z, x); + return new Frame3(origin, Quaternion.FromAxes(x, y, z)); + } + #region FromSurface /// diff --git a/Sources/ModelingEvolution.Drawing/Units/SpeedInterop.cs b/Sources/ModelingEvolution.Drawing/Units/SpeedInterop.cs new file mode 100644 index 0000000..f70238a --- /dev/null +++ b/Sources/ModelingEvolution.Drawing/Units/SpeedInterop.cs @@ -0,0 +1,21 @@ +using System.Numerics; + +namespace ModelingEvolution.Drawing.Units; + +/// +/// Bridges the unit-aware to the legacy unit-less +/// , whose canonical unit is millimetres per minute (mm/min). +/// Use this at the boundary with APIs that take the legacy type (e.g. Trajectory3Builder.MoveTo) so a +/// mm/s value is not silently read as mm/min — a 60× error. +/// +public static class SpeedInterop +{ + /// + /// Converts any unit-aware speed to the legacy (mm/min). + /// + public static Speed ToLegacyMmPerMin(this Speed speed) + where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, + ISignedNumber, IFloatingPointIeee754, IMinMaxValue + where TUnit : ISpeedUnit + => Speed.From(speed.To>().Value); +} diff --git a/Sources/ModelingEvolution.Drawing/Units/SpeedUnits.cs b/Sources/ModelingEvolution.Drawing/Units/SpeedUnits.cs index 0dbb4a1..8865dac 100644 --- a/Sources/ModelingEvolution.Drawing/Units/SpeedUnits.cs +++ b/Sources/ModelingEvolution.Drawing/Units/SpeedUnits.cs @@ -48,3 +48,18 @@ public interface ISpeedUnit : IUnit where T : INumber; /// public static string Symbol => "m/min"; } + +/// +/// Millimetres per minute. 1 mm/min = 1e-3 m / 60 s. This is the unit of the legacy unit-less +/// , so it is the bridge target for interop +/// (see ). +/// +public readonly struct MillimetrePerMinute : ISpeedUnit where T : INumber +{ + /// + public static T ToSi(T value) => value / T.CreateChecked(60000.0); + /// + public static T FromSi(T si) => si * T.CreateChecked(60000.0); + /// + public static string Symbol => "mm/min"; +}