From 8f120e004fb79c5e723151ee5b9fceb5d63d3fd3 Mon Sep 17 00:00:00 2001 From: Rafal Maciag Date: Sat, 24 Oct 2026 18:29:48 +0000 Subject: [PATCH 1/6] S3: Frame3 + KinematicChain; Pose3 is a wire format (epic-129) - Frame3 (position + unit quaternion): Multiply/*, Inverse, TransformPoint, TransformVector, Interpolate (slerp), Distance (position, angle), FromSurface (3 overloads, moved from Pose3), FromPose3/ToPose3 (the one Euler rule). - Quaternion.FromRpy (Rz*Ry*Rx), FromTo, FromAxes; Rotation3.ToQuaternion now delegates to FromRpy. - ModelingEvolution.Drawing.Kinematics: JointType, Link, Joint, ChainFrame, KinematicChain (Forward, FramePose, Jacobian, FrameIndex, Dof); spans in, spans out, no allocation. - Pose3: TransformPoint, TransformVector, Inverse, Multiply, operator * / + / -, Lerp, Distance, FromSurface deleted (no forwarders; breaking, ADR-11 major). - Drawing's callers (PosePath3, Trajectory3, Trajectory3Builder, Triangle3, Polyline3) moved to Frame3 / Point3. - Tests TS-129-01..05 and TS-129-55; grep-gate CI job with rows G-17, G-18. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_014wH5aL3TZhJNvLqa8YTku8 --- .github/scripts/grep-gate.sh | 25 + .github/workflows/ci.yml | 12 + README.md | 17 +- .../Frame3EulerTests.cs | 93 +++ .../Frame3SurfaceTests.cs | 526 ++++++++++++++++ .../Frame3TestData.cs | 39 ++ .../Frame3Tests.cs | 197 ++++++ .../Kinematics/ForwardTests.cs | 125 ++++ .../Kinematics/JacobianTests.cs | 117 ++++ .../Kinematics/KinematicChainTests.cs | 127 ++++ .../Kinematics/TestChains.cs | 92 +++ .../Pose3SurfaceTests.cs | 81 +++ .../Pose3Tests.cs | 574 ------------------ .../QuaternionFactoryTests.cs | 94 +++ Sources/ModelingEvolution.Drawing/Frame3.cs | 270 ++++++++ .../Kinematics/ChainFrame.cs | 11 + .../Kinematics/Joint.cs | 24 + .../Kinematics/JointType.cs | 11 + .../Kinematics/KinematicChain.cs | 188 ++++++ .../Kinematics/Link.cs | 6 + .../ModelingEvolution.Drawing/Polyline3.cs | 3 +- Sources/ModelingEvolution.Drawing/Pose3.cs | 367 +---------- .../ModelingEvolution.Drawing/PosePath3.cs | 9 +- .../ModelingEvolution.Drawing/Quaternion.cs | 87 +++ Sources/ModelingEvolution.Drawing/README.md | 17 +- .../ModelingEvolution.Drawing/Rotation3.cs | 18 +- .../ModelingEvolution.Drawing/Trajectory3.cs | 9 +- .../Trajectory3Builder.cs | 6 +- .../ModelingEvolution.Drawing/Triangle3.cs | 6 +- 29 files changed, 2167 insertions(+), 984 deletions(-) create mode 100755 .github/scripts/grep-gate.sh create mode 100644 Sources/ModelingEvolution.Drawing.Tests/Frame3EulerTests.cs create mode 100644 Sources/ModelingEvolution.Drawing.Tests/Frame3SurfaceTests.cs create mode 100644 Sources/ModelingEvolution.Drawing.Tests/Frame3TestData.cs create mode 100644 Sources/ModelingEvolution.Drawing.Tests/Frame3Tests.cs create mode 100644 Sources/ModelingEvolution.Drawing.Tests/Kinematics/ForwardTests.cs create mode 100644 Sources/ModelingEvolution.Drawing.Tests/Kinematics/JacobianTests.cs create mode 100644 Sources/ModelingEvolution.Drawing.Tests/Kinematics/KinematicChainTests.cs create mode 100644 Sources/ModelingEvolution.Drawing.Tests/Kinematics/TestChains.cs create mode 100644 Sources/ModelingEvolution.Drawing.Tests/Pose3SurfaceTests.cs create mode 100644 Sources/ModelingEvolution.Drawing.Tests/QuaternionFactoryTests.cs create mode 100644 Sources/ModelingEvolution.Drawing/Frame3.cs create mode 100644 Sources/ModelingEvolution.Drawing/Kinematics/ChainFrame.cs create mode 100644 Sources/ModelingEvolution.Drawing/Kinematics/Joint.cs create mode 100644 Sources/ModelingEvolution.Drawing/Kinematics/JointType.cs create mode 100644 Sources/ModelingEvolution.Drawing/Kinematics/KinematicChain.cs create mode 100644 Sources/ModelingEvolution.Drawing/Kinematics/Link.cs diff --git a/.github/scripts/grep-gate.sh b/.github/scripts/grep-gate.sh new file mode 100755 index 0000000..1066d9a --- /dev/null +++ b/.github/scripts/grep-gate.sh @@ -0,0 +1,25 @@ +#!/usr/bin/env bash +# Grep gate (epic-129 test-scenarios TS-129-48). Each row is: id extended regex pathspecs. +# A row passes when `git grep -nE` finds 0 lines. Add a row per slice as the G-rows are switched on. +set -uo pipefail +cd "$(git rev-parse --show-toplevel)" + +ROWS=( + $'G-17\t\\b(TransformPoint|TransformVector|Inverse|Multiply|Lerp|Distance|FromSurface)\\s*[<(]|operator\\s*[-+*]\tSources/ModelingEvolution.Drawing/Pose3.cs' + $'G-18\tPose3(<[^>]*>)?\\.(Lerp|Distance|FromSurface)\\b\tSources/' +) + +failed=0 +for row in "${ROWS[@]}"; do + IFS=$'\t' read -r id pattern paths <<<"$row" + # shellcheck disable=SC2086 # pathspecs are space-separated on purpose + hits=$(git grep -nE "$pattern" -- $paths) + count=$(printf '%s' "$hits" | grep -c . || true) + echo "$id: $count line(s) [git grep -nE '$pattern' -- $paths]" + if [ "$count" -ne 0 ]; then + printf '%s\n' "$hits" + echo "::error::grep gate $id found $count line(s)" + failed=1 + fi +done +exit $failed diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 535c1f4..67135f3 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -10,6 +10,7 @@ on: - '.github/workflows/ci.yml' - '.github/workflows/publish-nuget.yml' - '.github/workflows/release.yml' + - '.github/scripts/**' pull_request: branches: [ main, master, develop ] paths: @@ -19,6 +20,7 @@ on: - '.github/workflows/ci.yml' - '.github/workflows/publish-nuget.yml' - '.github/workflows/release.yml' + - '.github/scripts/**' jobs: build-and-test: @@ -116,3 +118,13 @@ jobs: - name: Test release.sh run: ./release.test.sh + + grep-gate: + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Grep gate (epic-129 TS-129-48 rows; each must find 0 lines) + run: ./.github/scripts/grep-gate.sh diff --git a/README.md b/README.md index 76d0a07..13d924f 100644 --- a/README.md +++ b/README.md @@ -63,7 +63,7 @@ A high-performance .NET generic math library for 2D and 3D geometry, coordinate - **Polygon booleans** -- union, intersection, difference, and clustering via Clipper2. - **Exact Bezier clipping** -- `Path.Intersect(Rectangle)` computes analytically exact sub-curves, not polygon approximations. - **Trajectory planning** -- `Trajectory3` with time-parameterized waypoints, interpolation, resampling, and a real-time `TrajectoryController`. -- **Robotics-ready** -- `Joints6` for 6-DOF joint representations, `Pose3.FromSurface()` for plane-based frame construction. +- **Robotics-ready** -- `Joints6` for 6-DOF joint representations, `Frame3` (position + unit quaternion) for all pose maths, `Frame3.FromSurface()` for plane-based frame construction, and `KinematicChain` for forward kinematics and geometric Jacobians of serial and tree chains. - **Pooled allocations** -- `AllocationScope` provides arena-style pooled memory for zero-GC geometry pipelines. - **Serialization built in** -- Protobuf (protobuf-net) attributes on all core types, plus custom JSON converters and SVG exporters. @@ -203,14 +203,15 @@ All 2D shapes implement `IShape`, providing `Area()`, `Perimeter()`, ` | Type | Description | |---|---| | `Rotation3` | Euler angle rotation (Roll/Pitch/Yaw in degrees, ZYX convention). Create from degrees, radians, quaternion, or `Matrix3x3`. `Rotate(Vector3)`, `Rotate(Point3)`. `Combine`, `Inverse`, `Negate`, `Slerp` (via quaternion). Convert to matrix, quaternion, or direction vector. Protobuf and JSON serialization. | -| `Quaternion` | Unit quaternion for 3D rotations. From axis-angle, rotate vector, slerp, dot, normalize, conjugate, inverse. Hamilton product operator (`*`). Conversions to/from `System.Numerics.Quaternion`. Protobuf serialization. | +| `Quaternion` | Unit quaternion for 3D rotations. From axis-angle, `FromRpy(roll, pitch, yaw)` (Rz·Ry·Rx, radians; the URDF rpy rule), `FromTo(from, to)` (shortest arc), `FromAxes(x, y, z)` (orthonormal basis), rotate vector, slerp, dot, normalize, conjugate, inverse. Hamilton product operator (`*`). Conversions to/from `System.Numerics.Quaternion`. Protobuf serialization. | | `Matrix3x3` | Also used as rotation representation. `RotationZYX(rx, ry, rz)` creates ZYX Euler rotation matrix. `ToEulerZYX()` extracts angles (with gimbal-lock handling). `ToQuaternion()` via Shepperd's method. | ### Poses and Trajectories | Type | Description | |---|---| -| `Pose3` | 3D pose = position (`Point3`) + orientation (`Rotation3`). `TransformPoint`, `TransformVector`, `Inverse`, `Multiply` (compose). `Lerp` (position lerp + rotation slerp). `FromSurface(a, b, c)` creates a frame from three coplanar points (right-hand rule). Overload `FromSurface(a, b, c, h)` uses a hint point to determine Z direction. Operators: `*` (compose), `+`/`-` with `Vector3` or `Rotation3`. Tuple conversions, parsing. Protobuf and JSON serialization. | +| `Frame3` | Rigid frame = position (`Point3`) + unit `Quaternion`. All pose maths: `Multiply` / `*` (compose), `Inverse`, `TransformPoint`, `TransformVector`, `Interpolate` (position lerp + slerp), `Distance` (position error and angle error in radians). `FromSurface(a, b, c)` creates a frame from three coplanar points (right-hand rule); `FromSurface(a, b, c, h)` uses a hint point for Z; `FromSurface(points)` fits a plane. `FromPose3(pose, lengthScale)` / `ToPose3(lengthScale)` convert to and from the wire format (ZYX degrees). | +| `Pose3` | Wire and edit format only (since 2.0): position (`Point3`) + ZYX Euler orientation (`Rotation3`, degrees). Constructors, `Identity`, tuple conversions, deconstruction, parsing, equality, `Truncating()`. Protobuf and JSON serialization. Convert to `Frame3` for any maths. | | `Waypoint3` | Timestamped `Pose3` -- an element of a trajectory. | | `PosePath3` | Immutable sequence of `Pose3` (no time). Length, reverse, transform, extract `Polyline3`, convert to `Trajectory3` with uniform timing. | | `Trajectory3` | Immutable time-parameterized path of `Waypoint3`. `Duration`, `Length`, `AtTime(t)` (binary-search interpolation with position lerp + rotation slerp). `Resample(interval)`, `Reverse`, `Transform`. Convert to `PosePath3` or `Polyline3`. Create a real-time `TrajectoryController` via `Control()`. | @@ -221,6 +222,7 @@ All 2D shapes implement `IShape`, providing `Area()`, `Perimeter()`, ` | Type | Description | |---|---| +| `KinematicChain` | Namespace `ModelingEvolution.Drawing.Kinematics`. A tree of `Link`s moved by revolute/prismatic `Joint`s (origin at q = 0, unit axis, limits) with named `ChainFrame`s; metres and radians. `Forward(q, links)` writes every link pose, `FramePose(links, frame)`, `Jacobian(links, frame, j)` writes the 6 × Dof geometric Jacobian (column-major, v then ω). Spans in, spans out, no allocation; the constructor validates topological order, unit axes, limits and unique names. | | `Joints6` | 6-DOF joint angles stored as six `Degree` fields (no array allocation). Indexer `[0..5]`, arithmetic operators, `Lerp`, `MaxAbsDelta`, `IsWithin(tolerance)`, `IsBetween(min, max)`. `ToArray()`, `FromSpan()`, `FromArray()`, `CopyTo(Span)`. Protobuf and JSON serialization. | | `Speed` | Scalar speed in units/second (unitless magnitude). Arithmetic, `TimeFor(distance)`, `DistanceIn(time)`. Parsing. | | `Velocity3` | 3D velocity vector (direction + magnitude). Create from direction + speed. `Speed`, `Direction`, `DisplacementIn(time)`. Arithmetic operators. | @@ -538,16 +540,17 @@ SphericalPoint back = cyl; // implicit conversio // Pose = position + orientation var pose = new Pose3(100, 200, 300, 45, 0, 0); // x,y,z, rx,ry,rz (degrees) -// Compose poses -var world = new Pose3(new Point3(10, 0, 0), Rotation3.Identity); -var local = new Pose3(new Point3(5, 0, 0), Rotation3.FromDegrees(0, 0, 90)); +// Maths composes Frame3 (position + quaternion); Pose3 converts once, here mm -> m +var world = Frame3.FromPose3(new Pose3(10, 0, 0, 0, 0, 0), 0.001); +var local = Frame3.FromPose3(new Pose3(5, 0, 0, 0, 0, 90), 0.001); var combined = world * local; +Pose3 back = combined.ToPose3(1000); // m -> mm, ZYX degrees // Create frame from a surface (3 points define a plane) var a = new Point3(0, 0, 0); var b = new Point3(1, 0, 0); var c = new Point3(0, 1, 0); -var surfacePose = Pose3.FromSurface(a, b, c); // Z-axis via right-hand rule +var surface = Frame3.FromSurface(a, b, c); // Z-axis via right-hand rule // Trajectory with timed waypoints var trajectory = new Trajectory3( diff --git a/Sources/ModelingEvolution.Drawing.Tests/Frame3EulerTests.cs b/Sources/ModelingEvolution.Drawing.Tests/Frame3EulerTests.cs new file mode 100644 index 0000000..6b86232 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Frame3EulerTests.cs @@ -0,0 +1,93 @@ +using FluentAssertions; +using static ModelingEvolution.Drawing.Tests.Frame3TestData; + +namespace ModelingEvolution.Drawing.Tests; + +/// TS-129-02: the one Euler rule on the Drawing side (Frame3.FromPose3 / ToPose3, Quaternion.FromRpy). +public class Frame3EulerTests +{ + private static double AngleDiffDegrees(double a, double b) + { + var d = (a - b) % 360.0; + if (d > 180) d -= 360; + if (d <= -180) d += 360; + return Math.Abs(d); + } + + [Fact] + public void FromPose3_ToPose3_RoundTripsOver10000RandomPoses() + { + var rng = new Random(129); + for (int i = 0; i < 10_000; i++) + { + var p = new Pose3(Uniform(rng, 2000), Uniform(rng, 2000), Uniform(rng, 2000), + Uniform(rng, 180), Uniform(rng, 89.9), Uniform(rng, 180)); + + var back = Frame3.FromPose3(p, 0.001).ToPose3(1000); + + Math.Abs(back.X - p.X).Should().BeLessThan(1e-9); + Math.Abs(back.Y - p.Y).Should().BeLessThan(1e-9); + Math.Abs(back.Z - p.Z).Should().BeLessThan(1e-9); + AngleDiffDegrees((double)back.Rx, (double)p.Rx).Should().BeLessThan(1e-9, $"rx of {p}"); + AngleDiffDegrees((double)back.Ry, (double)p.Ry).Should().BeLessThan(1e-9, $"ry of {p}"); + AngleDiffDegrees((double)back.Rz, (double)p.Rz).Should().BeLessThan(1e-9, $"rz of {p}"); + } + } + + [Fact] + public void FromPose3_ScalesPositionAndKeepsRotation() + { + var p = new Pose3(1000, -500, 250, 0, 0, 90); + + var f = Frame3.FromPose3(p, 0.001); + + f.Position.Should().Be(new Point3(1, -0.5, 0.25)); + var x = f.TransformVector(Vector3.EX); + x.X.Should().BeApproximately(0, 1e-12); + x.Y.Should().BeApproximately(1, 1e-12); + } + + [Fact] + public void FromRpy_EqualsRzRyRxOver10000RandomAngles() + { + var rng = new Random(129); + for (int n = 0; n < 10_000; n++) + { + double roll = Uniform(rng, Math.PI), pitch = Uniform(rng, Math.PI), yaw = Uniform(rng, Math.PI); + + var actual = ToMatrix(Quaternion.FromRpy(roll, pitch, yaw)); + var expected = Mul(Mul(Rz(yaw), Ry(pitch)), Rx(roll)); + + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + Math.Abs(actual[i, j] - expected[i, j]).Should().BeLessThan(1e-12); + } + } + + [Fact] + public void FromRpy_IsTheRuleRotation3Uses() + { + var rng = new Random(129); + for (int n = 0; n < 1000; n++) + { + var r = new Rotation3(Uniform(rng, 180), Uniform(rng, 90), Uniform(rng, 180)); + var (rx, ry, rz) = r.ToRadians(); + + r.ToQuaternion().Should().Be(Quaternion.FromRpy((double)rx, (double)ry, (double)rz)); + } + } + + private static double[,] Rx(double a) => new[,] { { 1, 0, 0 }, { 0, Math.Cos(a), -Math.Sin(a) }, { 0, Math.Sin(a), Math.Cos(a) } }; + private static double[,] Ry(double a) => new[,] { { Math.Cos(a), 0, Math.Sin(a) }, { 0, 1, 0 }, { -Math.Sin(a), 0, Math.Cos(a) } }; + private static double[,] Rz(double a) => new[,] { { Math.Cos(a), -Math.Sin(a), 0 }, { Math.Sin(a), Math.Cos(a), 0 }, { 0, 0, 1 } }; + + private static double[,] Mul(double[,] a, double[,] b) + { + var r = new double[3, 3]; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + for (int k = 0; k < 3; k++) + r[i, j] += a[i, k] * b[k, j]; + return r; + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Frame3SurfaceTests.cs b/Sources/ModelingEvolution.Drawing.Tests/Frame3SurfaceTests.cs new file mode 100644 index 0000000..d11131f --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Frame3SurfaceTests.cs @@ -0,0 +1,526 @@ +using FluentAssertions; + +namespace ModelingEvolution.Drawing.Tests; + +/// +/// Frame3.FromSurface (moved from the pose type in epic-129 S3, design § "Pose3 is a wire and edit format only"). +/// +public class Frame3SurfaceTests +{ + private const double Tolerance = 1e-10; + + #region FromSurface + + [Fact] + public void FromSurface_HorizontalPlane_HintAbove_ZPointsUp() + { + // Horizontal plane at Z=0 (XY plane) + var a = new Point3(0, 0, 0); + var b = new Point3(1, 0, 0); + var c = new Point3(0, 1, 0); + // Hint point above the plane + var h = new Point3(0.5, 0.5, 10); + + var pose = Frame3.FromSurface(a, b, c, h); + + // Origin should be projection of h onto plane + pose.Position.X.Should().BeApproximately(0.5, Tolerance); + pose.Position.Y.Should().BeApproximately(0.5, Tolerance); + pose.Position.Z.Should().BeApproximately(0, Tolerance); + + // Z-axis should point up (toward h) + var zDir = pose.Rotation.Rotate(Vector3.EZ); + zDir.X.Should().BeApproximately(0, Tolerance); + zDir.Y.Should().BeApproximately(0, Tolerance); + zDir.Z.Should().BeApproximately(1, Tolerance); + } + + [Fact] + public void FromSurface_HorizontalPlane_HintBelow_ZPointsDown() + { + // Horizontal plane at Z=0 + var a = new Point3(0, 0, 0); + var b = new Point3(1, 0, 0); + var c = new Point3(0, 1, 0); + // Hint point below the plane + var h = new Point3(0.5, 0.5, -10); + + var pose = Frame3.FromSurface(a, b, c, h); + + // Origin should be projection of h onto plane + pose.Position.X.Should().BeApproximately(0.5, Tolerance); + pose.Position.Y.Should().BeApproximately(0.5, Tolerance); + pose.Position.Z.Should().BeApproximately(0, Tolerance); + + // Z-axis should point down (toward h) + var zDir = pose.Rotation.Rotate(Vector3.EZ); + zDir.X.Should().BeApproximately(0, Tolerance); + zDir.Y.Should().BeApproximately(0, Tolerance); + zDir.Z.Should().BeApproximately(-1, Tolerance); + } + + [Fact] + public void FromSurface_VerticalPlane_XZ_HintInFront() + { + // Vertical plane (XZ plane, Y=0) + var a = new Point3(0, 0, 0); + var b = new Point3(1, 0, 0); + var c = new Point3(0, 0, 1); + // Hint point in front (positive Y) + var h = new Point3(0.5, 5, 0.5); + + var pose = Frame3.FromSurface(a, b, c, h); + + // Origin should be projection of h onto plane (Y=0) + pose.Position.X.Should().BeApproximately(0.5, Tolerance); + pose.Position.Y.Should().BeApproximately(0, Tolerance); + pose.Position.Z.Should().BeApproximately(0.5, Tolerance); + + // Z-axis should point toward positive Y + var zDir = pose.Rotation.Rotate(Vector3.EZ); + zDir.X.Should().BeApproximately(0, Tolerance); + zDir.Y.Should().BeApproximately(1, Tolerance); + zDir.Z.Should().BeApproximately(0, Tolerance); + } + + [Fact] + public void FromSurface_XAxisAlongEdgeAB() + { + // Horizontal plane + var a = new Point3(0, 0, 0); + var b = new Point3(2, 0, 0); // X direction + var c = new Point3(0, 2, 0); + var h = new Point3(1, 1, 5); + + var pose = Frame3.FromSurface(a, b, c, h); + + // X-axis should be along a→b direction (positive X) + var xDir = pose.Rotation.Rotate(Vector3.EX); + xDir.X.Should().BeApproximately(1, Tolerance); + xDir.Y.Should().BeApproximately(0, Tolerance); + xDir.Z.Should().BeApproximately(0, Tolerance); + } + + [Fact] + public void FromSurface_TiltedPlane() + { + // Plane tilted 45° around X axis (normal points in Y-Z direction) + // Plane equation: z = y (passes through origin with normal (0, -1, 1)) + var a = new Point3(0, 0, 0); + var b = new Point3(1, 0, 0); + var c = new Point3(0, 1, 1); // 45° tilt + // Hint point OFF the plane (z ≠ y, so z - y ≠ 0) + var h = new Point3(0.5, 2, 5); // z=5, y=2, so h is above the plane + + var pose = Frame3.FromSurface(a, b, c, h); + + // Z should point toward h - normal is (0, -1/√2, 1/√2) + // Since h has z > y, h is on the +normal side, so Z = +normal + var zDir = pose.Rotation.Rotate(Vector3.EZ); + zDir.X.Should().BeApproximately(0, Tolerance); + // Y and Z components should have same magnitude (45° angle from Y-Z plane) + Math.Abs(zDir.Y).Should().BeApproximately(Math.Abs(zDir.Z), 0.01); + // Z component should be positive (normal points toward +Z more than -Y) + zDir.Z.Should().BeGreaterThan(0); + } + + [Fact] + public void FromSurface_ThrowsWhenHintOnPlane() + { + var a = new Point3(0, 0, 0); + var b = new Point3(1, 0, 0); + var c = new Point3(0, 1, 0); + // Hint point ON the plane + var h = new Point3(0.5, 0.5, 0); + + var act = () => Frame3.FromSurface(a, b, c, h); + + act.Should().Throw() + .WithMessage("*lies on the surface*"); + } + + [Fact] + public void FromSurface_ThrowsWhenPointsCollinear() + { + // All three points on a line + var a = new Point3(0, 0, 0); + var b = new Point3(1, 0, 0); + var c = new Point3(2, 0, 0); + var h = new Point3(0, 1, 0); + + var act = () => Frame3.FromSurface(a, b, c, h); + + act.Should().Throw() + .WithMessage("*collinear*"); + } + + [Fact] + public void FromSurface_TransformPointOnSurface_HasZeroZ() + { + // Create any surface + var a = new Point3(10, 20, 5); + var b = new Point3(15, 20, 5); + var c = new Point3(10, 25, 8); + var h = new Point3(12, 22, 20); + + var pose = Frame3.FromSurface(a, b, c, h); + + // Point 'a' is on the surface, so in local coordinates its Z should be 0 + var inverse = pose.Inverse(); + var aLocal = inverse.TransformPoint(a); + + aLocal.Z.Should().BeApproximately(0, 0.0001); + } + + [Fact] + public void FromSurface_HintPointTransforms_ToPositiveZ() + { + var a = new Point3(0, 0, 0); + var b = new Point3(1, 0, 0); + var c = new Point3(0, 1, 0); + var h = new Point3(0.5, 0.5, 10); + + var pose = Frame3.FromSurface(a, b, c, h); + + // Transform h to local coordinates - should have positive Z + var inverse = pose.Inverse(); + var hLocal = inverse.TransformPoint(h); + + hLocal.Z.Should().BeGreaterThan(0); + // Local X and Y should be near 0 (h is directly above origin) + hLocal.X.Should().BeApproximately(0, 0.0001); + hLocal.Y.Should().BeApproximately(0, 0.0001); + } + + #endregion + + #region FromSurface (3-point, right-hand rule) + + [Fact] + public void FromSurface_ThreePoints_OriginAtA() + { + var a = new Point3(10, 20, 30); + var b = new Point3(11, 20, 30); + var c = new Point3(10, 21, 30); + + var pose = Frame3.FromSurface(a, b, c); + + pose.Position.X.Should().Be(10); + pose.Position.Y.Should().Be(20); + pose.Position.Z.Should().Be(30); + } + + [Fact] + public void FromSurface_ThreePoints_XAxisAlongAB() + { + var a = new Point3(0, 0, 0); + var b = new Point3(5, 0, 0); + var c = new Point3(0, 5, 0); + + var pose = Frame3.FromSurface(a, b, c); + + var xDir = pose.Rotation.Rotate(Vector3.EX); + xDir.X.Should().BeApproximately(1, Tolerance); + xDir.Y.Should().BeApproximately(0, Tolerance); + xDir.Z.Should().BeApproximately(0, Tolerance); + } + + [Fact] + public void FromSurface_ThreePoints_RightHandRule_ZPointsUp() + { + // Counter-clockwise in XY plane when viewed from above + var a = new Point3(0, 0, 0); + var b = new Point3(1, 0, 0); // +X + var c = new Point3(0, 1, 0); // +Y + // Right-hand rule: X×Y = +Z + + var pose = Frame3.FromSurface(a, b, c); + + var zDir = pose.Rotation.Rotate(Vector3.EZ); + zDir.X.Should().BeApproximately(0, Tolerance); + zDir.Y.Should().BeApproximately(0, Tolerance); + zDir.Z.Should().BeApproximately(1, Tolerance); + } + + [Fact] + public void FromSurface_ThreePoints_ReversedOrder_ZPointsDown() + { + // Reversed order: c, b, a (clockwise when viewed from above) + var a = new Point3(0, 1, 0); // was c + var b = new Point3(1, 0, 0); // same + var c = new Point3(0, 0, 0); // was a + // Right-hand rule with reversed winding: Z points down + + var pose = Frame3.FromSurface(a, b, c); + + var zDir = pose.Rotation.Rotate(Vector3.EZ); + zDir.Z.Should().BeLessThan(0); + } + + [Fact] + public void FromSurface_ThreePoints_VerticalPlane() + { + // XZ plane (vertical wall) + var a = new Point3(0, 0, 0); + var b = new Point3(1, 0, 0); // +X + var c = new Point3(0, 0, 1); // +Z + // Right-hand rule: X×Z = -Y + + var pose = Frame3.FromSurface(a, b, c); + + var zDir = pose.Rotation.Rotate(Vector3.EZ); + zDir.X.Should().BeApproximately(0, Tolerance); + zDir.Y.Should().BeApproximately(-1, Tolerance); + zDir.Z.Should().BeApproximately(0, Tolerance); + } + + [Fact] + public void FromSurface_ThreePoints_ThrowsWhenCollinear() + { + var a = new Point3(0, 0, 0); + var b = new Point3(1, 0, 0); + var c = new Point3(2, 0, 0); + + var act = () => Frame3.FromSurface(a, b, c); + + act.Should().Throw() + .WithMessage("*collinear*"); + } + + [Fact] + public void FromSurface_ThreePoints_PointsOnSurface_HaveZeroLocalZ() + { + var a = new Point3(10, 20, 5); + var b = new Point3(15, 20, 5); + var c = new Point3(10, 25, 5); + + var pose = Frame3.FromSurface(a, b, c); + var inverse = pose.Inverse(); + + // All three points should have Z=0 in local coordinates + inverse.TransformPoint(a).Z.Should().BeApproximately(0, 0.0001); + inverse.TransformPoint(b).Z.Should().BeApproximately(0, 0.0001); + inverse.TransformPoint(c).Z.Should().BeApproximately(0, 0.0001); + } + + #endregion + + #region FromSurface (N-point, best-fit plane) + + [Fact] + public void FromSurface_NPoints_OriginIsCentroid() + { + var points = new Point3[] + { + new(0, 0, 0), new(10, 0, 0), new(10, 10, 0), new(0, 10, 0) + }; + + var pose = Frame3.FromSurface(points); + + pose.Position.X.Should().BeApproximately(5, 0.0001); + pose.Position.Y.Should().BeApproximately(5, 0.0001); + pose.Position.Z.Should().BeApproximately(0, 0.0001); + } + + [Fact] + public void FromSurface_NPoints_HorizontalPlane_ZPointsUp() + { + // Counter-clockwise in XY plane → right-hand rule → Z points up + var points = new Point3[] + { + new(0, 0, 0), new(10, 0, 0), new(10, 10, 0), new(0, 10, 0) + }; + + var pose = Frame3.FromSurface(points); + var zDir = pose.Rotation.Rotate(Vector3.EZ); + + zDir.X.Should().BeApproximately(0, Tolerance); + zDir.Y.Should().BeApproximately(0, Tolerance); + zDir.Z.Should().BeApproximately(1, Tolerance); + } + + [Fact] + public void FromSurface_NPoints_XAxisAlongFirstEdge() + { + var points = new Point3[] + { + new(0, 0, 0), new(5, 0, 0), new(5, 5, 0), new(0, 5, 0) + }; + + var pose = Frame3.FromSurface(points); + var xDir = pose.Rotation.Rotate(Vector3.EX); + + // X-axis should be along points[0]→points[1] projected onto plane = +X + xDir.X.Should().BeApproximately(1, Tolerance); + xDir.Y.Should().BeApproximately(0, Tolerance); + xDir.Z.Should().BeApproximately(0, Tolerance); + } + + [Fact] + public void FromSurface_NPoints_AllPointsHaveNearZeroLocalZ() + { + // Coplanar points: all at Z=5 + var points = new Point3[] + { + new(0, 0, 5), new(10, 0, 5), new(10, 8, 5), + new(5, 12, 5), new(0, 8, 5) + }; + + var pose = Frame3.FromSurface(points); + var inverse = pose.Inverse(); + + for (int i = 0; i < points.Length; i++) + { + var local = inverse.TransformPoint(points[i]); + local.Z.Should().BeApproximately(0, 0.0001, $"Point {i} should have Z≈0 in local frame"); + } + } + + [Fact] + public void FromSurface_NPoints_NoisyPlane_NormalIsClose() + { + // Points approximately on Z=0 plane with small noise + var points = new Point3[] + { + new(0, 0, 0.01), new(10, 0, -0.01), new(10, 10, 0.02), + new(0, 10, -0.02), new(5, 5, 0.005), new(3, 7, -0.01) + }; + + var pose = Frame3.FromSurface(points); + var zDir = pose.Rotation.Rotate(Vector3.EZ); + + // Normal should be approximately +Z + Math.Abs(zDir.Z).Should().BeGreaterThan(0.999); + } + + [Fact] + public void FromSurface_NPoints_TiltedPlane() + { + // Points on the plane z = x (45° tilt around Y-axis) + var points = new Point3[] + { + new(0, 0, 0), new(1, 0, 1), new(1, 1, 1), + new(0, 1, 0), new(0.5, 0.5, 0.5) + }; + + var pose = Frame3.FromSurface(points); + var inverse = pose.Inverse(); + + for (int i = 0; i < points.Length; i++) + { + var local = inverse.TransformPoint(points[i]); + local.Z.Should().BeApproximately(0, 0.001, $"Point {i} should lie on the surface"); + } + } + + [Fact] + public void FromSurface_NPoints_MatchesThreePointForExactSquare() + { + // 4 points: first 3 define the same plane + var a = new Point3(0, 0, 0); + var b = new Point3(10, 0, 0); + var c = new Point3(10, 10, 0); + var d = new Point3(0, 10, 0); + + var pose3 = Frame3.FromSurface(a, b, c); + var poseN = Frame3.FromSurface([a, b, c, d]); + + // Normal directions should match + var z3 = pose3.Rotation.Rotate(Vector3.EZ); + var zN = poseN.Rotation.Rotate(Vector3.EZ); + z3.X.Should().BeApproximately(zN.X, 0.001); + z3.Y.Should().BeApproximately(zN.Y, 0.001); + z3.Z.Should().BeApproximately(zN.Z, 0.001); + + // X-axis directions should match + var x3 = pose3.Rotation.Rotate(Vector3.EX); + var xN = poseN.Rotation.Rotate(Vector3.EX); + x3.X.Should().BeApproximately(xN.X, 0.001); + x3.Y.Should().BeApproximately(xN.Y, 0.001); + x3.Z.Should().BeApproximately(xN.Z, 0.001); + } + + [Fact] + public void FromSurface_NPoints_ThrowsForFewerThan3() + { + var points = new Point3[] { new(0, 0, 0), new(1, 0, 0) }; + + var act = () => Frame3.FromSurface(points); + + act.Should().Throw() + .WithMessage("*3 points*"); + } + + [Fact] + public void FromSurface_NPoints_ThrowsForCollinearPoints() + { + var points = new Point3[] + { + new(0, 0, 0), new(1, 0, 0), new(2, 0, 0), new(3, 0, 0) + }; + + var act = () => Frame3.FromSurface(points); + + act.Should().Throw(); + } + + [Fact] + public void FromSurface_NPoints_DelegatesToThreePointFor3() + { + var a = new Point3(10, 20, 30); + var b = new Point3(11, 20, 30); + var c = new Point3(10, 21, 30); + + var pose3 = Frame3.FromSurface(a, b, c); + var poseN = Frame3.FromSurface([a, b, c]); + + // Exact same result — delegates to 3-point overload + pose3.Position.X.Should().Be(poseN.Position.X); + pose3.Position.Y.Should().Be(poseN.Position.Y); + pose3.Position.Z.Should().Be(poseN.Position.Z); + } + + #endregion + + #region Same frame as before the move (TS-129-01) + + // The pose each overload returned on Drawing 1.13.0.71 (master 3c711a8), printed with round-trip precision. + // The moved method must give the same frame within 1e-12 as that pose converted through FromPose3. + private static void ShouldMatchOldResult(Frame3 actual, Pose3 old) + { + var expected = Frame3.FromPose3(old, 1.0); + var (dp, da) = Frame3.Distance(actual, expected); + dp.Should().BeLessThan(1e-12); + da.Should().BeLessThan(1e-12); + } + + [Fact] + public void FromSurface_ThreePoints_SameFrameAsBeforeTheMove() + { + var frame = Frame3.FromSurface(new Point3(10, 20, 5), new Point3(15, 21, 6), new Point3(9, 25, 8)); + + ShouldMatchOldResult(frame, new Pose3(10, 20, 5, 30.000000000000004, -11.095803283136393, 11.309932474020215)); + } + + [Fact] + public void FromSurface_HintPoint_SameFrameAsBeforeTheMove() + { + var frame = Frame3.FromSurface(new Point3(10, 20, 5), new Point3(15, 20, 5), + new Point3(10, 25, 8), new Point3(12, 22, 20)); + + ShouldMatchOldResult(frame, new Pose3(12, 28.08823529411765, 9.852941176470589, 30.963756532073525, -0.0, 0)); + } + + [Fact] + public void FromSurface_NPoints_SameFrameAsBeforeTheMove() + { + var frame = Frame3.FromSurface(new Point3[] + { + new(0, 0, 0), new(1, 0, 1), new(1, 1, 1.1), new(0, 1, 0.05), new(0.5, 0.5, 0.52) + }); + + ShouldMatchOldResult(frame, new Pose3(0.5, 0.5, 0.534, 3.037244313390312, -45.71745485306115, -0.05383375807035245)); + } + + #endregion +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Frame3TestData.cs b/Sources/ModelingEvolution.Drawing.Tests/Frame3TestData.cs new file mode 100644 index 0000000..c7e0ec2 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Frame3TestData.cs @@ -0,0 +1,39 @@ +namespace ModelingEvolution.Drawing.Tests; + +/// Random frames and comparisons shared by the Frame3 scenarios (seed 129, epic-129 test rules). +internal static class Frame3TestData +{ + /// A uniformly random unit quaternion (Shoemake). + public static Quaternion RandomRotation(Random rng) + { + double u1 = rng.NextDouble(), u2 = rng.NextDouble() * 2 * Math.PI, u3 = rng.NextDouble() * 2 * Math.PI; + double a = Math.Sqrt(1 - u1), b = Math.Sqrt(u1); + return new Quaternion(b * Math.Cos(u3), a * Math.Sin(u2), a * Math.Cos(u2), b * Math.Sin(u3)); + } + + public static double Uniform(Random rng, double half) => (rng.NextDouble() * 2 - 1) * half; + + /// Position in ± per axis, random unit quaternion. + public static Frame3 RandomFrame(Random rng, double half = 2.0) => + new(new Point3(Uniform(rng, half), Uniform(rng, half), Uniform(rng, half)), RandomRotation(rng)); + + /// The largest component difference of two quaternions, taking q and −q as the same rotation. + public static double QuaternionError(Quaternion a, Quaternion b) + { + double plus = Math.Max(Math.Max(Math.Abs(a.W - b.W), Math.Abs(a.X - b.X)), Math.Max(Math.Abs(a.Y - b.Y), Math.Abs(a.Z - b.Z))); + double minus = Math.Max(Math.Max(Math.Abs(a.W + b.W), Math.Abs(a.X + b.X)), Math.Max(Math.Abs(a.Y + b.Y), Math.Abs(a.Z + b.Z))); + return Math.Min(plus, minus); + } + + /// Rotation matrix (row-major) of a unit quaternion. + public static double[,] ToMatrix(Quaternion q) + { + double w = q.W, x = q.X, y = q.Y, z = q.Z; + return new[,] + { + { 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y) }, + { 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x) }, + { 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) }, + }; + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Frame3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Frame3Tests.cs new file mode 100644 index 0000000..4b9fe5f --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Frame3Tests.cs @@ -0,0 +1,197 @@ +using FluentAssertions; +using static ModelingEvolution.Drawing.Tests.Frame3TestData; + +namespace ModelingEvolution.Drawing.Tests; + +/// TS-129-01: the algebra (epic-129 design § "Drawing: the kinematic chain"). +public class Frame3Tests +{ + private const int Samples = 10_000; + private const double PositionTolerance = 1e-12; + private const double AngleTolerance = 1e-9; + + [Fact] + public void Multiply_ByInverse_IsIdentityOver10000RandomFrames() + { + var rng = new Random(129); + for (int i = 0; i < Samples; i++) + { + var a = RandomFrame(rng); + + var (dp, da) = Frame3.Distance(a * a.Inverse(), Frame3.Identity); + var (dp2, da2) = Frame3.Distance(a.Inverse() * a, Frame3.Identity); + + dp.Should().BeLessThan(PositionTolerance); + da.Should().BeLessThan(AngleTolerance); + dp2.Should().BeLessThan(PositionTolerance); + da2.Should().BeLessThan(AngleTolerance); + } + } + + [Fact] + public void Multiply_IsAssociativeOver10000RandomFrames() + { + var rng = new Random(129); + for (int i = 0; i < Samples; i++) + { + var a = RandomFrame(rng); + var b = RandomFrame(rng); + var c = RandomFrame(rng); + + var (dp, da) = Frame3.Distance((a * b) * c, a * (b * c)); + + dp.Should().BeLessThan(PositionTolerance); + da.Should().BeLessThan(AngleTolerance); + } + } + + [Fact] + public void TransformPoint_EqualsRotatePlusPositionOver10000RandomFrames() + { + var rng = new Random(129); + for (int i = 0; i < Samples; i++) + { + var a = RandomFrame(rng); + var p = new Point3(Uniform(rng, 2), Uniform(rng, 2), Uniform(rng, 2)); + + var expected = (Point3)a.Rotation.Rotate(p) + (Vector3)a.Position; + + Point3.Distance(a.TransformPoint(p), expected).Should().BeLessThan(PositionTolerance); + } + } + + [Fact] + public void Multiply_TransformsLikeComposition() + { + var rng = new Random(129); + for (int i = 0; i < 1000; i++) + { + var a = RandomFrame(rng); + var b = RandomFrame(rng); + var p = new Point3(Uniform(rng, 2), Uniform(rng, 2), Uniform(rng, 2)); + + Point3.Distance((a * b).TransformPoint(p), a.TransformPoint(b.TransformPoint(p))) + .Should().BeLessThan(PositionTolerance); + Point3.Distance(a.Inverse().TransformPoint(a.TransformPoint(p)), p) + .Should().BeLessThan(PositionTolerance); + } + } + + [Fact] + public void Multiply_MethodAndOperatorAgree() + { + var rng = new Random(129); + var a = RandomFrame(rng); + var b = RandomFrame(rng); + + a.Multiply(b).Should().Be(a * b); + } + + [Fact] + public void Identity_IsNeutral() + { + var a = RandomFrame(new Random(129)); + + (Frame3.Identity * a).Should().Be(a); + (a * Frame3.Identity).Should().Be(a); + } + + [Fact] + public void TransformPoint_WithTranslationOnly() + { + var frame = new Frame3(new Point3(10, 20, 30), Quaternion.Identity); + + frame.TransformPoint(new Point3(1, 2, 3)).Should().Be(new Point3(11, 22, 33)); + } + + [Fact] + public void TransformPoint_WithRotationOnly() + { + var frame = new Frame3(Point3.Zero, Quaternion.FromAxisAngle(Vector3.EZ, Math.PI / 2)); + + var p = frame.TransformPoint(new Point3(1, 0, 0)); + + p.X.Should().BeApproximately(0, PositionTolerance); + p.Y.Should().BeApproximately(1, PositionTolerance); + p.Z.Should().BeApproximately(0, PositionTolerance); + } + + [Fact] + public void TransformVector_RotatesButDoesNotTranslate() + { + var frame = new Frame3(new Point3(100, 200, 300), Quaternion.FromAxisAngle(Vector3.EZ, Math.PI / 2)); + + var v = frame.TransformVector(Vector3.EX); + + v.X.Should().BeApproximately(0, PositionTolerance); + v.Y.Should().BeApproximately(1, PositionTolerance); + v.Z.Should().BeApproximately(0, PositionTolerance); + } + + [Fact] + public void Interpolate_AtZeroAndOne_ReturnsTheEnds() + { + var rng = new Random(129); + for (int i = 0; i < 1000; i++) + { + var a = RandomFrame(rng); + var b = RandomFrame(rng); + + var at0 = Frame3.Interpolate(a, b, 0); + var at1 = Frame3.Interpolate(a, b, 1); + + Point3.Distance(at0.Position, a.Position).Should().BeLessThan(PositionTolerance); + QuaternionError(at0.Rotation, a.Rotation).Should().BeLessThan(1e-12); + Point3.Distance(at1.Position, b.Position).Should().BeLessThan(PositionTolerance); + QuaternionError(at1.Rotation, b.Rotation).Should().BeLessThan(1e-12); + } + } + + [Fact] + public void Interpolate_AtHalf_GivesMidpointAndHalfAngle() + { + var rng = new Random(129); + for (int i = 0; i < 1000; i++) + { + var a = RandomFrame(rng); + var b = RandomFrame(rng); + + var mid = Frame3.Interpolate(a, b, 0.5); + + var expected = new Point3((a.Position.X + b.Position.X) / 2, (a.Position.Y + b.Position.Y) / 2, (a.Position.Z + b.Position.Z) / 2); + Point3.Distance(mid.Position, expected).Should().BeLessThan(PositionTolerance); + var whole = Frame3.Distance(a, b).Angle; + Frame3.Distance(a, mid).Angle.Should().BeApproximately(whole / 2, 1e-12); + Frame3.Distance(mid, b).Angle.Should().BeApproximately(whole / 2, 1e-12); + } + } + + [Fact] + public void Distance_GivesPositionAndAngleError() + { + var rng = new Random(129); + for (int i = 0; i < 1000; i++) + { + var a = RandomFrame(rng); + var offset = new Vector3(Uniform(rng, 1), Uniform(rng, 1), Uniform(rng, 1)); + var axis = new Vector3(Uniform(rng, 1) + 2, Uniform(rng, 1), Uniform(rng, 1)).Normalize(); + var angle = 0.01 + rng.NextDouble() * (Math.PI - 0.02); + var b = a * new Frame3((Point3)offset, Quaternion.FromAxisAngle(axis, angle)); + + var (position, rotation) = Frame3.Distance(a, b); + + position.Should().BeApproximately(offset.Length, 1e-12); + rotation.Should().BeApproximately(angle, 1e-12); + Frame3.Distance(b, a).Angle.Should().BeApproximately(angle, 1e-12); + } + } + + [Fact] + public void Distance_PositionPart_IsEuclidean() + { + var a = new Frame3(Point3.Zero, Quaternion.Identity); + var b = new Frame3(new Point3(3, 4, 0), Quaternion.FromRpy(1, 0.5, 0.3)); + + Frame3.Distance(a, b).Position.Should().Be(5); + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Kinematics/ForwardTests.cs b/Sources/ModelingEvolution.Drawing.Tests/Kinematics/ForwardTests.cs new file mode 100644 index 0000000..763e8f5 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Kinematics/ForwardTests.cs @@ -0,0 +1,125 @@ +using FluentAssertions; + +namespace ModelingEvolution.Drawing.Tests.Kinematics; + +/// TS-129-04: Forward against closed-form answers. +public class ForwardTests +{ + [Fact] + public void Planar2R_TipMatchesClosedFormAt1000RandomQ() + { + var chain = TestChains.Planar2R(); + var tip = chain.FrameIndex("tip"); + var rng = new Random(129); + Span q = stackalloc double[chain.Dof]; + var links = new Frame3[chain.Links.Length]; + + for (int i = 0; i < 1000; i++) + { + TestChains.RandomQ(rng, chain, q); + chain.Forward(q, links); + var p = chain.FramePose(links, tip).Position; + + p.X.Should().BeApproximately(0.4 * Math.Cos(q[0]) + 0.3 * Math.Cos(q[0] + q[1]), 1e-12); + p.Y.Should().BeApproximately(0.4 * Math.Sin(q[0]) + 0.3 * Math.Sin(q[0] + q[1]), 1e-12); + p.Z.Should().BeApproximately(0, 1e-12); + } + } + + [Fact] + public void Slider1P_TipMatchesClosedFormAt1000RandomQ() + { + var chain = TestChains.Slider1P(); + var tip = chain.FrameIndex("tip"); + var rng = new Random(129); + Span q = stackalloc double[chain.Dof]; + var links = new Frame3[chain.Links.Length]; + + for (int i = 0; i < 1000; i++) + { + TestChains.RandomQ(rng, chain, q); + chain.Forward(q, links); + var p = chain.FramePose(links, tip).Position; + + p.X.Should().BeApproximately(q[0], 1e-12); + p.Y.Should().BeApproximately(0, 1e-12); + p.Z.Should().BeApproximately(0, 1e-12); + } + } + + [Fact] + public void Forward_AtZero_ComposesTheOrigins() + { + var chain = TestChains.Arm6(); + var links = new Frame3[chain.Links.Length]; + + chain.Forward(new double[chain.Dof], links); + + var expected = Frame3.Identity; + for (int c = 1; c < links.Length; c++) + { + expected = expected * chain.Joints[c - 1].Origin; + var (dp, da) = Frame3.Distance(links[c], expected); + dp.Should().BeLessThan(1e-12); + da.Should().BeLessThan(1e-9); + } + } + + [Fact] + public void Forward_LinksOfWrongLength_Throws() + { + var chain = TestChains.Planar2R(); + var q = new double[chain.Dof]; + + var tooShort = () => chain.Forward(q, new Frame3[chain.Links.Length - 1]); + var tooLong = () => chain.Forward(q, new Frame3[chain.Links.Length + 1]); + + tooShort.Should().Throw().WithParameterName("links"); + tooLong.Should().Throw().WithParameterName("links"); + } + + [Fact] + public void Forward_QOfWrongLength_Throws() + { + var chain = TestChains.Planar2R(); + + var act = () => chain.Forward(new double[chain.Dof + 1], new Frame3[chain.Links.Length]); + + act.Should().Throw().WithParameterName("q"); + } + + [Fact] + public void FramePose_LinksOfWrongLength_Throws() + { + var chain = TestChains.Planar2R(); + + var act = () => chain.FramePose(new Frame3[1], 0); + + act.Should().Throw().WithParameterName("links"); + } + + [Fact] + public void ForwardAndJacobian_DoNotAllocate() + { + var chain = TestChains.RailArm6(); + var flange = chain.FrameIndex("flange"); + var q = new double[chain.Dof]; + var links = new Frame3[chain.Links.Length]; + var j = new double[6 * chain.Dof]; + var rng = new Random(129); + TestChains.RandomQ(rng, chain, q); + chain.Forward(q, links); + chain.Jacobian(links, flange, j); + + var before = GC.GetAllocatedBytesForCurrentThread(); + for (int i = 0; i < 100; i++) + { + chain.Forward(q, links); + chain.FramePose(links, flange); + chain.Jacobian(links, flange, j); + } + var allocated = GC.GetAllocatedBytesForCurrentThread() - before; + + allocated.Should().Be(0); + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Kinematics/JacobianTests.cs b/Sources/ModelingEvolution.Drawing.Tests/Kinematics/JacobianTests.cs new file mode 100644 index 0000000..3386f7c --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Kinematics/JacobianTests.cs @@ -0,0 +1,117 @@ +using FluentAssertions; +using ModelingEvolution.Drawing.Kinematics; + +namespace ModelingEvolution.Drawing.Tests.Kinematics; + +/// TS-129-05: the geometric Jacobian against central differences of Forward (h = 1e-7). +public class JacobianTests +{ + private const double H = 1e-7; + + private static void AssertMatchesCentralDifferences(KinematicChain chain) + { + var flange = chain.FrameIndex("flange"); + int dof = chain.Dof; + var rng = new Random(129); + var q = new double[dof]; + var links = new Frame3[chain.Links.Length]; + var j = new double[6 * dof]; + + for (int n = 0; n < 1000; n++) + { + TestChains.RandomQ(rng, chain, q); + chain.Forward(q, links); + chain.Jacobian(links, flange, j); + + for (int k = 0; k < dof; k++) + { + var qk = q[k]; + q[k] = qk + H; + chain.Forward(q, links); + var plus = chain.FramePose(links, flange); + q[k] = qk - H; + chain.Forward(q, links); + var minus = chain.FramePose(links, flange); + q[k] = qk; + + var v = (plus.Position - minus.Position) / (2 * H); + // World-frame rotation between the two samples: dR = R+ · R-^T ≈ exp([ω] 2h). + var d = plus.Rotation * minus.Rotation.Conjugate(); + var sign = d.W < 0 ? -1.0 : 1.0; + var w = new Vector3(d.X, d.Y, d.Z) * (sign / H); + + var col = j.AsSpan(6 * k, 6); + col[0].Should().BeApproximately(v.X, 1e-6, $"vx of joint {k} at sample {n}"); + col[1].Should().BeApproximately(v.Y, 1e-6, $"vy of joint {k} at sample {n}"); + col[2].Should().BeApproximately(v.Z, 1e-6, $"vz of joint {k} at sample {n}"); + col[3].Should().BeApproximately(w.X, 1e-6, $"wx of joint {k} at sample {n}"); + col[4].Should().BeApproximately(w.Y, 1e-6, $"wy of joint {k} at sample {n}"); + col[5].Should().BeApproximately(w.Z, 1e-6, $"wz of joint {k} at sample {n}"); + } + } + } + + [Fact] + public void Arm6_FlangeJacobianMatchesCentralDifferencesAt1000RandomQ() => + AssertMatchesCentralDifferences(TestChains.Arm6()); + + [Fact] + public void RailArm6_FlangeJacobianMatchesCentralDifferencesAt1000RandomQ() => + AssertMatchesCentralDifferences(TestChains.RailArm6()); + + [Fact] + public void RailArm6_PrismaticColumnIsAxisAndZeroRotation() + { + var chain = TestChains.RailArm6(); + var flange = chain.FrameIndex("flange"); + var rng = new Random(129); + var q = new double[chain.Dof]; + var links = new Frame3[chain.Links.Length]; + var j = new double[6 * chain.Dof]; + + for (int n = 0; n < 100; n++) + { + TestChains.RandomQ(rng, chain, q); + chain.Forward(q, links); + chain.Jacobian(links, flange, j); + + j.AsSpan(0, 6).ToArray().Should().Equal(1.0, 0.0, 0.0, 0.0, 0.0, 0.0); + } + } + + [Fact] + public void Jacobian_OfAFrameOnTheBase_IsZero() + { + var chain = TestChains.Arm6(); + var links = new Frame3[chain.Links.Length]; + var j = new double[6 * chain.Dof]; + Array.Fill(j, double.NaN); + chain.Forward(new double[] { 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 }, links); + + chain.Jacobian(links, chain.FrameIndex("base"), j); + + j.Should().OnlyContain(x => x == 0); + } + + [Fact] + public void Jacobian_OutputOfWrongLength_Throws() + { + var chain = TestChains.Arm6(); + var links = new Frame3[chain.Links.Length]; + chain.Forward(new double[chain.Dof], links); + + var act = () => chain.Jacobian(links, 1, new double[6 * chain.Dof - 1]); + + act.Should().Throw().WithParameterName("j"); + } + + [Fact] + public void Jacobian_LinksOfWrongLength_Throws() + { + var chain = TestChains.Arm6(); + + var act = () => chain.Jacobian(new Frame3[2], 1, new double[6 * chain.Dof]); + + act.Should().Throw().WithParameterName("links"); + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Kinematics/KinematicChainTests.cs b/Sources/ModelingEvolution.Drawing.Tests/Kinematics/KinematicChainTests.cs new file mode 100644 index 0000000..d1915db --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Kinematics/KinematicChainTests.cs @@ -0,0 +1,127 @@ +using System.Collections.Immutable; +using FluentAssertions; +using ModelingEvolution.Drawing.Kinematics; + +namespace ModelingEvolution.Drawing.Tests.Kinematics; + +/// TS-129-03: chain validation. +public class KinematicChainTests +{ + private static readonly ImmutableArray TwoLinks = ImmutableArray.Create(new Link("base", -1), new Link("arm", 0)); + + private static Joint Hinge(string name = "hinge", Vector3? axis = null, double min = -1, double max = 1, double? maxAcc = null) => + new(name, JointType.Revolute, 1, Frame3.Identity, axis ?? Vector3.EZ, min, max, 2, maxAcc); + + private static ImmutableArray> Frames(params string[] names) => + names.Select(n => new ChainFrame(n, 1, Frame3.Identity)).ToImmutableArray(); + + [Fact] + public void Constructor_LinkParentNotBeforeItself_ThrowsNamingTheLink() + { + var links = ImmutableArray.Create(new Link("base", -1), new Link("arm", 0), new Link("loop", 2)); + var joints = ImmutableArray.Create(Hinge("j1"), Hinge("j2") with { Child = 2 }); + + var act = () => new KinematicChain(links, joints, Frames("tip")); + + act.Should().Throw().WithMessage("*'loop'*"); + } + + [Fact] + public void Constructor_ParentAfterChild_ThrowsNamingTheLink() + { + var links = ImmutableArray.Create(new Link("base", -1), new Link("early", 2), new Link("late", 0)); + var joints = ImmutableArray.Create(Hinge("j1"), Hinge("j2") with { Child = 2 }); + + var act = () => new KinematicChain(links, joints, Frames("tip")); + + act.Should().Throw().WithMessage("*'early'*"); + } + + [Fact] + public void Constructor_ZeroAxis_ThrowsNamingTheJoint() + { + var act = () => new KinematicChain(TwoLinks, ImmutableArray.Create(Hinge("wrist", Vector3.Zero)), Frames("tip")); + + act.Should().Throw().WithMessage("*'wrist'*unit*"); + } + + [Fact] + public void Constructor_NonUnitAxis_ThrowsNamingTheJoint() + { + var act = () => new KinematicChain(TwoLinks, ImmutableArray.Create(Hinge("wrist", new Vector3(0, 0, 2))), Frames("tip")); + + act.Should().Throw().WithMessage("*'wrist'*unit*"); + } + + [Fact] + public void Constructor_MinGreaterThanMax_ThrowsNamingTheJoint() + { + var act = () => new KinematicChain(TwoLinks, ImmutableArray.Create(Hinge("elbow", min: 1, max: -1)), Frames("tip")); + + act.Should().Throw().WithMessage("*'elbow'*Min*Max*"); + } + + [Fact] + public void Constructor_DuplicateFrameNames_ThrowsNamingTheFrame() + { + var act = () => new KinematicChain(TwoLinks, ImmutableArray.Create(Hinge()), Frames("tcp", "flange", "tcp")); + + act.Should().Throw().WithMessage("*'tcp'*duplicate*"); + } + + [Fact] + public void Constructor_LinkWithoutJoint_ThrowsNamingTheLink() + { + var act = () => new KinematicChain(TwoLinks, ImmutableArray>.Empty, Frames("tip")); + + act.Should().Throw().WithMessage("*'arm'*no joint*"); + } + + [Fact] + public void Constructor_TwoJointsOnOneLink_ThrowsNamingTheJoint() + { + var act = () => new KinematicChain(TwoLinks, ImmutableArray.Create(Hinge("first"), Hinge("second")), Frames("tip")); + + act.Should().Throw().WithMessage("*'second'*"); + } + + [Fact] + public void Constructor_FrameOnMissingLink_ThrowsNamingTheFrame() + { + var frames = ImmutableArray.Create(new ChainFrame("ghost", 5, Frame3.Identity)); + + var act = () => new KinematicChain(TwoLinks, ImmutableArray.Create(Hinge()), frames); + + act.Should().Throw().WithMessage("*'ghost'*"); + } + + [Fact] + public void ValidChain_DofIsTheJointCount() + { + TestChains.Arm6().Dof.Should().Be(6); + TestChains.RailArm6().Dof.Should().Be(7); + TestChains.Planar2R().Dof.Should().Be(2); + TestChains.Slider1P().Dof.Should().Be(1); + var chain = TestChains.Arm6(); + chain.Dof.Should().Be(chain.Joints.Length); + } + + [Fact] + public void ValidChain_FrameIndex() + { + var chain = TestChains.Arm6(); + + chain.FrameIndex("absent").Should().Be(-1); + chain.FrameIndex("base").Should().Be(0); + chain.FrameIndex("flange").Should().Be(1); + } + + [Fact] + public void ValidChain_MaxAccelerationMayBeNull() + { + var chain = new KinematicChain(TwoLinks, ImmutableArray.Create(Hinge(maxAcc: null)), Frames("tip")); + + chain.Joints[0].MaxAcceleration.Should().BeNull(); + TestChains.Arm6().Joints[0].MaxAcceleration.Should().Be(10); + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Kinematics/TestChains.cs b/Sources/ModelingEvolution.Drawing.Tests/Kinematics/TestChains.cs new file mode 100644 index 0000000..264e4a6 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Kinematics/TestChains.cs @@ -0,0 +1,92 @@ +using System.Collections.Immutable; +using ModelingEvolution.Drawing.Kinematics; + +namespace ModelingEvolution.Drawing.Tests.Kinematics; + +/// +/// Test data for the kinematic chain scenarios (epic-129 TS-129-03..05). Metres and radians. +/// test.arm6 is a synthetic 6R arm with non-trivial joint origins and axes; Matrix.Fixtures copies this +/// definition later, so a change here is a change of the fixture. +/// +internal static class TestChains +{ + public static Frame3 Origin(double x, double y, double z, double roll, double pitch, double yaw) => + new(new Point3(x, y, z), Quaternion.FromRpy(roll, pitch, yaw)); + + public static Vector3 Unit(double x, double y, double z) => new Vector3(x, y, z).Normalize(); + + private static Joint R(string name, int child, Frame3 origin, Vector3 axis, double min, double max, double? maxAcc = null) => + new(name, JointType.Revolute, child, origin, axis, min, max, 3.14, maxAcc); + + /// The six revolute joints of test.arm6, child links 1..6 shifted by . + private static IEnumerable> Arm6Joints(int linkOffset, Frame3 j1Origin) => + [ + R("j1", 1 + linkOffset, j1Origin, Unit(0, 0, 1), -3.05, 3.05, 10), + R("j2", 2 + linkOffset, Origin(0.050, -0.030, 0.120, Math.PI / 2, 0, 0.1), Unit(0, 0, 1), -2.60, 2.60, 10), + R("j3", 3 + linkOffset, Origin(0.420, 0.010, 0.000, 0, 0, 0.2), Unit(0, 0, -1), -2.80, 2.80, 10), + R("j4", 4 + linkOffset, Origin(0.380, -0.020, 0.090, -Math.PI / 2, 0.05, 0), Unit(0, 0.6, 0.8), -3.05, 3.05), + R("j5", 5 + linkOffset, Origin(0.010, 0.110, 0.030, Math.PI / 2, 0, 0), Unit(1, 0, 0), -2.10, 2.10), + R("j6", 6 + linkOffset, Origin(0.000, 0.020, 0.095, 0, -Math.PI / 2, 0.3), Unit(0, 0, 1), -6.28, 6.28), + ]; + + /// test.arm6: links base, link1..link6; frames base (on base) and flange (on link6). + public static KinematicChain Arm6() + { + var links = ImmutableArray.Create( + new Link("base", -1), new Link("link1", 0), new Link("link2", 1), new Link("link3", 2), + new Link("link4", 3), new Link("link5", 4), new Link("link6", 5)); + var joints = Arm6Joints(0, Origin(0, 0, 0.180, 0, 0, 0)).ToImmutableArray(); + var frames = ImmutableArray.Create( + new ChainFrame("base", 0, Frame3.Identity), + new ChainFrame("flange", 6, Origin(0, 0, 0.045, Math.PI, 0, 0))); + return new KinematicChain(links, joints, frames); + } + + /// + /// A 1P+6R chain: a rail (prismatic along +X, 0..3 m) whose carriage carries test.arm6 + /// (arm base 0.10 m off the carriage in Y, 0.05 m up). + /// + public static KinematicChain RailArm6() + { + var links = ImmutableArray.Create( + new Link("base", -1), new Link("carriage", 0), new Link("link1", 1), new Link("link2", 2), new Link("link3", 3), + new Link("link4", 4), new Link("link5", 5), new Link("link6", 6)); + var rail = new Joint("rail", JointType.Prismatic, 1, Origin(0, 0, 0.25, 0, 0, 0), Unit(1, 0, 0), 0, 3, 1.0, null); + var joints = ImmutableArray.Create(rail).AddRange(Arm6Joints(1, Origin(0, 0.10, 0.230, 0, 0, 0))); + var frames = ImmutableArray.Create( + new ChainFrame("base", 0, Frame3.Identity), + new ChainFrame("flange", 7, Origin(0, 0, 0.045, Math.PI, 0, 0))); + return new KinematicChain(links, joints, frames); + } + + /// A planar 2R chain: link lengths 0.4 m and 0.3 m, axes +Z; frame tip at the end of link 2. + public static KinematicChain Planar2R() + { + var links = ImmutableArray.Create(new Link("base", -1), new Link("upper", 0), new Link("lower", 1)); + var joints = ImmutableArray.Create( + new Joint("shoulder", JointType.Revolute, 1, Frame3.Identity, Vector3.EZ, -Math.PI, Math.PI, 2, null), + new Joint("elbow", JointType.Revolute, 2, Origin(0.4, 0, 0, 0, 0, 0), Vector3.EZ, -Math.PI, Math.PI, 2, null)); + var frames = ImmutableArray.Create(new ChainFrame("tip", 2, Origin(0.3, 0, 0, 0, 0, 0))); + return new KinematicChain(links, joints, frames); + } + + /// A 1P chain: a slider along +X, range 0..3 m; frame tip on the slider. + public static KinematicChain Slider1P() + { + var links = ImmutableArray.Create(new Link("base", -1), new Link("slider", 0)); + var joints = ImmutableArray.Create( + new Joint("slide", JointType.Prismatic, 1, Frame3.Identity, Vector3.EX, 0, 3, 1, 2)); + var frames = ImmutableArray.Create(new ChainFrame("tip", 1, Frame3.Identity)); + return new KinematicChain(links, joints, frames); + } + + /// Fills uniformly within each joint's limits. + public static void RandomQ(Random rng, KinematicChain chain, Span q) + { + for (int i = 0; i < q.Length; i++) + { + var j = chain.Joints[i]; + q[i] = j.Min + rng.NextDouble() * (j.Max - j.Min); + } + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Pose3SurfaceTests.cs b/Sources/ModelingEvolution.Drawing.Tests/Pose3SurfaceTests.cs new file mode 100644 index 0000000..289a21f --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Pose3SurfaceTests.cs @@ -0,0 +1,81 @@ +using System.Reflection; +using FluentAssertions; + +namespace ModelingEvolution.Drawing.Tests; + +/// +/// TS-129-55: is a wire and edit format only (epic-129 design C1, ADR-11). Its maths lives in +/// ; a new member fails this test until the kept list is changed on purpose. +/// +public class Pose3SurfaceTests +{ + private const BindingFlags All = BindingFlags.Public | BindingFlags.NonPublic | BindingFlags.Static | BindingFlags.Instance | BindingFlags.DeclaredOnly; + + [Theory] + [InlineData("TransformPoint")] + [InlineData("TransformVector")] + [InlineData("Inverse")] + [InlineData("Multiply")] + [InlineData("op_Multiply")] + [InlineData("op_Addition")] + [InlineData("op_Subtraction")] + [InlineData("Lerp")] + [InlineData("Distance")] + [InlineData("FromSurface")] + public void DeletedMathsMember_DoesNotExist(string name) + { + typeof(Pose3).GetMembers(All).Where(m => m.Name == name).Should().BeEmpty(); + } + + [Fact] + public void PublicMembers_AreExactlyTheWireAndEditSet() + { + var actual = typeof(Pose3) + .GetMembers(BindingFlags.Public | BindingFlags.Static | BindingFlags.Instance | BindingFlags.DeclaredOnly) + .Select(Describe) + .OrderBy(s => s, StringComparer.Ordinal) + .ToArray(); + + string[] kept = + [ + // constructors + ".ctor(Point3`1, Rotation3`1)", + ".ctor(Double, Double, Double, Degree`1, Degree`1, Degree`1)", + ".ctor(Double, Double, Double, Double, Double, Double)", + // identity + "Identity", + // factories (named constructors) + "From(Point3`1, Rotation3`1)", + "From(Vector3`1, Rotation3`1)", + // properties and their accessors (Position/Rotation are init-only for `with`) + "IsIdentity", "get_IsIdentity()", + "Position", "get_Position()", "set_Position(Point3`1)", + "Rotation", "get_Rotation()", "set_Rotation(Rotation3`1)", + "Rx", "get_Rx()", "Ry", "get_Ry()", "Rz", "get_Rz()", + "X", "get_X()", "Y", "get_Y()", "Z", "get_Z()", + // collection-expression literals ([x, y, z, rx, ry, rz]) + "Enumerator", "GetEnumerator()", + // parsing and formatting + "Parse(String, IFormatProvider)", + "TryParse(String, IFormatProvider, Pose3`1&)", + "ToString()", + // tuple conversions and deconstruction + "op_Implicit(Pose3`1)", + "op_Implicit(ValueTuple`6)", + "Deconstruct(Double&, Double&, Double&, Double&, Double&, Double&)", + "Deconstruct(Point3`1&, Rotation3`1&)", + // equality + "Equals(Object)", "Equals(Pose3`1)", "GetHashCode()", "op_Equality(Pose3`1, Pose3`1)", "op_Inequality(Pose3`1, Pose3`1)", + // numeric type conversion + "Truncating()", + ]; + + actual.Should().Equal(kept.OrderBy(s => s, StringComparer.Ordinal)); + } + + private static string Describe(MemberInfo m) => m switch + { + MethodBase mb => $"{mb.Name}({string.Join(", ", mb.GetParameters().Select(p => p.ParameterType.Name))})", + _ => m.Name, + }; +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Pose3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Pose3Tests.cs index 88e1106..3d51664 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Pose3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Pose3Tests.cs @@ -85,107 +85,6 @@ public void Pose3_Identity_IsAtOriginWithNoRotation() #endregion - #region Transform Operations - - [Fact] - public void Pose3_TransformPoint_WithTranslationOnly() - { - var pose = new Pose3(10, 20, 30, 0, 0, 0); - var localPoint = new Point3(1, 2, 3); - - var worldPoint = pose.TransformPoint(localPoint); - - worldPoint.X.Should().Be(11); - worldPoint.Y.Should().Be(22); - worldPoint.Z.Should().Be(33); - } - - [Fact] - public void Pose3_TransformPoint_WithRotationOnly() - { - var pose = new Pose3(0, 0, 0, 0, 0, 90); // 90 deg around Z - var localPoint = new Point3(1, 0, 0); - - var worldPoint = pose.TransformPoint(localPoint); - - worldPoint.X.Should().BeApproximately(0, Tolerance); - worldPoint.Y.Should().BeApproximately(1, Tolerance); - worldPoint.Z.Should().BeApproximately(0, Tolerance); - } - - [Fact] - public void Pose3_TransformVector_RotatesButDoesNotTranslate() - { - var pose = new Pose3(100, 200, 300, 0, 0, 90); - var localVector = new Vector3(1, 0, 0); - - var worldVector = pose.TransformVector(localVector); - - worldVector.X.Should().BeApproximately(0, Tolerance); - worldVector.Y.Should().BeApproximately(1, Tolerance); - worldVector.Z.Should().BeApproximately(0, Tolerance); - } - - #endregion - - #region Lerp - - [Fact] - public void Pose3_Lerp_AtZero_ReturnsFirst() - { - var a = new Pose3(0, 0, 0, 0, 0, 0); - var b = new Pose3(100, 200, 300, 90, 0, 0); - - var result = Pose3.Lerp(a, b, 0); - - result.X.Should().Be(0); - result.Y.Should().Be(0); - result.Z.Should().Be(0); - } - - [Fact] - public void Pose3_Lerp_AtOne_ReturnsSecond() - { - var a = new Pose3(0, 0, 0, 0, 0, 0); - var b = new Pose3(100, 200, 300, 90, 0, 0); - - var result = Pose3.Lerp(a, b, 1); - - result.X.Should().Be(100); - result.Y.Should().Be(200); - result.Z.Should().Be(300); - } - - [Fact] - public void Pose3_Lerp_AtHalf_InterpolatesPosition() - { - var a = new Pose3(0, 0, 0, 0, 0, 0); - var b = new Pose3(100, 200, 300, 0, 0, 0); - - var result = Pose3.Lerp(a, b, 0.5); - - result.X.Should().Be(50); - result.Y.Should().Be(100); - result.Z.Should().Be(150); - } - - #endregion - - #region Distance - - [Fact] - public void Pose3_Distance_CalculatesPositionDistance() - { - var a = new Pose3(0, 0, 0, 0, 0, 0); - var b = new Pose3(3, 4, 0, 90, 45, 30); - - var distance = Pose3.Distance(a, b); - - distance.Should().Be(5); - } - - #endregion - #region Operators [Fact] @@ -282,477 +181,4 @@ public void Pose3_TryParse_ReturnsFalse_ForInvalidInput() } #endregion - - #region FromSurface - - [Fact] - public void FromSurface_HorizontalPlane_HintAbove_ZPointsUp() - { - // Horizontal plane at Z=0 (XY plane) - var a = new Point3(0, 0, 0); - var b = new Point3(1, 0, 0); - var c = new Point3(0, 1, 0); - // Hint point above the plane - var h = new Point3(0.5, 0.5, 10); - - var pose = Pose3.FromSurface(a, b, c, h); - - // Origin should be projection of h onto plane - pose.X.Should().BeApproximately(0.5, Tolerance); - pose.Y.Should().BeApproximately(0.5, Tolerance); - pose.Z.Should().BeApproximately(0, Tolerance); - - // Z-axis should point up (toward h) - var zDir = pose.Rotation.Rotate(Vector3.EZ); - zDir.X.Should().BeApproximately(0, Tolerance); - zDir.Y.Should().BeApproximately(0, Tolerance); - zDir.Z.Should().BeApproximately(1, Tolerance); - } - - [Fact] - public void FromSurface_HorizontalPlane_HintBelow_ZPointsDown() - { - // Horizontal plane at Z=0 - var a = new Point3(0, 0, 0); - var b = new Point3(1, 0, 0); - var c = new Point3(0, 1, 0); - // Hint point below the plane - var h = new Point3(0.5, 0.5, -10); - - var pose = Pose3.FromSurface(a, b, c, h); - - // Origin should be projection of h onto plane - pose.X.Should().BeApproximately(0.5, Tolerance); - pose.Y.Should().BeApproximately(0.5, Tolerance); - pose.Z.Should().BeApproximately(0, Tolerance); - - // Z-axis should point down (toward h) - var zDir = pose.Rotation.Rotate(Vector3.EZ); - zDir.X.Should().BeApproximately(0, Tolerance); - zDir.Y.Should().BeApproximately(0, Tolerance); - zDir.Z.Should().BeApproximately(-1, Tolerance); - } - - [Fact] - public void FromSurface_VerticalPlane_XZ_HintInFront() - { - // Vertical plane (XZ plane, Y=0) - var a = new Point3(0, 0, 0); - var b = new Point3(1, 0, 0); - var c = new Point3(0, 0, 1); - // Hint point in front (positive Y) - var h = new Point3(0.5, 5, 0.5); - - var pose = Pose3.FromSurface(a, b, c, h); - - // Origin should be projection of h onto plane (Y=0) - pose.X.Should().BeApproximately(0.5, Tolerance); - pose.Y.Should().BeApproximately(0, Tolerance); - pose.Z.Should().BeApproximately(0.5, Tolerance); - - // Z-axis should point toward positive Y - var zDir = pose.Rotation.Rotate(Vector3.EZ); - zDir.X.Should().BeApproximately(0, Tolerance); - zDir.Y.Should().BeApproximately(1, Tolerance); - zDir.Z.Should().BeApproximately(0, Tolerance); - } - - [Fact] - public void FromSurface_XAxisAlongEdgeAB() - { - // Horizontal plane - var a = new Point3(0, 0, 0); - var b = new Point3(2, 0, 0); // X direction - var c = new Point3(0, 2, 0); - var h = new Point3(1, 1, 5); - - var pose = Pose3.FromSurface(a, b, c, h); - - // X-axis should be along a→b direction (positive X) - var xDir = pose.Rotation.Rotate(Vector3.EX); - xDir.X.Should().BeApproximately(1, Tolerance); - xDir.Y.Should().BeApproximately(0, Tolerance); - xDir.Z.Should().BeApproximately(0, Tolerance); - } - - [Fact] - public void FromSurface_TiltedPlane() - { - // Plane tilted 45° around X axis (normal points in Y-Z direction) - // Plane equation: z = y (passes through origin with normal (0, -1, 1)) - var a = new Point3(0, 0, 0); - var b = new Point3(1, 0, 0); - var c = new Point3(0, 1, 1); // 45° tilt - // Hint point OFF the plane (z ≠ y, so z - y ≠ 0) - var h = new Point3(0.5, 2, 5); // z=5, y=2, so h is above the plane - - var pose = Pose3.FromSurface(a, b, c, h); - - // Z should point toward h - normal is (0, -1/√2, 1/√2) - // Since h has z > y, h is on the +normal side, so Z = +normal - var zDir = pose.Rotation.Rotate(Vector3.EZ); - zDir.X.Should().BeApproximately(0, Tolerance); - // Y and Z components should have same magnitude (45° angle from Y-Z plane) - Math.Abs(zDir.Y).Should().BeApproximately(Math.Abs(zDir.Z), 0.01); - // Z component should be positive (normal points toward +Z more than -Y) - zDir.Z.Should().BeGreaterThan(0); - } - - [Fact] - public void FromSurface_ThrowsWhenHintOnPlane() - { - var a = new Point3(0, 0, 0); - var b = new Point3(1, 0, 0); - var c = new Point3(0, 1, 0); - // Hint point ON the plane - var h = new Point3(0.5, 0.5, 0); - - var act = () => Pose3.FromSurface(a, b, c, h); - - act.Should().Throw() - .WithMessage("*lies on the surface*"); - } - - [Fact] - public void FromSurface_ThrowsWhenPointsCollinear() - { - // All three points on a line - var a = new Point3(0, 0, 0); - var b = new Point3(1, 0, 0); - var c = new Point3(2, 0, 0); - var h = new Point3(0, 1, 0); - - var act = () => Pose3.FromSurface(a, b, c, h); - - act.Should().Throw() - .WithMessage("*collinear*"); - } - - [Fact] - public void FromSurface_TransformPointOnSurface_HasZeroZ() - { - // Create any surface - var a = new Point3(10, 20, 5); - var b = new Point3(15, 20, 5); - var c = new Point3(10, 25, 8); - var h = new Point3(12, 22, 20); - - var pose = Pose3.FromSurface(a, b, c, h); - - // Point 'a' is on the surface, so in local coordinates its Z should be 0 - var inverse = pose.Inverse(); - var aLocal = inverse.TransformPoint(a); - - aLocal.Z.Should().BeApproximately(0, 0.0001); - } - - [Fact] - public void FromSurface_HintPointTransforms_ToPositiveZ() - { - var a = new Point3(0, 0, 0); - var b = new Point3(1, 0, 0); - var c = new Point3(0, 1, 0); - var h = new Point3(0.5, 0.5, 10); - - var pose = Pose3.FromSurface(a, b, c, h); - - // Transform h to local coordinates - should have positive Z - var inverse = pose.Inverse(); - var hLocal = inverse.TransformPoint(h); - - hLocal.Z.Should().BeGreaterThan(0); - // Local X and Y should be near 0 (h is directly above origin) - hLocal.X.Should().BeApproximately(0, 0.0001); - hLocal.Y.Should().BeApproximately(0, 0.0001); - } - - #endregion - - #region FromSurface (3-point, right-hand rule) - - [Fact] - public void FromSurface_ThreePoints_OriginAtA() - { - var a = new Point3(10, 20, 30); - var b = new Point3(11, 20, 30); - var c = new Point3(10, 21, 30); - - var pose = Pose3.FromSurface(a, b, c); - - pose.X.Should().Be(10); - pose.Y.Should().Be(20); - pose.Z.Should().Be(30); - } - - [Fact] - public void FromSurface_ThreePoints_XAxisAlongAB() - { - var a = new Point3(0, 0, 0); - var b = new Point3(5, 0, 0); - var c = new Point3(0, 5, 0); - - var pose = Pose3.FromSurface(a, b, c); - - var xDir = pose.Rotation.Rotate(Vector3.EX); - xDir.X.Should().BeApproximately(1, Tolerance); - xDir.Y.Should().BeApproximately(0, Tolerance); - xDir.Z.Should().BeApproximately(0, Tolerance); - } - - [Fact] - public void FromSurface_ThreePoints_RightHandRule_ZPointsUp() - { - // Counter-clockwise in XY plane when viewed from above - var a = new Point3(0, 0, 0); - var b = new Point3(1, 0, 0); // +X - var c = new Point3(0, 1, 0); // +Y - // Right-hand rule: X×Y = +Z - - var pose = Pose3.FromSurface(a, b, c); - - var zDir = pose.Rotation.Rotate(Vector3.EZ); - zDir.X.Should().BeApproximately(0, Tolerance); - zDir.Y.Should().BeApproximately(0, Tolerance); - zDir.Z.Should().BeApproximately(1, Tolerance); - } - - [Fact] - public void FromSurface_ThreePoints_ReversedOrder_ZPointsDown() - { - // Reversed order: c, b, a (clockwise when viewed from above) - var a = new Point3(0, 1, 0); // was c - var b = new Point3(1, 0, 0); // same - var c = new Point3(0, 0, 0); // was a - // Right-hand rule with reversed winding: Z points down - - var pose = Pose3.FromSurface(a, b, c); - - var zDir = pose.Rotation.Rotate(Vector3.EZ); - zDir.Z.Should().BeLessThan(0); - } - - [Fact] - public void FromSurface_ThreePoints_VerticalPlane() - { - // XZ plane (vertical wall) - var a = new Point3(0, 0, 0); - var b = new Point3(1, 0, 0); // +X - var c = new Point3(0, 0, 1); // +Z - // Right-hand rule: X×Z = -Y - - var pose = Pose3.FromSurface(a, b, c); - - var zDir = pose.Rotation.Rotate(Vector3.EZ); - zDir.X.Should().BeApproximately(0, Tolerance); - zDir.Y.Should().BeApproximately(-1, Tolerance); - zDir.Z.Should().BeApproximately(0, Tolerance); - } - - [Fact] - public void FromSurface_ThreePoints_ThrowsWhenCollinear() - { - var a = new Point3(0, 0, 0); - var b = new Point3(1, 0, 0); - var c = new Point3(2, 0, 0); - - var act = () => Pose3.FromSurface(a, b, c); - - act.Should().Throw() - .WithMessage("*collinear*"); - } - - [Fact] - public void FromSurface_ThreePoints_PointsOnSurface_HaveZeroLocalZ() - { - var a = new Point3(10, 20, 5); - var b = new Point3(15, 20, 5); - var c = new Point3(10, 25, 5); - - var pose = Pose3.FromSurface(a, b, c); - var inverse = pose.Inverse(); - - // All three points should have Z=0 in local coordinates - inverse.TransformPoint(a).Z.Should().BeApproximately(0, 0.0001); - inverse.TransformPoint(b).Z.Should().BeApproximately(0, 0.0001); - inverse.TransformPoint(c).Z.Should().BeApproximately(0, 0.0001); - } - - #endregion - - #region FromSurface (N-point, best-fit plane) - - [Fact] - public void FromSurface_NPoints_OriginIsCentroid() - { - var points = new Point3[] - { - new(0, 0, 0), new(10, 0, 0), new(10, 10, 0), new(0, 10, 0) - }; - - var pose = Pose3.FromSurface(points); - - pose.X.Should().BeApproximately(5, 0.0001); - pose.Y.Should().BeApproximately(5, 0.0001); - pose.Z.Should().BeApproximately(0, 0.0001); - } - - [Fact] - public void FromSurface_NPoints_HorizontalPlane_ZPointsUp() - { - // Counter-clockwise in XY plane → right-hand rule → Z points up - var points = new Point3[] - { - new(0, 0, 0), new(10, 0, 0), new(10, 10, 0), new(0, 10, 0) - }; - - var pose = Pose3.FromSurface(points); - var zDir = pose.Rotation.Rotate(Vector3.EZ); - - zDir.X.Should().BeApproximately(0, Tolerance); - zDir.Y.Should().BeApproximately(0, Tolerance); - zDir.Z.Should().BeApproximately(1, Tolerance); - } - - [Fact] - public void FromSurface_NPoints_XAxisAlongFirstEdge() - { - var points = new Point3[] - { - new(0, 0, 0), new(5, 0, 0), new(5, 5, 0), new(0, 5, 0) - }; - - var pose = Pose3.FromSurface(points); - var xDir = pose.Rotation.Rotate(Vector3.EX); - - // X-axis should be along points[0]→points[1] projected onto plane = +X - xDir.X.Should().BeApproximately(1, Tolerance); - xDir.Y.Should().BeApproximately(0, Tolerance); - xDir.Z.Should().BeApproximately(0, Tolerance); - } - - [Fact] - public void FromSurface_NPoints_AllPointsHaveNearZeroLocalZ() - { - // Coplanar points: all at Z=5 - var points = new Point3[] - { - new(0, 0, 5), new(10, 0, 5), new(10, 8, 5), - new(5, 12, 5), new(0, 8, 5) - }; - - var pose = Pose3.FromSurface(points); - var inverse = pose.Inverse(); - - for (int i = 0; i < points.Length; i++) - { - var local = inverse.TransformPoint(points[i]); - local.Z.Should().BeApproximately(0, 0.0001, $"Point {i} should have Z≈0 in local frame"); - } - } - - [Fact] - public void FromSurface_NPoints_NoisyPlane_NormalIsClose() - { - // Points approximately on Z=0 plane with small noise - var points = new Point3[] - { - new(0, 0, 0.01), new(10, 0, -0.01), new(10, 10, 0.02), - new(0, 10, -0.02), new(5, 5, 0.005), new(3, 7, -0.01) - }; - - var pose = Pose3.FromSurface(points); - var zDir = pose.Rotation.Rotate(Vector3.EZ); - - // Normal should be approximately +Z - Math.Abs(zDir.Z).Should().BeGreaterThan(0.999); - } - - [Fact] - public void FromSurface_NPoints_TiltedPlane() - { - // Points on the plane z = x (45° tilt around Y-axis) - var points = new Point3[] - { - new(0, 0, 0), new(1, 0, 1), new(1, 1, 1), - new(0, 1, 0), new(0.5, 0.5, 0.5) - }; - - var pose = Pose3.FromSurface(points); - var inverse = pose.Inverse(); - - for (int i = 0; i < points.Length; i++) - { - var local = inverse.TransformPoint(points[i]); - local.Z.Should().BeApproximately(0, 0.001, $"Point {i} should lie on the surface"); - } - } - - [Fact] - public void FromSurface_NPoints_MatchesThreePointForExactSquare() - { - // 4 points: first 3 define the same plane - var a = new Point3(0, 0, 0); - var b = new Point3(10, 0, 0); - var c = new Point3(10, 10, 0); - var d = new Point3(0, 10, 0); - - var pose3 = Pose3.FromSurface(a, b, c); - var poseN = Pose3.FromSurface([a, b, c, d]); - - // Normal directions should match - var z3 = pose3.Rotation.Rotate(Vector3.EZ); - var zN = poseN.Rotation.Rotate(Vector3.EZ); - z3.X.Should().BeApproximately(zN.X, 0.001); - z3.Y.Should().BeApproximately(zN.Y, 0.001); - z3.Z.Should().BeApproximately(zN.Z, 0.001); - - // X-axis directions should match - var x3 = pose3.Rotation.Rotate(Vector3.EX); - var xN = poseN.Rotation.Rotate(Vector3.EX); - x3.X.Should().BeApproximately(xN.X, 0.001); - x3.Y.Should().BeApproximately(xN.Y, 0.001); - x3.Z.Should().BeApproximately(xN.Z, 0.001); - } - - [Fact] - public void FromSurface_NPoints_ThrowsForFewerThan3() - { - var points = new Point3[] { new(0, 0, 0), new(1, 0, 0) }; - - var act = () => Pose3.FromSurface(points); - - act.Should().Throw() - .WithMessage("*3 points*"); - } - - [Fact] - public void FromSurface_NPoints_ThrowsForCollinearPoints() - { - var points = new Point3[] - { - new(0, 0, 0), new(1, 0, 0), new(2, 0, 0), new(3, 0, 0) - }; - - var act = () => Pose3.FromSurface(points); - - act.Should().Throw(); - } - - [Fact] - public void FromSurface_NPoints_DelegatesToThreePointFor3() - { - var a = new Point3(10, 20, 30); - var b = new Point3(11, 20, 30); - var c = new Point3(10, 21, 30); - - var pose3 = Pose3.FromSurface(a, b, c); - var poseN = Pose3.FromSurface([a, b, c]); - - // Exact same result — delegates to 3-point overload - pose3.X.Should().Be(poseN.X); - pose3.Y.Should().Be(poseN.Y); - pose3.Z.Should().Be(poseN.Z); - } - - #endregion } diff --git a/Sources/ModelingEvolution.Drawing.Tests/QuaternionFactoryTests.cs b/Sources/ModelingEvolution.Drawing.Tests/QuaternionFactoryTests.cs new file mode 100644 index 0000000..f124cbf --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/QuaternionFactoryTests.cs @@ -0,0 +1,94 @@ +using FluentAssertions; +using static ModelingEvolution.Drawing.Tests.Frame3TestData; + +namespace ModelingEvolution.Drawing.Tests; + +/// Quaternion.FromTo and Quaternion.FromAxes (epic-129 design § "Drawing: the kinematic chain"). +public class QuaternionFactoryTests +{ + private static Vector3 RandomVector(Random rng) => new(Uniform(rng, 1), Uniform(rng, 1), Uniform(rng, 1)); + + private static void ShouldBeClose(Vector3 a, Vector3 b, double tol) + { + a.X.Should().BeApproximately(b.X, tol); + a.Y.Should().BeApproximately(b.Y, tol); + a.Z.Should().BeApproximately(b.Z, tol); + } + + [Fact] + public void FromTo_TurnsFromIntoTo() + { + var rng = new Random(129); + for (int i = 0; i < 1000; i++) + { + var from = RandomVector(rng); + var to = RandomVector(rng); + + var q = Quaternion.FromTo(from, to); + + q.Length.Should().BeApproximately(1, 1e-12); + ShouldBeClose(q.Rotate(from.Normalize()), to.Normalize(), 1e-12); + } + } + + [Fact] + public void FromTo_IsTheShortestArc() + { + var q = Quaternion.FromTo(Vector3.EX, Vector3.EY); + + // 90° about +Z + QuaternionError(q, Quaternion.FromAxisAngle(Vector3.EZ, Math.PI / 2)).Should().BeLessThan(1e-12); + } + + [Fact] + public void FromTo_SameDirection_IsIdentity() + { + var q = Quaternion.FromTo(new Vector3(0, 0, 2), Vector3.EZ); + + QuaternionError(q, Quaternion.Identity).Should().BeLessThan(1e-12); + } + + [Theory] + [InlineData(1, 0, 0)] + [InlineData(0, 1, 0)] + [InlineData(0, 0, 1)] + [InlineData(0.3, -0.4, 0.5)] + public void FromTo_OppositeDirections_TurnsByPi(double x, double y, double z) + { + var from = new Vector3(x, y, z); + + var q = Quaternion.FromTo(from, -from); + + ShouldBeClose(q.Rotate(from.Normalize()), -from.Normalize(), 1e-12); + q.W.Should().BeApproximately(0, 1e-12); + } + + [Fact] + public void FromTo_ZeroVector_Throws() + { + var act = () => Quaternion.FromTo(Vector3.Zero, Vector3.EX); + + act.Should().Throw(); + } + + [Fact] + public void FromAxes_RecoversTheRotationOfItsAxes() + { + var rng = new Random(129); + for (int i = 0; i < 10_000; i++) + { + var q = RandomRotation(rng); + + var back = Quaternion.FromAxes(q.Rotate(Vector3.EX), q.Rotate(Vector3.EY), q.Rotate(Vector3.EZ)); + + QuaternionError(back, q).Should().BeLessThan(1e-12); + } + } + + [Fact] + public void FromAxes_Identity() + { + QuaternionError(Quaternion.FromAxes(Vector3.EX, Vector3.EY, Vector3.EZ), Quaternion.Identity) + .Should().BeLessThan(1e-15); + } +} diff --git a/Sources/ModelingEvolution.Drawing/Frame3.cs b/Sources/ModelingEvolution.Drawing/Frame3.cs new file mode 100644 index 0000000..788c89e --- /dev/null +++ b/Sources/ModelingEvolution.Drawing/Frame3.cs @@ -0,0 +1,270 @@ +using System.Numerics; +using System.Runtime.CompilerServices; + +namespace ModelingEvolution.Drawing; + +/// +/// A rigid 3D frame: a position and a unit-quaternion orientation. All pose maths composes ; +/// is only the wire and edit format and is converted once, by +/// and (epic-129 design § "Drawing: the kinematic chain"). +/// The frame carries no unit: its lengths are in whatever unit its producer chose. +/// +/// The origin of the frame in its parent frame. +/// The orientation of the frame in its parent frame; a unit quaternion. +/// The numeric type used for coordinates. +public readonly record struct Frame3(Point3 Position, Quaternion Rotation) + where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue +{ + private static readonly T Two = T.CreateTruncating(2); + + /// The identity frame: origin at zero, no rotation. + public static Frame3 Identity { get; } = new(Point3.Zero, Quaternion.Identity); + + /// Composes this frame with a child frame expressed in it: this · child. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Frame3 Multiply(in Frame3 child) => + new(Position + Rotation.Rotate(child.Position), Rotation * child.Rotation); + + /// Composes two frames: a · b, where is expressed in . + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Frame3 operator *(in Frame3 a, in Frame3 b) => a.Multiply(b); + + /// Returns the inverse frame, so that f · f.Inverse() is the identity. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Frame3 Inverse() + { + var inv = Rotation.Conjugate(); + return new Frame3((Point3)(-inv.Rotate(Position)), inv); + } + + /// Maps a point from this frame's local coordinates into its parent's coordinates. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Point3 TransformPoint(Point3 p) => Position + Rotation.Rotate(p); + + /// Rotates a vector from this frame's local coordinates into its parent's (no translation). + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Vector3 TransformVector(Vector3 v) => Rotation.Rotate(v); + + /// + /// Interpolates between two frames: the position linearly, the rotation by spherical linear interpolation + /// along the shorter arc. = 0 gives , 1 gives . + /// + public static Frame3 Interpolate(in Frame3 a, in Frame3 b, T t) => + new(Point3.Lerp(a.Position, b.Position, t), Quaternion.Slerp(a.Rotation, b.Rotation, t)); + + /// + /// The distance between two frames: the position error |p_a − p_b| and the angle error + /// 2·acos(|q_a·q_b|) in radians (computed in its well-conditioned atan2 form). + /// + public static (T Position, T Angle) Distance(in Frame3 a, in Frame3 b) + { + var d = a.Rotation.Conjugate() * b.Rotation; + var s = T.Sqrt(d.X * d.X + d.Y * d.Y + d.Z * d.Z); + return (Point3.Distance(a.Position, b.Position), Two * T.Atan2(s, T.Abs(d.W))); + } + + /// + /// Converts a wire/edit pose (ZYX degrees) into a frame: position scaled by + /// (0.001 turns mm into m), rotation by . This is the one Euler rule. + /// + public static Frame3 FromPose3(Pose3 p, T lengthScale) + { + var (rx, ry, rz) = p.Rotation.ToRadians(); + return new Frame3(p.Position * lengthScale, Quaternion.FromRpy((T)rx, (T)ry, (T)rz)); + } + + /// + /// Converts this frame into a wire/edit pose: position scaled by + /// (1000 turns m into mm), rotation as ZYX degrees (the inverse of ). + /// + public Pose3 ToPose3(T lengthScale) => + new(Position * lengthScale, Rotation3.FromQuaternion(Rotation)); + + #region FromSurface + + /// + /// Creates a frame from three points defining a surface plane using the right-hand rule. + /// Z direction is determined by (b-a) x (c-a). Reversing point order reverses Z direction. + /// + /// First point - becomes the origin, also defines X-axis direction with b. + /// Second point - defines X-axis direction from a. + /// Third point - completes the plane; a-b-c order determines Z via right-hand rule. + /// A frame whose origin is at a, X-axis along a-b, Z-axis per right-hand rule. + /// Thrown when points a, b, c are collinear. + public static Frame3 FromSurface(Point3 a, Point3 b, Point3 c) + { + var epsilon = T.CreateTruncating(1e-9); + var ab = b - a; + var ac = c - a; + + var zAxis = Vector3.Cross(ab, ac); + var normalLength = zAxis.Length; + if (normalLength < epsilon) + throw new ArgumentException("Points a, b, c are collinear and do not define a plane."); + zAxis = zAxis / normalLength; + + var xAxis = ab.Normalize(); + var yAxis = Vector3.Cross(zAxis, xAxis); + return new Frame3(a, Quaternion.FromAxes(xAxis, yAxis, zAxis)); + } + + /// + /// Creates a frame from three points defining a surface plane and a hint point indicating the Z direction. + /// + /// First point on the surface (also defines X-axis origin direction with b). + /// Second point on the surface (defines X-axis direction from a). + /// Third point on the surface (completes the plane definition). + /// Hint point off the surface - Z-axis will point toward this point. + /// A frame whose origin is the projection of h onto the plane, X-Y plane is the surface, and Z points toward h. + /// Thrown when h lies on the surface plane, or when a, b, c are collinear. + public static Frame3 FromSurface(Point3 a, Point3 b, Point3 c, Point3 h) + { + var epsilon = T.CreateTruncating(1e-9); + var ab = b - a; + var ac = c - a; + + var normal = Vector3.Cross(ab, ac); + var normalLength = normal.Length; + if (normalLength < epsilon) + throw new ArgumentException("Points a, b, c are collinear and do not define a plane."); + normal = normal / normalLength; + + var d = Vector3.Dot(h - a, normal); + if (T.Abs(d) < epsilon) + throw new ArgumentException("Point h lies on the surface plane. Cannot determine Z direction.", nameof(h)); + + var origin = new Point3(h.X - d * normal.X, h.Y - d * normal.Y, h.Z - d * normal.Z); + var zAxis = d > T.Zero ? normal : -normal; + var xAxis = ab.Normalize(); + var yAxis = Vector3.Cross(zAxis, xAxis); + return new Frame3(origin, Quaternion.FromAxes(xAxis, yAxis, zAxis)); + } + + /// + /// Creates a frame from N points defining a surface plane using least-squares best-fit. + /// Origin is the centroid. Z-axis is the best-fit normal (oriented via right-hand rule + /// of the first three points). X-axis is the projection of edge a→b onto the plane. + /// + /// At least 3 non-collinear points on or near the surface. + /// A frame whose origin is the centroid and whose Z-axis is the best-fit surface normal. + /// Thrown when fewer than 3 points or points are collinear. + public static Frame3 FromSurface(ReadOnlySpan> points) + { + if (points.Length < 3) + throw new ArgumentException("At least 3 points are required to define a surface.", nameof(points)); + if (points.Length == 3) + return FromSurface(points[0], points[1], points[2]); + + var epsilon = T.CreateTruncating(1e-9); + var n = T.CreateTruncating(points.Length); + + T cx = T.Zero, cy = T.Zero, cz = T.Zero; + for (int i = 0; i < points.Length; i++) + { + cx += points[i].X; + cy += points[i].Y; + cz += points[i].Z; + } + cx /= n; + cy /= n; + cz /= n; + var centroid = new Point3(cx, cy, cz); + + // Scatter matrix (symmetric 3x3): S = Σ (pi - centroid)(pi - centroid)^T + T sxx = T.Zero, sxy = T.Zero, sxz = T.Zero, syy = T.Zero, syz = T.Zero, szz = T.Zero; + for (int i = 0; i < points.Length; i++) + { + var dx = points[i].X - cx; + var dy = points[i].Y - cy; + var dz = points[i].Z - cz; + sxx += dx * dx; + sxy += dx * dy; + sxz += dx * dz; + syy += dy * dy; + syz += dy * dz; + szz += dz * dz; + } + + // Normal = eigenvector of the smallest eigenvalue, oriented by the right-hand rule of the first three points. + var zAxis = SmallestEigenvector3x3Symmetric(sxx, sxy, sxz, syy, syz, szz); + var ab = points[1] - points[0]; + var ac = points[2] - points[0]; + if (Vector3.Dot(Vector3.Cross(ab, ac), zAxis) < T.Zero) + zAxis = -zAxis; + + // X-axis: edge a→b projected onto the plane. + var proj = Vector3.Dot(ab, zAxis); + var xAxis = new Vector3(ab.X - proj * zAxis.X, ab.Y - proj * zAxis.Y, ab.Z - proj * zAxis.Z); + var xLen = xAxis.Length; + if (xLen < epsilon) + throw new ArgumentException("Cannot determine X-axis: first two points project to the same location on the best-fit plane."); + xAxis = xAxis / xLen; + + var yAxis = Vector3.Cross(zAxis, xAxis); + return new Frame3(centroid, Quaternion.FromAxes(xAxis, yAxis, zAxis)); + } + + /// + /// Finds the eigenvector of the smallest eigenvalue of a 3×3 symmetric matrix: the analytical trigonometric + /// method for the eigenvalue, then the cross product of rows for the eigenvector. + /// + private static Vector3 SmallestEigenvector3x3Symmetric(T sxx, T sxy, T sxz, T syy, T syz, T szz) + { + var three = T.CreateTruncating(3); + var six = T.CreateTruncating(6); + var epsilon = T.CreateTruncating(1e-12); + + var m = (sxx + syy + szz) / three; + var kxx = sxx - m; + var kyy = syy - m; + var kzz = szz - m; + + var q = (kxx * kxx + kyy * kyy + kzz * kzz + Two * (sxy * sxy + sxz * sxz + syz * syz)) / six; + var p = T.Sqrt(q); + if (p < epsilon) + throw new ArgumentException("Points are collinear or coincident and do not define a plane."); + + var invP = T.One / p; + var bxx = kxx * invP; + var bxy = sxy * invP; + var bxz = sxz * invP; + var byy = kyy * invP; + var byz = syz * invP; + var bzz = kzz * invP; + + var halfDetB = (bxx * (byy * bzz - byz * byz) + - bxy * (bxy * bzz - byz * bxz) + + bxz * (bxy * byz - byy * bxz)) / Two; + if (halfDetB > T.One) halfDetB = T.One; + if (halfDetB < -T.One) halfDetB = -T.One; + + var phi = T.Acos(halfDetB) / three; + var e3 = m + Two * p * T.Cos(phi + Two * T.Pi / three); + + var r0 = new Vector3(sxx - e3, sxy, sxz); + var r1 = new Vector3(sxy, syy - e3, syz); + var r2 = new Vector3(sxz, syz, szz - e3); + + var c01 = Vector3.Cross(r0, r1); + var c02 = Vector3.Cross(r0, r2); + var c12 = Vector3.Cross(r1, r2); + + var l01 = Vector3.Dot(c01, c01); + var l02 = Vector3.Dot(c02, c02); + var l12 = Vector3.Dot(c12, c12); + + var best = l01; + if (l02 > best) best = l02; + if (l12 > best) best = l12; + if (best < epsilon) + throw new ArgumentException("Points are collinear or coincident and do not define a plane."); + + if (l01 >= l02 && l01 >= l12) + return c01 / T.Sqrt(l01); + if (l02 >= l12) + return c02 / T.Sqrt(l02); + return c12 / T.Sqrt(l12); + } + + #endregion +} diff --git a/Sources/ModelingEvolution.Drawing/Kinematics/ChainFrame.cs b/Sources/ModelingEvolution.Drawing/Kinematics/ChainFrame.cs new file mode 100644 index 0000000..6501f90 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing/Kinematics/ChainFrame.cs @@ -0,0 +1,11 @@ +using System.Numerics; + +namespace ModelingEvolution.Drawing.Kinematics; + +/// A named frame fixed to a link of a (for example base or flange). +/// The frame name, unique in the chain. +/// The index of the link the frame is fixed to. +/// The frame in the link frame (metres). +/// The numeric type. +public readonly record struct ChainFrame(string Name, int Link, Frame3 InLink) + where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue; diff --git a/Sources/ModelingEvolution.Drawing/Kinematics/Joint.cs b/Sources/ModelingEvolution.Drawing/Kinematics/Joint.cs new file mode 100644 index 0000000..9f5c6e6 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing/Kinematics/Joint.cs @@ -0,0 +1,24 @@ +using System.Numerics; + +namespace ModelingEvolution.Drawing.Kinematics; + +/// +/// The joint that moves link relative to its parent link (Links[Child].Parent). +/// Units are metres and radians; the joint value is the vendor joint value (epic-129 design § "Drawing: the kinematic chain"). +/// +/// The joint name. +/// Revolute or prismatic. +/// The index of the child link this joint moves. +/// The child link frame in the parent link frame at joint value 0. +/// The unit joint axis in the child link frame. +/// The lower joint limit (radians or metres). +/// The upper joint limit (radians or metres). +/// The velocity limit (radians or metres per second). +/// The acceleration limit (per second squared), or null when the vendor gives none. +/// The numeric type. +public readonly record struct Joint( + string Name, JointType Type, int Child, + Frame3 Origin, + Vector3 Axis, + T Min, T Max, T MaxVelocity, T? MaxAcceleration) + where T : struct, INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue; diff --git a/Sources/ModelingEvolution.Drawing/Kinematics/JointType.cs b/Sources/ModelingEvolution.Drawing/Kinematics/JointType.cs new file mode 100644 index 0000000..e365322 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing/Kinematics/JointType.cs @@ -0,0 +1,11 @@ +namespace ModelingEvolution.Drawing.Kinematics; + +/// The motion a adds between its parent and child link. +public enum JointType : byte +{ + /// A rotation about the joint axis; the joint value is in radians. + Revolute, + + /// A translation along the joint axis; the joint value is in metres. + Prismatic +} diff --git a/Sources/ModelingEvolution.Drawing/Kinematics/KinematicChain.cs b/Sources/ModelingEvolution.Drawing/Kinematics/KinematicChain.cs new file mode 100644 index 0000000..2b651f8 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing/Kinematics/KinematicChain.cs @@ -0,0 +1,188 @@ +using System.Collections.Immutable; +using System.Numerics; + +namespace ModelingEvolution.Drawing.Kinematics; + +/// +/// A tree of links connected by revolute and prismatic joints, with named frames fixed to the links. +/// Units are metres and radians. Link 0 is the root (the chain base frame); every other link is moved by exactly +/// one joint. and take spans in and write spans out and do not allocate. +/// (epic-129 design § "Drawing: the kinematic chain (feature 001)".) +/// +/// The numeric type. +public sealed class KinematicChain + where T : struct, INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue +{ + private static readonly T UnitTolerance = T.CreateTruncating(1e-6); + + // _jointOf[link] = index of the joint whose child is that link; -1 for the root. + private readonly int[] _jointOf; + + /// + /// Creates a chain and validates it: links in topological order (link 0 is the only root, each parent index is + /// lower than its child's), one joint per non-root link, unit axes, Min ≤ Max, frames on existing links, + /// unique link and frame names. + /// + /// A rule is broken; the message names the offending link, joint or frame. + public KinematicChain(ImmutableArray links, ImmutableArray> joints, ImmutableArray> frames) + { + if (links.IsDefaultOrEmpty) + throw new ArgumentException("A chain needs at least the root link.", nameof(links)); + if (joints.IsDefault) + throw new ArgumentException("Joints must not be a default array.", nameof(joints)); + if (frames.IsDefault) + throw new ArgumentException("Frames must not be a default array.", nameof(frames)); + + var linkNames = new HashSet(StringComparer.Ordinal); + for (int i = 0; i < links.Length; i++) + { + var link = links[i]; + if (string.IsNullOrEmpty(link.Name)) + throw new ArgumentException($"Link {i} has no name.", nameof(links)); + if (!linkNames.Add(link.Name)) + throw new ArgumentException($"Link '{link.Name}' (index {i}) has a duplicate name.", nameof(links)); + if (i == 0 && link.Parent != -1) + throw new ArgumentException($"Link '{link.Name}' (index 0) is the root and must have parent -1, not {link.Parent}.", nameof(links)); + if (i > 0 && (link.Parent < 0 || link.Parent >= i)) + throw new ArgumentException($"Link '{link.Name}' (index {i}) has parent {link.Parent}; a parent index must be in [0, {i}) (topological order).", nameof(links)); + } + + var jointOf = new int[links.Length]; + Array.Fill(jointOf, -1); + for (int j = 0; j < joints.Length; j++) + { + var joint = joints[j]; + var name = string.IsNullOrEmpty(joint.Name) ? $"#{j}" : joint.Name; + if (string.IsNullOrEmpty(joint.Name)) + throw new ArgumentException($"Joint {name} has no name.", nameof(joints)); + if (joint.Child <= 0 || joint.Child >= links.Length) + throw new ArgumentException($"Joint '{name}' has child link {joint.Child}; it must be in [1, {links.Length}).", nameof(joints)); + if (jointOf[joint.Child] != -1) + throw new ArgumentException($"Joint '{name}' moves link '{links[joint.Child].Name}', which joint '{joints[jointOf[joint.Child]].Name}' already moves.", nameof(joints)); + if (joint.Type != JointType.Revolute && joint.Type != JointType.Prismatic) + throw new ArgumentException($"Joint '{name}' has unknown type {joint.Type}.", nameof(joints)); + var axisLength = joint.Axis.Length; + if (!(T.Abs(axisLength - T.One) <= UnitTolerance)) + throw new ArgumentException($"Joint '{name}' axis {joint.Axis} is not a unit vector (length {axisLength}).", nameof(joints)); + if (!(joint.Min <= joint.Max)) + throw new ArgumentException($"Joint '{name}' has Min {joint.Min} > Max {joint.Max}.", nameof(joints)); + jointOf[joint.Child] = j; + } + for (int i = 1; i < links.Length; i++) + if (jointOf[i] == -1) + throw new ArgumentException($"Link '{links[i].Name}' (index {i}) is moved by no joint.", nameof(joints)); + + var frameNames = new HashSet(StringComparer.Ordinal); + for (int f = 0; f < frames.Length; f++) + { + var frame = frames[f]; + if (string.IsNullOrEmpty(frame.Name)) + throw new ArgumentException($"Frame {f} has no name.", nameof(frames)); + if (!frameNames.Add(frame.Name)) + throw new ArgumentException($"Frame '{frame.Name}' (index {f}) has a duplicate name.", nameof(frames)); + if (frame.Link < 0 || frame.Link >= links.Length) + throw new ArgumentException($"Frame '{frame.Name}' is on link {frame.Link}; it must be in [0, {links.Length}).", nameof(frames)); + } + + Links = links; + Joints = joints; + Frames = frames; + _jointOf = jointOf; + } + + /// The links, in topological order; link 0 is the root. + public ImmutableArray Links { get; } + + /// The joints; joint j reads the joint value q[j]. + public ImmutableArray> Joints { get; } + + /// The named frames fixed to links. + public ImmutableArray> Frames { get; } + + /// The number of joint values: Joints.Length. + public int Dof => Joints.Length; + + /// The index of the frame named , or -1 when absent. + public int FrameIndex(string name) + { + for (int i = 0; i < Frames.Length; i++) + if (string.Equals(Frames[i].Name, name, StringComparison.Ordinal)) + return i; + return -1; + } + + /// + /// Forward kinematics: writes the pose of every link in the chain base frame, + /// links[c] = links[parent] · Origin · J(q). Does not allocate. + /// + /// The joint values, q.Length == Dof (radians or metres). + /// The output, links.Length == Links.Length. + /// A span has the wrong length. + public void Forward(ReadOnlySpan q, Span> links) + { + if (q.Length != Joints.Length) + throw new ArgumentException($"q has {q.Length} values; the chain has {Joints.Length} joints.", nameof(q)); + if (links.Length != Links.Length) + throw new ArgumentException($"links has {links.Length} elements; the chain has {Links.Length} links.", nameof(links)); + + links[0] = Frame3.Identity; + for (int c = 1; c < links.Length; c++) + { + int j = _jointOf[c]; + ref readonly var joint = ref Joints.ItemRef(j); + var motion = joint.Type == JointType.Revolute + ? new Frame3(Point3.Zero, Quaternion.FromAxisAngle(joint.Axis, q[j])) + : new Frame3((Point3)(joint.Axis * q[j]), Quaternion.Identity); + links[c] = links[Links.ItemRef(c).Parent] * joint.Origin * motion; + } + } + + /// The pose of frame in the chain base frame, from link poses written by . + /// A span has the wrong length. + /// The frame index is out of range. + public Frame3 FramePose(ReadOnlySpan> links, int frame) + { + if (links.Length != Links.Length) + throw new ArgumentException($"links has {links.Length} elements; the chain has {Links.Length} links.", nameof(links)); + if ((uint)frame >= (uint)Frames.Length) + throw new ArgumentOutOfRangeException(nameof(frame), frame, $"The chain has {Frames.Length} frames."); + ref readonly var f = ref Frames.ItemRef(frame); + return links[f.Link] * f.InLink; + } + + /// + /// The geometric Jacobian of frame in the chain base frame: 6 × Dof, column-major, + /// rows (vx vy vz ωx ωy ωz). Revolute: v = a × (p_frame − p_joint), ω = a. Prismatic: v = a, ω = 0. + /// Joints that do not move the frame give zero columns. Does not allocate. + /// + /// Link poses written by . + /// The frame index (see ). + /// The output, j.Length == 6 * Dof. + /// A span has the wrong length. + /// The frame index is out of range. + public void Jacobian(ReadOnlySpan> links, int frame, Span j) + { + if (j.Length != 6 * Joints.Length) + throw new ArgumentException($"j has {j.Length} elements; a 6 × {Joints.Length} Jacobian needs {6 * Joints.Length}.", nameof(j)); + var p = FramePose(links, frame).Position; + + j.Clear(); + for (int c = Frames.ItemRef(frame).Link; c > 0; c = Links.ItemRef(c).Parent) + { + int k = _jointOf[c]; + ref readonly var joint = ref Joints.ItemRef(k); + var a = links[c].Rotation.Rotate(joint.Axis); + var col = j.Slice(6 * k, 6); + if (joint.Type == JointType.Revolute) + { + var v = Vector3.Cross(a, p - links[c].Position); + col[0] = v.X; col[1] = v.Y; col[2] = v.Z; + col[3] = a.X; col[4] = a.Y; col[5] = a.Z; + } + else + { + col[0] = a.X; col[1] = a.Y; col[2] = a.Z; + } + } + } +} diff --git a/Sources/ModelingEvolution.Drawing/Kinematics/Link.cs b/Sources/ModelingEvolution.Drawing/Kinematics/Link.cs new file mode 100644 index 0000000..f1efb76 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing/Kinematics/Link.cs @@ -0,0 +1,6 @@ +namespace ModelingEvolution.Drawing.Kinematics; + +/// A rigid link of a . +/// The link name, unique in the chain. +/// The index of the parent link; -1 for the root link (the chain base), otherwise lower than this link's index. +public readonly record struct Link(string Name, int Parent); diff --git a/Sources/ModelingEvolution.Drawing/Polyline3.cs b/Sources/ModelingEvolution.Drawing/Polyline3.cs index 2588782..9a1411a 100644 --- a/Sources/ModelingEvolution.Drawing/Polyline3.cs +++ b/Sources/ModelingEvolution.Drawing/Polyline3.cs @@ -327,11 +327,12 @@ public Polyline3 Transform(Rotation3 rotation) /// public Polyline3 Transform(Pose3 pose) { + var frame = Frame3.FromPose3(pose, T.One); var span = AsSpan(); var mem = Alloc.Memory>(span.Length); var points = mem.Span; for (int i = 0; i < span.Length; i++) - points[i] = pose.TransformPoint(span[i]); + points[i] = frame.TransformPoint(span[i]); return new Polyline3(mem); } diff --git a/Sources/ModelingEvolution.Drawing/Pose3.cs b/Sources/ModelingEvolution.Drawing/Pose3.cs index 71a05d2..5a1601a 100644 --- a/Sources/ModelingEvolution.Drawing/Pose3.cs +++ b/Sources/ModelingEvolution.Drawing/Pose3.cs @@ -7,7 +7,9 @@ namespace ModelingEvolution.Drawing; /// -/// Represents a 3D pose (position + orientation) using generic numeric types. +/// Represents a 3D pose (position + orientation) using generic numeric types: the wire and edit format +/// (for example mm plus ZYX degrees). It carries no maths; convert it to with +/// to compose, invert, transform or interpolate. /// Combines Point3{T} (x, y, z) and Rotation3{T} (rx, ry, rz). /// /// The numeric type used for coordinates and angles. @@ -142,69 +144,6 @@ public struct Enumerator /// public bool IsIdentity => _position.IsEmpty && _rotation.IsIdentity; - /// - /// Transforms a point from local coordinates to world coordinates using this pose. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Point3 TransformPoint(Point3 localPoint) - { - var rotated = _rotation.Rotate(localPoint); - return rotated + (Vector3)_position; - } - - /// - /// Transforms a vector from local coordinates to world coordinates using this pose's rotation. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Vector3 TransformVector(Vector3 localVector) - { - return _rotation.Rotate(localVector); - } - - /// - /// Returns the inverse of this pose. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Pose3 Inverse() - { - var invRotation = _rotation.Inverse(); - var negPosition = new Vector3(-_position.X, -_position.Y, -_position.Z); - var invPosition = invRotation.Rotate(negPosition); - return new Pose3((Point3)invPosition, invRotation); - } - - /// - /// Combines two poses (this * other). - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Pose3 Multiply(Pose3 other) - { - var newRotation = Rotation3.Combine(_rotation, other._rotation); - var rotatedPosition = _rotation.Rotate(other._position); - var newPosition = _position + (Vector3)rotatedPosition; - return new Pose3(newPosition, newRotation); - } - - /// - /// Linearly interpolates between two poses. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Pose3 Lerp(Pose3 a, Pose3 b, T t) - { - var position = Point3.Lerp(a._position, b._position, t); - var rotation = Rotation3.Slerp(a._rotation, b._rotation, t); - return new Pose3(position, rotation); - } - - /// - /// Calculates the distance between two poses (position only). - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static T Distance(Pose3 a, Pose3 b) - { - return Point3.Distance(a._position, b._position); - } - /// /// Converts this pose to a different numeric type. /// @@ -217,14 +156,6 @@ public Pose3 Truncating() #region Operators - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Pose3 operator *(Pose3 a, Pose3 b) => a.Multiply(b); - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Pose3 operator +(Pose3 pose, Vector3 offset) => new(pose._position + offset, pose._rotation); - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Pose3 operator -(Pose3 pose, Vector3 offset) => new(pose._position - offset, pose._rotation); - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Pose3 operator +(Pose3 pose, Rotation3 rotation) => new(pose._position, Rotation3.Combine(pose._rotation, rotation)); [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool operator ==(Pose3 a, Pose3 b) => a._position == b._position && a._rotation == b._rotation; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -246,298 +177,6 @@ public Pose3 Truncating() [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Pose3 From(Vector3 position, Rotation3 rotation) => new((Point3)position, rotation); - /// - /// Creates a pose from three points defining a surface plane using the right-hand rule. - /// Z direction is determined by (b-a) x (c-a). Reversing point order reverses Z direction. - /// - /// First point - becomes the origin, also defines X-axis direction with b. - /// Second point - defines X-axis direction from a. - /// Third point - completes the plane; a-b-c order determines Z via right-hand rule. - /// A pose where origin is at a, X-axis along a-b, Z-axis per right-hand rule. - /// Thrown when points a, b, c are collinear. - public static Pose3 FromSurface(Point3 a, Point3 b, Point3 c) - { - var epsilon = T.CreateTruncating(1e-9); - - // Edge vectors - var ab = b - a; - var ac = c - a; - - // Plane normal via right-hand rule: Z = (b-a) x (c-a) - var zAxis = Vector3.Cross(ab, ac); - var normalLength = zAxis.Length; - - if (normalLength < epsilon) - throw new ArgumentException("Points a, b, c are collinear and do not define a plane."); - - zAxis = zAxis / normalLength; // Normalize - - // X-axis along edge a-b, normalized - var xAxis = ab.Normalize(); - - // Y-axis completes right-hand system: Y = Z x X - var yAxis = Vector3.Cross(zAxis, xAxis); - - // Convert orthonormal basis to Euler angles (ZYX convention) - var rotation = RotationFromAxes(xAxis, yAxis, zAxis); - - return new Pose3(a, rotation); - } - - /// - /// Creates a pose from three points defining a surface plane and a hint point indicating the Z direction. - /// - /// First point on the surface (also defines X-axis origin direction with b). - /// Second point on the surface (defines X-axis direction from a). - /// Third point on the surface (completes the plane definition). - /// Hint point off the surface - Z-axis will point toward this point. - /// A pose where origin is the projection of h onto the plane, X-Y plane is the surface, and Z points toward h. - /// Thrown when h lies on the surface plane. - /// Thrown when points a, b, c are collinear. - public static Pose3 FromSurface(Point3 a, Point3 b, Point3 c, Point3 h) - { - var epsilon = T.CreateTruncating(1e-9); - - // Edge vectors - var ab = b - a; - var ac = c - a; - - // Plane normal (unsigned) - var normal = Vector3.Cross(ab, ac); - var normalLength = normal.Length; - - if (normalLength < epsilon) - throw new ArgumentException("Points a, b, c are collinear and do not define a plane."); - - normal = normal / normalLength; // Normalize - - // Signed distance from h to plane - var ah = h - a; - var d = Vector3.Dot(ah, normal); - - if (T.Abs(d) < epsilon) - throw new ArgumentException("Point h lies on the surface plane. Cannot determine Z direction.", nameof(h)); - - // Origin = foot of perpendicular from h to plane - var origin = new Point3( - h.X - d * normal.X, - h.Y - d * normal.Y, - h.Z - d * normal.Z); - - // Z-axis points toward h - var zAxis = d > T.Zero ? normal : -normal; - - // X-axis along edge a-b, normalized - var xAxis = ab.Normalize(); - - // Y-axis completes right-hand system: Y = Z x X - var yAxis = Vector3.Cross(zAxis, xAxis); - - // Convert orthonormal basis to Euler angles (ZYX convention) - var rotation = RotationFromAxes(xAxis, yAxis, zAxis); - - return new Pose3(origin, rotation); - } - - /// - /// Creates a pose from N points defining a surface plane using least-squares best-fit. - /// Origin is the centroid. Z-axis is the best-fit normal (oriented via right-hand rule - /// of the first three points). X-axis is the projection of edge a→b onto the plane. - /// - /// At least 3 non-collinear points on or near the surface. - /// A pose where origin is the centroid, Z-axis is the best-fit surface normal. - /// Thrown when fewer than 3 points or points are collinear. - public static Pose3 FromSurface(ReadOnlySpan> points) - { - if (points.Length < 3) - throw new ArgumentException("At least 3 points are required to define a surface.", nameof(points)); - - if (points.Length == 3) - return FromSurface(points[0], points[1], points[2]); - - var epsilon = T.CreateTruncating(1e-9); - var n = T.CreateTruncating(points.Length); - - // 1. Centroid - var cx = T.Zero; - var cy = T.Zero; - var cz = T.Zero; - for (int i = 0; i < points.Length; i++) - { - cx += points[i].X; - cy += points[i].Y; - cz += points[i].Z; - } - cx /= n; - cy /= n; - cz /= n; - var centroid = new Point3(cx, cy, cz); - - // 2. Scatter matrix (symmetric 3x3): S = Σ (pi - centroid)(pi - centroid)^T - var sxx = T.Zero; - var sxy = T.Zero; - var sxz = T.Zero; - var syy = T.Zero; - var syz = T.Zero; - var szz = T.Zero; - for (int i = 0; i < points.Length; i++) - { - var dx = points[i].X - cx; - var dy = points[i].Y - cy; - var dz = points[i].Z - cz; - sxx += dx * dx; - sxy += dx * dy; - sxz += dx * dz; - syy += dy * dy; - syz += dy * dz; - szz += dz * dz; - } - - // 3. Normal = eigenvector of smallest eigenvalue of scatter matrix - var zAxis = SmallestEigenvector3x3Symmetric(sxx, sxy, sxz, syy, syz, szz); - - // 4. Orient via right-hand rule of first three points - var ab = points[1] - points[0]; - var ac = points[2] - points[0]; - var cross = Vector3.Cross(ab, ac); - if (Vector3.Dot(cross, zAxis) < T.Zero) - zAxis = -zAxis; - - // 5. X-axis: project edge a→b onto the plane, normalize - var proj = Vector3.Dot(ab, zAxis); - var xAxis = new Vector3( - ab.X - proj * zAxis.X, - ab.Y - proj * zAxis.Y, - ab.Z - proj * zAxis.Z); - var xLen = xAxis.Length; - if (xLen < epsilon) - throw new ArgumentException("Cannot determine X-axis: first two points project to the same location on the best-fit plane."); - xAxis = xAxis / xLen; - - // 6. Y = Z × X (right-hand system) - var yAxis = Vector3.Cross(zAxis, xAxis); - - var rotation = RotationFromAxes(xAxis, yAxis, zAxis); - return new Pose3(centroid, rotation); - } - - /// - /// Finds the eigenvector corresponding to the smallest eigenvalue of a 3×3 symmetric matrix. - /// Uses the analytical trigonometric method for eigenvalues, then cross-product of rows for the eigenvector. - /// - private static Vector3 SmallestEigenvector3x3Symmetric(T sxx, T sxy, T sxz, T syy, T syz, T szz) - { - var two = T.CreateTruncating(2); - var three = T.CreateTruncating(3); - var six = T.CreateTruncating(6); - var epsilon = T.CreateTruncating(1e-12); - - // Shift to zero-mean: K = S - m*I where m = trace/3 - var m = (sxx + syy + szz) / three; - var kxx = sxx - m; - var kyy = syy - m; - var kzz = szz - m; - - // q = ||K||_F^2 / 6 - var q = (kxx * kxx + kyy * kyy + kzz * kzz - + two * (sxy * sxy + sxz * sxz + syz * syz)) / six; - var p = T.Sqrt(q); - - if (p < epsilon) - { - // Scatter is (near-)isotropic — no well-defined plane. - // Fall back: try cross-product of first available pair - throw new ArgumentException("Points are collinear or coincident and do not define a plane."); - } - - // B = K / p (normalized) - var invP = T.One / p; - var bxx = kxx * invP; - var bxy = sxy * invP; - var bxz = sxz * invP; - var byy = kyy * invP; - var byz = syz * invP; - var bzz = kzz * invP; - - // halfDetB = det(B) / 2, clamped to [-1, 1] - var halfDetB = (bxx * (byy * bzz - byz * byz) - - bxy * (bxy * bzz - byz * bxz) - + bxz * (bxy * byz - byy * bxz)) / two; - if (halfDetB > T.One) halfDetB = T.One; - if (halfDetB < -T.One) halfDetB = -T.One; - - var phi = T.Acos(halfDetB) / three; - - // Smallest eigenvalue: e3 = m + 2p·cos(φ + 2π/3) - var twoPiOverThree = two * T.Pi / three; - var e3 = m + two * p * T.Cos(phi + twoPiOverThree); - - // Eigenvector via cross-product of rows of (S - e3·I) - var r0 = new Vector3(sxx - e3, sxy, sxz); - var r1 = new Vector3(sxy, syy - e3, syz); - var r2 = new Vector3(sxz, syz, szz - e3); - - var c01 = Vector3.Cross(r0, r1); - var c02 = Vector3.Cross(r0, r2); - var c12 = Vector3.Cross(r1, r2); - - var l01 = Vector3.Dot(c01, c01); - var l02 = Vector3.Dot(c02, c02); - var l12 = Vector3.Dot(c12, c12); - - var best = l01; - if (l02 > best) best = l02; - if (l12 > best) best = l12; - - if (best < epsilon) - throw new ArgumentException("Points are collinear or coincident and do not define a plane."); - - if (l01 >= l02 && l01 >= l12) - return c01 / T.Sqrt(l01); - if (l02 >= l12) - return c02 / T.Sqrt(l02); - return c12 / T.Sqrt(l12); - } - - /// - /// Creates a Rotation3 from orthonormal basis vectors (X, Y, Z axes). - /// The resulting rotation satisfies: Rotate(EX)=X, Rotate(EY)=Y, Rotate(EZ)=Z. - /// - private static Rotation3 RotationFromAxes(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis) - { - var one = T.One; - var epsilon = T.CreateTruncating(1e-6); - - // Clamp to [-1, 1] to avoid NaN from asin - var sinPitch = -xAxis.Z; - if (sinPitch > one) sinPitch = one; - if (sinPitch < -one) sinPitch = -one; - - var ry = T.Asin(sinPitch); // Pitch - var cosPitch = T.Cos(ry); - - T rx, rz; - - if (T.Abs(cosPitch) > epsilon) - { - // Normal case - rx = T.Atan2(yAxis.Z, zAxis.Z); // Roll - rz = T.Atan2(xAxis.Y, xAxis.X); // Yaw - } - else - { - // Gimbal lock - pitch is +/-90 deg - rx = T.Atan2(-zAxis.Y, yAxis.Y); - rz = T.Zero; - } - - // rx, ry, rz are in radians — convert via Radian → Degree - return new Rotation3( - (Degree)Radian.FromRadian(rx), - (Degree)Radian.FromRadian(ry), - (Degree)Radian.FromRadian(rz)); - } - #endregion #region Conversions diff --git a/Sources/ModelingEvolution.Drawing/PosePath3.cs b/Sources/ModelingEvolution.Drawing/PosePath3.cs index a295e2f..3d7765d 100644 --- a/Sources/ModelingEvolution.Drawing/PosePath3.cs +++ b/Sources/ModelingEvolution.Drawing/PosePath3.cs @@ -133,7 +133,7 @@ public T Length() T length = T.Zero; for (int i = 0; i < n - 1; i++) - length += Pose3.Distance(span[i], span[i + 1]); + length += Point3.Distance(span[i].Position, span[i + 1].Position); return length; } @@ -155,11 +155,12 @@ public PosePath3 Reverse() /// public PosePath3 Transform(Pose3 pose) { + var frame = Frame3.FromPose3(pose, T.One); var span = AsSpan(); var mem = Alloc.Memory>(span.Length); var dst = mem.Span; for (int i = 0; i < span.Length; i++) - dst[i] = pose.Multiply(span[i]); + dst[i] = (frame * Frame3.FromPose3(span[i], T.One)).ToPose3(T.One); return new PosePath3(mem); } @@ -209,7 +210,7 @@ public Trajectory3 ToTrajectory(T totalTime) var mem = Alloc.Memory>(span.Length); var dst = mem.Span; for (int i = 0; i < span.Length; i++) - dst[i] = span[i] + f; + dst[i] = span[i] with { Position = span[i].Position + f }; return new PosePath3(mem); } @@ -222,7 +223,7 @@ public Trajectory3 ToTrajectory(T totalTime) var mem = Alloc.Memory>(span.Length); var dst = mem.Span; for (int i = 0; i < span.Length; i++) - dst[i] = span[i] - f; + dst[i] = span[i] with { Position = span[i].Position - f }; return new PosePath3(mem); } diff --git a/Sources/ModelingEvolution.Drawing/Quaternion.cs b/Sources/ModelingEvolution.Drawing/Quaternion.cs index 4394743..a9bf8bf 100644 --- a/Sources/ModelingEvolution.Drawing/Quaternion.cs +++ b/Sources/ModelingEvolution.Drawing/Quaternion.cs @@ -20,6 +20,7 @@ namespace ModelingEvolution.Drawing; private static readonly T Two = T.CreateTruncating(2); private static readonly T SlerpThreshold = T.One - T.CreateTruncating(1e-6); + private static readonly T OppositeThreshold = T.CreateTruncating(1e-12); /// /// Represents the identity quaternion (no rotation). @@ -133,6 +134,92 @@ public static Quaternion FromAxisAngle(Vector3 axis, T angleRadians) axis.Z * s); } + /// + /// Creates the rotation Rz(yaw)·Ry(pitch)·Rx(roll) from angles in radians: the URDF rpy convention and + /// the ZYX convention of and of the asset files. + /// + /// Rotation about X, applied first, in radians. + /// Rotation about Y, in radians. + /// Rotation about Z, applied last, in radians. + public static Quaternion FromRpy(T roll, T pitch, T yaw) + { + var cx = T.Cos(roll / Two); + var sx = T.Sin(roll / Two); + var cy = T.Cos(pitch / Two); + var sy = T.Sin(pitch / Two); + var cz = T.Cos(yaw / Two); + var sz = T.Sin(yaw / Two); + + return new Quaternion( + cx * cy * cz + sx * sy * sz, + sx * cy * cz - cx * sy * sz, + cx * sy * cz + sx * cy * sz, + cx * cy * sz - sx * sy * cz); + } + + /// + /// Creates the shortest-arc rotation that turns the direction of into the direction of + /// . For opposite directions it turns by π about an axis perpendicular to . + /// + /// Thrown when either vector is zero. + public static Quaternion FromTo(Vector3 from, Vector3 to) + { + var f = from.Normalize(); + var t = to.Normalize(); + var d = Vector3.Dot(f, t); + + if (T.One + d < OppositeThreshold) + { + var axis = Vector3.Cross(Vector3.EX, f); + if (axis.LengthSquared < OppositeThreshold) + axis = Vector3.Cross(Vector3.EY, f); + axis = axis.Normalize(); + return new Quaternion(T.Zero, axis.X, axis.Y, axis.Z); + } + + var c = Vector3.Cross(f, t); + return new Quaternion(T.One + d, c.X, c.Y, c.Z).Normalize(); + } + + /// + /// Creates the rotation whose rotated unit axes are , and + /// : Rotate(EX) = x, Rotate(EY) = y, Rotate(EZ) = z. The axes must form a right-handed + /// orthonormal basis (the columns of the rotation matrix). + /// + public static Quaternion FromAxes(Vector3 x, Vector3 y, Vector3 z) + { + // Shepperd's method on the matrix whose columns are x, y, z. + T m00 = x.X, m10 = x.Y, m20 = x.Z; + T m01 = y.X, m11 = y.Y, m21 = y.Z; + T m02 = z.X, m12 = z.Y, m22 = z.Z; + var quarter = T.One / (Two * Two); + var trace = m00 + m11 + m22; + Quaternion q; + + if (trace > T.Zero) + { + var s = T.Sqrt(trace + T.One) * Two; + q = new Quaternion(quarter * s, (m21 - m12) / s, (m02 - m20) / s, (m10 - m01) / s); + } + else if (m00 > m11 && m00 > m22) + { + var s = T.Sqrt(T.One + m00 - m11 - m22) * Two; + q = new Quaternion((m21 - m12) / s, quarter * s, (m01 + m10) / s, (m02 + m20) / s); + } + else if (m11 > m22) + { + var s = T.Sqrt(T.One + m11 - m00 - m22) * Two; + q = new Quaternion((m02 - m20) / s, (m01 + m10) / s, quarter * s, (m12 + m21) / s); + } + else + { + var s = T.Sqrt(T.One + m22 - m00 - m11) * Two; + q = new Quaternion((m10 - m01) / s, (m02 + m20) / s, (m12 + m21) / s, quarter * s); + } + + return q.Normalize(); + } + /// /// Rotates a vector by this quaternion. /// diff --git a/Sources/ModelingEvolution.Drawing/README.md b/Sources/ModelingEvolution.Drawing/README.md index 1162b46..65e60b0 100644 --- a/Sources/ModelingEvolution.Drawing/README.md +++ b/Sources/ModelingEvolution.Drawing/README.md @@ -63,7 +63,7 @@ A high-performance .NET generic math library for 2D and 3D geometry, coordinate - **Polygon booleans** -- union, intersection, difference, and clustering via Clipper2. - **Exact Bezier clipping** -- `Path.Intersect(Rectangle)` computes analytically exact sub-curves, not polygon approximations. - **Trajectory planning** -- `Trajectory3` with time-parameterized waypoints, interpolation, resampling, and a real-time `TrajectoryController`. -- **Robotics-ready** -- `Joints6` for 6-DOF joint representations, `Pose3.FromSurface()` for plane-based frame construction. +- **Robotics-ready** -- `Joints6` for 6-DOF joint representations, `Frame3` (position + unit quaternion) for all pose maths, `Frame3.FromSurface()` for plane-based frame construction, and `KinematicChain` for forward kinematics and geometric Jacobians of serial and tree chains. - **Pooled allocations** -- `AllocationScope` provides arena-style pooled memory for zero-GC geometry pipelines. - **Serialization built in** -- Protobuf (protobuf-net) attributes on all core types, plus custom JSON converters and SVG exporters. @@ -203,14 +203,15 @@ All 2D shapes implement `IShape`, providing `Area()`, `Perimeter()`, ` | Type | Description | |---|---| | `Rotation3` | Euler angle rotation (Roll/Pitch/Yaw in degrees, ZYX convention). Create from degrees, radians, quaternion, or `Matrix3x3`. `Rotate(Vector3)`, `Rotate(Point3)`. `Combine`, `Inverse`, `Negate`, `Slerp` (via quaternion). Convert to matrix, quaternion, or direction vector. Protobuf and JSON serialization. | -| `Quaternion` | Unit quaternion for 3D rotations. From axis-angle, rotate vector, slerp, dot, normalize, conjugate, inverse. Hamilton product operator (`*`). Conversions to/from `System.Numerics.Quaternion`. Protobuf serialization. | +| `Quaternion` | Unit quaternion for 3D rotations. From axis-angle, `FromRpy(roll, pitch, yaw)` (Rz·Ry·Rx, radians; the URDF rpy rule), `FromTo(from, to)` (shortest arc), `FromAxes(x, y, z)` (orthonormal basis), rotate vector, slerp, dot, normalize, conjugate, inverse. Hamilton product operator (`*`). Conversions to/from `System.Numerics.Quaternion`. Protobuf serialization. | | `Matrix3x3` | Also used as rotation representation. `RotationZYX(rx, ry, rz)` creates ZYX Euler rotation matrix. `ToEulerZYX()` extracts angles (with gimbal-lock handling). `ToQuaternion()` via Shepperd's method. | ### Poses and Trajectories | Type | Description | |---|---| -| `Pose3` | 3D pose = position (`Point3`) + orientation (`Rotation3`). `TransformPoint`, `TransformVector`, `Inverse`, `Multiply` (compose). `Lerp` (position lerp + rotation slerp). `FromSurface(a, b, c)` creates a frame from three coplanar points (right-hand rule). Overload `FromSurface(a, b, c, h)` uses a hint point to determine Z direction. Operators: `*` (compose), `+`/`-` with `Vector3` or `Rotation3`. Tuple conversions, parsing. Protobuf and JSON serialization. | +| `Frame3` | Rigid frame = position (`Point3`) + unit `Quaternion`. All pose maths: `Multiply` / `*` (compose), `Inverse`, `TransformPoint`, `TransformVector`, `Interpolate` (position lerp + slerp), `Distance` (position error and angle error in radians). `FromSurface(a, b, c)` creates a frame from three coplanar points (right-hand rule); `FromSurface(a, b, c, h)` uses a hint point for Z; `FromSurface(points)` fits a plane. `FromPose3(pose, lengthScale)` / `ToPose3(lengthScale)` convert to and from the wire format (ZYX degrees). | +| `Pose3` | Wire and edit format only (since 2.0): position (`Point3`) + ZYX Euler orientation (`Rotation3`, degrees). Constructors, `Identity`, tuple conversions, deconstruction, parsing, equality, `Truncating()`. Protobuf and JSON serialization. Convert to `Frame3` for any maths. | | `Waypoint3` | Timestamped `Pose3` -- an element of a trajectory. | | `PosePath3` | Immutable sequence of `Pose3` (no time). Length, reverse, transform, extract `Polyline3`, convert to `Trajectory3` with uniform timing. | | `Trajectory3` | Immutable time-parameterized path of `Waypoint3`. `Duration`, `Length`, `AtTime(t)` (binary-search interpolation with position lerp + rotation slerp). `Resample(interval)`, `Reverse`, `Transform`. Convert to `PosePath3` or `Polyline3`. Create a real-time `TrajectoryController` via `Control()`. | @@ -221,6 +222,7 @@ All 2D shapes implement `IShape`, providing `Area()`, `Perimeter()`, ` | Type | Description | |---|---| +| `KinematicChain` | Namespace `ModelingEvolution.Drawing.Kinematics`. A tree of `Link`s moved by revolute/prismatic `Joint`s (origin at q = 0, unit axis, limits) with named `ChainFrame`s; metres and radians. `Forward(q, links)` writes every link pose, `FramePose(links, frame)`, `Jacobian(links, frame, j)` writes the 6 × Dof geometric Jacobian (column-major, v then ω). Spans in, spans out, no allocation; the constructor validates topological order, unit axes, limits and unique names. | | `Joints6` | 6-DOF joint angles stored as six `Degree` fields (no array allocation). Indexer `[0..5]`, arithmetic operators, `Lerp`, `MaxAbsDelta`, `IsWithin(tolerance)`, `IsBetween(min, max)`. `ToArray()`, `FromSpan()`, `FromArray()`, `CopyTo(Span)`. Protobuf and JSON serialization. | | `Speed` | Scalar speed in units/second (unitless magnitude). Arithmetic, `TimeFor(distance)`, `DistanceIn(time)`. Parsing. | | `Velocity3` | 3D velocity vector (direction + magnitude). Create from direction + speed. `Speed`, `Direction`, `DisplacementIn(time)`. Arithmetic operators. | @@ -540,16 +542,17 @@ SphericalPoint back = cyl; // implicit conversio // Pose = position + orientation var pose = new Pose3(100, 200, 300, 45, 0, 0); // x,y,z, rx,ry,rz (degrees) -// Compose poses -var world = new Pose3(new Point3(10, 0, 0), Rotation3.Identity); -var local = new Pose3(new Point3(5, 0, 0), Rotation3.FromDegrees(0, 0, 90)); +// Maths composes Frame3 (position + quaternion); Pose3 converts once, here mm -> m +var world = Frame3.FromPose3(new Pose3(10, 0, 0, 0, 0, 0), 0.001); +var local = Frame3.FromPose3(new Pose3(5, 0, 0, 0, 0, 90), 0.001); var combined = world * local; +Pose3 back = combined.ToPose3(1000); // m -> mm, ZYX degrees // Create frame from a surface (3 points define a plane) var a = new Point3(0, 0, 0); var b = new Point3(1, 0, 0); var c = new Point3(0, 1, 0); -var surfacePose = Pose3.FromSurface(a, b, c); // Z-axis via right-hand rule +var surface = Frame3.FromSurface(a, b, c); // Z-axis via right-hand rule // Trajectory with timed waypoints var trajectory = new Trajectory3( diff --git a/Sources/ModelingEvolution.Drawing/Rotation3.cs b/Sources/ModelingEvolution.Drawing/Rotation3.cs index 907421f..68a4c88 100644 --- a/Sources/ModelingEvolution.Drawing/Rotation3.cs +++ b/Sources/ModelingEvolution.Drawing/Rotation3.cs @@ -138,23 +138,7 @@ private T ProtoRz public Quaternion ToQuaternion() { var (rx, ry, rz) = ToRadians(); - var halfRx = (T)rx / Two; - var halfRy = (T)ry / Two; - var halfRz = (T)rz / Two; - - var cx = T.Cos(halfRx); - var sx = T.Sin(halfRx); - var cy = T.Cos(halfRy); - var sy = T.Sin(halfRy); - var cz = T.Cos(halfRz); - var sz = T.Sin(halfRz); - - // ZYX order - return new Quaternion( - w: cx * cy * cz + sx * sy * sz, - x: sx * cy * cz - cx * sy * sz, - y: cx * sy * cz + sx * cy * sz, - z: cx * cy * sz - sx * sy * cz); + return Quaternion.FromRpy((T)rx, (T)ry, (T)rz); } /// diff --git a/Sources/ModelingEvolution.Drawing/Trajectory3.cs b/Sources/ModelingEvolution.Drawing/Trajectory3.cs index 00cd1b7..3554bc3 100644 --- a/Sources/ModelingEvolution.Drawing/Trajectory3.cs +++ b/Sources/ModelingEvolution.Drawing/Trajectory3.cs @@ -179,7 +179,7 @@ public Pose3 AtTime(T time) if (dt == T.Zero) return a.Pose; var t = (time - a.Time) / dt; - return Pose3.Lerp(a.Pose, b.Pose, t); + return Frame3.Interpolate(Frame3.FromPose3(a.Pose, T.One), Frame3.FromPose3(b.Pose, T.One), t).ToPose3(T.One); } /// @@ -210,11 +210,12 @@ public Trajectory3 Reverse() /// public Trajectory3 Transform(Pose3 pose) { + var frame = Frame3.FromPose3(pose, T.One); var span = AsSpan(); var mem = Alloc.Memory>(span.Length); var dst = mem.Span; for (int i = 0; i < span.Length; i++) - dst[i] = new Waypoint3(pose.Multiply(span[i].Pose), span[i].Time); + dst[i] = new Waypoint3((frame * Frame3.FromPose3(span[i].Pose, T.One)).ToPose3(T.One), span[i].Time); return new Trajectory3(mem); } @@ -287,7 +288,7 @@ public Trajectory3 Resample(T interval) var mem = Alloc.Memory>(span.Length); var dst = mem.Span; for (int i = 0; i < span.Length; i++) - dst[i] = new Waypoint3(span[i].Pose + f, span[i].Time); + dst[i] = new Waypoint3(span[i].Pose with { Position = span[i].Pose.Position + f }, span[i].Time); return new Trajectory3(mem); } @@ -300,7 +301,7 @@ public Trajectory3 Resample(T interval) var mem = Alloc.Memory>(span.Length); var dst = mem.Span; for (int i = 0; i < span.Length; i++) - dst[i] = new Waypoint3(span[i].Pose - f, span[i].Time); + dst[i] = new Waypoint3(span[i].Pose with { Position = span[i].Pose.Position - f }, span[i].Time); return new Trajectory3(mem); } diff --git a/Sources/ModelingEvolution.Drawing/Trajectory3Builder.cs b/Sources/ModelingEvolution.Drawing/Trajectory3Builder.cs index 7342172..48da1aa 100644 --- a/Sources/ModelingEvolution.Drawing/Trajectory3Builder.cs +++ b/Sources/ModelingEvolution.Drawing/Trajectory3Builder.cs @@ -102,7 +102,7 @@ public Trajectory3Builder MoveTo(Pose3 pose, Speed speed) return Add(pose); var lastPose = _buffer[_count - 1].Pose; - var distance = Pose3.Distance(lastPose, pose); + var distance = Point3.Distance(lastPose.Position, pose.Position); var dt = distance / (T)speed; _currentTime += dt; @@ -120,7 +120,7 @@ public Trajectory3Builder MoveTo(Pose3 pose, Speed speed, Corner cor return Add(pose); var lastPose = _buffer[_count - 1].Pose; - var distance = Pose3.Distance(lastPose, pose); + var distance = Point3.Distance(lastPose.Position, pose.Position); var dt = distance / (T)speed; _currentTime += dt; @@ -140,7 +140,7 @@ public Trajectory3Builder MoveTo(Pose3 pose, Speed speed, Corner cor return Add(pose); var lastPose = _buffer[_count - 1].Pose; - var distance = Pose3.Distance(lastPose, pose); + var distance = Point3.Distance(lastPose.Position, pose.Position); var dt = distance / (T)speed; _currentTime += dt; diff --git a/Sources/ModelingEvolution.Drawing/Triangle3.cs b/Sources/ModelingEvolution.Drawing/Triangle3.cs index e53efad..1c5d55c 100644 --- a/Sources/ModelingEvolution.Drawing/Triangle3.cs +++ b/Sources/ModelingEvolution.Drawing/Triangle3.cs @@ -86,9 +86,9 @@ public T Area /// A pose representing the triangle's plane orientation at its center. public Pose3 ToPose() { - // Get rotation from surface, then create pose at centroid - var surfacePose = Pose3.FromSurface(_a, _b, _c); - return new Pose3(Centroid, surfacePose.Rotation); + // Rotation from the surface frame, position at the centroid + var surface = Frame3.FromSurface(_a, _b, _c); + return new Frame3(Centroid, surface.Rotation).ToPose3(T.One); } #region Operators From 5217f0efc89898d456d97bf8537de9992df37ce4 Mon Sep 17 00:00:00 2001 From: Rafal Maciag Date: Sat, 24 Oct 2026 18:38:43 +0000 Subject: [PATCH 2/6] S3 review A: Transform takes Frame3, Triangle3.ToFrame (epic-129) Polyline3/PosePath3/Trajectory3.Transform(Pose3) become Transform(in Frame3): the pose maths composes Frame3 and the hidden lengthScale=1 conversion of the argument is gone; PosePath3/Trajectory3 keep the per-element FromPose3/ToPose3 because their elements are wire Pose3. Triangle3.ToPose() becomes ToFrame(), built from Frame3.FromSurface's rotation at the centroid. One test per Transform added; Triangle3 tests assert the frame. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_014wH5aL3TZhJNvLqa8YTku8 --- .../Polyline3Tests.cs | 935 +++++++++--------- .../PosePath3Tests.cs | 319 +++--- .../Trajectory3Tests.cs | 464 ++++----- .../Triangle3Tests.cs | 36 +- .../ModelingEvolution.Drawing/Polyline3.cs | 873 ++++++++-------- .../ModelingEvolution.Drawing/PosePath3.cs | 524 +++++----- .../ModelingEvolution.Drawing/Trajectory3.cs | 680 ++++++------- .../ModelingEvolution.Drawing/Triangle3.cs | 12 +- 8 files changed, 1943 insertions(+), 1900 deletions(-) diff --git a/Sources/ModelingEvolution.Drawing.Tests/Polyline3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Polyline3Tests.cs index 2483fa2..cbff910 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Polyline3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Polyline3Tests.cs @@ -1,461 +1,474 @@ -using FluentAssertions; -using ModelingEvolution.Drawing; - -namespace ModelingEvolution.Drawing.Tests; - -public class Polyline3Tests -{ - private const float Tol = 1e-4f; - - // ───────────────────────────────────────────── - // Construction - // ───────────────────────────────────────────── - - [Fact] - public void DefaultCtor_Empty() - { - var pl = new Polyline3(); - pl.Count.Should().Be(0); - pl.AsSpan().Length.Should().Be(0); - } - - [Fact] - public void ParamsCtor_StoresPoints() - { - var p1 = new Point3(0, 0, 0); - var p2 = new Point3(1, 0, 0); - var p3 = new Point3(1, 1, 0); - var pl = new Polyline3(p1, p2, p3); - pl.Count.Should().Be(3); - pl[0].Should().Be(p1); - pl[1].Should().Be(p2); - pl[2].Should().Be(p3); - } - - [Fact] - public void ListCtor_CopiesPoints() - { - var list = new List> - { - new(0, 0, 0), new(5, 5, 5), new(10, 0, 0) - }; - var pl = new Polyline3(list); - pl.Count.Should().Be(3); - pl[2].Should().Be(new Point3(10, 0, 0)); - } - - [Fact] - public void CoordsCtor_ParsesTriplets() - { - var coords = new float[] { 0, 0, 0, 10, 0, 0, 10, 10, 0 }; - var pl = new Polyline3(coords); - pl.Count.Should().Be(3); - pl[1].Should().Be(new Point3(10, 0, 0)); - } - - [Fact] - public void MemoryCtor_ZeroCopy() - { - var arr = new Point3[] { new(1, 2, 3), new(4, 5, 6), new(7, 8, 9) }; - var mem = new ReadOnlyMemory>(arr); - var pl = new Polyline3(mem); - pl.Count.Should().Be(3); - pl[0].Should().Be(new Point3(1, 2, 3)); - } - - // ───────────────────────────────────────────── - // Start / End - // ───────────────────────────────────────────── - - [Fact] - public void Start_ReturnsFirstPoint() - { - var pl = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); - pl.Start.Should().Be(new Point3(1, 2, 3)); - } - - [Fact] - public void End_ReturnsLastPoint() - { - var pl = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); - pl.End.Should().Be(new Point3(4, 5, 6)); - } - - // ───────────────────────────────────────────── - // Length - // ───────────────────────────────────────────── - - [Fact] - public void Length_StraightLine() - { - var pl = new Polyline3(new Point3(0, 0, 0), new Point3(10, 0, 0)); - pl.Length().Should().BeApproximately(10f, Tol); - } - - [Fact] - public void Length_MultiSegment() - { - // L-shaped in 3D: (0,0,0)->(10,0,0)->(10,5,0) - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0), - new Point3(10, 5, 0)); - pl.Length().Should().BeApproximately(15f, Tol); - } - - [Fact] - public void Length_Diagonal3D() - { - // Diagonal from origin to (1,1,1), length = sqrt(3) - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(1, 1, 1)); - pl.Length().Should().BeApproximately(MathF.Sqrt(3f), Tol); - } - - [Fact] - public void Length_SinglePoint_ReturnsZero() - { - var pl = new Polyline3(new Point3(5, 5, 5)); - pl.Length().Should().Be(0f); - } - - // ───────────────────────────────────────────── - // Centroid - // ───────────────────────────────────────────── - - [Fact] - public void Centroid_SinglePoint_ReturnsThatPoint() - { - var pl = new Polyline3(new Point3(3, 4, 5)); - var c = pl.Centroid(); - c.X.Should().BeApproximately(3f, Tol); - c.Y.Should().BeApproximately(4f, Tol); - c.Z.Should().BeApproximately(5f, Tol); - } - - [Fact] - public void Centroid_TwoPoints_IsMidpoint() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0)); - var c = pl.Centroid(); - c.X.Should().BeApproximately(5f, Tol); - c.Y.Should().BeApproximately(0f, Tol); - c.Z.Should().BeApproximately(0f, Tol); - } - - [Fact] - public void Centroid_EqualSegments_IsGeometricCenter() - { - // Three equal-length segments along X: 0->10->20->30 - // Segment midpoints: 5, 15, 25 — all with equal length - // Centroid X = (5+15+25)/3 = 15 - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0), - new Point3(20, 0, 0), - new Point3(30, 0, 0)); - var c = pl.Centroid(); - c.X.Should().BeApproximately(15f, Tol); - c.Y.Should().BeApproximately(0f, Tol); - c.Z.Should().BeApproximately(0f, Tol); - } - - [Fact] - public void Centroid_UnequalSegments_WeightedByLength() - { - // Segment 1: (0,0,0)->(10,0,0) length=10 midpoint=(5,0,0) - // Segment 2: (10,0,0)->(11,0,0) length=1 midpoint=(10.5,0,0) - // Centroid X = (5*10 + 10.5*1) / 11 = 60.5/11 ≈ 5.5 - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0), - new Point3(11, 0, 0)); - var c = pl.Centroid(); - c.X.Should().BeApproximately(60.5f / 11f, Tol); - } - - [Fact] - public void Centroid_3D_AllAxes() - { - // Segment along Z axis: (0,0,0)->(0,0,10) - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(0, 0, 10)); - var c = pl.Centroid(); - c.X.Should().BeApproximately(0f, Tol); - c.Y.Should().BeApproximately(0f, Tol); - c.Z.Should().BeApproximately(5f, Tol); - } - - // ───────────────────────────────────────────── - // Reverse - // ───────────────────────────────────────────── - - [Fact] - public void Reverse_FlipsOrder() - { - var p1 = new Point3(0, 0, 0); - var p2 = new Point3(1, 0, 0); - var p3 = new Point3(2, 0, 0); - var pl = new Polyline3(p1, p2, p3).Reverse(); - pl[0].Should().Be(p3); - pl[1].Should().Be(p2); - pl[2].Should().Be(p1); - } - - // ───────────────────────────────────────────── - // Edges (open — n-1 segments, no wrap-around) - // ───────────────────────────────────────────── - - [Fact] - public void Edges_ReturnsNMinus1Segments() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0), - new Point3(10, 10, 0)); - var edges = pl.Edges(); - edges.Length.Should().Be(2); - var edgesSpan = edges.Span; - edgesSpan[0].Start.Should().Be(new Point3(0, 0, 0)); - edgesSpan[0].End.Should().Be(new Point3(10, 0, 0)); - edgesSpan[1].Start.Should().Be(new Point3(10, 0, 0)); - edgesSpan[1].End.Should().Be(new Point3(10, 10, 0)); - } - - [Fact] - public void Edges_SinglePoint_Empty() - { - var pl = new Polyline3(new Point3(5, 5, 5)); - pl.Edges().Span.ToArray().Should().BeEmpty(); - } - - // ───────────────────────────────────────────── - // Operators - // ───────────────────────────────────────────── - - [Fact] - public void PlusVector_TranslatesAll() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0)); - var moved = pl + new Vector3(5, 3, 1); - moved[0].Should().Be(new Point3(5, 3, 1)); - moved[1].Should().Be(new Point3(15, 3, 1)); - } - - [Fact] - public void MinusVector_TranslatesAll() - { - var pl = new Polyline3( - new Point3(5, 3, 1), - new Point3(15, 3, 1)); - var moved = pl - new Vector3(5, 3, 1); - moved[0].X.Should().BeApproximately(0f, Tol); - moved[0].Y.Should().BeApproximately(0f, Tol); - moved[0].Z.Should().BeApproximately(0f, Tol); - } - - [Fact] - public void MultiplyScalar_ScalesAll() - { - var pl = new Polyline3( - new Point3(1, 2, 3), - new Point3(4, 5, 6)); - var scaled = pl * 2f; - scaled[0].Should().Be(new Point3(2, 4, 6)); - scaled[1].Should().Be(new Point3(8, 10, 12)); - } - - [Fact] - public void DivideScalar_DividesAll() - { - var pl = new Polyline3( - new Point3(2, 4, 6), - new Point3(8, 10, 12)); - var scaled = pl / 2f; - scaled[0].Should().Be(new Point3(1, 2, 3)); - scaled[1].Should().Be(new Point3(4, 5, 6)); - } - - // ───────────────────────────────────────────── - // Equality - // ───────────────────────────────────────────── - - [Fact] - public void Equality_SamePoints_AreEqual() - { - var a = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); - var b = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); - a.Should().Be(b); - a.GetHashCode().Should().Be(b.GetHashCode()); - } - - [Fact] - public void Equality_DifferentPoints_AreNotEqual() - { - var a = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); - var b = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 7)); - a.Should().NotBe(b); - } - - // ───────────────────────────────────────────── - // Simplify (Ramer-Douglas-Peucker 3D) - // ───────────────────────────────────────────── - - [Fact] - public void Simplify_StraightLine_RemovesCollinearPoints() - { - // 5 collinear points along X → should reduce to 2 endpoints - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(2, 0, 0), - new Point3(5, 0, 0), - new Point3(7, 0, 0), - new Point3(10, 0, 0)); - var simplified = pl.Simplify(0.1f); - simplified.Count.Should().Be(2); - simplified[0].Should().Be(new Point3(0, 0, 0)); - simplified[1].Should().Be(new Point3(10, 0, 0)); - } - - [Fact] - public void Simplify_CollinearAlongDiagonal3D() - { - // Points along (1,1,1) direction — all collinear - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(1, 1, 1), - new Point3(2, 2, 2), - new Point3(3, 3, 3)); - var simplified = pl.Simplify(0.1f); - simplified.Count.Should().Be(2); - } - - [Fact] - public void Simplify_LShape_KeepsCorner() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0), - new Point3(10, 10, 0)); - var simplified = pl.Simplify(1f); - simplified.Count.Should().Be(3); - } - - [Fact] - public void Simplify_3DDeviation() - { - // Point deviates in Z only - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(5, 0, 2f), // 2 units off the line in Z - new Point3(10, 0, 0)); - pl.Simplify(1f).Count.Should().Be(3); // 2 > 1 → kept - pl.Simplify(3f).Count.Should().Be(2); // 2 < 3 → removed - } - - [Fact] - public void Simplify_TwoPoints_ReturnsSelf() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0)); - var simplified = pl.Simplify(1f); - simplified.Count.Should().Be(2); - } - - // ───────────────────────────────────────────── - // Scale - // ───────────────────────────────────────────── - - [Fact] - public void Scale_AroundCentroid_PreservesCentroid() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0)); - var centroid = pl.Centroid(); - var scaled = pl.Scale(2f); - var scaledCentroid = scaled.Centroid(); - scaledCentroid.X.Should().BeApproximately(centroid.X, Tol); - scaledCentroid.Y.Should().BeApproximately(centroid.Y, Tol); - scaledCentroid.Z.Should().BeApproximately(centroid.Z, Tol); - } - - [Fact] - public void Scale_DoublesLength() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0)); - var scaled = pl.Scale(2f); - scaled.Length().Should().BeApproximately(20f, Tol); - } - - // ───────────────────────────────────────────── - // Points list - // ───────────────────────────────────────────── - - [Fact] - public void Points_ReturnsReadOnlyList() - { - var pl = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); - pl.Points.Count.Should().Be(2); - pl.Points[0].Should().Be(new Point3(1, 2, 3)); - } - - // ───────────────────────────────────────────── - // ToString - // ───────────────────────────────────────────── - - [Fact] - public void ToString_ShowsCount() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(1, 1, 1)); - pl.ToString().Should().Be("Polyline3(2 points)"); - } - - // ───────────────────────────────────────────── - // JSON round-trip - // ───────────────────────────────────────────── - - [Fact] - public void Json_RoundTrip_Float() - { - var pl = new Polyline3( - new Point3(1, 2, 3), - new Point3(4, 5, 6)); - var json = System.Text.Json.JsonSerializer.Serialize(pl); - json.Should().Be("[1,2,3,4,5,6]"); - var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); - deserialized.Should().Be(pl); - } - - [Fact] - public void Json_RoundTrip_Double() - { - var pl = new Polyline3( - new Point3(1.5, 2.5, 3.5), - new Point3(4.5, 5.5, 6.5)); - var json = System.Text.Json.JsonSerializer.Serialize(pl); - var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); - deserialized.Should().Be(pl); - } - - [Fact] - public void Json_Empty() - { - var pl = new Polyline3(); - var json = System.Text.Json.JsonSerializer.Serialize(pl); - json.Should().Be("[]"); - var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); - deserialized.Count.Should().Be(0); - } -} +using FluentAssertions; +using ModelingEvolution.Drawing; + +namespace ModelingEvolution.Drawing.Tests; + +public class Polyline3Tests +{ + private const float Tol = 1e-4f; + + // ───────────────────────────────────────────── + // Construction + // ───────────────────────────────────────────── + + [Fact] + public void DefaultCtor_Empty() + { + var pl = new Polyline3(); + pl.Count.Should().Be(0); + pl.AsSpan().Length.Should().Be(0); + } + + [Fact] + public void ParamsCtor_StoresPoints() + { + var p1 = new Point3(0, 0, 0); + var p2 = new Point3(1, 0, 0); + var p3 = new Point3(1, 1, 0); + var pl = new Polyline3(p1, p2, p3); + pl.Count.Should().Be(3); + pl[0].Should().Be(p1); + pl[1].Should().Be(p2); + pl[2].Should().Be(p3); + } + + [Fact] + public void ListCtor_CopiesPoints() + { + var list = new List> + { + new(0, 0, 0), new(5, 5, 5), new(10, 0, 0) + }; + var pl = new Polyline3(list); + pl.Count.Should().Be(3); + pl[2].Should().Be(new Point3(10, 0, 0)); + } + + [Fact] + public void CoordsCtor_ParsesTriplets() + { + var coords = new float[] { 0, 0, 0, 10, 0, 0, 10, 10, 0 }; + var pl = new Polyline3(coords); + pl.Count.Should().Be(3); + pl[1].Should().Be(new Point3(10, 0, 0)); + } + + [Fact] + public void MemoryCtor_ZeroCopy() + { + var arr = new Point3[] { new(1, 2, 3), new(4, 5, 6), new(7, 8, 9) }; + var mem = new ReadOnlyMemory>(arr); + var pl = new Polyline3(mem); + pl.Count.Should().Be(3); + pl[0].Should().Be(new Point3(1, 2, 3)); + } + + // ───────────────────────────────────────────── + // Start / End + // ───────────────────────────────────────────── + + [Fact] + public void Start_ReturnsFirstPoint() + { + var pl = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); + pl.Start.Should().Be(new Point3(1, 2, 3)); + } + + [Fact] + public void End_ReturnsLastPoint() + { + var pl = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); + pl.End.Should().Be(new Point3(4, 5, 6)); + } + + // ───────────────────────────────────────────── + // Length + // ───────────────────────────────────────────── + + [Fact] + public void Length_StraightLine() + { + var pl = new Polyline3(new Point3(0, 0, 0), new Point3(10, 0, 0)); + pl.Length().Should().BeApproximately(10f, Tol); + } + + [Fact] + public void Length_MultiSegment() + { + // L-shaped in 3D: (0,0,0)->(10,0,0)->(10,5,0) + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0), + new Point3(10, 5, 0)); + pl.Length().Should().BeApproximately(15f, Tol); + } + + [Fact] + public void Length_Diagonal3D() + { + // Diagonal from origin to (1,1,1), length = sqrt(3) + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(1, 1, 1)); + pl.Length().Should().BeApproximately(MathF.Sqrt(3f), Tol); + } + + [Fact] + public void Length_SinglePoint_ReturnsZero() + { + var pl = new Polyline3(new Point3(5, 5, 5)); + pl.Length().Should().Be(0f); + } + + // ───────────────────────────────────────────── + // Centroid + // ───────────────────────────────────────────── + + [Fact] + public void Centroid_SinglePoint_ReturnsThatPoint() + { + var pl = new Polyline3(new Point3(3, 4, 5)); + var c = pl.Centroid(); + c.X.Should().BeApproximately(3f, Tol); + c.Y.Should().BeApproximately(4f, Tol); + c.Z.Should().BeApproximately(5f, Tol); + } + + [Fact] + public void Centroid_TwoPoints_IsMidpoint() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0)); + var c = pl.Centroid(); + c.X.Should().BeApproximately(5f, Tol); + c.Y.Should().BeApproximately(0f, Tol); + c.Z.Should().BeApproximately(0f, Tol); + } + + [Fact] + public void Centroid_EqualSegments_IsGeometricCenter() + { + // Three equal-length segments along X: 0->10->20->30 + // Segment midpoints: 5, 15, 25 — all with equal length + // Centroid X = (5+15+25)/3 = 15 + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0), + new Point3(20, 0, 0), + new Point3(30, 0, 0)); + var c = pl.Centroid(); + c.X.Should().BeApproximately(15f, Tol); + c.Y.Should().BeApproximately(0f, Tol); + c.Z.Should().BeApproximately(0f, Tol); + } + + [Fact] + public void Centroid_UnequalSegments_WeightedByLength() + { + // Segment 1: (0,0,0)->(10,0,0) length=10 midpoint=(5,0,0) + // Segment 2: (10,0,0)->(11,0,0) length=1 midpoint=(10.5,0,0) + // Centroid X = (5*10 + 10.5*1) / 11 = 60.5/11 ≈ 5.5 + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0), + new Point3(11, 0, 0)); + var c = pl.Centroid(); + c.X.Should().BeApproximately(60.5f / 11f, Tol); + } + + [Fact] + public void Centroid_3D_AllAxes() + { + // Segment along Z axis: (0,0,0)->(0,0,10) + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(0, 0, 10)); + var c = pl.Centroid(); + c.X.Should().BeApproximately(0f, Tol); + c.Y.Should().BeApproximately(0f, Tol); + c.Z.Should().BeApproximately(5f, Tol); + } + + // ───────────────────────────────────────────── + // Reverse + // ───────────────────────────────────────────── + + [Fact] + public void Reverse_FlipsOrder() + { + var p1 = new Point3(0, 0, 0); + var p2 = new Point3(1, 0, 0); + var p3 = new Point3(2, 0, 0); + var pl = new Polyline3(p1, p2, p3).Reverse(); + pl[0].Should().Be(p3); + pl[1].Should().Be(p2); + pl[2].Should().Be(p1); + } + + // ───────────────────────────────────────────── + // Edges (open — n-1 segments, no wrap-around) + // ───────────────────────────────────────────── + + [Fact] + public void Edges_ReturnsNMinus1Segments() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0), + new Point3(10, 10, 0)); + var edges = pl.Edges(); + edges.Length.Should().Be(2); + var edgesSpan = edges.Span; + edgesSpan[0].Start.Should().Be(new Point3(0, 0, 0)); + edgesSpan[0].End.Should().Be(new Point3(10, 0, 0)); + edgesSpan[1].Start.Should().Be(new Point3(10, 0, 0)); + edgesSpan[1].End.Should().Be(new Point3(10, 10, 0)); + } + + [Fact] + public void Edges_SinglePoint_Empty() + { + var pl = new Polyline3(new Point3(5, 5, 5)); + pl.Edges().Span.ToArray().Should().BeEmpty(); + } + + // ───────────────────────────────────────────── + // Operators + // ───────────────────────────────────────────── + + [Fact] + public void PlusVector_TranslatesAll() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0)); + var moved = pl + new Vector3(5, 3, 1); + moved[0].Should().Be(new Point3(5, 3, 1)); + moved[1].Should().Be(new Point3(15, 3, 1)); + } + + [Fact] + public void MinusVector_TranslatesAll() + { + var pl = new Polyline3( + new Point3(5, 3, 1), + new Point3(15, 3, 1)); + var moved = pl - new Vector3(5, 3, 1); + moved[0].X.Should().BeApproximately(0f, Tol); + moved[0].Y.Should().BeApproximately(0f, Tol); + moved[0].Z.Should().BeApproximately(0f, Tol); + } + + [Fact] + public void MultiplyScalar_ScalesAll() + { + var pl = new Polyline3( + new Point3(1, 2, 3), + new Point3(4, 5, 6)); + var scaled = pl * 2f; + scaled[0].Should().Be(new Point3(2, 4, 6)); + scaled[1].Should().Be(new Point3(8, 10, 12)); + } + + [Fact] + public void DivideScalar_DividesAll() + { + var pl = new Polyline3( + new Point3(2, 4, 6), + new Point3(8, 10, 12)); + var scaled = pl / 2f; + scaled[0].Should().Be(new Point3(1, 2, 3)); + scaled[1].Should().Be(new Point3(4, 5, 6)); + } + + // ───────────────────────────────────────────── + // Equality + // ───────────────────────────────────────────── + + [Fact] + public void Equality_SamePoints_AreEqual() + { + var a = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); + var b = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); + a.Should().Be(b); + a.GetHashCode().Should().Be(b.GetHashCode()); + } + + [Fact] + public void Equality_DifferentPoints_AreNotEqual() + { + var a = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); + var b = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 7)); + a.Should().NotBe(b); + } + + // ───────────────────────────────────────────── + // Simplify (Ramer-Douglas-Peucker 3D) + // ───────────────────────────────────────────── + + [Fact] + public void Simplify_StraightLine_RemovesCollinearPoints() + { + // 5 collinear points along X → should reduce to 2 endpoints + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(2, 0, 0), + new Point3(5, 0, 0), + new Point3(7, 0, 0), + new Point3(10, 0, 0)); + var simplified = pl.Simplify(0.1f); + simplified.Count.Should().Be(2); + simplified[0].Should().Be(new Point3(0, 0, 0)); + simplified[1].Should().Be(new Point3(10, 0, 0)); + } + + [Fact] + public void Simplify_CollinearAlongDiagonal3D() + { + // Points along (1,1,1) direction — all collinear + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(1, 1, 1), + new Point3(2, 2, 2), + new Point3(3, 3, 3)); + var simplified = pl.Simplify(0.1f); + simplified.Count.Should().Be(2); + } + + [Fact] + public void Simplify_LShape_KeepsCorner() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0), + new Point3(10, 10, 0)); + var simplified = pl.Simplify(1f); + simplified.Count.Should().Be(3); + } + + [Fact] + public void Simplify_3DDeviation() + { + // Point deviates in Z only + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(5, 0, 2f), // 2 units off the line in Z + new Point3(10, 0, 0)); + pl.Simplify(1f).Count.Should().Be(3); // 2 > 1 → kept + pl.Simplify(3f).Count.Should().Be(2); // 2 < 3 → removed + } + + [Fact] + public void Simplify_TwoPoints_ReturnsSelf() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0)); + var simplified = pl.Simplify(1f); + simplified.Count.Should().Be(2); + } + + // ───────────────────────────────────────────── + // Scale + // ───────────────────────────────────────────── + + [Fact] + public void Scale_AroundCentroid_PreservesCentroid() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0)); + var centroid = pl.Centroid(); + var scaled = pl.Scale(2f); + var scaledCentroid = scaled.Centroid(); + scaledCentroid.X.Should().BeApproximately(centroid.X, Tol); + scaledCentroid.Y.Should().BeApproximately(centroid.Y, Tol); + scaledCentroid.Z.Should().BeApproximately(centroid.Z, Tol); + } + + [Fact] + public void Scale_DoublesLength() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0)); + var scaled = pl.Scale(2f); + scaled.Length().Should().BeApproximately(20f, Tol); + } + + // ───────────────────────────────────────────── + // Points list + // ───────────────────────────────────────────── + + [Fact] + public void Points_ReturnsReadOnlyList() + { + var pl = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); + pl.Points.Count.Should().Be(2); + pl.Points[0].Should().Be(new Point3(1, 2, 3)); + } + + // ───────────────────────────────────────────── + // ToString + // ───────────────────────────────────────────── + + [Fact] + public void ToString_ShowsCount() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(1, 1, 1)); + pl.ToString().Should().Be("Polyline3(2 points)"); + } + + // ───────────────────────────────────────────── + // JSON round-trip + // ───────────────────────────────────────────── + + [Fact] + public void Json_RoundTrip_Float() + { + var pl = new Polyline3( + new Point3(1, 2, 3), + new Point3(4, 5, 6)); + var json = System.Text.Json.JsonSerializer.Serialize(pl); + json.Should().Be("[1,2,3,4,5,6]"); + var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); + deserialized.Should().Be(pl); + } + + [Fact] + public void Json_RoundTrip_Double() + { + var pl = new Polyline3( + new Point3(1.5, 2.5, 3.5), + new Point3(4.5, 5.5, 6.5)); + var json = System.Text.Json.JsonSerializer.Serialize(pl); + var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); + deserialized.Should().Be(pl); + } + + [Fact] + public void Json_Empty() + { + var pl = new Polyline3(); + var json = System.Text.Json.JsonSerializer.Serialize(pl); + json.Should().Be("[]"); + var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); + deserialized.Count.Should().Be(0); + } + + [Fact] + public void Transform_Frame_RotatesThenTranslatesEveryPoint() + { + var pl = new Polyline3(new Point3(1, 0, 0), new Point3(0, 2, 0)); + var frame = new Frame3(new Point3(10, 0, 0), Quaternion.FromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2)); + + var r = pl.Transform(frame); + + r.Count.Should().Be(2); + r[0].X.Should().BeApproximately(10, Tol); r[0].Y.Should().BeApproximately(1, Tol); r[0].Z.Should().BeApproximately(0, Tol); + r[1].X.Should().BeApproximately(8, Tol); r[1].Y.Should().BeApproximately(0, Tol); r[1].Z.Should().BeApproximately(0, Tol); + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/PosePath3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/PosePath3Tests.cs index 034671c..5777332 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/PosePath3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/PosePath3Tests.cs @@ -1,152 +1,167 @@ -using FluentAssertions; -using ModelingEvolution.Drawing; - -namespace ModelingEvolution.Drawing.Tests; - -public class PosePath3Tests -{ - private const float Tol = 1e-3f; - - private static Pose3 P(float x, float y, float z) => new(x, y, z, 0, 0, 0); - - // ───────────────────────────────────────────── - // Construction - // ───────────────────────────────────────────── - - [Fact] - public void DefaultCtor_Empty() - { - var path = new PosePath3(); - path.Count.Should().Be(0); - } - - [Fact] - public void ParamsCtor_StoresPoses() - { - var path = new PosePath3(P(0, 0, 0), P(10, 0, 0)); - path.Count.Should().Be(2); - path[0].Should().Be(P(0, 0, 0)); - path[1].Should().Be(P(10, 0, 0)); - } - - [Fact] - public void Start_End() - { - var path = new PosePath3(P(1, 2, 3), P(4, 5, 6)); - path.Start.Should().Be(P(1, 2, 3)); - path.End.Should().Be(P(4, 5, 6)); - } - - // ───────────────────────────────────────────── - // Length - // ───────────────────────────────────────────── - - [Fact] - public void Length_StraightLine() - { - var path = new PosePath3(P(0, 0, 0), P(10, 0, 0)); - path.Length().Should().BeApproximately(10f, Tol); - } - - [Fact] - public void Length_MultiSegment() - { - var path = new PosePath3(P(0, 0, 0), P(10, 0, 0), P(10, 5, 0)); - path.Length().Should().BeApproximately(15f, Tol); - } - - // ───────────────────────────────────────────── - // Reverse - // ───────────────────────────────────────────── - - [Fact] - public void Reverse_FlipsOrder() - { - var path = new PosePath3(P(0, 0, 0), P(10, 0, 0), P(20, 0, 0)); - var rev = path.Reverse(); - rev[0].Should().Be(P(20, 0, 0)); - rev[1].Should().Be(P(10, 0, 0)); - rev[2].Should().Be(P(0, 0, 0)); - } - - // ───────────────────────────────────────────── - // ToPolyline3 - // ───────────────────────────────────────────── - - [Fact] - public void ToPolyline3_ExtractsPositions() - { - var path = new PosePath3(P(1, 2, 3), P(4, 5, 6)); - var polyline = path.ToPolyline3(); - polyline.Count.Should().Be(2); - polyline[0].Should().Be(new Point3(1, 2, 3)); - polyline[1].Should().Be(new Point3(4, 5, 6)); - } - - // ───────────────────────────────────────────── - // ToTrajectory - // ───────────────────────────────────────────── - - [Fact] - public void ToTrajectory_AssignsUniformTimestamps() - { - var path = new PosePath3(P(0, 0, 0), P(5, 0, 0), P(10, 0, 0)); - var traj = path.ToTrajectory(4f); - traj.Count.Should().Be(3); - traj[0].Time.Should().BeApproximately(0f, Tol); - traj[1].Time.Should().BeApproximately(2f, Tol); - traj[2].Time.Should().BeApproximately(4f, Tol); - } - - // ───────────────────────────────────────────── - // Operators - // ───────────────────────────────────────────── - - [Fact] - public void PlusVector_TranslatesPositions() - { - var path = new PosePath3(P(0, 0, 0), P(10, 0, 0)); - var moved = path + new Vector3(1, 2, 3); - moved[0].X.Should().BeApproximately(1f, Tol); - moved[0].Y.Should().BeApproximately(2f, Tol); - moved[0].Z.Should().BeApproximately(3f, Tol); - } - - // ───────────────────────────────────────────── - // Equality - // ───────────────────────────────────────────── - - [Fact] - public void Equality_SamePoses_AreEqual() - { - var a = new PosePath3(P(1, 2, 3), P(4, 5, 6)); - var b = new PosePath3(P(1, 2, 3), P(4, 5, 6)); - a.Should().Be(b); - a.GetHashCode().Should().Be(b.GetHashCode()); - } - - // ───────────────────────────────────────────── - // JSON round-trip - // ───────────────────────────────────────────── - - [Fact] - public void Json_RoundTrip() - { - var path = new PosePath3(P(1, 2, 3), P(4, 5, 6)); - var json = System.Text.Json.JsonSerializer.Serialize(path); - json.Should().Be("[1,2,3,0,0,0,4,5,6,0,0,0]"); - var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); - deserialized.Should().Be(path); - } - - // ───────────────────────────────────────────── - // ToString - // ───────────────────────────────────────────── - - [Fact] - public void ToString_ShowsCount() - { - var path = new PosePath3(P(0, 0, 0), P(1, 1, 1)); - path.ToString().Should().Be("PosePath3(2 poses)"); - } -} +using FluentAssertions; +using ModelingEvolution.Drawing; + +namespace ModelingEvolution.Drawing.Tests; + +public class PosePath3Tests +{ + private const float Tol = 1e-3f; + + private static Pose3 P(float x, float y, float z) => new(x, y, z, 0, 0, 0); + + // ───────────────────────────────────────────── + // Construction + // ───────────────────────────────────────────── + + [Fact] + public void DefaultCtor_Empty() + { + var path = new PosePath3(); + path.Count.Should().Be(0); + } + + [Fact] + public void ParamsCtor_StoresPoses() + { + var path = new PosePath3(P(0, 0, 0), P(10, 0, 0)); + path.Count.Should().Be(2); + path[0].Should().Be(P(0, 0, 0)); + path[1].Should().Be(P(10, 0, 0)); + } + + [Fact] + public void Start_End() + { + var path = new PosePath3(P(1, 2, 3), P(4, 5, 6)); + path.Start.Should().Be(P(1, 2, 3)); + path.End.Should().Be(P(4, 5, 6)); + } + + // ───────────────────────────────────────────── + // Length + // ───────────────────────────────────────────── + + [Fact] + public void Length_StraightLine() + { + var path = new PosePath3(P(0, 0, 0), P(10, 0, 0)); + path.Length().Should().BeApproximately(10f, Tol); + } + + [Fact] + public void Length_MultiSegment() + { + var path = new PosePath3(P(0, 0, 0), P(10, 0, 0), P(10, 5, 0)); + path.Length().Should().BeApproximately(15f, Tol); + } + + // ───────────────────────────────────────────── + // Reverse + // ───────────────────────────────────────────── + + [Fact] + public void Reverse_FlipsOrder() + { + var path = new PosePath3(P(0, 0, 0), P(10, 0, 0), P(20, 0, 0)); + var rev = path.Reverse(); + rev[0].Should().Be(P(20, 0, 0)); + rev[1].Should().Be(P(10, 0, 0)); + rev[2].Should().Be(P(0, 0, 0)); + } + + // ───────────────────────────────────────────── + // ToPolyline3 + // ───────────────────────────────────────────── + + [Fact] + public void ToPolyline3_ExtractsPositions() + { + var path = new PosePath3(P(1, 2, 3), P(4, 5, 6)); + var polyline = path.ToPolyline3(); + polyline.Count.Should().Be(2); + polyline[0].Should().Be(new Point3(1, 2, 3)); + polyline[1].Should().Be(new Point3(4, 5, 6)); + } + + // ───────────────────────────────────────────── + // ToTrajectory + // ───────────────────────────────────────────── + + [Fact] + public void ToTrajectory_AssignsUniformTimestamps() + { + var path = new PosePath3(P(0, 0, 0), P(5, 0, 0), P(10, 0, 0)); + var traj = path.ToTrajectory(4f); + traj.Count.Should().Be(3); + traj[0].Time.Should().BeApproximately(0f, Tol); + traj[1].Time.Should().BeApproximately(2f, Tol); + traj[2].Time.Should().BeApproximately(4f, Tol); + } + + // ───────────────────────────────────────────── + // Operators + // ───────────────────────────────────────────── + + [Fact] + public void PlusVector_TranslatesPositions() + { + var path = new PosePath3(P(0, 0, 0), P(10, 0, 0)); + var moved = path + new Vector3(1, 2, 3); + moved[0].X.Should().BeApproximately(1f, Tol); + moved[0].Y.Should().BeApproximately(2f, Tol); + moved[0].Z.Should().BeApproximately(3f, Tol); + } + + // ───────────────────────────────────────────── + // Equality + // ───────────────────────────────────────────── + + [Fact] + public void Equality_SamePoses_AreEqual() + { + var a = new PosePath3(P(1, 2, 3), P(4, 5, 6)); + var b = new PosePath3(P(1, 2, 3), P(4, 5, 6)); + a.Should().Be(b); + a.GetHashCode().Should().Be(b.GetHashCode()); + } + + // ───────────────────────────────────────────── + // JSON round-trip + // ───────────────────────────────────────────── + + [Fact] + public void Json_RoundTrip() + { + var path = new PosePath3(P(1, 2, 3), P(4, 5, 6)); + var json = System.Text.Json.JsonSerializer.Serialize(path); + json.Should().Be("[1,2,3,0,0,0,4,5,6,0,0,0]"); + var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); + deserialized.Should().Be(path); + } + + // ───────────────────────────────────────────── + // ToString + // ───────────────────────────────────────────── + + [Fact] + public void ToString_ShowsCount() + { + var path = new PosePath3(P(0, 0, 0), P(1, 1, 1)); + path.ToString().Should().Be("PosePath3(2 poses)"); + } + + [Fact] + public void Transform_Frame_ComposesFrameTimesEachPose() + { + var path = new PosePath3(P(1, 0, 0), P(0, 2, 0)); + var frame = new Frame3(new Point3(10, 0, 0), Quaternion.FromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2)); + + var r = path.Transform(frame); + + r.Count.Should().Be(2); + r[0].X.Should().BeApproximately(10, Tol); r[0].Y.Should().BeApproximately(1, Tol); r[0].Z.Should().BeApproximately(0, Tol); + ((float)r[0].Rz).Should().BeApproximately(90, Tol); + r[1].X.Should().BeApproximately(8, Tol); r[1].Y.Should().BeApproximately(0, Tol); + ((float)r[1].Rz).Should().BeApproximately(90, Tol); + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Trajectory3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Trajectory3Tests.cs index 9bece25..0ea71fd 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Trajectory3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Trajectory3Tests.cs @@ -1,224 +1,240 @@ -using FluentAssertions; -using ModelingEvolution.Drawing; - -namespace ModelingEvolution.Drawing.Tests; - -public class Trajectory3Tests -{ - private const float Tol = 1e-3f; - - private static Pose3 P(float x, float y, float z) => new(x, y, z, 0, 0, 0); - private static Waypoint3 W(float x, float y, float z, float t) => new(P(x, y, z), t); - - // ───────────────────────────────────────────── - // Construction - // ───────────────────────────────────────────── - - [Fact] - public void DefaultCtor_Empty() - { - var traj = new Trajectory3(); - traj.Count.Should().Be(0); - } - - [Fact] - public void ParamsCtor_StoresWaypoints() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1)); - traj.Count.Should().Be(2); - traj[0].Position.Should().Be(new Point3(0, 0, 0)); - traj[0].Time.Should().Be(0f); - traj[1].Time.Should().Be(1f); - } - - [Fact] - public void Start_End() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 5)); - traj.Start.Time.Should().Be(0f); - traj.End.Time.Should().Be(5f); - } - - // ───────────────────────────────────────────── - // Duration & Length - // ───────────────────────────────────────────── - - [Fact] - public void Duration_IsLastMinusFirst() - { - var traj = new Trajectory3(W(0, 0, 0, 1), W(10, 0, 0, 4)); - traj.Duration().Should().BeApproximately(3f, Tol); - } - - [Fact] - public void Length_PositionalDistance() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1)); - traj.Length().Should().BeApproximately(10f, Tol); - } - - // ───────────────────────────────────────────── - // AtTime — interpolation - // ───────────────────────────────────────────── - - [Fact] - public void AtTime_AtStart_ReturnsFirstPose() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); - var pose = traj.AtTime(0f); - pose.X.Should().BeApproximately(0f, Tol); - } - - [Fact] - public void AtTime_AtEnd_ReturnsLastPose() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); - var pose = traj.AtTime(2f); - pose.X.Should().BeApproximately(10f, Tol); - } - - [Fact] - public void AtTime_AtMidpoint_InterpolatesPosition() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); - var pose = traj.AtTime(1f); - pose.X.Should().BeApproximately(5f, Tol); - pose.Y.Should().BeApproximately(0f, Tol); - pose.Z.Should().BeApproximately(0f, Tol); - } - - [Fact] - public void AtTime_BeforeStart_ClampsToFirst() - { - var traj = new Trajectory3(W(0, 0, 0, 1), W(10, 0, 0, 3)); - var pose = traj.AtTime(0f); - pose.X.Should().BeApproximately(0f, Tol); - } - - [Fact] - public void AtTime_AfterEnd_ClampsToLast() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); - var pose = traj.AtTime(100f); - pose.X.Should().BeApproximately(10f, Tol); - } - - [Fact] - public void AtTime_MultiSegment() - { - // 0->10 over [0,2], 10->30 over [2,4] - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2), W(30, 0, 0, 4)); - traj.AtTime(1f).X.Should().BeApproximately(5f, Tol); // midway first segment - traj.AtTime(3f).X.Should().BeApproximately(20f, Tol); // midway second segment - } - - // ───────────────────────────────────────────── - // Reverse - // ───────────────────────────────────────────── - - [Fact] - public void Reverse_FlipsOrderPreservesTimingIntervals() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1), W(30, 0, 0, 3)); - var rev = traj.Reverse(); - rev.Count.Should().Be(3); - // Original intervals: [0-1], [1-3] → reversed order with same intervals: [0-2], [2-3] - rev[0].Position.X.Should().BeApproximately(30f, Tol); - rev[0].Time.Should().BeApproximately(0f, Tol); - rev[1].Position.X.Should().BeApproximately(10f, Tol); - rev[1].Time.Should().BeApproximately(2f, Tol); - rev[2].Position.X.Should().BeApproximately(0f, Tol); - rev[2].Time.Should().BeApproximately(3f, Tol); - } - - // ───────────────────────────────────────────── - // ToPosePath & ToPolyline3 - // ───────────────────────────────────────────── - - [Fact] - public void ToPosePath_StripsTimestamps() - { - var traj = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); - var path = traj.ToPosePath(); - path.Count.Should().Be(2); - path[0].X.Should().BeApproximately(1f, Tol); - path[1].X.Should().BeApproximately(4f, Tol); - } - - [Fact] - public void ToPolyline3_ExtractsPositions() - { - var traj = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); - var poly = traj.ToPolyline3(); - poly.Count.Should().Be(2); - poly[0].Should().Be(new Point3(1, 2, 3)); - } - - // ───────────────────────────────────────────── - // Resample - // ───────────────────────────────────────────── - - [Fact] - public void Resample_UniformInterval() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 10)); - var resampled = traj.Resample(5f); - resampled.Count.Should().Be(3); // t=0, t=5, t=10 - resampled[0].Time.Should().BeApproximately(0f, Tol); - resampled[1].Time.Should().BeApproximately(5f, Tol); - resampled[2].Time.Should().BeApproximately(10f, Tol); - resampled[1].Position.X.Should().BeApproximately(5f, Tol); - } - - // ───────────────────────────────────────────── - // Operators - // ───────────────────────────────────────────── - - [Fact] - public void PlusVector_TranslatesPositions() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1)); - var moved = traj + new Vector3(1, 2, 3); - moved[0].Position.X.Should().BeApproximately(1f, Tol); - moved[0].Position.Y.Should().BeApproximately(2f, Tol); - moved[0].Time.Should().Be(0f); // time unchanged - } - - // ───────────────────────────────────────────── - // Equality - // ───────────────────────────────────────────── - - [Fact] - public void Equality_SameWaypoints_AreEqual() - { - var a = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); - var b = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); - a.Should().Be(b); - a.GetHashCode().Should().Be(b.GetHashCode()); - } - - // ───────────────────────────────────────────── - // JSON round-trip - // ───────────────────────────────────────────── - - [Fact] - public void Json_RoundTrip() - { - var traj = new Trajectory3(W(1, 2, 3, 0.5f), W(4, 5, 6, 1.5f)); - var json = System.Text.Json.JsonSerializer.Serialize(traj); - json.Should().Be("[1,2,3,0,0,0,0.5,4,5,6,0,0,0,1.5]"); - var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); - deserialized.Should().Be(traj); - } - - // ───────────────────────────────────────────── - // ToString - // ───────────────────────────────────────────── - - [Fact] - public void ToString_ShowsCount() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(1, 1, 1, 1)); - traj.ToString().Should().Be("Trajectory3(2 waypoints)"); - } -} +using FluentAssertions; +using ModelingEvolution.Drawing; + +namespace ModelingEvolution.Drawing.Tests; + +public class Trajectory3Tests +{ + private const float Tol = 1e-3f; + + private static Pose3 P(float x, float y, float z) => new(x, y, z, 0, 0, 0); + private static Waypoint3 W(float x, float y, float z, float t) => new(P(x, y, z), t); + + // ───────────────────────────────────────────── + // Construction + // ───────────────────────────────────────────── + + [Fact] + public void DefaultCtor_Empty() + { + var traj = new Trajectory3(); + traj.Count.Should().Be(0); + } + + [Fact] + public void ParamsCtor_StoresWaypoints() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1)); + traj.Count.Should().Be(2); + traj[0].Position.Should().Be(new Point3(0, 0, 0)); + traj[0].Time.Should().Be(0f); + traj[1].Time.Should().Be(1f); + } + + [Fact] + public void Start_End() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 5)); + traj.Start.Time.Should().Be(0f); + traj.End.Time.Should().Be(5f); + } + + // ───────────────────────────────────────────── + // Duration & Length + // ───────────────────────────────────────────── + + [Fact] + public void Duration_IsLastMinusFirst() + { + var traj = new Trajectory3(W(0, 0, 0, 1), W(10, 0, 0, 4)); + traj.Duration().Should().BeApproximately(3f, Tol); + } + + [Fact] + public void Length_PositionalDistance() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1)); + traj.Length().Should().BeApproximately(10f, Tol); + } + + // ───────────────────────────────────────────── + // AtTime — interpolation + // ───────────────────────────────────────────── + + [Fact] + public void AtTime_AtStart_ReturnsFirstPose() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); + var pose = traj.AtTime(0f); + pose.X.Should().BeApproximately(0f, Tol); + } + + [Fact] + public void AtTime_AtEnd_ReturnsLastPose() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); + var pose = traj.AtTime(2f); + pose.X.Should().BeApproximately(10f, Tol); + } + + [Fact] + public void AtTime_AtMidpoint_InterpolatesPosition() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); + var pose = traj.AtTime(1f); + pose.X.Should().BeApproximately(5f, Tol); + pose.Y.Should().BeApproximately(0f, Tol); + pose.Z.Should().BeApproximately(0f, Tol); + } + + [Fact] + public void AtTime_BeforeStart_ClampsToFirst() + { + var traj = new Trajectory3(W(0, 0, 0, 1), W(10, 0, 0, 3)); + var pose = traj.AtTime(0f); + pose.X.Should().BeApproximately(0f, Tol); + } + + [Fact] + public void AtTime_AfterEnd_ClampsToLast() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); + var pose = traj.AtTime(100f); + pose.X.Should().BeApproximately(10f, Tol); + } + + [Fact] + public void AtTime_MultiSegment() + { + // 0->10 over [0,2], 10->30 over [2,4] + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2), W(30, 0, 0, 4)); + traj.AtTime(1f).X.Should().BeApproximately(5f, Tol); // midway first segment + traj.AtTime(3f).X.Should().BeApproximately(20f, Tol); // midway second segment + } + + // ───────────────────────────────────────────── + // Reverse + // ───────────────────────────────────────────── + + [Fact] + public void Reverse_FlipsOrderPreservesTimingIntervals() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1), W(30, 0, 0, 3)); + var rev = traj.Reverse(); + rev.Count.Should().Be(3); + // Original intervals: [0-1], [1-3] → reversed order with same intervals: [0-2], [2-3] + rev[0].Position.X.Should().BeApproximately(30f, Tol); + rev[0].Time.Should().BeApproximately(0f, Tol); + rev[1].Position.X.Should().BeApproximately(10f, Tol); + rev[1].Time.Should().BeApproximately(2f, Tol); + rev[2].Position.X.Should().BeApproximately(0f, Tol); + rev[2].Time.Should().BeApproximately(3f, Tol); + } + + // ───────────────────────────────────────────── + // ToPosePath & ToPolyline3 + // ───────────────────────────────────────────── + + [Fact] + public void ToPosePath_StripsTimestamps() + { + var traj = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); + var path = traj.ToPosePath(); + path.Count.Should().Be(2); + path[0].X.Should().BeApproximately(1f, Tol); + path[1].X.Should().BeApproximately(4f, Tol); + } + + [Fact] + public void ToPolyline3_ExtractsPositions() + { + var traj = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); + var poly = traj.ToPolyline3(); + poly.Count.Should().Be(2); + poly[0].Should().Be(new Point3(1, 2, 3)); + } + + // ───────────────────────────────────────────── + // Resample + // ───────────────────────────────────────────── + + [Fact] + public void Resample_UniformInterval() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 10)); + var resampled = traj.Resample(5f); + resampled.Count.Should().Be(3); // t=0, t=5, t=10 + resampled[0].Time.Should().BeApproximately(0f, Tol); + resampled[1].Time.Should().BeApproximately(5f, Tol); + resampled[2].Time.Should().BeApproximately(10f, Tol); + resampled[1].Position.X.Should().BeApproximately(5f, Tol); + } + + // ───────────────────────────────────────────── + // Operators + // ───────────────────────────────────────────── + + [Fact] + public void PlusVector_TranslatesPositions() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1)); + var moved = traj + new Vector3(1, 2, 3); + moved[0].Position.X.Should().BeApproximately(1f, Tol); + moved[0].Position.Y.Should().BeApproximately(2f, Tol); + moved[0].Time.Should().Be(0f); // time unchanged + } + + // ───────────────────────────────────────────── + // Equality + // ───────────────────────────────────────────── + + [Fact] + public void Equality_SameWaypoints_AreEqual() + { + var a = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); + var b = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); + a.Should().Be(b); + a.GetHashCode().Should().Be(b.GetHashCode()); + } + + // ───────────────────────────────────────────── + // JSON round-trip + // ───────────────────────────────────────────── + + [Fact] + public void Json_RoundTrip() + { + var traj = new Trajectory3(W(1, 2, 3, 0.5f), W(4, 5, 6, 1.5f)); + var json = System.Text.Json.JsonSerializer.Serialize(traj); + json.Should().Be("[1,2,3,0,0,0,0.5,4,5,6,0,0,0,1.5]"); + var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); + deserialized.Should().Be(traj); + } + + // ───────────────────────────────────────────── + // ToString + // ───────────────────────────────────────────── + + [Fact] + public void ToString_ShowsCount() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(1, 1, 1, 1)); + traj.ToString().Should().Be("Trajectory3(2 waypoints)"); + } + + [Fact] + public void Transform_Frame_ComposesFrameTimesEachPose_PreservesTime() + { + var traj = new Trajectory3(W(1, 0, 0, 0), W(0, 2, 0, 0.5f)); + var frame = new Frame3(new Point3(10, 0, 0), Quaternion.FromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2)); + + var r = traj.Transform(frame); + + r.Count.Should().Be(2); + r[0].Pose.X.Should().BeApproximately(10, Tol); r[0].Pose.Y.Should().BeApproximately(1, Tol); + ((float)r[0].Pose.Rz).Should().BeApproximately(90, Tol); + r[0].Time.Should().Be(0); + r[1].Pose.X.Should().BeApproximately(8, Tol); r[1].Pose.Y.Should().BeApproximately(0, Tol); + r[1].Time.Should().Be(0.5f); + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Triangle3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Triangle3Tests.cs index bc91ce4..a2b1931 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Triangle3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Triangle3Tests.cs @@ -134,22 +134,23 @@ public void Triangle3_Normal_VerticalTriangleXZ_PointsAlongY() #region ToPose [Fact] - public void Triangle3_ToPose_PositionAtCentroid() + public void Triangle3_ToFrame_PositionAtCentroid_RotationFromSurface() { - var triangle = new Triangle3( - new Point3(0, 0, 0), - new Point3(3, 0, 0), - new Point3(0, 3, 0)); + var a = new Point3(0, 0, 0); + var b = new Point3(3, 0, 0); + var c = new Point3(0, 3, 0); + var triangle = new Triangle3(a, b, c); - var pose = triangle.ToPose(); + var frame = triangle.ToFrame(); - pose.X.Should().Be(1); - pose.Y.Should().Be(1); - pose.Z.Should().Be(0); + frame.Position.X.Should().Be(1); + frame.Position.Y.Should().Be(1); + frame.Position.Z.Should().Be(0); + frame.Rotation.Should().Be(Frame3.FromSurface(a, b, c).Rotation); } [Fact] - public void Triangle3_ToPose_HorizontalTriangle_ZAxisPointsUp() + public void Triangle3_ToFrame_HorizontalTriangle_ZAxisPointsUp() { // Horizontal triangle in XY plane var triangle = new Triangle3( @@ -157,13 +158,16 @@ public void Triangle3_ToPose_HorizontalTriangle_ZAxisPointsUp() new Point3(1, 0, 5), new Point3(0, 1, 5)); - var pose = triangle.ToPose(); + var frame = triangle.ToFrame(); - // Check that Z-axis of pose points along (0,0,1) - var rotation = pose.Rotation; - // For a horizontal surface, Rx and Ry should be near 0 - ((double)rotation.Rx).Should().BeApproximately(0, 1); - ((double)rotation.Ry).Should().BeApproximately(0, 1); + // The frame's Z-axis is the surface normal (0,0,1); its X-axis is along A→B + var z = frame.TransformVector(new Vector3(0, 0, 1)); + z.X.Should().BeApproximately(0, 1e-12); + z.Y.Should().BeApproximately(0, 1e-12); + z.Z.Should().BeApproximately(1, 1e-12); + var x = frame.TransformVector(new Vector3(1, 0, 0)); + x.X.Should().BeApproximately(1, 1e-12); + x.Y.Should().BeApproximately(0, 1e-12); } #endregion diff --git a/Sources/ModelingEvolution.Drawing/Polyline3.cs b/Sources/ModelingEvolution.Drawing/Polyline3.cs index 9a1411a..c35c988 100644 --- a/Sources/ModelingEvolution.Drawing/Polyline3.cs +++ b/Sources/ModelingEvolution.Drawing/Polyline3.cs @@ -1,437 +1,436 @@ -using System.Numerics; -using System.Runtime.InteropServices; -using System.Text.Json.Serialization; -using ProtoBuf; - -namespace ModelingEvolution.Drawing; - -/// -/// Represents an immutable open polyline defined by a sequence of 3D points. -/// Unlike Polygon, a polyline does not close — Edges() yields n-1 segments with no wrap-around. -/// -/// The numeric type used for coordinates. -[JsonConverter(typeof(Polyline3JsonConverterFactory))] -[ProtoContract] -public readonly record struct Polyline3 : IPoolable, Lease>> - where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, - IFloatingPointIeee754, IMinMaxValue, IParsable -{ - internal readonly ReadOnlyMemory> _points; - - [ProtoMember(1)] - private Point3[] ProtoPoints - { - get - { - if (_points.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_points, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _points.ToArray(); - } - init => _points = value ?? ReadOnlyMemory>.Empty; - } - - /// - /// Gets the number of vertices in this polyline. - /// - [JsonIgnore] - public int Count => _points.Length; - - /// - /// Returns a read-only span over the polyline's vertices. - /// Hoist the result before loops — this is a method call, not a field access. - /// - public ReadOnlySpan> AsSpan() => _points.Span; - - /// - /// Gets the vertex at the specified index. - /// - public Point3 this[int index] => _points.Span[index]; - - /// - /// Gets a read-only list of all vertices. For high-performance code, prefer AsSpan() instead. - /// - public IReadOnlyList> Points - { - get - { - if (_points.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_points, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _points.ToArray(); - } - } - - /// - /// Gets the first vertex of the polyline. - /// - [JsonIgnore] - public Point3 Start => _points.Span[0]; - - /// - /// Gets the last vertex of the polyline. - /// - [JsonIgnore] - public Point3 End => _points.Span[^1]; - - #region Constructors - - /// - /// Initializes a new empty polyline. - /// - public Polyline3() - { - _points = ReadOnlyMemory>.Empty; - } - - /// - /// Initializes a new polyline wrapping a ReadOnlyMemory of points. Zero-copy. - /// - public Polyline3(ReadOnlyMemory> memory) - { - _points = memory; - } - - /// - /// Initializes a new polyline from the specified points array. - /// - public Polyline3(params Point3[] points) - { - _points = points; - } - - /// - /// Initializes a new polyline from the specified list of points (copies to array). - /// - public Polyline3(IList> points) - { - if (points is Point3[] arr) - _points = arr; - else - { - var tmp = new Point3[points.Count]; - for (int i = 0; i < points.Count; i++) - tmp[i] = points[i]; - _points = tmp; - } - } - - /// - /// Initializes a new polyline from a flat list of coordinate values (x1, y1, z1, x2, y2, z2, ...). - /// - public Polyline3(IReadOnlyList coords) - { - var tmp = new Point3[coords.Count / 3]; - for (int i = 0; i < coords.Count; i += 3) - tmp[i / 3] = new Point3(coords[i], coords[i + 1], coords[i + 2]); - _points = tmp; - } - - #endregion - - #region Geometry - - /// - /// Computes the total length of this polyline (sum of all edge lengths). - /// - public T Length() - { - var span = AsSpan(); - int n = span.Length; - if (n < 2) return T.Zero; - - T length = T.Zero; - for (int i = 0; i < n - 1; i++) - length += Point3.Distance(span[i], span[i + 1]); - return length; - } - - /// - /// Computes the centroid (center of mass) of this polyline, weighted by segment length. - /// Each segment contributes its midpoint weighted by its length. - /// - public Point3 Centroid() - { - var span = AsSpan(); - if (span.Length == 0) return Point3.Zero; - if (span.Length == 1) return span[0]; - - T totalLength = T.Zero; - T cx = T.Zero; - T cy = T.Zero; - T cz = T.Zero; - - for (int i = 0; i < span.Length - 1; i++) - { - var segLen = Point3.Distance(span[i], span[i + 1]); - var two = T.CreateTruncating(2); - cx += (span[i].X + span[i + 1].X) / two * segLen; - cy += (span[i].Y + span[i + 1].Y) / two * segLen; - cz += (span[i].Z + span[i + 1].Z) / two * segLen; - totalLength += segLen; - } - - if (totalLength == T.Zero) return span[0]; - - return new Point3(cx / totalLength, cy / totalLength, cz / totalLength); - } - - /// - /// Returns a new polyline with the vertex order reversed. - /// - public Polyline3 Reverse() - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var reversed = mem.Span; - for (int i = 0; i < span.Length; i++) - reversed[i] = span[span.Length - 1 - i]; - return new Polyline3(mem); - } - - /// - /// Returns the edges of this polyline as 3D segments. Open — yields n-1 segments with no wrap-around. - /// - public ReadOnlyMemory> Edges() - { - var span = AsSpan(); - int n = span.Length; - if (n < 2) return ReadOnlyMemory>.Empty; - var mem = Alloc.Memory>(n - 1); - var edges = mem.Span; - for (int i = 0; i < n - 1; i++) - edges[i] = new Segment3(span[i], span[i + 1]); - return mem; - } - - /// - /// Returns a new polyline scaled by the given factor around the centroid. - /// - public Polyline3 Scale(T factor) - { - var span = AsSpan(); - if (span.Length == 0) return this; - - var c = Centroid(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - { - points[i] = new Point3( - c.X + (span[i].X - c.X) * factor, - c.Y + (span[i].Y - c.Y) * factor, - c.Z + (span[i].Z - c.Z) * factor); - } - return new Polyline3(mem); - } - - /// - /// Simplifies this polyline using the Ramer-Douglas-Peucker algorithm in 3D. - /// Removes points closer than to the line between their neighbors. - /// - public Polyline3 Simplify(T epsilon) - { - var span = AsSpan(); - if (span.Length < 3) return this; - - var epsSq = epsilon * epsilon; - Span keep = span.Length <= 1024 ? stackalloc bool[span.Length] : new bool[span.Length]; - keep[0] = true; - keep[span.Length - 1] = true; - - // Iterative stack-based RDP - int stackSize = span.Length * 2; - Span stack = stackSize <= 1024 ? stackalloc int[stackSize] : new int[stackSize]; - int top = 0; - stack[top++] = 0; - stack[top++] = span.Length - 1; - - while (top >= 2) - { - int end = stack[--top]; - int start = stack[--top]; - - var maxDistSq = T.Zero; - int maxIdx = start; - - var a = span[start]; - var b = span[end]; - var ab = (Vector3)(b - a); - var abLenSq = ab.LengthSquared; - - for (int i = start + 1; i < end; i++) - { - T distSq; - if (abLenSq < T.CreateTruncating(1e-18)) - { - // Degenerate segment — distance to start point - distSq = Point3.DistanceSquared(span[i], a); - } - else - { - // Distance from point to line via cross product: |AP x AB|^2 / |AB|^2 - var ap = (Vector3)(span[i] - a); - var cross = Vector3.Cross(ap, ab); - distSq = cross.LengthSquared / abLenSq; - } - - if (distSq > maxDistSq) - { - maxDistSq = distSq; - maxIdx = i; - } - } - - if (maxDistSq > epsSq) - { - keep[maxIdx] = true; - stack[top++] = start; - stack[top++] = maxIdx; - stack[top++] = maxIdx; - stack[top++] = end; - } - } - - int count = 0; - for (int i = 0; i < span.Length; i++) - if (keep[i]) count++; - - if (count == span.Length) return this; - - var mem = Alloc.Memory>(count); - var dst = mem.Span; - int idx = 0; - for (int i = 0; i < span.Length; i++) - if (keep[i]) dst[idx++] = span[i]; - - return new Polyline3(mem); - } - - /// - /// Returns a new polyline with all points rotated by the given rotation. - /// - public Polyline3 Transform(Rotation3 rotation) - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - points[i] = rotation.Rotate(span[i]); - return new Polyline3(mem); - } - - /// - /// Returns a new polyline with all points transformed by the given pose (rotation + translation). - /// - public Polyline3 Transform(Pose3 pose) - { - var frame = Frame3.FromPose3(pose, T.One); - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - points[i] = frame.TransformPoint(span[i]); - return new Polyline3(mem); - } - - #endregion - - #region Operators - - /// - /// Translates all points by adding the given vector. - /// - public static Polyline3 operator +(in Polyline3 a, in Vector3 f) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - points[i] = span[i] + f; - return new Polyline3(mem); - } - - /// - /// Translates all points by subtracting the given vector. - /// - public static Polyline3 operator -(in Polyline3 a, in Vector3 f) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - points[i] = span[i] - f; - return new Polyline3(mem); - } - - /// - /// Scales all points by a uniform scalar. - /// - public static Polyline3 operator *(in Polyline3 a, T scalar) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - points[i] = span[i] * scalar; - return new Polyline3(mem); - } - - /// - /// Divides all points by a uniform scalar. - /// - public static Polyline3 operator /(in Polyline3 a, T scalar) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - points[i] = span[i] / scalar; - return new Polyline3(mem); - } - - #endregion - - #region Equality - - /// - public bool Equals(Polyline3 other) => AsSpan().SequenceEqual(other.AsSpan()); - - /// - public override int GetHashCode() - { - var hash = new HashCode(); - var span = AsSpan(); - for (int i = 0; i < span.Length; i++) - hash.Add(span[i]); - return hash.ToHashCode(); - } - - #endregion - - /// - /// Detaches this polyline's backing memory from the given scope and returns a lease. - /// The caller becomes responsible for disposing the lease to return memory to the pool. - /// - public Lease> DetachFrom(AllocationScope scope) - { - if (!MemoryMarshal.TryGetArray(_points, out var seg)) - throw new InvalidOperationException("Cannot detach non-array-backed memory."); - var owner = scope.UntrackMemory(new Memory>(seg.Array!, seg.Offset, seg.Count)); - return new Lease> { _owner = owner }; - } - - internal Point3[] AsArray() - { - if (_points.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_points, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _points.ToArray(); - } - - /// - public override string ToString() => $"Polyline3({Count} points)"; -} +using System.Numerics; +using System.Runtime.InteropServices; +using System.Text.Json.Serialization; +using ProtoBuf; + +namespace ModelingEvolution.Drawing; + +/// +/// Represents an immutable open polyline defined by a sequence of 3D points. +/// Unlike Polygon, a polyline does not close — Edges() yields n-1 segments with no wrap-around. +/// +/// The numeric type used for coordinates. +[JsonConverter(typeof(Polyline3JsonConverterFactory))] +[ProtoContract] +public readonly record struct Polyline3 : IPoolable, Lease>> + where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, + IFloatingPointIeee754, IMinMaxValue, IParsable +{ + internal readonly ReadOnlyMemory> _points; + + [ProtoMember(1)] + private Point3[] ProtoPoints + { + get + { + if (_points.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_points, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _points.ToArray(); + } + init => _points = value ?? ReadOnlyMemory>.Empty; + } + + /// + /// Gets the number of vertices in this polyline. + /// + [JsonIgnore] + public int Count => _points.Length; + + /// + /// Returns a read-only span over the polyline's vertices. + /// Hoist the result before loops — this is a method call, not a field access. + /// + public ReadOnlySpan> AsSpan() => _points.Span; + + /// + /// Gets the vertex at the specified index. + /// + public Point3 this[int index] => _points.Span[index]; + + /// + /// Gets a read-only list of all vertices. For high-performance code, prefer AsSpan() instead. + /// + public IReadOnlyList> Points + { + get + { + if (_points.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_points, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _points.ToArray(); + } + } + + /// + /// Gets the first vertex of the polyline. + /// + [JsonIgnore] + public Point3 Start => _points.Span[0]; + + /// + /// Gets the last vertex of the polyline. + /// + [JsonIgnore] + public Point3 End => _points.Span[^1]; + + #region Constructors + + /// + /// Initializes a new empty polyline. + /// + public Polyline3() + { + _points = ReadOnlyMemory>.Empty; + } + + /// + /// Initializes a new polyline wrapping a ReadOnlyMemory of points. Zero-copy. + /// + public Polyline3(ReadOnlyMemory> memory) + { + _points = memory; + } + + /// + /// Initializes a new polyline from the specified points array. + /// + public Polyline3(params Point3[] points) + { + _points = points; + } + + /// + /// Initializes a new polyline from the specified list of points (copies to array). + /// + public Polyline3(IList> points) + { + if (points is Point3[] arr) + _points = arr; + else + { + var tmp = new Point3[points.Count]; + for (int i = 0; i < points.Count; i++) + tmp[i] = points[i]; + _points = tmp; + } + } + + /// + /// Initializes a new polyline from a flat list of coordinate values (x1, y1, z1, x2, y2, z2, ...). + /// + public Polyline3(IReadOnlyList coords) + { + var tmp = new Point3[coords.Count / 3]; + for (int i = 0; i < coords.Count; i += 3) + tmp[i / 3] = new Point3(coords[i], coords[i + 1], coords[i + 2]); + _points = tmp; + } + + #endregion + + #region Geometry + + /// + /// Computes the total length of this polyline (sum of all edge lengths). + /// + public T Length() + { + var span = AsSpan(); + int n = span.Length; + if (n < 2) return T.Zero; + + T length = T.Zero; + for (int i = 0; i < n - 1; i++) + length += Point3.Distance(span[i], span[i + 1]); + return length; + } + + /// + /// Computes the centroid (center of mass) of this polyline, weighted by segment length. + /// Each segment contributes its midpoint weighted by its length. + /// + public Point3 Centroid() + { + var span = AsSpan(); + if (span.Length == 0) return Point3.Zero; + if (span.Length == 1) return span[0]; + + T totalLength = T.Zero; + T cx = T.Zero; + T cy = T.Zero; + T cz = T.Zero; + + for (int i = 0; i < span.Length - 1; i++) + { + var segLen = Point3.Distance(span[i], span[i + 1]); + var two = T.CreateTruncating(2); + cx += (span[i].X + span[i + 1].X) / two * segLen; + cy += (span[i].Y + span[i + 1].Y) / two * segLen; + cz += (span[i].Z + span[i + 1].Z) / two * segLen; + totalLength += segLen; + } + + if (totalLength == T.Zero) return span[0]; + + return new Point3(cx / totalLength, cy / totalLength, cz / totalLength); + } + + /// + /// Returns a new polyline with the vertex order reversed. + /// + public Polyline3 Reverse() + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var reversed = mem.Span; + for (int i = 0; i < span.Length; i++) + reversed[i] = span[span.Length - 1 - i]; + return new Polyline3(mem); + } + + /// + /// Returns the edges of this polyline as 3D segments. Open — yields n-1 segments with no wrap-around. + /// + public ReadOnlyMemory> Edges() + { + var span = AsSpan(); + int n = span.Length; + if (n < 2) return ReadOnlyMemory>.Empty; + var mem = Alloc.Memory>(n - 1); + var edges = mem.Span; + for (int i = 0; i < n - 1; i++) + edges[i] = new Segment3(span[i], span[i + 1]); + return mem; + } + + /// + /// Returns a new polyline scaled by the given factor around the centroid. + /// + public Polyline3 Scale(T factor) + { + var span = AsSpan(); + if (span.Length == 0) return this; + + var c = Centroid(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + { + points[i] = new Point3( + c.X + (span[i].X - c.X) * factor, + c.Y + (span[i].Y - c.Y) * factor, + c.Z + (span[i].Z - c.Z) * factor); + } + return new Polyline3(mem); + } + + /// + /// Simplifies this polyline using the Ramer-Douglas-Peucker algorithm in 3D. + /// Removes points closer than to the line between their neighbors. + /// + public Polyline3 Simplify(T epsilon) + { + var span = AsSpan(); + if (span.Length < 3) return this; + + var epsSq = epsilon * epsilon; + Span keep = span.Length <= 1024 ? stackalloc bool[span.Length] : new bool[span.Length]; + keep[0] = true; + keep[span.Length - 1] = true; + + // Iterative stack-based RDP + int stackSize = span.Length * 2; + Span stack = stackSize <= 1024 ? stackalloc int[stackSize] : new int[stackSize]; + int top = 0; + stack[top++] = 0; + stack[top++] = span.Length - 1; + + while (top >= 2) + { + int end = stack[--top]; + int start = stack[--top]; + + var maxDistSq = T.Zero; + int maxIdx = start; + + var a = span[start]; + var b = span[end]; + var ab = (Vector3)(b - a); + var abLenSq = ab.LengthSquared; + + for (int i = start + 1; i < end; i++) + { + T distSq; + if (abLenSq < T.CreateTruncating(1e-18)) + { + // Degenerate segment — distance to start point + distSq = Point3.DistanceSquared(span[i], a); + } + else + { + // Distance from point to line via cross product: |AP x AB|^2 / |AB|^2 + var ap = (Vector3)(span[i] - a); + var cross = Vector3.Cross(ap, ab); + distSq = cross.LengthSquared / abLenSq; + } + + if (distSq > maxDistSq) + { + maxDistSq = distSq; + maxIdx = i; + } + } + + if (maxDistSq > epsSq) + { + keep[maxIdx] = true; + stack[top++] = start; + stack[top++] = maxIdx; + stack[top++] = maxIdx; + stack[top++] = end; + } + } + + int count = 0; + for (int i = 0; i < span.Length; i++) + if (keep[i]) count++; + + if (count == span.Length) return this; + + var mem = Alloc.Memory>(count); + var dst = mem.Span; + int idx = 0; + for (int i = 0; i < span.Length; i++) + if (keep[i]) dst[idx++] = span[i]; + + return new Polyline3(mem); + } + + /// + /// Returns a new polyline with all points rotated by the given rotation. + /// + public Polyline3 Transform(Rotation3 rotation) + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + points[i] = rotation.Rotate(span[i]); + return new Polyline3(mem); + } + + /// + /// Returns a new polyline with all points transformed by the given frame (rotation + translation). + /// + public Polyline3 Transform(in Frame3 frame) + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + points[i] = frame.TransformPoint(span[i]); + return new Polyline3(mem); + } + + #endregion + + #region Operators + + /// + /// Translates all points by adding the given vector. + /// + public static Polyline3 operator +(in Polyline3 a, in Vector3 f) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + points[i] = span[i] + f; + return new Polyline3(mem); + } + + /// + /// Translates all points by subtracting the given vector. + /// + public static Polyline3 operator -(in Polyline3 a, in Vector3 f) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + points[i] = span[i] - f; + return new Polyline3(mem); + } + + /// + /// Scales all points by a uniform scalar. + /// + public static Polyline3 operator *(in Polyline3 a, T scalar) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + points[i] = span[i] * scalar; + return new Polyline3(mem); + } + + /// + /// Divides all points by a uniform scalar. + /// + public static Polyline3 operator /(in Polyline3 a, T scalar) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + points[i] = span[i] / scalar; + return new Polyline3(mem); + } + + #endregion + + #region Equality + + /// + public bool Equals(Polyline3 other) => AsSpan().SequenceEqual(other.AsSpan()); + + /// + public override int GetHashCode() + { + var hash = new HashCode(); + var span = AsSpan(); + for (int i = 0; i < span.Length; i++) + hash.Add(span[i]); + return hash.ToHashCode(); + } + + #endregion + + /// + /// Detaches this polyline's backing memory from the given scope and returns a lease. + /// The caller becomes responsible for disposing the lease to return memory to the pool. + /// + public Lease> DetachFrom(AllocationScope scope) + { + if (!MemoryMarshal.TryGetArray(_points, out var seg)) + throw new InvalidOperationException("Cannot detach non-array-backed memory."); + var owner = scope.UntrackMemory(new Memory>(seg.Array!, seg.Offset, seg.Count)); + return new Lease> { _owner = owner }; + } + + internal Point3[] AsArray() + { + if (_points.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_points, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _points.ToArray(); + } + + /// + public override string ToString() => $"Polyline3({Count} points)"; +} diff --git a/Sources/ModelingEvolution.Drawing/PosePath3.cs b/Sources/ModelingEvolution.Drawing/PosePath3.cs index 3d7765d..0cee618 100644 --- a/Sources/ModelingEvolution.Drawing/PosePath3.cs +++ b/Sources/ModelingEvolution.Drawing/PosePath3.cs @@ -1,262 +1,262 @@ -using System.Numerics; -using System.Runtime.InteropServices; -using System.Text.Json.Serialization; -using ProtoBuf; - -namespace ModelingEvolution.Drawing; - -/// -/// Represents an immutable open path defined by a sequence of 3D poses (position + orientation). -/// This is the geometric counterpart — no time information. For time-parameterized paths, use . -/// -/// The numeric type used for coordinates. -[JsonConverter(typeof(PosePath3JsonConverterFactory))] -[ProtoContract] -public readonly record struct PosePath3 : IPoolable, Lease>> - where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, - IFloatingPointIeee754, IMinMaxValue, IParsable -{ - internal readonly ReadOnlyMemory> _poses; - - [ProtoMember(1)] - private Pose3[] ProtoPoses - { - get - { - if (_poses.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_poses, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _poses.ToArray(); - } - init => _poses = value ?? ReadOnlyMemory>.Empty; - } - - /// - /// Gets the number of poses in this path. - /// - [JsonIgnore] - public int Count => _poses.Length; - - /// - /// Returns a read-only span over the path's poses. - /// Hoist the result before loops — this is a method call, not a field access. - /// - public ReadOnlySpan> AsSpan() => _poses.Span; - - /// - /// Gets the pose at the specified index. - /// - public Pose3 this[int index] => _poses.Span[index]; - - /// - /// Gets a read-only list of all poses. For high-performance code, prefer AsSpan() instead. - /// - public IReadOnlyList> Poses - { - get - { - if (_poses.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_poses, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _poses.ToArray(); - } - } - - /// - /// Gets the first pose of the path. - /// - [JsonIgnore] - public Pose3 Start => _poses.Span[0]; - - /// - /// Gets the last pose of the path. - /// - [JsonIgnore] - public Pose3 End => _poses.Span[^1]; - - #region Constructors - - /// - /// Initializes a new empty path. - /// - public PosePath3() - { - _poses = ReadOnlyMemory>.Empty; - } - - /// - /// Initializes a new path wrapping a ReadOnlyMemory of poses. Zero-copy. - /// - public PosePath3(ReadOnlyMemory> memory) - { - _poses = memory; - } - - /// - /// Initializes a new path from the specified poses array. - /// - public PosePath3(params Pose3[] poses) - { - _poses = poses; - } - - /// - /// Initializes a new path from the specified list of poses (copies to array). - /// - public PosePath3(IList> poses) - { - if (poses is Pose3[] arr) - _poses = arr; - else - { - var tmp = new Pose3[poses.Count]; - for (int i = 0; i < poses.Count; i++) - tmp[i] = poses[i]; - _poses = tmp; - } - } - - #endregion - - #region Geometry - - /// - /// Computes the total positional length of this path (sum of distances between consecutive positions). - /// - public T Length() - { - var span = AsSpan(); - int n = span.Length; - if (n < 2) return T.Zero; - - T length = T.Zero; - for (int i = 0; i < n - 1; i++) - length += Point3.Distance(span[i].Position, span[i + 1].Position); - return length; - } - - /// - /// Returns a new path with the pose order reversed. - /// - public PosePath3 Reverse() - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var reversed = mem.Span; - for (int i = 0; i < span.Length; i++) - reversed[i] = span[span.Length - 1 - i]; - return new PosePath3(mem); - } - - /// - /// Returns a new path with all poses transformed by the given pose (rotation + translation). - /// - public PosePath3 Transform(Pose3 pose) - { - var frame = Frame3.FromPose3(pose, T.One); - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = (frame * Frame3.FromPose3(span[i], T.One)).ToPose3(T.One); - return new PosePath3(mem); - } - - /// - /// Extracts just the positions as a . - /// - public Polyline3 ToPolyline3() - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = span[i].Position; - return new Polyline3(mem); - } - - /// - /// Creates a by assigning uniform timestamps from 0 to . - /// - public Trajectory3 ToTrajectory(T totalTime) - { - var span = AsSpan(); - if (span.Length == 0) return new Trajectory3(); - if (span.Length == 1) return new Trajectory3(new Waypoint3(span[0], T.Zero)); - - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - var n = T.CreateTruncating(span.Length - 1); - for (int i = 0; i < span.Length; i++) - { - var t = totalTime * T.CreateTruncating(i) / n; - dst[i] = new Waypoint3(span[i], t); - } - return new Trajectory3(mem); - } - - #endregion - - #region Operators - - /// - /// Translates all positions by adding the given vector (rotations unchanged). - /// - public static PosePath3 operator +(in PosePath3 a, in Vector3 f) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = span[i] with { Position = span[i].Position + f }; - return new PosePath3(mem); - } - - /// - /// Translates all positions by subtracting the given vector (rotations unchanged). - /// - public static PosePath3 operator -(in PosePath3 a, in Vector3 f) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = span[i] with { Position = span[i].Position - f }; - return new PosePath3(mem); - } - - #endregion - - #region Equality - - /// - public bool Equals(PosePath3 other) => AsSpan().SequenceEqual(other.AsSpan()); - - /// - public override int GetHashCode() - { - var hash = new HashCode(); - var span = AsSpan(); - for (int i = 0; i < span.Length; i++) - hash.Add(span[i]); - return hash.ToHashCode(); - } - - #endregion - - /// - /// Detaches this path's backing memory from the given scope and returns a lease. - /// - public Lease> DetachFrom(AllocationScope scope) - { - if (!MemoryMarshal.TryGetArray(_poses, out var seg)) - throw new InvalidOperationException("Cannot detach non-array-backed memory."); - var owner = scope.UntrackMemory(new Memory>(seg.Array!, seg.Offset, seg.Count)); - return new Lease> { _owner = owner }; - } - - /// - public override string ToString() => $"PosePath3({Count} poses)"; -} +using System.Numerics; +using System.Runtime.InteropServices; +using System.Text.Json.Serialization; +using ProtoBuf; + +namespace ModelingEvolution.Drawing; + +/// +/// Represents an immutable open path defined by a sequence of 3D poses (position + orientation). +/// This is the geometric counterpart — no time information. For time-parameterized paths, use . +/// +/// The numeric type used for coordinates. +[JsonConverter(typeof(PosePath3JsonConverterFactory))] +[ProtoContract] +public readonly record struct PosePath3 : IPoolable, Lease>> + where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, + IFloatingPointIeee754, IMinMaxValue, IParsable +{ + internal readonly ReadOnlyMemory> _poses; + + [ProtoMember(1)] + private Pose3[] ProtoPoses + { + get + { + if (_poses.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_poses, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _poses.ToArray(); + } + init => _poses = value ?? ReadOnlyMemory>.Empty; + } + + /// + /// Gets the number of poses in this path. + /// + [JsonIgnore] + public int Count => _poses.Length; + + /// + /// Returns a read-only span over the path's poses. + /// Hoist the result before loops — this is a method call, not a field access. + /// + public ReadOnlySpan> AsSpan() => _poses.Span; + + /// + /// Gets the pose at the specified index. + /// + public Pose3 this[int index] => _poses.Span[index]; + + /// + /// Gets a read-only list of all poses. For high-performance code, prefer AsSpan() instead. + /// + public IReadOnlyList> Poses + { + get + { + if (_poses.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_poses, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _poses.ToArray(); + } + } + + /// + /// Gets the first pose of the path. + /// + [JsonIgnore] + public Pose3 Start => _poses.Span[0]; + + /// + /// Gets the last pose of the path. + /// + [JsonIgnore] + public Pose3 End => _poses.Span[^1]; + + #region Constructors + + /// + /// Initializes a new empty path. + /// + public PosePath3() + { + _poses = ReadOnlyMemory>.Empty; + } + + /// + /// Initializes a new path wrapping a ReadOnlyMemory of poses. Zero-copy. + /// + public PosePath3(ReadOnlyMemory> memory) + { + _poses = memory; + } + + /// + /// Initializes a new path from the specified poses array. + /// + public PosePath3(params Pose3[] poses) + { + _poses = poses; + } + + /// + /// Initializes a new path from the specified list of poses (copies to array). + /// + public PosePath3(IList> poses) + { + if (poses is Pose3[] arr) + _poses = arr; + else + { + var tmp = new Pose3[poses.Count]; + for (int i = 0; i < poses.Count; i++) + tmp[i] = poses[i]; + _poses = tmp; + } + } + + #endregion + + #region Geometry + + /// + /// Computes the total positional length of this path (sum of distances between consecutive positions). + /// + public T Length() + { + var span = AsSpan(); + int n = span.Length; + if (n < 2) return T.Zero; + + T length = T.Zero; + for (int i = 0; i < n - 1; i++) + length += Point3.Distance(span[i].Position, span[i + 1].Position); + return length; + } + + /// + /// Returns a new path with the pose order reversed. + /// + public PosePath3 Reverse() + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var reversed = mem.Span; + for (int i = 0; i < span.Length; i++) + reversed[i] = span[span.Length - 1 - i]; + return new PosePath3(mem); + } + + /// + /// Returns a new path with every pose composed as frame · pose. + /// The elements are stored as wire ; each is converted with lengthScale 1. + /// + public PosePath3 Transform(in Frame3 frame) + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = (frame * Frame3.FromPose3(span[i], T.One)).ToPose3(T.One); + return new PosePath3(mem); + } + + /// + /// Extracts just the positions as a . + /// + public Polyline3 ToPolyline3() + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = span[i].Position; + return new Polyline3(mem); + } + + /// + /// Creates a by assigning uniform timestamps from 0 to . + /// + public Trajectory3 ToTrajectory(T totalTime) + { + var span = AsSpan(); + if (span.Length == 0) return new Trajectory3(); + if (span.Length == 1) return new Trajectory3(new Waypoint3(span[0], T.Zero)); + + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + var n = T.CreateTruncating(span.Length - 1); + for (int i = 0; i < span.Length; i++) + { + var t = totalTime * T.CreateTruncating(i) / n; + dst[i] = new Waypoint3(span[i], t); + } + return new Trajectory3(mem); + } + + #endregion + + #region Operators + + /// + /// Translates all positions by adding the given vector (rotations unchanged). + /// + public static PosePath3 operator +(in PosePath3 a, in Vector3 f) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = span[i] with { Position = span[i].Position + f }; + return new PosePath3(mem); + } + + /// + /// Translates all positions by subtracting the given vector (rotations unchanged). + /// + public static PosePath3 operator -(in PosePath3 a, in Vector3 f) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = span[i] with { Position = span[i].Position - f }; + return new PosePath3(mem); + } + + #endregion + + #region Equality + + /// + public bool Equals(PosePath3 other) => AsSpan().SequenceEqual(other.AsSpan()); + + /// + public override int GetHashCode() + { + var hash = new HashCode(); + var span = AsSpan(); + for (int i = 0; i < span.Length; i++) + hash.Add(span[i]); + return hash.ToHashCode(); + } + + #endregion + + /// + /// Detaches this path's backing memory from the given scope and returns a lease. + /// + public Lease> DetachFrom(AllocationScope scope) + { + if (!MemoryMarshal.TryGetArray(_poses, out var seg)) + throw new InvalidOperationException("Cannot detach non-array-backed memory."); + var owner = scope.UntrackMemory(new Memory>(seg.Array!, seg.Offset, seg.Count)); + return new Lease> { _owner = owner }; + } + + /// + public override string ToString() => $"PosePath3({Count} poses)"; +} diff --git a/Sources/ModelingEvolution.Drawing/Trajectory3.cs b/Sources/ModelingEvolution.Drawing/Trajectory3.cs index 3554bc3..6f7e34f 100644 --- a/Sources/ModelingEvolution.Drawing/Trajectory3.cs +++ b/Sources/ModelingEvolution.Drawing/Trajectory3.cs @@ -1,340 +1,340 @@ -using System.Numerics; -using System.Runtime.InteropServices; -using System.Text.Json.Serialization; -using ProtoBuf; - -namespace ModelingEvolution.Drawing; - -/// -/// Represents an immutable time-parameterized path through 3D poses. -/// Each element is a containing a and a timestamp. -/// For untimed paths, use . -/// -/// The numeric type used for coordinates and time. -[JsonConverter(typeof(Trajectory3JsonConverterFactory))] -[ProtoContract] -public readonly record struct Trajectory3 : IPoolable, Lease>> - where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, - IFloatingPointIeee754, IMinMaxValue, IParsable -{ - internal readonly ReadOnlyMemory> _waypoints; - - [ProtoMember(1)] - private Waypoint3[] ProtoWaypoints - { - get - { - if (_waypoints.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_waypoints, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _waypoints.ToArray(); - } - init => _waypoints = value ?? ReadOnlyMemory>.Empty; - } - - /// - /// Gets the number of waypoints in this trajectory. - /// - [JsonIgnore] - public int Count => _waypoints.Length; - - /// - /// Returns a read-only span over the trajectory's waypoints. - /// Hoist the result before loops — this is a method call, not a field access. - /// - public ReadOnlySpan> AsSpan() => _waypoints.Span; - - /// - /// Gets the waypoint at the specified index. - /// - public Waypoint3 this[int index] => _waypoints.Span[index]; - - /// - /// Gets a read-only list of all waypoints. For high-performance code, prefer AsSpan() instead. - /// - public IReadOnlyList> Waypoints - { - get - { - if (_waypoints.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_waypoints, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _waypoints.ToArray(); - } - } - - /// - /// Gets the first waypoint. - /// - [JsonIgnore] - public Waypoint3 Start => _waypoints.Span[0]; - - /// - /// Gets the last waypoint. - /// - [JsonIgnore] - public Waypoint3 End => _waypoints.Span[^1]; - - #region Constructors - - /// - /// Initializes a new empty trajectory. - /// - public Trajectory3() - { - _waypoints = ReadOnlyMemory>.Empty; - } - - /// - /// Initializes a new trajectory wrapping a ReadOnlyMemory of waypoints. Zero-copy. - /// - public Trajectory3(ReadOnlyMemory> memory) - { - _waypoints = memory; - } - - /// - /// Initializes a new trajectory from the specified waypoints array. - /// - public Trajectory3(params Waypoint3[] waypoints) - { - _waypoints = waypoints; - } - - /// - /// Initializes a new trajectory from the specified list of waypoints (copies to array). - /// - public Trajectory3(IList> waypoints) - { - if (waypoints is Waypoint3[] arr) - _waypoints = arr; - else - { - var tmp = new Waypoint3[waypoints.Count]; - for (int i = 0; i < waypoints.Count; i++) - tmp[i] = waypoints[i]; - _waypoints = tmp; - } - } - - #endregion - - #region Geometry & Kinematics - - /// - /// Gets the total duration of this trajectory (last time minus first time). - /// - public T Duration() - { - var span = AsSpan(); - if (span.Length < 2) return T.Zero; - return span[^1].Time - span[0].Time; - } - - /// - /// Computes the total positional length of this trajectory (sum of distances between consecutive positions). - /// - public T Length() - { - var span = AsSpan(); - int n = span.Length; - if (n < 2) return T.Zero; - - T length = T.Zero; - for (int i = 0; i < n - 1; i++) - length += Point3.Distance(span[i].Position, span[i + 1].Position); - return length; - } - - /// - /// Interpolates the pose at the given time using linear position interpolation and Slerp for rotation. - /// Clamps to the first/last waypoint if is outside the trajectory range. - /// - public Pose3 AtTime(T time) - { - var span = AsSpan(); - if (span.Length == 0) return Pose3.Identity; - if (span.Length == 1) return span[0].Pose; - - // Clamp to range - if (time <= span[0].Time) return span[0].Pose; - if (time >= span[^1].Time) return span[^1].Pose; - - // Binary search for the interval - int lo = 0, hi = span.Length - 2; - while (lo < hi) - { - int mid = (lo + hi) / 2; - if (span[mid + 1].Time < time) - lo = mid + 1; - else - hi = mid; - } - - var a = span[lo]; - var b = span[lo + 1]; - var dt = b.Time - a.Time; - if (dt == T.Zero) return a.Pose; - - var t = (time - a.Time) / dt; - return Frame3.Interpolate(Frame3.FromPose3(a.Pose, T.One), Frame3.FromPose3(b.Pose, T.One), t).ToPose3(T.One); - } - - /// - /// Returns a new trajectory with the waypoint order and timestamps reversed. - /// The new first waypoint has time 0, preserving the original time intervals. - /// - public Trajectory3 Reverse() - { - var span = AsSpan(); - if (span.Length == 0) return this; - - var totalTime = Duration(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - var t0 = span[0].Time; - for (int i = 0; i < span.Length; i++) - { - var original = span[span.Length - 1 - i]; - var newTime = totalTime - (original.Time - t0); - dst[i] = new Waypoint3(original.Pose, t0 + newTime); - } - return new Trajectory3(mem); - } - - /// - /// Returns a new trajectory with all poses transformed by the given pose (rotation + translation). - /// Timestamps are preserved. - /// - public Trajectory3 Transform(Pose3 pose) - { - var frame = Frame3.FromPose3(pose, T.One); - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = new Waypoint3((frame * Frame3.FromPose3(span[i].Pose, T.One)).ToPose3(T.One), span[i].Time); - return new Trajectory3(mem); - } - - /// - /// Strips timestamps and returns a . - /// - public PosePath3 ToPosePath() - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = span[i].Pose; - return new PosePath3(mem); - } - - /// - /// Extracts just the positions as a . - /// - public Polyline3 ToPolyline3() - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = span[i].Position; - return new Polyline3(mem); - } - - /// - /// Resamples the trajectory at uniform time intervals. - /// - /// Time between consecutive samples. - public Trajectory3 Resample(T interval) - { - var span = AsSpan(); - if (span.Length < 2) return this; - - var duration = Duration(); - var t0 = span[0].Time; - int sampleCount = int.CreateChecked(T.Floor(duration / interval)) + 1; - var mem = Alloc.Memory>(sampleCount); - var dst = mem.Span; - - for (int i = 0; i < sampleCount; i++) - { - var time = t0 + interval * T.CreateTruncating(i); - if (time > t0 + duration) time = t0 + duration; - dst[i] = new Waypoint3(AtTime(time), time); - } - - return new Trajectory3(mem); - } - - /// - /// Creates a for real-time playback of this trajectory. - /// - public TrajectoryController Control() => new(this); - - #endregion - - #region Operators - - /// - /// Translates all positions by adding the given vector (rotations and timestamps unchanged). - /// - public static Trajectory3 operator +(in Trajectory3 a, in Vector3 f) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = new Waypoint3(span[i].Pose with { Position = span[i].Pose.Position + f }, span[i].Time); - return new Trajectory3(mem); - } - - /// - /// Translates all positions by subtracting the given vector (rotations and timestamps unchanged). - /// - public static Trajectory3 operator -(in Trajectory3 a, in Vector3 f) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = new Waypoint3(span[i].Pose with { Position = span[i].Pose.Position - f }, span[i].Time); - return new Trajectory3(mem); - } - - #endregion - - #region Equality - - /// - public bool Equals(Trajectory3 other) => AsSpan().SequenceEqual(other.AsSpan()); - - /// - public override int GetHashCode() - { - var hash = new HashCode(); - var span = AsSpan(); - for (int i = 0; i < span.Length; i++) - hash.Add(span[i]); - return hash.ToHashCode(); - } - - #endregion - - /// - /// Detaches this trajectory's backing memory from the given scope and returns a lease. - /// - public Lease> DetachFrom(AllocationScope scope) - { - if (!MemoryMarshal.TryGetArray(_waypoints, out var seg)) - throw new InvalidOperationException("Cannot detach non-array-backed memory."); - var owner = scope.UntrackMemory(new Memory>(seg.Array!, seg.Offset, seg.Count)); - return new Lease> { _owner = owner }; - } - - /// - public override string ToString() => $"Trajectory3({Count} waypoints)"; -} +using System.Numerics; +using System.Runtime.InteropServices; +using System.Text.Json.Serialization; +using ProtoBuf; + +namespace ModelingEvolution.Drawing; + +/// +/// Represents an immutable time-parameterized path through 3D poses. +/// Each element is a containing a and a timestamp. +/// For untimed paths, use . +/// +/// The numeric type used for coordinates and time. +[JsonConverter(typeof(Trajectory3JsonConverterFactory))] +[ProtoContract] +public readonly record struct Trajectory3 : IPoolable, Lease>> + where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, + IFloatingPointIeee754, IMinMaxValue, IParsable +{ + internal readonly ReadOnlyMemory> _waypoints; + + [ProtoMember(1)] + private Waypoint3[] ProtoWaypoints + { + get + { + if (_waypoints.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_waypoints, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _waypoints.ToArray(); + } + init => _waypoints = value ?? ReadOnlyMemory>.Empty; + } + + /// + /// Gets the number of waypoints in this trajectory. + /// + [JsonIgnore] + public int Count => _waypoints.Length; + + /// + /// Returns a read-only span over the trajectory's waypoints. + /// Hoist the result before loops — this is a method call, not a field access. + /// + public ReadOnlySpan> AsSpan() => _waypoints.Span; + + /// + /// Gets the waypoint at the specified index. + /// + public Waypoint3 this[int index] => _waypoints.Span[index]; + + /// + /// Gets a read-only list of all waypoints. For high-performance code, prefer AsSpan() instead. + /// + public IReadOnlyList> Waypoints + { + get + { + if (_waypoints.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_waypoints, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _waypoints.ToArray(); + } + } + + /// + /// Gets the first waypoint. + /// + [JsonIgnore] + public Waypoint3 Start => _waypoints.Span[0]; + + /// + /// Gets the last waypoint. + /// + [JsonIgnore] + public Waypoint3 End => _waypoints.Span[^1]; + + #region Constructors + + /// + /// Initializes a new empty trajectory. + /// + public Trajectory3() + { + _waypoints = ReadOnlyMemory>.Empty; + } + + /// + /// Initializes a new trajectory wrapping a ReadOnlyMemory of waypoints. Zero-copy. + /// + public Trajectory3(ReadOnlyMemory> memory) + { + _waypoints = memory; + } + + /// + /// Initializes a new trajectory from the specified waypoints array. + /// + public Trajectory3(params Waypoint3[] waypoints) + { + _waypoints = waypoints; + } + + /// + /// Initializes a new trajectory from the specified list of waypoints (copies to array). + /// + public Trajectory3(IList> waypoints) + { + if (waypoints is Waypoint3[] arr) + _waypoints = arr; + else + { + var tmp = new Waypoint3[waypoints.Count]; + for (int i = 0; i < waypoints.Count; i++) + tmp[i] = waypoints[i]; + _waypoints = tmp; + } + } + + #endregion + + #region Geometry & Kinematics + + /// + /// Gets the total duration of this trajectory (last time minus first time). + /// + public T Duration() + { + var span = AsSpan(); + if (span.Length < 2) return T.Zero; + return span[^1].Time - span[0].Time; + } + + /// + /// Computes the total positional length of this trajectory (sum of distances between consecutive positions). + /// + public T Length() + { + var span = AsSpan(); + int n = span.Length; + if (n < 2) return T.Zero; + + T length = T.Zero; + for (int i = 0; i < n - 1; i++) + length += Point3.Distance(span[i].Position, span[i + 1].Position); + return length; + } + + /// + /// Interpolates the pose at the given time using linear position interpolation and Slerp for rotation. + /// Clamps to the first/last waypoint if is outside the trajectory range. + /// + public Pose3 AtTime(T time) + { + var span = AsSpan(); + if (span.Length == 0) return Pose3.Identity; + if (span.Length == 1) return span[0].Pose; + + // Clamp to range + if (time <= span[0].Time) return span[0].Pose; + if (time >= span[^1].Time) return span[^1].Pose; + + // Binary search for the interval + int lo = 0, hi = span.Length - 2; + while (lo < hi) + { + int mid = (lo + hi) / 2; + if (span[mid + 1].Time < time) + lo = mid + 1; + else + hi = mid; + } + + var a = span[lo]; + var b = span[lo + 1]; + var dt = b.Time - a.Time; + if (dt == T.Zero) return a.Pose; + + var t = (time - a.Time) / dt; + return Frame3.Interpolate(Frame3.FromPose3(a.Pose, T.One), Frame3.FromPose3(b.Pose, T.One), t).ToPose3(T.One); + } + + /// + /// Returns a new trajectory with the waypoint order and timestamps reversed. + /// The new first waypoint has time 0, preserving the original time intervals. + /// + public Trajectory3 Reverse() + { + var span = AsSpan(); + if (span.Length == 0) return this; + + var totalTime = Duration(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + var t0 = span[0].Time; + for (int i = 0; i < span.Length; i++) + { + var original = span[span.Length - 1 - i]; + var newTime = totalTime - (original.Time - t0); + dst[i] = new Waypoint3(original.Pose, t0 + newTime); + } + return new Trajectory3(mem); + } + + /// + /// Returns a new trajectory with every waypoint pose composed as frame · pose. + /// The waypoint poses are stored as wire ; each is converted with lengthScale 1. + /// Timestamps are preserved. + /// + public Trajectory3 Transform(in Frame3 frame) + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = new Waypoint3((frame * Frame3.FromPose3(span[i].Pose, T.One)).ToPose3(T.One), span[i].Time); + return new Trajectory3(mem); + } + + /// + /// Strips timestamps and returns a . + /// + public PosePath3 ToPosePath() + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = span[i].Pose; + return new PosePath3(mem); + } + + /// + /// Extracts just the positions as a . + /// + public Polyline3 ToPolyline3() + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = span[i].Position; + return new Polyline3(mem); + } + + /// + /// Resamples the trajectory at uniform time intervals. + /// + /// Time between consecutive samples. + public Trajectory3 Resample(T interval) + { + var span = AsSpan(); + if (span.Length < 2) return this; + + var duration = Duration(); + var t0 = span[0].Time; + int sampleCount = int.CreateChecked(T.Floor(duration / interval)) + 1; + var mem = Alloc.Memory>(sampleCount); + var dst = mem.Span; + + for (int i = 0; i < sampleCount; i++) + { + var time = t0 + interval * T.CreateTruncating(i); + if (time > t0 + duration) time = t0 + duration; + dst[i] = new Waypoint3(AtTime(time), time); + } + + return new Trajectory3(mem); + } + + /// + /// Creates a for real-time playback of this trajectory. + /// + public TrajectoryController Control() => new(this); + + #endregion + + #region Operators + + /// + /// Translates all positions by adding the given vector (rotations and timestamps unchanged). + /// + public static Trajectory3 operator +(in Trajectory3 a, in Vector3 f) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = new Waypoint3(span[i].Pose with { Position = span[i].Pose.Position + f }, span[i].Time); + return new Trajectory3(mem); + } + + /// + /// Translates all positions by subtracting the given vector (rotations and timestamps unchanged). + /// + public static Trajectory3 operator -(in Trajectory3 a, in Vector3 f) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = new Waypoint3(span[i].Pose with { Position = span[i].Pose.Position - f }, span[i].Time); + return new Trajectory3(mem); + } + + #endregion + + #region Equality + + /// + public bool Equals(Trajectory3 other) => AsSpan().SequenceEqual(other.AsSpan()); + + /// + public override int GetHashCode() + { + var hash = new HashCode(); + var span = AsSpan(); + for (int i = 0; i < span.Length; i++) + hash.Add(span[i]); + return hash.ToHashCode(); + } + + #endregion + + /// + /// Detaches this trajectory's backing memory from the given scope and returns a lease. + /// + public Lease> DetachFrom(AllocationScope scope) + { + if (!MemoryMarshal.TryGetArray(_waypoints, out var seg)) + throw new InvalidOperationException("Cannot detach non-array-backed memory."); + var owner = scope.UntrackMemory(new Memory>(seg.Array!, seg.Offset, seg.Count)); + return new Lease> { _owner = owner }; + } + + /// + public override string ToString() => $"Trajectory3({Count} waypoints)"; +} diff --git a/Sources/ModelingEvolution.Drawing/Triangle3.cs b/Sources/ModelingEvolution.Drawing/Triangle3.cs index 1c5d55c..292696e 100644 --- a/Sources/ModelingEvolution.Drawing/Triangle3.cs +++ b/Sources/ModelingEvolution.Drawing/Triangle3.cs @@ -80,16 +80,12 @@ public T Area Point3.Distance(_c, _a); /// - /// Creates a pose at the triangle's centroid with Z-axis along the normal. + /// Creates a frame at the triangle's centroid with Z-axis along the normal. /// X-axis is along edge A→B, Y-axis completes the right-handed system. + /// The orientation is the one builds. /// - /// A pose representing the triangle's plane orientation at its center. - public Pose3 ToPose() - { - // Rotation from the surface frame, position at the centroid - var surface = Frame3.FromSurface(_a, _b, _c); - return new Frame3(Centroid, surface.Rotation).ToPose3(T.One); - } + /// A frame representing the triangle's plane orientation at its center. + public Frame3 ToFrame() => new(Centroid, Frame3.FromSurface(_a, _b, _c).Rotation); #region Operators From 5e2f1a3f10649b6a2b01197f372579d59e48dc30 Mon Sep 17 00:00:00 2001 From: Rafal Maciag Date: Sat, 24 Oct 2026 18:49:18 +0000 Subject: [PATCH 3/6] S3 Q7: delete the Euler rotation algebra; matrix bridge through the quaternion (epic-129) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Rotation3 is wire/edit only: Combine, operator +, Inverse, operator -, Slerp, Rotate(Vector3), Rotate(Point3), ToMatrix3x3, Negate and the explicit cast to Vector3 are deleted (Rotation3SurfaceTests pins the kept list). PosePath3Builder.Delta (Euler components added) is replaced by Step(in Frame3), composing previous · step. Polyline3.Transform(Rotation3) is deleted. Trajectory3Builder's corner blend slerps quaternions from FromPose3(·,1). One Shepperd and one Euler formula each way: Quaternion.FromMatrix3x3 / ToMatrix3x3 added; FromAxes uses FromMatrix3x3; Matrix3x3.ToQuaternion deleted; RotationZYX = FromRpy(...).ToMatrix3x3(); ToEulerZYX and Rotation3.FromMatrix3x3 = Rotation3.FromQuaternion(Quaternion.FromMatrix3x3(m)); Vector3.RotationTo's private Euler extraction goes through FromQuaternion. Tests pin RotationZYX/ToEulerZYX to master's closed form over 10 000 random rotations and the matrix -> quaternion -> matrix round trip to 1e-12. Grep gate row G-19d added. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_014wH5aL3TZhJNvLqa8YTku8 --- .github/scripts/grep-gate.sh | 2 + README.md | 1575 ++++++++-------- .../Frame3SurfaceTests.cs | 32 +- .../Frame3Tests.cs | 3 +- .../Matrix3x3Tests.cs | 1128 ++++++------ .../PosePath3BuilderTests.cs | 78 +- .../Rotation3SurfaceTests.cs | 80 + .../Rotation3Tests.cs | 67 +- .../TorchRotationTests.cs | 50 +- .../Vector3Tests.cs | 20 +- .../Kinematics/KinematicChain.cs | 2 +- .../ModelingEvolution.Drawing/Matrix3x3.cs | 602 +++--- .../ModelingEvolution.Drawing/Polyline3.cs | 13 - .../PosePath3Builder.cs | 30 +- .../ModelingEvolution.Drawing/Quaternion.cs | 50 +- Sources/ModelingEvolution.Drawing/README.md | 1639 +++++++++-------- .../ModelingEvolution.Drawing/Rotation3.cs | 86 +- .../Trajectory3Builder.cs | 862 ++++----- Sources/ModelingEvolution.Drawing/Vector3.cs | 41 +- 19 files changed, 3156 insertions(+), 3204 deletions(-) create mode 100644 Sources/ModelingEvolution.Drawing.Tests/Rotation3SurfaceTests.cs diff --git a/.github/scripts/grep-gate.sh b/.github/scripts/grep-gate.sh index 1066d9a..20605ed 100755 --- a/.github/scripts/grep-gate.sh +++ b/.github/scripts/grep-gate.sh @@ -7,6 +7,8 @@ cd "$(git rev-parse --show-toplevel)" ROWS=( $'G-17\t\\b(TransformPoint|TransformVector|Inverse|Multiply|Lerp|Distance|FromSurface)\\s*[<(]|operator\\s*[-+*]\tSources/ModelingEvolution.Drawing/Pose3.cs' $'G-18\tPose3(<[^>]*>)?\\.(Lerp|Distance|FromSurface)\\b\tSources/' + # G-19d (epic-129 Q7; the lead renumbers it in the docs): no Euler rotation algebra on Rotation3 or a Pose3's rotation. + $'G-19d\tRotation3(<[^>]*>)?\\.(Combine|Slerp)\\b|\\.Rotation\\.(Rotate|Inverse|Combine)\\(\tSources/' ) failed=0 diff --git a/README.md b/README.md index 13d924f..20bb0f0 100644 --- a/README.md +++ b/README.md @@ -1,787 +1,788 @@ -# ModelingEvolution.Drawing - -[![NuGet](https://img.shields.io/nuget/v/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) -[![NuGet Downloads](https://img.shields.io/nuget/dt/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) -[![CI](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml/badge.svg)](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml) -[![License](https://img.shields.io/github/license/modelingevolution/drawing.svg)](https://github.com/modelingevolution/drawing/blob/main/LICENSE) - -A high-performance .NET generic math library for 2D and 3D geometry, coordinate systems, shapes, intersections, trajectories, and drawing primitives. Built on `INumber` constraints from .NET generic math, all types work with `float`, `double`, `decimal`, or any compatible numeric type -- with zero boxing and full type safety. - -## Table of Contents - -- [Why This Library](#why-this-library) -- [Installation](#installation) -- [Quick Start](#quick-start) -- [API Reference](#api-reference) - - [2D Core Types](#2d-core-types) - - [3D Core Types](#3d-core-types) - - [Shapes (2D)](#shapes-2d) - - [Shapes (3D)](#shapes-3d) - - [Lines and Segments](#lines-and-segments) - - [Curves and Paths (2D)](#curves-and-paths-2d) - - [Polylines (2D and 3D)](#polylines-2d-and-3d) - - [Angles and Coordinate Systems](#angles-and-coordinate-systems) - - [Rotations and Orientation](#rotations-and-orientation) - - [Poses and Trajectories](#poses-and-trajectories) - - [Kinematics](#kinematics) - - [Equations](#equations) - - [Colors](#colors) - - [Data Structures and Algorithms](#data-structures-and-algorithms) - - [Interfaces](#interfaces) -- [Key Features](#key-features) - - [Generic Math](#generic-math) - - [IShape Interface Hierarchy](#ishape-interface-hierarchy) - - [Intersections](#intersections) - - [Polygon Boolean Operations](#polygon-boolean-operations) - - [Bezier Path Clipping](#bezier-path-clipping) - - [Pooled Allocation Scopes](#pooled-allocation-scopes) - - [Serialization](#serialization) - - [SVG Export](#svg-export) -- [Code Examples](#code-examples) - - [Points: Distance, Lerp, Rotate](#points-distance-lerp-rotate) - - [3D Points, Vectors, and Rotations](#3d-points-vectors-and-rotations) - - [Spherical Coordinates](#spherical-coordinates) - - [Poses and Trajectories](#poses-and-trajectories-1) - - [Shapes: Area and Perimeter](#shapes-area-and-perimeter) - - [Line-Circle Intersection](#line-circle-intersection) - - [Segment-Rectangle Clipping](#segment-rectangle-clipping) - - [Path-Rectangle Clipping](#path-rectangle-clipping) - - [Triangle Properties](#triangle-properties) - - [Polygon Operations](#polygon-operations) - - [Matrix Transformations (2D and 3D)](#matrix-transformations-2d-and-3d) -- [Requirements](#requirements) -- [License](#license) - -## Why This Library - -- **Generic math** -- write geometry code once; use it with `float`, `double`, or any `IFloatingPointIeee754` type. -- **No boxing** -- all core types are `struct` or `readonly record struct`, avoiding heap allocations. -- **Full 2D and 3D** -- from `Point` and `Vector` to `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Quaternion`, and `Trajectory3`. -- **Rich coordinate systems** -- Cartesian, polar, cylindrical, and spherical with implicit/explicit conversions between them. -- **Rich interface hierarchy** -- `IShape` unifies area, perimeter, centroid, bounding box, containment, rotation, and scaling across all shape types. -- **Comprehensive intersections** -- 21 intersection combinations (line, segment, circle, triangle, rectangle, polygon) with zero-alloc `FirstOf` variants. -- **Polygon booleans** -- union, intersection, difference, and clustering via Clipper2. -- **Exact Bezier clipping** -- `Path.Intersect(Rectangle)` computes analytically exact sub-curves, not polygon approximations. -- **Trajectory planning** -- `Trajectory3` with time-parameterized waypoints, interpolation, resampling, and a real-time `TrajectoryController`. -- **Robotics-ready** -- `Joints6` for 6-DOF joint representations, `Frame3` (position + unit quaternion) for all pose maths, `Frame3.FromSurface()` for plane-based frame construction, and `KinematicChain` for forward kinematics and geometric Jacobians of serial and tree chains. -- **Pooled allocations** -- `AllocationScope` provides arena-style pooled memory for zero-GC geometry pipelines. -- **Serialization built in** -- Protobuf (protobuf-net) attributes on all core types, plus custom JSON converters and SVG exporters. - -## Installation - -```bash -dotnet add package ModelingEvolution.Drawing -``` - -Or via the NuGet Package Manager: - -``` -Install-Package ModelingEvolution.Drawing -``` - -## Quick Start - -```csharp -using ModelingEvolution.Drawing; - -// 2D points and vectors (using double precision) -var a = new Point(1.0, 2.0); -var b = new Point(4.0, 6.0); - -double distance = a.DistanceTo(b); // 5.0 -Point mid = a.Lerp(b, 0.5); // {X=2.5, Y=4} - -// 3D points and rotations -var p = new Point3(1.0, 2.0, 3.0); -var rotation = new Rotation3(45.0, 0.0, 0.0); // 45 deg roll -Point3 rotated = rotation.Rotate(p); - -// Spherical coordinates -var orbit = SphericalPoint.OnEquator(5.0, Degree.Create(30)); -Point3 cartesian = orbit; // implicit conversion - -// Shapes with area and perimeter -var circle = new Circle(Point.Zero, 10.0); -double area = circle.Area(); // ~314.159 -bool inside = circle.Contains(new Point(3.0, 4.0)); // true - -// Polygon boolean operations -var poly1 = new Polygon( - new Point(0, 0), new Point(10, 0), - new Point(10, 10), new Point(0, 10)); -var poly2 = new Polygon( - new Point(5, 5), new Point(15, 5), - new Point(15, 15), new Point(5, 15)); - -Polygon merged = poly1 | poly2; // union -Polygon overlap = poly1 & poly2; // intersection - -// 3D trajectory with time-parameterized waypoints -var trajectory = new Trajectory3( - new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), - new Waypoint3(new Pose3(100, 0, 50, 0, 45, 0), 5.0)); -Pose3 atHalf = trajectory.AtTime(2.5); // interpolated pose -``` - -## API Reference - -### 2D Core Types - -| Type | Description | -|---|---| -| `Point` | 2D Cartesian point. Supports arithmetic with vectors and sizes, distance, lerp, reflect, rotate, clamp, midpoint, matrix transform, tuple conversion, and parsing. | -| `Vector` | 2D vector with magnitude, direction, dot/cross product, normalize, projection, reflect, rotate, perpendicular (CW/CCW), angle between, lerp, and matrix transform. | -| `Size` | Width/height pair with arithmetic operators, component-wise multiply/divide, and parsing. | -| `Matrix` | 3x3 affine transformation matrix. Supports translate, rotate, scale, skew, invert, append/prepend, and point/vector transforms. | -| `Matrix2x2` | 2x2 matrix for linear algebra: determinant, trace, transpose, inverse, eigendecomposition (closed-form for symmetric matrices), and point/vector transforms. | -| `Rectangle` | Axis-aligned rectangle with contains, intersect, inflate, offset, bounds, tiling, diagonal, distance-to-point, and LTRB construction. | -| `Thickness` | Four-sided thickness (top, right, bottom, left) for rectangular borders. | - -### 3D Core Types - -| Type | Description | -|---|---| -| `Point3` | 3D Cartesian point. Distance, lerp, midpoint, random generation, arithmetic with `Vector3`. Conversions to/from `System.Numerics.Vector3` and tuples. Protobuf and JSON serialization. | -| `Vector3` | 3D vector with length, normalize, dot product, cross product, angle between, projection, lerp, random unit vector, and `RotationTo()` for computing the rotation between two directions. Static basis vectors `EX`, `EY`, `EZ`. | -| `Matrix3x3` | 3x3 matrix for 3D transforms. Determinant, trace, transpose, inverse. Rotation factories: `RotationX`, `RotationY`, `RotationZ`, `RotationZYX`. Scale, column/row construction. Euler angle and quaternion extraction. | -| `Quaternion` | Quaternion for 3D rotations. Normalize, conjugate, inverse, from axis-angle, rotate vector, slerp, dot product. Conversions to/from `System.Numerics.Quaternion`. | -| `Segment3` | Bounded line segment in 3D. Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | - -### Shapes (2D) - -All 2D shapes implement `IShape`, providing `Area()`, `Perimeter()`, `Centroid()`, `BoundingBox()`, `Contains(Point)`, `Rotate(Degree, Point)`, and `Scale(T)`. - -| Type | Description | -|---|---| -| `Circle` | Center + radius. Intersects with lines, segments, circles, and rectangles. Tangent point detection. `PointAt(Radian)` for parameterized access. | -| `Triangle` | Three vertices. Incircle, circumcircle, orthocenter, angles, edges. Classification: `IsRight()`, `IsAcute()`, `IsObtuse()`, `IsEquilateral()`, `IsIsosceles()`, `IsScalene()`. Similarity and congruence tests. | -| `Polygon` | Immutable polygon backed by `ReadOnlyMemory>`. Shoelace area, ray-casting containment, convex hull (Graham scan), `IsConvex()`, edges, simplify, skeleton (medial axis). Boolean operations: union, intersect, subtract, cluster. Operators: `\|` (union), `&` (intersect), `-` (subtract). | -| `Rectangle` | Implements `IArea`, `IPerimeter`, `ICentroid`, `IBoundingBox`, `IScalable>`. Rotation returns `Polygon`. | - -### Shapes (3D) - -| Type | Description | -|---|---| -| `Triangle3` | Three vertices in 3D space. Area (cross product), perimeter, centroid, unit normal vector. `ToPose()` creates a coordinate frame from the triangle plane. Translate by `Vector3`. | - -### Lines and Segments - -| Type | Description | -|---|---| -| `Line` | Infinite 2D line (supports vertical lines). From two points, point + direction, or equation. Intersect with lines, segments, circles, triangles, rectangles, polygons. Distance-to-point, angle-between, parallel/perpendicular tests, project point, reflect point. | -| `Segment` | Bounded 2D line segment (start + end). Length, midpoint, direction, lerp, split, reverse. Intersect with segments, lines, circles, rectangles. Distance-to-point, project point, parallel test. Liang-Barsky rectangle clipping. | -| `Segment3` | Bounded 3D line segment (start + end). Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | - -### Curves and Paths (2D) - -| Type | Description | -|---|---| -| `BezierCurve` | Cubic Bezier curve (4 control points). `Evaluate(t)`, `Split(t)`, `SubCurve(t0, t1)` via De Casteljau. Extremum points, linear equation intersection, rectangle edge crossings. | -| `Path` | Immutable sequence of `BezierCurve` segments. Create from points (`FromPoints`) with smoothing, or from SVG path data (`Parse`). `Length()`, `PointAt(t)` with arc-length parameterization. Exact `Intersect(Rectangle)` that splits Bezier curves at crossing points. Transform, rotate, reverse, append. `Close()` and `ToPolygon()` for rasterization. | -| `ComplexCurve` | Composite curve of mixed Bezier and polyline segments packed in a contiguous byte buffer for zero-copy decoding. SVG export and JSON serialization. | -| `PolygonalCurve` | Mutable sequence of connected points that generates smooth Bezier segments via `GetSmoothSegment(i)`. | - -### Polylines (2D and 3D) - -| Type | Description | -|---|---| -| `Polyline` | Immutable open polyline in 2D (non-closing). Length, edges, bounding box, simplification. JSON, Protobuf, and SVG serialization. | -| `Polyline3` | Immutable open polyline in 3D. Length, edges, translate, transform. JSON and Protobuf serialization. | - -### Angles and Coordinate Systems - -| Type | Description | -|---|---| -| `Degree` | Angle in degrees. Implicit conversion from numeric values and from `Radian`. Arithmetic (+, -, *, /), normalize to (-180, 180], abs, comparison operators. | -| `Radian` | Angle in radians. Implicit conversion from `Degree`. Arithmetic (+, -, *, /), normalize to (-pi, pi], abs, comparison operators. Static `Sin`/`Cos` helpers. | -| `PolarPoint` | Polar coordinates (radius, angle). Implicit conversions to/from `Point`, explicit to `Vector2` and `Vector`. | -| `CylindricalPoint` | Cylindrical coordinates (radial distance, angle, height). Factory: `OnPlane(r, angle)`. Builder methods: `WithZ`, `WithRadialDistance`, `WithAngle`. `RotateAngle` for angular displacement. `+`/`-` operators with `Radian` and `Point3`. Conversions to/from `Point3`, `Vector3`, `SphericalPoint`, and `System.Numerics.Vector3`. | -| `SphericalPoint` | Spherical coordinates (radius, azimuth, inclination) with Degree-based storage (ISO convention: inclination from Z-axis). Factories: `OnEquator`, `FromElevation`, `NorthPole`, `SouthPole`. `Normalized`, `WithRadius`, `RotateAzimuth`, `RotateInclination`, `Rotate(az, inc)`. `AngularDistance` (Vincenty formula), `Slerp`, `Lerp`, Euclidean `Distance`. Radius scaling operators (`*`, `/`). `Point3` offset operators (`+`, `-`). Conversions to/from `Point3`, `Vector3`, `CylindricalPoint`, and `System.Numerics.Vector3`. | - -### Rotations and Orientation - -| Type | Description | -|---|---| -| `Rotation3` | Euler angle rotation (Roll/Pitch/Yaw in degrees, ZYX convention). Create from degrees, radians, quaternion, or `Matrix3x3`. `Rotate(Vector3)`, `Rotate(Point3)`. `Combine`, `Inverse`, `Negate`, `Slerp` (via quaternion). Convert to matrix, quaternion, or direction vector. Protobuf and JSON serialization. | -| `Quaternion` | Unit quaternion for 3D rotations. From axis-angle, `FromRpy(roll, pitch, yaw)` (Rz·Ry·Rx, radians; the URDF rpy rule), `FromTo(from, to)` (shortest arc), `FromAxes(x, y, z)` (orthonormal basis), rotate vector, slerp, dot, normalize, conjugate, inverse. Hamilton product operator (`*`). Conversions to/from `System.Numerics.Quaternion`. Protobuf serialization. | -| `Matrix3x3` | Also used as rotation representation. `RotationZYX(rx, ry, rz)` creates ZYX Euler rotation matrix. `ToEulerZYX()` extracts angles (with gimbal-lock handling). `ToQuaternion()` via Shepperd's method. | - -### Poses and Trajectories - -| Type | Description | -|---|---| -| `Frame3` | Rigid frame = position (`Point3`) + unit `Quaternion`. All pose maths: `Multiply` / `*` (compose), `Inverse`, `TransformPoint`, `TransformVector`, `Interpolate` (position lerp + slerp), `Distance` (position error and angle error in radians). `FromSurface(a, b, c)` creates a frame from three coplanar points (right-hand rule); `FromSurface(a, b, c, h)` uses a hint point for Z; `FromSurface(points)` fits a plane. `FromPose3(pose, lengthScale)` / `ToPose3(lengthScale)` convert to and from the wire format (ZYX degrees). | -| `Pose3` | Wire and edit format only (since 2.0): position (`Point3`) + ZYX Euler orientation (`Rotation3`, degrees). Constructors, `Identity`, tuple conversions, deconstruction, parsing, equality, `Truncating()`. Protobuf and JSON serialization. Convert to `Frame3` for any maths. | -| `Waypoint3` | Timestamped `Pose3` -- an element of a trajectory. | -| `PosePath3` | Immutable sequence of `Pose3` (no time). Length, reverse, transform, extract `Polyline3`, convert to `Trajectory3` with uniform timing. | -| `Trajectory3` | Immutable time-parameterized path of `Waypoint3`. `Duration`, `Length`, `AtTime(t)` (binary-search interpolation with position lerp + rotation slerp). `Resample(interval)`, `Reverse`, `Transform`. Convert to `PosePath3` or `Polyline3`. Create a real-time `TrajectoryController` via `Control()`. | -| `TrajectoryController` | Real-time trajectory playback using `Stopwatch` clock. Start, stop, pause, resume, seek. Fires `PoseChanged` events with current pose, time, and progress. | -| `Corner` | Corner blending descriptor: `Sharp` (exact waypoint) or `Round(radius)` (arc blend before/after the waypoint). | - -### Kinematics - -| Type | Description | -|---|---| -| `KinematicChain` | Namespace `ModelingEvolution.Drawing.Kinematics`. A tree of `Link`s moved by revolute/prismatic `Joint`s (origin at q = 0, unit axis, limits) with named `ChainFrame`s; metres and radians. `Forward(q, links)` writes every link pose, `FramePose(links, frame)`, `Jacobian(links, frame, j)` writes the 6 × Dof geometric Jacobian (column-major, v then ω). Spans in, spans out, no allocation; the constructor validates topological order, unit axes, limits and unique names. | -| `Joints6` | 6-DOF joint angles stored as six `Degree` fields (no array allocation). Indexer `[0..5]`, arithmetic operators, `Lerp`, `MaxAbsDelta`, `IsWithin(tolerance)`, `IsBetween(min, max)`. `ToArray()`, `FromSpan()`, `FromArray()`, `CopyTo(Span)`. Protobuf and JSON serialization. | -| `Speed` | Scalar speed in units/second (unitless magnitude). Arithmetic, `TimeFor(distance)`, `DistanceIn(time)`. Parsing. | -| `Velocity3` | 3D velocity vector (direction + magnitude). Create from direction + speed. `Speed`, `Direction`, `DisplacementIn(time)`. Arithmetic operators. | -| `Speed` | **Unit-aware** speed tagged at compile time with its unit (`ISpeedUnit`). Stores the value in `TUnit`; `ToSi()`/`FromSi()`/`To()` normalise via the SI base (m/s). Full arithmetic, comparisons, ratio (`Speed/Speed → T`), `IParsable`. Mixing units (or a non-speed unit) is a compile error. Verified zero-cost vs raw `double`. | -| `ISpeedUnit` | Marker for speed units. Built-ins: `MetrePerSecond` (SI base), `MillimetrePerSecond`, `MetrePerMinute`. Implement to add your own. | - -### Equations - -| Type | Description | -|---|---| -| `LinearEquation` | `y = Ax + B`. Create from points, angle, or direction. Compute, zero point, intersect, perpendicular, translate, mirror. | -| `QuadraticEquation` | `ax^2 + bx + c = 0`. Zero points using the discriminant. | -| `CubicEquation` | `ax^3 + bx^2 + cx + d = 0`. Root finding via Newton-Raphson. | -| `CircleEquation` | `(x-cx)^2 + (y-cy)^2 = r^2`. Intersect with `LinearEquation`. | - -### Colors - -| Type | Description | -|---|---| -| `Color` | ARGB color as a 32-bit uint. Parse from hex (`#RRGGBB`, `#AARRGGBB`), `rgba(r,g,b,a)`, or HSV strings. `GetHue()`, `GetSaturation()`, `GetLightness()`, `GetBrightness()`, `MakeTransparent(float)`. Implicit from string or tuple. JSON and Protobuf serialization. | -| `HsvColor` | HSV (hue, saturation, value) color space with optional alpha. Implicit conversions to/from `Color`. Parse from `hsv()`/`hsva()` strings, JSON arrays, or hex. | - -### Data Structures and Algorithms - -| Type | Description | -|---|---| -| `KdTree` | 2D KD-tree for fast nearest-neighbor queries on `Point`. Build from span, add individual points, find nearest neighbor. | -| `Skeleton` | Medial axis approximation of a polygon (interior nodes + edges). Three algorithms: `StraightSkeleton`, `ChordalAxis`, and `Voronoi`. | -| `MovingAverage` | Component-wise moving average over a sliding window of 2D points. O(1) add and query via circular buffer + running sums. | -| `MovingMedian` | Component-wise moving median over a sliding window of 2D points. O(1) median read, O(n) add via maintained sorted arrays. | -| `AllocationScope` | Arena-style pooled allocation scope. Library allocations use `MemoryPool.Shared` when active. Dispose returns all tracked memory at once. Supports `Persist()` to detach individual results. | -| `AlignmentResult` | Result of a rigid 2D alignment (rotation + translation) between two point clouds, with rotation angle and residual error. | - -### Interfaces - -```csharp -// Composable geometry traits -public interface IArea { T Area(); } -public interface IPerimeter { T Perimeter(); } -public interface ICentroid { Point Centroid(); } -public interface IBoundingBox { Rectangle BoundingBox(); } -public interface IRotatable { TSelf Rotate(Degree angle, Point origin = default); } -public interface IScalable { TSelf Scale(T factor); } - -// Unified shape interface -- combines all of the above plus point containment -public interface IShape : IArea, IPerimeter, ICentroid, - IBoundingBox, IRotatable, IScalable -{ - bool Contains(Point point); -} -``` - -Implementing types: `Circle`, `Triangle`, `Polygon`, and `Rectangle` (partial -- no `IRotatable` since rotation produces a `Polygon`). - -## Key Features - -### Generic Math - -All geometry types are parameterized by `T` with constraints like `IFloatingPointIeee754`, `IMinMaxValue`, etc. This means the same code works with any numeric type: - -```csharp -// Single precision -var pf = new Point(1f, 2f); -var cf = new Circle(pf, 5f); - -// Double precision -var pd = new Point(1.0, 2.0); -var cd = new Circle(pd, 5.0); - -// Both compute area with their respective precision -float areaF = cf.Area(); -double areaD = cd.Area(); -``` - -### IShape Interface Hierarchy - -Write generic algorithms over any shape: - -```csharp -void PrintShapeInfo(TShape shape) - where T : INumber, ITrigonometricFunctions, IRootFunctions, - IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue - where TShape : IShape -{ - Console.WriteLine($"Area: {shape.Area()}"); - Console.WriteLine($"Perimeter: {shape.Perimeter()}"); - Console.WriteLine($"Centroid: {shape.Centroid()}"); - Console.WriteLine($"Bounds: {shape.BoundingBox()}"); -} -``` - -### Intersections - -The `Intersections` static class provides 21 combination overloads covering every pair of: `Line`, `Segment`, `Circle`, `Triangle`, `Rectangle`, and `Polygon`. Each combination also has a zero-allocation `FirstOf` variant. - -```csharp -// Line x Line -Point? p = Intersections.Of(line1, line2); - -// Line x Circle -> chord (Segment) or null -Segment? chord = Intersections.Of(line, circle); - -// Tangent detection -Point? tangent = Intersections.TangentPoint(line, circle); - -// Circle x Circle -> radical chord or tangent point -Segment? radicalChord = Intersections.Of(circle1, circle2); - -// Segment x Segment -Point? hit = Intersections.Of(seg1, seg2); - -// Zero-alloc first-hit variants -Segment? first = Intersections.FirstOf(line, polygon); -``` - -Instance methods are also available on each shape for convenience: - -```csharp -Segment? chord = circle.Intersect(line); -Point? hit = segment.Intersect(otherSegment); -Segment? clipped = line.Intersect(rectangle); -``` - -### Polygon Boolean Operations - -Powered by [Clipper2](https://github.com/AngusJohnson/Clipper2): - -```csharp -var a = new Polygon( /* ... */ ); -var b = new Polygon( /* ... */ ); - -// Operators -Polygon union = a | b; -Polygon intersect = a & b; -Polygon difference = a - b; - -// Methods returning multiple result polygons -List> unions = a.Union(b, removeHoles: true); -List> diffs = a.Subtract(b); - -// Batch operations -List> merged = Polygon.Union(polygons); - -// Clustering overlapping polygons -IEnumerable> clusters = Polygon.Cluster(polygons); -``` - -### Bezier Path Clipping - -`Path.Intersect(Rectangle)` performs analytically exact Bezier-rectangle clipping. It finds the precise parameter values where each cubic Bezier segment crosses the rectangle edges, then uses `SubCurve(t0, t1)` via De Casteljau subdivision to return exact sub-paths -- not polygon approximations. - -```csharp -var path = Path.FromPoints(points); -IEnumerable> clipped = path.Intersect(viewport); - -foreach (var subPath in clipped) -{ - // Each sub-path contains only the Bezier segments inside the rectangle, - // with curves split precisely at the rectangle boundaries. -} -``` - -### Pooled Allocation Scopes - -When performing many geometry operations that allocate intermediate results (polygon booleans, skeleton computation, curve corrections), use `AllocationScope` to avoid GC pressure: - -```csharp -using var scope = AllocationScope.Begin(); - -var polygon = somePolygon * new Size(2, 2); // pooled -var skeleton = polygon.Skeleton(); // pooled -var lease = scope.Persist(ref skeleton); // detach from scope - -// scope.Dispose() returns everything except skeleton's memory -// lease.Dispose() returns skeleton's memory when you're done -``` - -### Serialization - -**Protobuf** -- all core types carry `[ProtoContract]` / `[ProtoMember]` attributes for use with protobuf-net: - -```csharp -var point = new Point(1f, 2f); -byte[] bytes = Serializer.Serialize(point); -``` - -**JSON** -- custom `System.Text.Json` converters for `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Polygon`, `Polyline`, `Polyline3`, `PosePath3`, `Trajectory3`, `Path`, `ComplexCurve`, `BezierCurve`, `Matrix2x2`, `Matrix3x3`, `Skeleton`, `Color`, and `HsvColor`: - -```csharp -var pose = new Pose3(100, 200, 300, 45, 90, 0); -string json = JsonSerializer.Serialize(pose); // [100,200,300,45,90,0] - -var joints = new Joints6(10, 20, 30, 40, 50, 60); -string j = JsonSerializer.Serialize(joints); // [10,20,30,40,50,60] -``` - -**String parsing** -- `IParsable` implementations on `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Rectangle`, `Size`, `Path`, `ComplexCurve`, `Speed`, `Color`, and `HsvColor`: - -```csharp -var point = Point.Parse("1.5, 2.5"); -var point3 = Point3.Parse("1.0 2.0 3.0"); -var pose = Pose3.Parse("100, 200, 300, 45, 90, 0"); -var rect = Rectangle.Parse("0 0 100 200"); -var color = Color.Parse("#FF8800"); -var path = Path.Parse("M 0 0 C 1 2, 3 4, 5 6", null); -``` - -### SVG Export - -`Polygon`, `Polyline`, `ComplexCurve`, `Skeleton`, and `Path` support SVG export via the `[SvgExporter]` attribute and the `SvgExporter` infrastructure: - -```csharp -// Path.ToString() produces SVG path data -var path = Path.FromPoints(points); -string svgData = path.ToString(); // "M 0 0 C 1.5 2.3, 3.1 4.2, 5 6 ..." -``` - -## Code Examples - -### Points: Distance, Lerp, Rotate - -```csharp -var a = new Point(0, 0); -var b = new Point(3, 4); - -double dist = a.DistanceTo(b); // 5.0 -Point quarter = a.Lerp(b, 0.25); // {X=0.75, Y=1} -Point reflected = a.Reflect(b); // {X=6, Y=8} - -// Rotate 90 degrees around the origin -Degree angle = 90.0; -Point rotated = b.Rotate(angle); // {X=-4, Y=3} - -// Rotate around a custom center -var center = new Point(1, 1); -Point rotated2 = b.Rotate(angle, center); - -// Clamp to a rectangle -var bounds = new Rectangle(0, 0, 10, 10); -Point clamped = new Point(15, -3).Clamp(bounds); // {X=10, Y=0} -``` - -### 3D Points, Vectors, and Rotations - -```csharp -// 3D point operations -var p1 = new Point3(1, 2, 3); -var p2 = new Point3(4, 6, 3); -double dist3d = Point3.Distance(p1, p2); // 5.0 -Point3 mid3d = Point3.Lerp(p1, p2, 0.5); - -// 3D vector operations -var v = new Vector3(1, 0, 0); -var w = new Vector3(0, 1, 0); -var cross = Vector3.Cross(v, w); // (0, 0, 1) -double dot = Vector3.Dot(v, w); // 0.0 - -// Compute rotation between two directions -Rotation3 rot = v.RotationTo(w); -Vector3 result = rot.Rotate(v); // approximately (0, 1, 0) - -// Euler angle rotation (ZYX convention) -var rotation = Rotation3.FromDegrees(45, 0, 0); -Point3 rotated = rotation.Rotate(p1); - -// Quaternion slerp -var r1 = Rotation3.FromDegrees(0, 0, 0); -var r2 = Rotation3.FromDegrees(0, 0, 90); -var halfway = Rotation3.Slerp(r1, r2, 0.5); - -// Matrix3x3 rotation -var m = Matrix3x3.RotationZ(Degree.Create(45)); -var transformed = m.Transform(new Point3(1, 0, 0)); -``` - -### Spherical Coordinates - -```csharp -// Create spherical points with factory methods -var northPole = SphericalPoint.NorthPole(10.0); // +Z, radius=10 -var equator = SphericalPoint.OnEquator(5.0, Degree.Create(45)); -var elevated = SphericalPoint.FromElevation(5.0, - Degree.Create(0), Degree.Create(30)); - -// Convert to/from Cartesian -Point3 cartesian = equator; // implicit conversion -SphericalPoint spherical = cartesian; // implicit conversion - -// Orbit around a 3D center -var center = new Point3(10, 50, 0); -var points = new List>(); -for (int az = 0; az < 360; az += 30) - points.Add(center + SphericalPoint.OnEquator(5.0, Degree.Create(az))); - -// Rotation (O(1) angle operations) -var sp = SphericalPoint.OnEquator(5.0, Degree.Create(0)); -var rotated = sp.RotateAzimuth(Degree.Create(45)); // rotate around Z - -// Angular distance (Vincenty formula -- numerically stable) -var a = SphericalPoint.NorthPole(1.0); -var b = SphericalPoint.SouthPole(1.0); -Degree angDist = SphericalPoint.AngularDistance(a, b); // 180 degrees - -// Spherical linear interpolation -var mid = SphericalPoint.Slerp(a, b, 0.5); - -// Cylindrical coordinate conversions -CylindricalPoint cyl = sp; // implicit conversion -SphericalPoint back = cyl; // implicit conversion -``` - -### Poses and Trajectories - -```csharp -// Pose = position + orientation -var pose = new Pose3(100, 200, 300, 45, 0, 0); // x,y,z, rx,ry,rz (degrees) - -// Maths composes Frame3 (position + quaternion); Pose3 converts once, here mm -> m -var world = Frame3.FromPose3(new Pose3(10, 0, 0, 0, 0, 0), 0.001); -var local = Frame3.FromPose3(new Pose3(5, 0, 0, 0, 0, 90), 0.001); -var combined = world * local; -Pose3 back = combined.ToPose3(1000); // m -> mm, ZYX degrees - -// Create frame from a surface (3 points define a plane) -var a = new Point3(0, 0, 0); -var b = new Point3(1, 0, 0); -var c = new Point3(0, 1, 0); -var surface = Frame3.FromSurface(a, b, c); // Z-axis via right-hand rule - -// Trajectory with timed waypoints -var trajectory = new Trajectory3( - new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), - new Waypoint3(new Pose3(50, 0, 25, 0, 30, 0), 2.5), - new Waypoint3(new Pose3(100, 0, 0, 0, 0, 0), 5.0)); - -// Interpolate at any time (binary search + lerp/slerp) -Pose3 atT = trajectory.AtTime(1.25); - -// Resample at uniform intervals -var resampled = trajectory.Resample(0.1); // every 0.1 seconds - -// Real-time playback -var controller = trajectory.Control(); -controller.PoseChanged += (_, e) => Console.WriteLine($"t={e.Time} pose={e.Pose}"); -controller.Start(); - -// 6-DOF joint angles for robotics -var home = new Joints6(0, -90, 0, 0, 90, 0); -var target = new Joints6(45, -60, 30, 0, 90, -45); -var halfway = Joints6.Lerp(home, target, 0.5); -bool inRange = target.IsBetween(minLimits, maxLimits); -``` - -### Shapes: Area and Perimeter - -```csharp -// Circle -var circle = new Circle(new Point(5, 5), 3.0); -double cArea = circle.Area(); // ~28.274 -double cPerim = circle.Perimeter(); // ~18.850 - -// Triangle -var triangle = new Triangle( - new Point(0, 0), - new Point(4, 0), - new Point(0, 3)); -double tArea = triangle.Area(); // 6.0 -double tPerim = triangle.Perimeter(); // 12.0 - -// Polygon (shoelace formula) -var poly = new Polygon( - new Point(0, 0), new Point(4, 0), - new Point(4, 3), new Point(0, 3)); -double pArea = poly.Area(); // 12.0 - -// 3D triangle -var t3 = new Triangle3( - new Point3(0, 0, 0), - new Point3(4, 0, 0), - new Point3(0, 3, 0)); -double area3 = t3.Area; // 6.0 -Vector3 normal = t3.Normal; // (0, 0, 1) -``` - -### Line-Circle Intersection - -```csharp -var circle = new Circle(new Point(0, 0), 5.0); -var line = Line.From(new Point(-10, 0), new Point(10, 0)); - -// Secant -- returns the chord as a segment -Segment? chord = circle.Intersect(line); -// chord.Value.Start ~ {X=-5, Y=0}, chord.Value.End ~ {X=5, Y=0} - -// Tangent detection -var tangentLine = Line.Horizontal(5.0); -Point? tangent = circle.TangentPoint(tangentLine); -// tangent.Value ~ {X=0, Y=5} -``` - -### Segment-Rectangle Clipping - -```csharp -var viewport = new Rectangle(0, 0, 100, 100); -var segment = new Segment( - new Point(-20, 50), - new Point(150, 50)); - -// Liang-Barsky clipping -Segment? clipped = segment.Intersect(viewport); -// clipped.Value: Start={X=0, Y=50}, End={X=100, Y=50} -``` - -### Path-Rectangle Clipping - -```csharp -var points = new[] -{ - new Point(10, 50), - new Point(50, 10), - new Point(90, 50), - new Point(130, 90) -}; -var path = Path.FromPoints(points); -var viewport = new Rectangle(20, 20, 60, 60); // 20,20 to 80,80 - -// Exact Bezier clipping -- returns sub-paths with curves split at boundaries -foreach (var subPath in path.Intersect(viewport)) -{ - Console.WriteLine($"Sub-path with {subPath.Count} Bezier segments"); -} -``` - -### Triangle Properties - -```csharp -var t = new Triangle( - new Point(0, 0), - new Point(4, 0), - new Point(2, 3)); - -// Derived circles -Circle incircle = t.Incircle(); // largest circle inside -Circle circumcircle = t.Circumcircle(); // passes through all vertices - -// Center points -Point centroid = t.Centroid(); -Point orthocenter = t.Orthocenter; - -// Classification -bool right = t.IsRight(); -bool equilateral = t.IsEquilateral(); -bool isosceles = t.IsIsosceles(); - -// Similarity and congruence -var t2 = new Triangle( - new Point(0, 0), - new Point(8, 0), - new Point(4, 6)); -bool similar = t.IsSimilarTo(t2); // true (same shape, scaled 2x) -bool congruent = t.IsCongruentTo(t2); // false (different size) - -// Interior angles -var (atA, atB, atC) = t.Angles; -``` - -### Polygon Operations - -```csharp -// Convex hull -var cloud = new Polygon( - new Point(0, 0), new Point(1, 3), - new Point(2, 1), new Point(4, 4), - new Point(3, 0), new Point(5, 2)); -Polygon hull = cloud.ConvexHull(); - -// Point containment (ray casting) -bool inside = hull.Contains(new Point(2, 2)); - -// Boolean operations -var a = new Polygon( /* ... */ ); -var b = new Polygon( /* ... */ ); -List> union = a.Union(b, removeHoles: true); -List> diff = a.Subtract(b); -List> inter = a.Intersect(b); - -// Simplify (remove collinear/close points) -Polygon simplified = hull.Simplify(epsilon: 0.1); - -// Convexity test -bool convex = hull.IsConvex(); - -// Clustering overlapping polygons -var clusters = Polygon.Cluster(polygons); -``` - -### Matrix Transformations (2D and 3D) - -```csharp -// 2D affine transformation -var matrix = Matrix.Identity; -matrix.Translate(10.0, 20.0); -matrix.Rotate(Degree.Create(45.0)); -matrix.Scale(2.0, 2.0); - -Point transformed = matrix.Transform(new Point(1, 0)); -Vector rotatedVec = matrix.Transform(new Vector(1, 0)); - -var combined = matrix1 * matrix2; -matrix.Invert(); - -// 3D rotation matrix -var rx = Matrix3x3.RotationX(Degree.Create(45)); -var ry = Matrix3x3.RotationY(Degree.Create(30)); -var rz = Matrix3x3.RotationZ(Degree.Create(60)); - -// Compose rotations -var rotation = rz * ry * rx; -var p = new Point3(1, 0, 0); -Point3 rotated3 = rotation * p; // or rotation.Transform(p) - -// Extract Euler angles -var (eRx, eRy, eRz) = rotation.ToEulerZYX(); - -// Convert to quaternion -Quaternion q = rotation.ToQuaternion(); -``` - -### Unit-Aware Speed - -```csharp -using ModelingEvolution.Drawing.Units; - -// The unit is part of the type — the value is stored in that unit. -Speed> feed = new(300); // 300 m/min - -// Normalise to the SI base (m/s) or convert to another speed unit. -double si = feed.ToSi(); // 5.0 (m/s) -var inMmPerS = feed.To>(); // 5000 mm/s - -// Same-unit arithmetic, scalar scaling, and a dimensionless ratio. -var faster = feed * 2.0; // 600 m/min -double ratio = faster / feed; // 2.0 - -// Cross-unit add normalises through SI; result stays in the left unit. -var total = inMmPerS.Add(feed); // (5000 + 5000) mm/s - -Console.WriteLine(feed); // "300 m/min" - -// Compile-time safety: -// feed + inMmPerS; // error: different units — use .Add(...) -// feed.To>(); // error: not an ISpeedUnit -``` - -## Requirements - -- **.NET 10.0** or later (uses generic math interfaces from .NET 7+) -- **Dependencies**: [Clipper2](https://www.nuget.org/packages/Clipper2) 1.4.0, [protobuf-net](https://www.nuget.org/packages/protobuf-net) 3.2.45 - -## License - -See [LICENSE](LICENSE) for details. +# ModelingEvolution.Drawing + +[![NuGet](https://img.shields.io/nuget/v/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) +[![NuGet Downloads](https://img.shields.io/nuget/dt/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) +[![CI](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml/badge.svg)](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml) +[![License](https://img.shields.io/github/license/modelingevolution/drawing.svg)](https://github.com/modelingevolution/drawing/blob/main/LICENSE) + +A high-performance .NET generic math library for 2D and 3D geometry, coordinate systems, shapes, intersections, trajectories, and drawing primitives. Built on `INumber` constraints from .NET generic math, all types work with `float`, `double`, `decimal`, or any compatible numeric type -- with zero boxing and full type safety. + +## Table of Contents + +- [Why This Library](#why-this-library) +- [Installation](#installation) +- [Quick Start](#quick-start) +- [API Reference](#api-reference) + - [2D Core Types](#2d-core-types) + - [3D Core Types](#3d-core-types) + - [Shapes (2D)](#shapes-2d) + - [Shapes (3D)](#shapes-3d) + - [Lines and Segments](#lines-and-segments) + - [Curves and Paths (2D)](#curves-and-paths-2d) + - [Polylines (2D and 3D)](#polylines-2d-and-3d) + - [Angles and Coordinate Systems](#angles-and-coordinate-systems) + - [Rotations and Orientation](#rotations-and-orientation) + - [Poses and Trajectories](#poses-and-trajectories) + - [Kinematics](#kinematics) + - [Equations](#equations) + - [Colors](#colors) + - [Data Structures and Algorithms](#data-structures-and-algorithms) + - [Interfaces](#interfaces) +- [Key Features](#key-features) + - [Generic Math](#generic-math) + - [IShape Interface Hierarchy](#ishape-interface-hierarchy) + - [Intersections](#intersections) + - [Polygon Boolean Operations](#polygon-boolean-operations) + - [Bezier Path Clipping](#bezier-path-clipping) + - [Pooled Allocation Scopes](#pooled-allocation-scopes) + - [Serialization](#serialization) + - [SVG Export](#svg-export) +- [Code Examples](#code-examples) + - [Points: Distance, Lerp, Rotate](#points-distance-lerp-rotate) + - [3D Points, Vectors, and Rotations](#3d-points-vectors-and-rotations) + - [Spherical Coordinates](#spherical-coordinates) + - [Poses and Trajectories](#poses-and-trajectories-1) + - [Shapes: Area and Perimeter](#shapes-area-and-perimeter) + - [Line-Circle Intersection](#line-circle-intersection) + - [Segment-Rectangle Clipping](#segment-rectangle-clipping) + - [Path-Rectangle Clipping](#path-rectangle-clipping) + - [Triangle Properties](#triangle-properties) + - [Polygon Operations](#polygon-operations) + - [Matrix Transformations (2D and 3D)](#matrix-transformations-2d-and-3d) +- [Requirements](#requirements) +- [License](#license) + +## Why This Library + +- **Generic math** -- write geometry code once; use it with `float`, `double`, or any `IFloatingPointIeee754` type. +- **No boxing** -- all core types are `struct` or `readonly record struct`, avoiding heap allocations. +- **Full 2D and 3D** -- from `Point` and `Vector` to `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Quaternion`, and `Trajectory3`. +- **Rich coordinate systems** -- Cartesian, polar, cylindrical, and spherical with implicit/explicit conversions between them. +- **Rich interface hierarchy** -- `IShape` unifies area, perimeter, centroid, bounding box, containment, rotation, and scaling across all shape types. +- **Comprehensive intersections** -- 21 intersection combinations (line, segment, circle, triangle, rectangle, polygon) with zero-alloc `FirstOf` variants. +- **Polygon booleans** -- union, intersection, difference, and clustering via Clipper2. +- **Exact Bezier clipping** -- `Path.Intersect(Rectangle)` computes analytically exact sub-curves, not polygon approximations. +- **Trajectory planning** -- `Trajectory3` with time-parameterized waypoints, interpolation, resampling, and a real-time `TrajectoryController`. +- **Robotics-ready** -- `Joints6` for 6-DOF joint representations, `Frame3` (position + unit quaternion) for all pose maths, `Frame3.FromSurface()` for plane-based frame construction, and `KinematicChain` for forward kinematics and geometric Jacobians of serial and tree chains. +- **Pooled allocations** -- `AllocationScope` provides arena-style pooled memory for zero-GC geometry pipelines. +- **Serialization built in** -- Protobuf (protobuf-net) attributes on all core types, plus custom JSON converters and SVG exporters. + +## Installation + +```bash +dotnet add package ModelingEvolution.Drawing +``` + +Or via the NuGet Package Manager: + +``` +Install-Package ModelingEvolution.Drawing +``` + +## Quick Start + +```csharp +using ModelingEvolution.Drawing; + +// 2D points and vectors (using double precision) +var a = new Point(1.0, 2.0); +var b = new Point(4.0, 6.0); + +double distance = a.DistanceTo(b); // 5.0 +Point mid = a.Lerp(b, 0.5); // {X=2.5, Y=4} + +// 3D points and rotations +var p = new Point3(1.0, 2.0, 3.0); +var rotation = new Rotation3(45.0, 0.0, 0.0); // 45 deg roll (wire/edit format) +var rotated = new Frame3(Point3.Zero, rotation.ToQuaternion()).TransformPoint(p); + +// Spherical coordinates +var orbit = SphericalPoint.OnEquator(5.0, Degree.Create(30)); +Point3 cartesian = orbit; // implicit conversion + +// Shapes with area and perimeter +var circle = new Circle(Point.Zero, 10.0); +double area = circle.Area(); // ~314.159 +bool inside = circle.Contains(new Point(3.0, 4.0)); // true + +// Polygon boolean operations +var poly1 = new Polygon( + new Point(0, 0), new Point(10, 0), + new Point(10, 10), new Point(0, 10)); +var poly2 = new Polygon( + new Point(5, 5), new Point(15, 5), + new Point(15, 15), new Point(5, 15)); + +Polygon merged = poly1 | poly2; // union +Polygon overlap = poly1 & poly2; // intersection + +// 3D trajectory with time-parameterized waypoints +var trajectory = new Trajectory3( + new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), + new Waypoint3(new Pose3(100, 0, 50, 0, 45, 0), 5.0)); +Pose3 atHalf = trajectory.AtTime(2.5); // interpolated pose +``` + +## API Reference + +### 2D Core Types + +| Type | Description | +|---|---| +| `Point` | 2D Cartesian point. Supports arithmetic with vectors and sizes, distance, lerp, reflect, rotate, clamp, midpoint, matrix transform, tuple conversion, and parsing. | +| `Vector` | 2D vector with magnitude, direction, dot/cross product, normalize, projection, reflect, rotate, perpendicular (CW/CCW), angle between, lerp, and matrix transform. | +| `Size` | Width/height pair with arithmetic operators, component-wise multiply/divide, and parsing. | +| `Matrix` | 3x3 affine transformation matrix. Supports translate, rotate, scale, skew, invert, append/prepend, and point/vector transforms. | +| `Matrix2x2` | 2x2 matrix for linear algebra: determinant, trace, transpose, inverse, eigendecomposition (closed-form for symmetric matrices), and point/vector transforms. | +| `Rectangle` | Axis-aligned rectangle with contains, intersect, inflate, offset, bounds, tiling, diagonal, distance-to-point, and LTRB construction. | +| `Thickness` | Four-sided thickness (top, right, bottom, left) for rectangular borders. | + +### 3D Core Types + +| Type | Description | +|---|---| +| `Point3` | 3D Cartesian point. Distance, lerp, midpoint, random generation, arithmetic with `Vector3`. Conversions to/from `System.Numerics.Vector3` and tuples. Protobuf and JSON serialization. | +| `Vector3` | 3D vector with length, normalize, dot product, cross product, angle between, projection, lerp, random unit vector, and `RotationTo()` for computing the rotation between two directions. Static basis vectors `EX`, `EY`, `EZ`. | +| `Matrix3x3` | 3x3 matrix for 3D transforms. Determinant, trace, transpose, inverse. Rotation factories: `RotationX`, `RotationY`, `RotationZ`, `RotationZYX`. Scale, column/row construction. Euler angle and quaternion extraction. | +| `Quaternion` | Quaternion for 3D rotations. Normalize, conjugate, inverse, from axis-angle, rotate vector, slerp, dot product. Conversions to/from `System.Numerics.Quaternion`. | +| `Segment3` | Bounded line segment in 3D. Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | + +### Shapes (2D) + +All 2D shapes implement `IShape`, providing `Area()`, `Perimeter()`, `Centroid()`, `BoundingBox()`, `Contains(Point)`, `Rotate(Degree, Point)`, and `Scale(T)`. + +| Type | Description | +|---|---| +| `Circle` | Center + radius. Intersects with lines, segments, circles, and rectangles. Tangent point detection. `PointAt(Radian)` for parameterized access. | +| `Triangle` | Three vertices. Incircle, circumcircle, orthocenter, angles, edges. Classification: `IsRight()`, `IsAcute()`, `IsObtuse()`, `IsEquilateral()`, `IsIsosceles()`, `IsScalene()`. Similarity and congruence tests. | +| `Polygon` | Immutable polygon backed by `ReadOnlyMemory>`. Shoelace area, ray-casting containment, convex hull (Graham scan), `IsConvex()`, edges, simplify, skeleton (medial axis). Boolean operations: union, intersect, subtract, cluster. Operators: `\|` (union), `&` (intersect), `-` (subtract). | +| `Rectangle` | Implements `IArea`, `IPerimeter`, `ICentroid`, `IBoundingBox`, `IScalable>`. Rotation returns `Polygon`. | + +### Shapes (3D) + +| Type | Description | +|---|---| +| `Triangle3` | Three vertices in 3D space. Area (cross product), perimeter, centroid, unit normal vector. `ToPose()` creates a coordinate frame from the triangle plane. Translate by `Vector3`. | + +### Lines and Segments + +| Type | Description | +|---|---| +| `Line` | Infinite 2D line (supports vertical lines). From two points, point + direction, or equation. Intersect with lines, segments, circles, triangles, rectangles, polygons. Distance-to-point, angle-between, parallel/perpendicular tests, project point, reflect point. | +| `Segment` | Bounded 2D line segment (start + end). Length, midpoint, direction, lerp, split, reverse. Intersect with segments, lines, circles, rectangles. Distance-to-point, project point, parallel test. Liang-Barsky rectangle clipping. | +| `Segment3` | Bounded 3D line segment (start + end). Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | + +### Curves and Paths (2D) + +| Type | Description | +|---|---| +| `BezierCurve` | Cubic Bezier curve (4 control points). `Evaluate(t)`, `Split(t)`, `SubCurve(t0, t1)` via De Casteljau. Extremum points, linear equation intersection, rectangle edge crossings. | +| `Path` | Immutable sequence of `BezierCurve` segments. Create from points (`FromPoints`) with smoothing, or from SVG path data (`Parse`). `Length()`, `PointAt(t)` with arc-length parameterization. Exact `Intersect(Rectangle)` that splits Bezier curves at crossing points. Transform, rotate, reverse, append. `Close()` and `ToPolygon()` for rasterization. | +| `ComplexCurve` | Composite curve of mixed Bezier and polyline segments packed in a contiguous byte buffer for zero-copy decoding. SVG export and JSON serialization. | +| `PolygonalCurve` | Mutable sequence of connected points that generates smooth Bezier segments via `GetSmoothSegment(i)`. | + +### Polylines (2D and 3D) + +| Type | Description | +|---|---| +| `Polyline` | Immutable open polyline in 2D (non-closing). Length, edges, bounding box, simplification. JSON, Protobuf, and SVG serialization. | +| `Polyline3` | Immutable open polyline in 3D. Length, edges, translate, transform. JSON and Protobuf serialization. | + +### Angles and Coordinate Systems + +| Type | Description | +|---|---| +| `Degree` | Angle in degrees. Implicit conversion from numeric values and from `Radian`. Arithmetic (+, -, *, /), normalize to (-180, 180], abs, comparison operators. | +| `Radian` | Angle in radians. Implicit conversion from `Degree`. Arithmetic (+, -, *, /), normalize to (-pi, pi], abs, comparison operators. Static `Sin`/`Cos` helpers. | +| `PolarPoint` | Polar coordinates (radius, angle). Implicit conversions to/from `Point`, explicit to `Vector2` and `Vector`. | +| `CylindricalPoint` | Cylindrical coordinates (radial distance, angle, height). Factory: `OnPlane(r, angle)`. Builder methods: `WithZ`, `WithRadialDistance`, `WithAngle`. `RotateAngle` for angular displacement. `+`/`-` operators with `Radian` and `Point3`. Conversions to/from `Point3`, `Vector3`, `SphericalPoint`, and `System.Numerics.Vector3`. | +| `SphericalPoint` | Spherical coordinates (radius, azimuth, inclination) with Degree-based storage (ISO convention: inclination from Z-axis). Factories: `OnEquator`, `FromElevation`, `NorthPole`, `SouthPole`. `Normalized`, `WithRadius`, `RotateAzimuth`, `RotateInclination`, `Rotate(az, inc)`. `AngularDistance` (Vincenty formula), `Slerp`, `Lerp`, Euclidean `Distance`. Radius scaling operators (`*`, `/`). `Point3` offset operators (`+`, `-`). Conversions to/from `Point3`, `Vector3`, `CylindricalPoint`, and `System.Numerics.Vector3`. | + +### Rotations and Orientation + +| Type | Description | +|---|---| +| `Rotation3` | Euler angle rotation (Roll/Pitch/Yaw in degrees, ZYX convention). Wire and edit format only (since 2.0): create from degrees, radians, quaternion (`FromQuaternion`) or `Matrix3x3` (`FromMatrix3x3`); `ToQuaternion()` for maths. No rotation algebra: compose, invert, slerp and rotate with `Quaternion` or `Frame3`. Protobuf and JSON serialization. | +| `Quaternion` | Unit quaternion for 3D rotations. From axis-angle, `FromRpy(roll, pitch, yaw)` (Rz·Ry·Rx, radians; the URDF rpy rule), `FromTo(from, to)` (shortest arc), `FromAxes(x, y, z)` (orthonormal basis), `FromMatrix3x3(m)` (Shepperd's method) and `ToMatrix3x3()`, rotate vector, slerp, dot, normalize, conjugate, inverse. Hamilton product operator (`*`). Conversions to/from `System.Numerics.Quaternion`. Protobuf serialization. | +| `Matrix3x3` | Also used as rotation representation. `RotationZYX(rx, ry, rz)` creates ZYX Euler rotation matrix. `ToEulerZYX()` extracts angles (gimbal lock: rz = 0). Both go through `Quaternion` (`FromRpy`/`ToMatrix3x3`, `FromMatrix3x3`/`Rotation3.FromQuaternion`), so there is one Euler formula each way. | + +### Poses and Trajectories + +| Type | Description | +|---|---| +| `Frame3` | Rigid frame = position (`Point3`) + unit `Quaternion`. All pose maths: `Multiply` / `*` (compose), `Inverse`, `TransformPoint`, `TransformVector`, `Interpolate` (position lerp + slerp), `Distance` (position error and angle error in radians). `FromSurface(a, b, c)` creates a frame from three coplanar points (right-hand rule); `FromSurface(a, b, c, h)` uses a hint point for Z; `FromSurface(points)` fits a plane. `FromPose3(pose, lengthScale)` / `ToPose3(lengthScale)` convert to and from the wire format (ZYX degrees). | +| `Pose3` | Wire and edit format only (since 2.0): position (`Point3`) + ZYX Euler orientation (`Rotation3`, degrees). Constructors, `Identity`, tuple conversions, deconstruction, parsing, equality, `Truncating()`. Protobuf and JSON serialization. Convert to `Frame3` for any maths. | +| `Waypoint3` | Timestamped `Pose3` -- an element of a trajectory. | +| `PosePath3` | Immutable sequence of `Pose3` (no time). Length, reverse, transform, extract `Polyline3`, convert to `Trajectory3` with uniform timing. | +| `Trajectory3` | Immutable time-parameterized path of `Waypoint3`. `Duration`, `Length`, `AtTime(t)` (binary-search interpolation with position lerp + rotation slerp). `Resample(interval)`, `Reverse`, `Transform`. Convert to `PosePath3` or `Polyline3`. Create a real-time `TrajectoryController` via `Control()`. | +| `TrajectoryController` | Real-time trajectory playback using `Stopwatch` clock. Start, stop, pause, resume, seek. Fires `PoseChanged` events with current pose, time, and progress. | +| `Corner` | Corner blending descriptor: `Sharp` (exact waypoint) or `Round(radius)` (arc blend before/after the waypoint). | + +### Kinematics + +| Type | Description | +|---|---| +| `KinematicChain` | Namespace `ModelingEvolution.Drawing.Kinematics`. A tree of `Link`s moved by revolute/prismatic `Joint`s (origin at q = 0, unit axis, limits) with named `ChainFrame`s; metres and radians. `Forward(q, links)` writes every link pose, `FramePose(links, frame)`, `Jacobian(links, frame, j)` writes the 6 × Dof geometric Jacobian (column-major, v then ω). Spans in, spans out, no allocation; the constructor validates topological order, unit axes, limits and unique names. | +| `Joints6` | 6-DOF joint angles stored as six `Degree` fields (no array allocation). Indexer `[0..5]`, arithmetic operators, `Lerp`, `MaxAbsDelta`, `IsWithin(tolerance)`, `IsBetween(min, max)`. `ToArray()`, `FromSpan()`, `FromArray()`, `CopyTo(Span)`. Protobuf and JSON serialization. | +| `Speed` | Scalar speed in units/second (unitless magnitude). Arithmetic, `TimeFor(distance)`, `DistanceIn(time)`. Parsing. | +| `Velocity3` | 3D velocity vector (direction + magnitude). Create from direction + speed. `Speed`, `Direction`, `DisplacementIn(time)`. Arithmetic operators. | +| `Speed` | **Unit-aware** speed tagged at compile time with its unit (`ISpeedUnit`). Stores the value in `TUnit`; `ToSi()`/`FromSi()`/`To()` normalise via the SI base (m/s). Full arithmetic, comparisons, ratio (`Speed/Speed → T`), `IParsable`. Mixing units (or a non-speed unit) is a compile error. Verified zero-cost vs raw `double`. | +| `ISpeedUnit` | Marker for speed units. Built-ins: `MetrePerSecond` (SI base), `MillimetrePerSecond`, `MetrePerMinute`. Implement to add your own. | + +### Equations + +| Type | Description | +|---|---| +| `LinearEquation` | `y = Ax + B`. Create from points, angle, or direction. Compute, zero point, intersect, perpendicular, translate, mirror. | +| `QuadraticEquation` | `ax^2 + bx + c = 0`. Zero points using the discriminant. | +| `CubicEquation` | `ax^3 + bx^2 + cx + d = 0`. Root finding via Newton-Raphson. | +| `CircleEquation` | `(x-cx)^2 + (y-cy)^2 = r^2`. Intersect with `LinearEquation`. | + +### Colors + +| Type | Description | +|---|---| +| `Color` | ARGB color as a 32-bit uint. Parse from hex (`#RRGGBB`, `#AARRGGBB`), `rgba(r,g,b,a)`, or HSV strings. `GetHue()`, `GetSaturation()`, `GetLightness()`, `GetBrightness()`, `MakeTransparent(float)`. Implicit from string or tuple. JSON and Protobuf serialization. | +| `HsvColor` | HSV (hue, saturation, value) color space with optional alpha. Implicit conversions to/from `Color`. Parse from `hsv()`/`hsva()` strings, JSON arrays, or hex. | + +### Data Structures and Algorithms + +| Type | Description | +|---|---| +| `KdTree` | 2D KD-tree for fast nearest-neighbor queries on `Point`. Build from span, add individual points, find nearest neighbor. | +| `Skeleton` | Medial axis approximation of a polygon (interior nodes + edges). Three algorithms: `StraightSkeleton`, `ChordalAxis`, and `Voronoi`. | +| `MovingAverage` | Component-wise moving average over a sliding window of 2D points. O(1) add and query via circular buffer + running sums. | +| `MovingMedian` | Component-wise moving median over a sliding window of 2D points. O(1) median read, O(n) add via maintained sorted arrays. | +| `AllocationScope` | Arena-style pooled allocation scope. Library allocations use `MemoryPool.Shared` when active. Dispose returns all tracked memory at once. Supports `Persist()` to detach individual results. | +| `AlignmentResult` | Result of a rigid 2D alignment (rotation + translation) between two point clouds, with rotation angle and residual error. | + +### Interfaces + +```csharp +// Composable geometry traits +public interface IArea { T Area(); } +public interface IPerimeter { T Perimeter(); } +public interface ICentroid { Point Centroid(); } +public interface IBoundingBox { Rectangle BoundingBox(); } +public interface IRotatable { TSelf Rotate(Degree angle, Point origin = default); } +public interface IScalable { TSelf Scale(T factor); } + +// Unified shape interface -- combines all of the above plus point containment +public interface IShape : IArea, IPerimeter, ICentroid, + IBoundingBox, IRotatable, IScalable +{ + bool Contains(Point point); +} +``` + +Implementing types: `Circle`, `Triangle`, `Polygon`, and `Rectangle` (partial -- no `IRotatable` since rotation produces a `Polygon`). + +## Key Features + +### Generic Math + +All geometry types are parameterized by `T` with constraints like `IFloatingPointIeee754`, `IMinMaxValue`, etc. This means the same code works with any numeric type: + +```csharp +// Single precision +var pf = new Point(1f, 2f); +var cf = new Circle(pf, 5f); + +// Double precision +var pd = new Point(1.0, 2.0); +var cd = new Circle(pd, 5.0); + +// Both compute area with their respective precision +float areaF = cf.Area(); +double areaD = cd.Area(); +``` + +### IShape Interface Hierarchy + +Write generic algorithms over any shape: + +```csharp +void PrintShapeInfo(TShape shape) + where T : INumber, ITrigonometricFunctions, IRootFunctions, + IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue + where TShape : IShape +{ + Console.WriteLine($"Area: {shape.Area()}"); + Console.WriteLine($"Perimeter: {shape.Perimeter()}"); + Console.WriteLine($"Centroid: {shape.Centroid()}"); + Console.WriteLine($"Bounds: {shape.BoundingBox()}"); +} +``` + +### Intersections + +The `Intersections` static class provides 21 combination overloads covering every pair of: `Line`, `Segment`, `Circle`, `Triangle`, `Rectangle`, and `Polygon`. Each combination also has a zero-allocation `FirstOf` variant. + +```csharp +// Line x Line +Point? p = Intersections.Of(line1, line2); + +// Line x Circle -> chord (Segment) or null +Segment? chord = Intersections.Of(line, circle); + +// Tangent detection +Point? tangent = Intersections.TangentPoint(line, circle); + +// Circle x Circle -> radical chord or tangent point +Segment? radicalChord = Intersections.Of(circle1, circle2); + +// Segment x Segment +Point? hit = Intersections.Of(seg1, seg2); + +// Zero-alloc first-hit variants +Segment? first = Intersections.FirstOf(line, polygon); +``` + +Instance methods are also available on each shape for convenience: + +```csharp +Segment? chord = circle.Intersect(line); +Point? hit = segment.Intersect(otherSegment); +Segment? clipped = line.Intersect(rectangle); +``` + +### Polygon Boolean Operations + +Powered by [Clipper2](https://github.com/AngusJohnson/Clipper2): + +```csharp +var a = new Polygon( /* ... */ ); +var b = new Polygon( /* ... */ ); + +// Operators +Polygon union = a | b; +Polygon intersect = a & b; +Polygon difference = a - b; + +// Methods returning multiple result polygons +List> unions = a.Union(b, removeHoles: true); +List> diffs = a.Subtract(b); + +// Batch operations +List> merged = Polygon.Union(polygons); + +// Clustering overlapping polygons +IEnumerable> clusters = Polygon.Cluster(polygons); +``` + +### Bezier Path Clipping + +`Path.Intersect(Rectangle)` performs analytically exact Bezier-rectangle clipping. It finds the precise parameter values where each cubic Bezier segment crosses the rectangle edges, then uses `SubCurve(t0, t1)` via De Casteljau subdivision to return exact sub-paths -- not polygon approximations. + +```csharp +var path = Path.FromPoints(points); +IEnumerable> clipped = path.Intersect(viewport); + +foreach (var subPath in clipped) +{ + // Each sub-path contains only the Bezier segments inside the rectangle, + // with curves split precisely at the rectangle boundaries. +} +``` + +### Pooled Allocation Scopes + +When performing many geometry operations that allocate intermediate results (polygon booleans, skeleton computation, curve corrections), use `AllocationScope` to avoid GC pressure: + +```csharp +using var scope = AllocationScope.Begin(); + +var polygon = somePolygon * new Size(2, 2); // pooled +var skeleton = polygon.Skeleton(); // pooled +var lease = scope.Persist(ref skeleton); // detach from scope + +// scope.Dispose() returns everything except skeleton's memory +// lease.Dispose() returns skeleton's memory when you're done +``` + +### Serialization + +**Protobuf** -- all core types carry `[ProtoContract]` / `[ProtoMember]` attributes for use with protobuf-net: + +```csharp +var point = new Point(1f, 2f); +byte[] bytes = Serializer.Serialize(point); +``` + +**JSON** -- custom `System.Text.Json` converters for `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Polygon`, `Polyline`, `Polyline3`, `PosePath3`, `Trajectory3`, `Path`, `ComplexCurve`, `BezierCurve`, `Matrix2x2`, `Matrix3x3`, `Skeleton`, `Color`, and `HsvColor`: + +```csharp +var pose = new Pose3(100, 200, 300, 45, 90, 0); +string json = JsonSerializer.Serialize(pose); // [100,200,300,45,90,0] + +var joints = new Joints6(10, 20, 30, 40, 50, 60); +string j = JsonSerializer.Serialize(joints); // [10,20,30,40,50,60] +``` + +**String parsing** -- `IParsable` implementations on `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Rectangle`, `Size`, `Path`, `ComplexCurve`, `Speed`, `Color`, and `HsvColor`: + +```csharp +var point = Point.Parse("1.5, 2.5"); +var point3 = Point3.Parse("1.0 2.0 3.0"); +var pose = Pose3.Parse("100, 200, 300, 45, 90, 0"); +var rect = Rectangle.Parse("0 0 100 200"); +var color = Color.Parse("#FF8800"); +var path = Path.Parse("M 0 0 C 1 2, 3 4, 5 6", null); +``` + +### SVG Export + +`Polygon`, `Polyline`, `ComplexCurve`, `Skeleton`, and `Path` support SVG export via the `[SvgExporter]` attribute and the `SvgExporter` infrastructure: + +```csharp +// Path.ToString() produces SVG path data +var path = Path.FromPoints(points); +string svgData = path.ToString(); // "M 0 0 C 1.5 2.3, 3.1 4.2, 5 6 ..." +``` + +## Code Examples + +### Points: Distance, Lerp, Rotate + +```csharp +var a = new Point(0, 0); +var b = new Point(3, 4); + +double dist = a.DistanceTo(b); // 5.0 +Point quarter = a.Lerp(b, 0.25); // {X=0.75, Y=1} +Point reflected = a.Reflect(b); // {X=6, Y=8} + +// Rotate 90 degrees around the origin +Degree angle = 90.0; +Point rotated = b.Rotate(angle); // {X=-4, Y=3} + +// Rotate around a custom center +var center = new Point(1, 1); +Point rotated2 = b.Rotate(angle, center); + +// Clamp to a rectangle +var bounds = new Rectangle(0, 0, 10, 10); +Point clamped = new Point(15, -3).Clamp(bounds); // {X=10, Y=0} +``` + +### 3D Points, Vectors, and Rotations + +```csharp +// 3D point operations +var p1 = new Point3(1, 2, 3); +var p2 = new Point3(4, 6, 3); +double dist3d = Point3.Distance(p1, p2); // 5.0 +Point3 mid3d = Point3.Lerp(p1, p2, 0.5); + +// 3D vector operations +var v = new Vector3(1, 0, 0); +var w = new Vector3(0, 1, 0); +var cross = Vector3.Cross(v, w); // (0, 0, 1) +double dot = Vector3.Dot(v, w); // 0.0 + +// Compute rotation between two directions +var rot = Quaternion.FromTo(v, w); +Vector3 result = rot.Rotate(v); // approximately (0, 1, 0) + +// Euler angles (ZYX convention) are a wire format: convert once, then use the quaternion +var rotation = Rotation3.FromDegrees(45, 0, 0).ToQuaternion(); +Vector3 rotated = rotation.Rotate((Vector3)p1); + +// Quaternion slerp +var r1 = Quaternion.Identity; +var r2 = Quaternion.FromAxisAngle(Vector3.EZ, Math.PI / 2); +var halfway = Quaternion.Slerp(r1, r2, 0.5); + +// Matrix3x3 rotation +var m = Matrix3x3.RotationZ(Degree.Create(45)); +var transformed = m.Transform(new Point3(1, 0, 0)); +``` + +### Spherical Coordinates + +```csharp +// Create spherical points with factory methods +var northPole = SphericalPoint.NorthPole(10.0); // +Z, radius=10 +var equator = SphericalPoint.OnEquator(5.0, Degree.Create(45)); +var elevated = SphericalPoint.FromElevation(5.0, + Degree.Create(0), Degree.Create(30)); + +// Convert to/from Cartesian +Point3 cartesian = equator; // implicit conversion +SphericalPoint spherical = cartesian; // implicit conversion + +// Orbit around a 3D center +var center = new Point3(10, 50, 0); +var points = new List>(); +for (int az = 0; az < 360; az += 30) + points.Add(center + SphericalPoint.OnEquator(5.0, Degree.Create(az))); + +// Rotation (O(1) angle operations) +var sp = SphericalPoint.OnEquator(5.0, Degree.Create(0)); +var rotated = sp.RotateAzimuth(Degree.Create(45)); // rotate around Z + +// Angular distance (Vincenty formula -- numerically stable) +var a = SphericalPoint.NorthPole(1.0); +var b = SphericalPoint.SouthPole(1.0); +Degree angDist = SphericalPoint.AngularDistance(a, b); // 180 degrees + +// Spherical linear interpolation +var mid = SphericalPoint.Slerp(a, b, 0.5); + +// Cylindrical coordinate conversions +CylindricalPoint cyl = sp; // implicit conversion +SphericalPoint back = cyl; // implicit conversion +``` + +### Poses and Trajectories + +```csharp +// Pose = position + orientation +var pose = new Pose3(100, 200, 300, 45, 0, 0); // x,y,z, rx,ry,rz (degrees) + +// Maths composes Frame3 (position + quaternion); Pose3 converts once, here mm -> m +var world = Frame3.FromPose3(new Pose3(10, 0, 0, 0, 0, 0), 0.001); +var local = Frame3.FromPose3(new Pose3(5, 0, 0, 0, 0, 90), 0.001); +var combined = world * local; +Pose3 back = combined.ToPose3(1000); // m -> mm, ZYX degrees + +// Create frame from a surface (3 points define a plane) +var a = new Point3(0, 0, 0); +var b = new Point3(1, 0, 0); +var c = new Point3(0, 1, 0); +var surface = Frame3.FromSurface(a, b, c); // Z-axis via right-hand rule + +// Trajectory with timed waypoints +var trajectory = new Trajectory3( + new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), + new Waypoint3(new Pose3(50, 0, 25, 0, 30, 0), 2.5), + new Waypoint3(new Pose3(100, 0, 0, 0, 0, 0), 5.0)); + +// Interpolate at any time (binary search + lerp/slerp) +Pose3 atT = trajectory.AtTime(1.25); + +// Resample at uniform intervals +var resampled = trajectory.Resample(0.1); // every 0.1 seconds + +// Real-time playback +var controller = trajectory.Control(); +controller.PoseChanged += (_, e) => Console.WriteLine($"t={e.Time} pose={e.Pose}"); +controller.Start(); + +// 6-DOF joint angles for robotics +var home = new Joints6(0, -90, 0, 0, 90, 0); +var target = new Joints6(45, -60, 30, 0, 90, -45); +var halfway = Joints6.Lerp(home, target, 0.5); +bool inRange = target.IsBetween(minLimits, maxLimits); +``` + +### Shapes: Area and Perimeter + +```csharp +// Circle +var circle = new Circle(new Point(5, 5), 3.0); +double cArea = circle.Area(); // ~28.274 +double cPerim = circle.Perimeter(); // ~18.850 + +// Triangle +var triangle = new Triangle( + new Point(0, 0), + new Point(4, 0), + new Point(0, 3)); +double tArea = triangle.Area(); // 6.0 +double tPerim = triangle.Perimeter(); // 12.0 + +// Polygon (shoelace formula) +var poly = new Polygon( + new Point(0, 0), new Point(4, 0), + new Point(4, 3), new Point(0, 3)); +double pArea = poly.Area(); // 12.0 + +// 3D triangle +var t3 = new Triangle3( + new Point3(0, 0, 0), + new Point3(4, 0, 0), + new Point3(0, 3, 0)); +double area3 = t3.Area; // 6.0 +Vector3 normal = t3.Normal; // (0, 0, 1) +``` + +### Line-Circle Intersection + +```csharp +var circle = new Circle(new Point(0, 0), 5.0); +var line = Line.From(new Point(-10, 0), new Point(10, 0)); + +// Secant -- returns the chord as a segment +Segment? chord = circle.Intersect(line); +// chord.Value.Start ~ {X=-5, Y=0}, chord.Value.End ~ {X=5, Y=0} + +// Tangent detection +var tangentLine = Line.Horizontal(5.0); +Point? tangent = circle.TangentPoint(tangentLine); +// tangent.Value ~ {X=0, Y=5} +``` + +### Segment-Rectangle Clipping + +```csharp +var viewport = new Rectangle(0, 0, 100, 100); +var segment = new Segment( + new Point(-20, 50), + new Point(150, 50)); + +// Liang-Barsky clipping +Segment? clipped = segment.Intersect(viewport); +// clipped.Value: Start={X=0, Y=50}, End={X=100, Y=50} +``` + +### Path-Rectangle Clipping + +```csharp +var points = new[] +{ + new Point(10, 50), + new Point(50, 10), + new Point(90, 50), + new Point(130, 90) +}; +var path = Path.FromPoints(points); +var viewport = new Rectangle(20, 20, 60, 60); // 20,20 to 80,80 + +// Exact Bezier clipping -- returns sub-paths with curves split at boundaries +foreach (var subPath in path.Intersect(viewport)) +{ + Console.WriteLine($"Sub-path with {subPath.Count} Bezier segments"); +} +``` + +### Triangle Properties + +```csharp +var t = new Triangle( + new Point(0, 0), + new Point(4, 0), + new Point(2, 3)); + +// Derived circles +Circle incircle = t.Incircle(); // largest circle inside +Circle circumcircle = t.Circumcircle(); // passes through all vertices + +// Center points +Point centroid = t.Centroid(); +Point orthocenter = t.Orthocenter; + +// Classification +bool right = t.IsRight(); +bool equilateral = t.IsEquilateral(); +bool isosceles = t.IsIsosceles(); + +// Similarity and congruence +var t2 = new Triangle( + new Point(0, 0), + new Point(8, 0), + new Point(4, 6)); +bool similar = t.IsSimilarTo(t2); // true (same shape, scaled 2x) +bool congruent = t.IsCongruentTo(t2); // false (different size) + +// Interior angles +var (atA, atB, atC) = t.Angles; +``` + +### Polygon Operations + +```csharp +// Convex hull +var cloud = new Polygon( + new Point(0, 0), new Point(1, 3), + new Point(2, 1), new Point(4, 4), + new Point(3, 0), new Point(5, 2)); +Polygon hull = cloud.ConvexHull(); + +// Point containment (ray casting) +bool inside = hull.Contains(new Point(2, 2)); + +// Boolean operations +var a = new Polygon( /* ... */ ); +var b = new Polygon( /* ... */ ); +List> union = a.Union(b, removeHoles: true); +List> diff = a.Subtract(b); +List> inter = a.Intersect(b); + +// Simplify (remove collinear/close points) +Polygon simplified = hull.Simplify(epsilon: 0.1); + +// Convexity test +bool convex = hull.IsConvex(); + +// Clustering overlapping polygons +var clusters = Polygon.Cluster(polygons); +``` + +### Matrix Transformations (2D and 3D) + +```csharp +// 2D affine transformation +var matrix = Matrix.Identity; +matrix.Translate(10.0, 20.0); +matrix.Rotate(Degree.Create(45.0)); +matrix.Scale(2.0, 2.0); + +Point transformed = matrix.Transform(new Point(1, 0)); +Vector rotatedVec = matrix.Transform(new Vector(1, 0)); + +var combined = matrix1 * matrix2; +matrix.Invert(); + +// 3D rotation matrix +var rx = Matrix3x3.RotationX(Degree.Create(45)); +var ry = Matrix3x3.RotationY(Degree.Create(30)); +var rz = Matrix3x3.RotationZ(Degree.Create(60)); + +// Compose rotations +var rotation = rz * ry * rx; +var p = new Point3(1, 0, 0); +Point3 rotated3 = rotation * p; // or rotation.Transform(p) + +// Extract Euler angles +var (eRx, eRy, eRz) = rotation.ToEulerZYX(); + +// Convert to quaternion and back +Quaternion q = Quaternion.FromMatrix3x3(rotation); +Matrix3x3 back = q.ToMatrix3x3(); +``` + +### Unit-Aware Speed + +```csharp +using ModelingEvolution.Drawing.Units; + +// The unit is part of the type — the value is stored in that unit. +Speed> feed = new(300); // 300 m/min + +// Normalise to the SI base (m/s) or convert to another speed unit. +double si = feed.ToSi(); // 5.0 (m/s) +var inMmPerS = feed.To>(); // 5000 mm/s + +// Same-unit arithmetic, scalar scaling, and a dimensionless ratio. +var faster = feed * 2.0; // 600 m/min +double ratio = faster / feed; // 2.0 + +// Cross-unit add normalises through SI; result stays in the left unit. +var total = inMmPerS.Add(feed); // (5000 + 5000) mm/s + +Console.WriteLine(feed); // "300 m/min" + +// Compile-time safety: +// feed + inMmPerS; // error: different units — use .Add(...) +// feed.To>(); // error: not an ISpeedUnit +``` + +## Requirements + +- **.NET 10.0** or later (uses generic math interfaces from .NET 7+) +- **Dependencies**: [Clipper2](https://www.nuget.org/packages/Clipper2) 1.4.0, [protobuf-net](https://www.nuget.org/packages/protobuf-net) 3.2.45 + +## License + +See [LICENSE](LICENSE) for details. diff --git a/Sources/ModelingEvolution.Drawing.Tests/Frame3SurfaceTests.cs b/Sources/ModelingEvolution.Drawing.Tests/Frame3SurfaceTests.cs index d11131f..fecc8db 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Frame3SurfaceTests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Frame3SurfaceTests.cs @@ -29,7 +29,7 @@ public void FromSurface_HorizontalPlane_HintAbove_ZPointsUp() pose.Position.Z.Should().BeApproximately(0, Tolerance); // Z-axis should point up (toward h) - var zDir = pose.Rotation.Rotate(Vector3.EZ); + var zDir = pose.TransformVector(Vector3.EZ); zDir.X.Should().BeApproximately(0, Tolerance); zDir.Y.Should().BeApproximately(0, Tolerance); zDir.Z.Should().BeApproximately(1, Tolerance); @@ -53,7 +53,7 @@ public void FromSurface_HorizontalPlane_HintBelow_ZPointsDown() pose.Position.Z.Should().BeApproximately(0, Tolerance); // Z-axis should point down (toward h) - var zDir = pose.Rotation.Rotate(Vector3.EZ); + var zDir = pose.TransformVector(Vector3.EZ); zDir.X.Should().BeApproximately(0, Tolerance); zDir.Y.Should().BeApproximately(0, Tolerance); zDir.Z.Should().BeApproximately(-1, Tolerance); @@ -77,7 +77,7 @@ public void FromSurface_VerticalPlane_XZ_HintInFront() pose.Position.Z.Should().BeApproximately(0.5, Tolerance); // Z-axis should point toward positive Y - var zDir = pose.Rotation.Rotate(Vector3.EZ); + var zDir = pose.TransformVector(Vector3.EZ); zDir.X.Should().BeApproximately(0, Tolerance); zDir.Y.Should().BeApproximately(1, Tolerance); zDir.Z.Should().BeApproximately(0, Tolerance); @@ -95,7 +95,7 @@ public void FromSurface_XAxisAlongEdgeAB() var pose = Frame3.FromSurface(a, b, c, h); // X-axis should be along a→b direction (positive X) - var xDir = pose.Rotation.Rotate(Vector3.EX); + var xDir = pose.TransformVector(Vector3.EX); xDir.X.Should().BeApproximately(1, Tolerance); xDir.Y.Should().BeApproximately(0, Tolerance); xDir.Z.Should().BeApproximately(0, Tolerance); @@ -116,7 +116,7 @@ public void FromSurface_TiltedPlane() // Z should point toward h - normal is (0, -1/√2, 1/√2) // Since h has z > y, h is on the +normal side, so Z = +normal - var zDir = pose.Rotation.Rotate(Vector3.EZ); + var zDir = pose.TransformVector(Vector3.EZ); zDir.X.Should().BeApproximately(0, Tolerance); // Y and Z components should have same magnitude (45° angle from Y-Z plane) Math.Abs(zDir.Y).Should().BeApproximately(Math.Abs(zDir.Z), 0.01); @@ -219,7 +219,7 @@ public void FromSurface_ThreePoints_XAxisAlongAB() var pose = Frame3.FromSurface(a, b, c); - var xDir = pose.Rotation.Rotate(Vector3.EX); + var xDir = pose.TransformVector(Vector3.EX); xDir.X.Should().BeApproximately(1, Tolerance); xDir.Y.Should().BeApproximately(0, Tolerance); xDir.Z.Should().BeApproximately(0, Tolerance); @@ -236,7 +236,7 @@ public void FromSurface_ThreePoints_RightHandRule_ZPointsUp() var pose = Frame3.FromSurface(a, b, c); - var zDir = pose.Rotation.Rotate(Vector3.EZ); + var zDir = pose.TransformVector(Vector3.EZ); zDir.X.Should().BeApproximately(0, Tolerance); zDir.Y.Should().BeApproximately(0, Tolerance); zDir.Z.Should().BeApproximately(1, Tolerance); @@ -253,7 +253,7 @@ public void FromSurface_ThreePoints_ReversedOrder_ZPointsDown() var pose = Frame3.FromSurface(a, b, c); - var zDir = pose.Rotation.Rotate(Vector3.EZ); + var zDir = pose.TransformVector(Vector3.EZ); zDir.Z.Should().BeLessThan(0); } @@ -268,7 +268,7 @@ public void FromSurface_ThreePoints_VerticalPlane() var pose = Frame3.FromSurface(a, b, c); - var zDir = pose.Rotation.Rotate(Vector3.EZ); + var zDir = pose.TransformVector(Vector3.EZ); zDir.X.Should().BeApproximately(0, Tolerance); zDir.Y.Should().BeApproximately(-1, Tolerance); zDir.Z.Should().BeApproximately(0, Tolerance); @@ -332,7 +332,7 @@ public void FromSurface_NPoints_HorizontalPlane_ZPointsUp() }; var pose = Frame3.FromSurface(points); - var zDir = pose.Rotation.Rotate(Vector3.EZ); + var zDir = pose.TransformVector(Vector3.EZ); zDir.X.Should().BeApproximately(0, Tolerance); zDir.Y.Should().BeApproximately(0, Tolerance); @@ -348,7 +348,7 @@ public void FromSurface_NPoints_XAxisAlongFirstEdge() }; var pose = Frame3.FromSurface(points); - var xDir = pose.Rotation.Rotate(Vector3.EX); + var xDir = pose.TransformVector(Vector3.EX); // X-axis should be along points[0]→points[1] projected onto plane = +X xDir.X.Should().BeApproximately(1, Tolerance); @@ -387,7 +387,7 @@ public void FromSurface_NPoints_NoisyPlane_NormalIsClose() }; var pose = Frame3.FromSurface(points); - var zDir = pose.Rotation.Rotate(Vector3.EZ); + var zDir = pose.TransformVector(Vector3.EZ); // Normal should be approximately +Z Math.Abs(zDir.Z).Should().BeGreaterThan(0.999); @@ -426,15 +426,15 @@ public void FromSurface_NPoints_MatchesThreePointForExactSquare() var poseN = Frame3.FromSurface([a, b, c, d]); // Normal directions should match - var z3 = pose3.Rotation.Rotate(Vector3.EZ); - var zN = poseN.Rotation.Rotate(Vector3.EZ); + var z3 = pose3.TransformVector(Vector3.EZ); + var zN = poseN.TransformVector(Vector3.EZ); z3.X.Should().BeApproximately(zN.X, 0.001); z3.Y.Should().BeApproximately(zN.Y, 0.001); z3.Z.Should().BeApproximately(zN.Z, 0.001); // X-axis directions should match - var x3 = pose3.Rotation.Rotate(Vector3.EX); - var xN = poseN.Rotation.Rotate(Vector3.EX); + var x3 = pose3.TransformVector(Vector3.EX); + var xN = poseN.TransformVector(Vector3.EX); x3.X.Should().BeApproximately(xN.X, 0.001); x3.Y.Should().BeApproximately(xN.Y, 0.001); x3.Z.Should().BeApproximately(xN.Z, 0.001); diff --git a/Sources/ModelingEvolution.Drawing.Tests/Frame3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Frame3Tests.cs index 4b9fe5f..5be2f6a 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Frame3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Frame3Tests.cs @@ -54,7 +54,8 @@ public void TransformPoint_EqualsRotatePlusPositionOver10000RandomFrames() var a = RandomFrame(rng); var p = new Point3(Uniform(rng, 2), Uniform(rng, 2), Uniform(rng, 2)); - var expected = (Point3)a.Rotation.Rotate(p) + (Vector3)a.Position; + var q = a.Rotation; // reference: the quaternion directly, not a Frame3 member + var expected = (Point3)q.Rotate(p) + (Vector3)a.Position; Point3.Distance(a.TransformPoint(p), expected).Should().BeLessThan(PositionTolerance); } diff --git a/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs index 1e9bd43..12aaed8 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs @@ -1,516 +1,612 @@ -using System.Text.Json; -using FluentAssertions; -using ModelingEvolution.Drawing; - -namespace ModelingEvolution.Drawing.Tests; - -public class Matrix3x3Tests -{ - private const double Tol = 1e-10; - private const double TolLoose = 1e-6; - - #region Identity & Properties - - [Fact] - public void Identity_HasCorrectValues() - { - var m = Matrix3x3.Identity; - m.M11.Should().Be(1d); m.M12.Should().Be(0d); m.M13.Should().Be(0d); - m.M21.Should().Be(0d); m.M22.Should().Be(1d); m.M23.Should().Be(0d); - m.M31.Should().Be(0d); m.M32.Should().Be(0d); m.M33.Should().Be(1d); - } - - [Fact] - public void Zero_HasCorrectValues() - { - var m = Matrix3x3.Zero; - m.M11.Should().Be(0d); m.M12.Should().Be(0d); m.M13.Should().Be(0d); - m.M21.Should().Be(0d); m.M22.Should().Be(0d); m.M23.Should().Be(0d); - m.M31.Should().Be(0d); m.M32.Should().Be(0d); m.M33.Should().Be(0d); - } - - [Fact] - public void Determinant_Identity_IsOne() - { - Matrix3x3.Identity.Determinant.Should().Be(1d); - } - - [Fact] - public void Determinant_KnownMatrix() - { - // |1 2 3; 0 1 4; 5 6 0| = 1*(0-24) - 2*(0-20) + 3*(0-5) = -24+40-15 = 1 - var m = new Matrix3x3(1, 2, 3, 0, 1, 4, 5, 6, 0); - m.Determinant.Should().BeApproximately(1d, Tol); - } - - [Fact] - public void Trace_KnownMatrix() - { - var m = new Matrix3x3(3, 0, 0, 0, 5, 0, 0, 0, 7); - m.Trace.Should().BeApproximately(15d, Tol); - } - - #endregion - - #region Transpose - - [Fact] - public void Transpose_General() - { - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var t = m.Transpose(); - t.M11.Should().Be(1d); t.M12.Should().Be(4d); t.M13.Should().Be(7d); - t.M21.Should().Be(2d); t.M22.Should().Be(5d); t.M23.Should().Be(8d); - t.M31.Should().Be(3d); t.M32.Should().Be(6d); t.M33.Should().Be(9d); - } - - [Fact] - public void Transpose_RotationMatrix_IsInverse() - { - var r = Matrix3x3.RotationX(Degree.Create(30)); - var product = r * r.Transpose(); - AssertIsIdentity(product); - } - - #endregion - - #region Inverse - - [Fact] - public void Inverse_Identity() - { - var inv = Matrix3x3.Identity.Inverse(); - AssertIsIdentity(inv); - } - - [Fact] - public void Inverse_KnownMatrix() - { - var m = new Matrix3x3(1, 2, 3, 0, 1, 4, 5, 6, 0); - var inv = m.Inverse(); - var product = m * inv; - AssertIsIdentity(product); - } - - [Fact] - public void Inverse_Singular_Throws() - { - // Third row = first row → det = 0 - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 1, 2, 3); - var act = () => m.Inverse(); - act.Should().Throw(); - } - - #endregion - - #region Multiplication - - [Fact] - public void Multiplication_IdentityIsNeutral() - { - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var r = m * Matrix3x3.Identity; - AssertMatricesEqual(r, m); - } - - [Fact] - public void Multiplication_KnownResult() - { - var a = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var b = new Matrix3x3(9, 8, 7, 6, 5, 4, 3, 2, 1); - var c = a * b; - // Row 0: (1*9+2*6+3*3, 1*8+2*5+3*2, 1*7+2*4+3*1) = (30, 24, 18) - c.M11.Should().BeApproximately(30, Tol); - c.M12.Should().BeApproximately(24, Tol); - c.M13.Should().BeApproximately(18, Tol); - // Row 1: (4*9+5*6+6*3, 4*8+5*5+6*2, 4*7+5*4+6*1) = (84, 69, 54) - c.M21.Should().BeApproximately(84, Tol); - c.M22.Should().BeApproximately(69, Tol); - c.M23.Should().BeApproximately(54, Tol); - // Row 2: (7*9+8*6+9*3, 7*8+8*5+9*2, 7*7+8*4+9*1) = (138, 114, 90) - c.M31.Should().BeApproximately(138, Tol); - c.M32.Should().BeApproximately(114, Tol); - c.M33.Should().BeApproximately(90, Tol); - } - - [Fact] - public void Multiplication_Associativity() - { - var a = Matrix3x3.RotationX(Degree.Create(30)); - var b = Matrix3x3.RotationY(Degree.Create(45)); - var c = Matrix3x3.RotationZ(Degree.Create(60)); - var ab_c = (a * b) * c; - var a_bc = a * (b * c); - AssertMatricesEqual(ab_c, a_bc); - } - - #endregion - - #region Transform - - [Fact] - public void Transform_Vector_IdentityPreserves() - { - var v = new Vector3(1, 2, 3); - var r = Matrix3x3.Identity.Transform(v); - r.X.Should().BeApproximately(1, Tol); - r.Y.Should().BeApproximately(2, Tol); - r.Z.Should().BeApproximately(3, Tol); - } - - [Fact] - public void Transform_Vector_RotationX90_RotatesBasisVectors() - { - var m = Matrix3x3.RotationX(Degree.Create(90)); - // Rotating Y axis by 90° around X → Z axis - var ey = new Vector3(0, 1, 0); - var result = m * ey; - result.X.Should().BeApproximately(0, TolLoose); - result.Y.Should().BeApproximately(0, TolLoose); - result.Z.Should().BeApproximately(1, TolLoose); - } - - [Fact] - public void Transform_Point_OperatorSyntax() - { - var m = Matrix3x3.Scale(2, 3, 4); - var p = new Point3(1, 2, 3); - var r = m * p; - r.X.Should().BeApproximately(2, Tol); - r.Y.Should().BeApproximately(6, Tol); - r.Z.Should().BeApproximately(12, Tol); - } - - #endregion - - #region Rotation Factories - - [Fact] - public void RotationX_90_RotatesYToZ() - { - var m = Matrix3x3.RotationX(Degree.Create(90)); - var v = new Vector3(0, 1, 0); - var r = m * v; - r.X.Should().BeApproximately(0, TolLoose); - r.Y.Should().BeApproximately(0, TolLoose); - r.Z.Should().BeApproximately(1, TolLoose); - } - - [Fact] - public void RotationY_90_RotatesZToX() - { - var m = Matrix3x3.RotationY(Degree.Create(90)); - var v = new Vector3(0, 0, 1); - var r = m * v; - r.X.Should().BeApproximately(1, TolLoose); - r.Y.Should().BeApproximately(0, TolLoose); - r.Z.Should().BeApproximately(0, TolLoose); - } - - [Fact] - public void RotationZ_90_RotatesXToY() - { - var m = Matrix3x3.RotationZ(Degree.Create(90)); - var v = new Vector3(1, 0, 0); - var r = m * v; - r.X.Should().BeApproximately(0, TolLoose); - r.Y.Should().BeApproximately(1, TolLoose); - r.Z.Should().BeApproximately(0, TolLoose); - } - - [Fact] - public void RotationZYX_MatchesComposedRotations() - { - var rx = Degree.Create(30d); - var ry = Degree.Create(45d); - var rz = Degree.Create(60d); - var composed = Matrix3x3.RotationZ(rz) * Matrix3x3.RotationY(ry) * Matrix3x3.RotationX(rx); - var direct = Matrix3x3.RotationZYX(rx, ry, rz); - AssertMatricesEqual(composed, direct); - } - - [Fact] - public void RotationMatrix_IsOrthogonal() - { - var m = Matrix3x3.RotationZYX(Degree.Create(25), Degree.Create(35), Degree.Create(45)); - var product = m * m.Transpose(); - AssertIsIdentity(product); - } - - [Fact] - public void RotationMatrix_Determinant_IsOne() - { - var m = Matrix3x3.RotationZYX(Degree.Create(10), Degree.Create(20), Degree.Create(30)); - m.Determinant.Should().BeApproximately(1d, TolLoose); - } - - #endregion - - #region Scale & Construction Factories - - [Fact] - public void Scale_ScalesVector() - { - var m = Matrix3x3.Scale(2, 3, 4); - var v = new Vector3(1, 1, 1); - var r = m * v; - r.X.Should().BeApproximately(2, Tol); - r.Y.Should().BeApproximately(3, Tol); - r.Z.Should().BeApproximately(4, Tol); - } - - [Fact] - public void FromColumns_BuildsCorrectMatrix() - { - var c0 = new Vector3(1, 4, 7); - var c1 = new Vector3(2, 5, 8); - var c2 = new Vector3(3, 6, 9); - var m = Matrix3x3.FromColumns(c0, c1, c2); - m.M11.Should().Be(1); m.M12.Should().Be(2); m.M13.Should().Be(3); - m.M21.Should().Be(4); m.M22.Should().Be(5); m.M23.Should().Be(6); - m.M31.Should().Be(7); m.M32.Should().Be(8); m.M33.Should().Be(9); - } - - [Fact] - public void FromRows_BuildsCorrectMatrix() - { - var r0 = new Vector3(1, 2, 3); - var r1 = new Vector3(4, 5, 6); - var r2 = new Vector3(7, 8, 9); - var m = Matrix3x3.FromRows(r0, r1, r2); - m.M11.Should().Be(1); m.M12.Should().Be(2); m.M13.Should().Be(3); - m.M21.Should().Be(4); m.M22.Should().Be(5); m.M23.Should().Be(6); - m.M31.Should().Be(7); m.M32.Should().Be(8); m.M33.Should().Be(9); - } - - #endregion - - #region Euler Round-trip - - [Theory] - [InlineData(10, 20, 30)] - [InlineData(-15, 45, -60)] - [InlineData(0, 0, 90)] - [InlineData(180, 0, 0)] - [InlineData(5, 10, 15)] - public void RotationZYX_ToEulerZYX_RoundTrip(double rxd, double ryd, double rzd) - { - Degree rx = Degree.Create(rxd), ry = Degree.Create(ryd), rz = Degree.Create(rzd); - var m = Matrix3x3.RotationZYX(rx, ry, rz); - var (erx, ery, erz) = m.ToEulerZYX(); - // Rebuild matrix from extracted angles and compare - var m2 = Matrix3x3.RotationZYX(erx, ery, erz); - AssertMatricesEqual(m, m2); - } - - [Fact] - public void ToEulerZYX_GimbalLock_AtPositive90() - { - var m = Matrix3x3.RotationZYX(Degree.Create(0), Degree.Create(90), Degree.Create(0)); - var (rx, ry, rz) = m.ToEulerZYX(); - // ry should be ~90° - ((double)ry).Should().BeApproximately(90, TolLoose); - // At gimbal lock, rx and rz share DOF; matrix should still reconstruct - var m2 = Matrix3x3.RotationZYX(rx, ry, rz); - AssertMatricesEqual(m, m2, 1e-5); - } - - #endregion - - #region Quaternion Round-trip - - [Theory] - [InlineData(10, 20, 30)] - [InlineData(-30, 45, 60)] - [InlineData(0, 0, 0)] - [InlineData(5, -10, 15)] - public void ToQuaternion_FromQuaternion_RoundTrip(double rxd, double ryd, double rzd) - { - var m = Matrix3x3.RotationZYX(Degree.Create(rxd), Degree.Create(ryd), Degree.Create(rzd)); - var q = m.ToQuaternion(); - // Convert quaternion back to rotation, then to matrix - var rot = Rotation3.FromQuaternion(q); - var m2 = rot.ToMatrix3x3(); - AssertMatricesEqual(m, m2, 1e-9); - } - - [Fact] - public void ToQuaternion_Identity_IsUnitQuaternion() - { - var q = Matrix3x3.Identity.ToQuaternion(); - q.W.Should().BeApproximately(1, TolLoose); - q.X.Should().BeApproximately(0, TolLoose); - q.Y.Should().BeApproximately(0, TolLoose); - q.Z.Should().BeApproximately(0, TolLoose); - } - - #endregion - - #region Rotation3 Integration - - [Theory] - [InlineData(10, 20, 30)] - [InlineData(-15, 45, -60)] - [InlineData(0, 0, 0)] - public void Rotation3_ToMatrix3x3_RoundTrip(double rxd, double ryd, double rzd) - { - var rot = new Rotation3(rxd, ryd, rzd); - var m = rot.ToMatrix3x3(); - var rot2 = Rotation3.FromMatrix3x3(m); - ((double)rot2.Rx).Should().BeApproximately(rxd, TolLoose); - ((double)rot2.Ry).Should().BeApproximately(ryd, TolLoose); - ((double)rot2.Rz).Should().BeApproximately(rzd, TolLoose); - } - - [Fact] - public void Rotation3_Rotate_MatchesMatrix_Transform() - { - var rot = new Rotation3(30, 45, 60); - var m = rot.ToMatrix3x3(); - var v = new Vector3(1, 2, 3); - - var rotated = rot.Rotate(v); - var transformed = m.Transform(v); - - rotated.X.Should().BeApproximately(transformed.X, TolLoose); - rotated.Y.Should().BeApproximately(transformed.Y, TolLoose); - rotated.Z.Should().BeApproximately(transformed.Z, TolLoose); - } - - #endregion - - #region Scalar & Element-wise - - [Fact] - public void ScalarMultiplication() - { - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var r = m * 2d; - r.M11.Should().Be(2); r.M12.Should().Be(4); r.M13.Should().Be(6); - r.M21.Should().Be(8); r.M22.Should().Be(10); r.M23.Should().Be(12); - r.M31.Should().Be(14); r.M32.Should().Be(16); r.M33.Should().Be(18); - } - - [Fact] - public void ScalarMultiplication_Commutative() - { - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var r1 = m * 3d; - var r2 = 3d * m; - r1.Should().Be(r2); - } - - [Fact] - public void Addition() - { - var a = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var b = new Matrix3x3(9, 8, 7, 6, 5, 4, 3, 2, 1); - var c = a + b; - c.M11.Should().Be(10); c.M12.Should().Be(10); c.M13.Should().Be(10); - c.M21.Should().Be(10); c.M22.Should().Be(10); c.M23.Should().Be(10); - c.M31.Should().Be(10); c.M32.Should().Be(10); c.M33.Should().Be(10); - } - - [Fact] - public void Subtraction() - { - var a = new Matrix3x3(9, 8, 7, 6, 5, 4, 3, 2, 1); - var b = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var c = a - b; - c.M11.Should().Be(8); c.M12.Should().Be(6); c.M13.Should().Be(4); - c.M21.Should().Be(2); c.M22.Should().Be(0); c.M23.Should().Be(-2); - c.M31.Should().Be(-4); c.M32.Should().Be(-6); c.M33.Should().Be(-8); - } - - #endregion - - #region JSON Serialization - - [Fact] - public void Json_Serialize_Double() - { - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var json = JsonSerializer.Serialize(m); - json.Should().Be("[[1,2,3],[4,5,6],[7,8,9]]"); - } - - [Fact] - public void Json_Deserialize_Double() - { - var m = JsonSerializer.Deserialize>("[[1.5,2.5,3.5],[4.5,5.5,6.5],[7.5,8.5,9.5]]"); - m.M11.Should().Be(1.5); m.M12.Should().Be(2.5); m.M13.Should().Be(3.5); - m.M21.Should().Be(4.5); m.M22.Should().Be(5.5); m.M23.Should().Be(6.5); - m.M31.Should().Be(7.5); m.M32.Should().Be(8.5); m.M33.Should().Be(9.5); - } - - [Fact] - public void Json_RoundTrip_Double() - { - var original = new Matrix3x3(1.23, 4.56, 7.89, 0.12, 3.45, 6.78, 9.01, 2.34, 5.67); - var json = JsonSerializer.Serialize(original); - var deserialized = JsonSerializer.Deserialize>(json); - deserialized.Should().Be(original); - } - - [Fact] - public void Json_RoundTrip_Float() - { - var original = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var json = JsonSerializer.Serialize(original); - var deserialized = JsonSerializer.Deserialize>(json); - deserialized.Should().Be(original); - } - - [Fact] - public void Json_Identity_RoundTrip() - { - var original = Matrix3x3.Identity; - var json = JsonSerializer.Serialize(original); - json.Should().Be("[[1,0,0],[0,1,0],[0,0,1]]"); - var deserialized = JsonSerializer.Deserialize>(json); - deserialized.Should().Be(original); - } - - #endregion - - #region ToString - - [Fact] - public void ToString_Format() - { - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - m.ToString().Should().Be("[1, 2, 3; 4, 5, 6; 7, 8, 9]"); - } - - #endregion - - #region Helpers - - private static void AssertIsIdentity(Matrix3x3 m, double tol = Tol) - { - m.M11.Should().BeApproximately(1, tol); - m.M12.Should().BeApproximately(0, tol); - m.M13.Should().BeApproximately(0, tol); - m.M21.Should().BeApproximately(0, tol); - m.M22.Should().BeApproximately(1, tol); - m.M23.Should().BeApproximately(0, tol); - m.M31.Should().BeApproximately(0, tol); - m.M32.Should().BeApproximately(0, tol); - m.M33.Should().BeApproximately(1, tol); - } - - private static void AssertMatricesEqual(Matrix3x3 a, Matrix3x3 b, double tol = Tol) - { - a.M11.Should().BeApproximately(b.M11, tol); - a.M12.Should().BeApproximately(b.M12, tol); - a.M13.Should().BeApproximately(b.M13, tol); - a.M21.Should().BeApproximately(b.M21, tol); - a.M22.Should().BeApproximately(b.M22, tol); - a.M23.Should().BeApproximately(b.M23, tol); - a.M31.Should().BeApproximately(b.M31, tol); - a.M32.Should().BeApproximately(b.M32, tol); - a.M33.Should().BeApproximately(b.M33, tol); - } - - #endregion -} +using System.Text.Json; +using FluentAssertions; +using ModelingEvolution.Drawing; + +namespace ModelingEvolution.Drawing.Tests; + +public class Matrix3x3Tests +{ + private const double Tol = 1e-10; + private const double TolLoose = 1e-6; + + #region Identity & Properties + + [Fact] + public void Identity_HasCorrectValues() + { + var m = Matrix3x3.Identity; + m.M11.Should().Be(1d); m.M12.Should().Be(0d); m.M13.Should().Be(0d); + m.M21.Should().Be(0d); m.M22.Should().Be(1d); m.M23.Should().Be(0d); + m.M31.Should().Be(0d); m.M32.Should().Be(0d); m.M33.Should().Be(1d); + } + + [Fact] + public void Zero_HasCorrectValues() + { + var m = Matrix3x3.Zero; + m.M11.Should().Be(0d); m.M12.Should().Be(0d); m.M13.Should().Be(0d); + m.M21.Should().Be(0d); m.M22.Should().Be(0d); m.M23.Should().Be(0d); + m.M31.Should().Be(0d); m.M32.Should().Be(0d); m.M33.Should().Be(0d); + } + + [Fact] + public void Determinant_Identity_IsOne() + { + Matrix3x3.Identity.Determinant.Should().Be(1d); + } + + [Fact] + public void Determinant_KnownMatrix() + { + // |1 2 3; 0 1 4; 5 6 0| = 1*(0-24) - 2*(0-20) + 3*(0-5) = -24+40-15 = 1 + var m = new Matrix3x3(1, 2, 3, 0, 1, 4, 5, 6, 0); + m.Determinant.Should().BeApproximately(1d, Tol); + } + + [Fact] + public void Trace_KnownMatrix() + { + var m = new Matrix3x3(3, 0, 0, 0, 5, 0, 0, 0, 7); + m.Trace.Should().BeApproximately(15d, Tol); + } + + #endregion + + #region Transpose + + [Fact] + public void Transpose_General() + { + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var t = m.Transpose(); + t.M11.Should().Be(1d); t.M12.Should().Be(4d); t.M13.Should().Be(7d); + t.M21.Should().Be(2d); t.M22.Should().Be(5d); t.M23.Should().Be(8d); + t.M31.Should().Be(3d); t.M32.Should().Be(6d); t.M33.Should().Be(9d); + } + + [Fact] + public void Transpose_RotationMatrix_IsInverse() + { + var r = Matrix3x3.RotationX(Degree.Create(30)); + var product = r * r.Transpose(); + AssertIsIdentity(product); + } + + #endregion + + #region Inverse + + [Fact] + public void Inverse_Identity() + { + var inv = Matrix3x3.Identity.Inverse(); + AssertIsIdentity(inv); + } + + [Fact] + public void Inverse_KnownMatrix() + { + var m = new Matrix3x3(1, 2, 3, 0, 1, 4, 5, 6, 0); + var inv = m.Inverse(); + var product = m * inv; + AssertIsIdentity(product); + } + + [Fact] + public void Inverse_Singular_Throws() + { + // Third row = first row → det = 0 + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 1, 2, 3); + var act = () => m.Inverse(); + act.Should().Throw(); + } + + #endregion + + #region Multiplication + + [Fact] + public void Multiplication_IdentityIsNeutral() + { + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var r = m * Matrix3x3.Identity; + AssertMatricesEqual(r, m); + } + + [Fact] + public void Multiplication_KnownResult() + { + var a = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var b = new Matrix3x3(9, 8, 7, 6, 5, 4, 3, 2, 1); + var c = a * b; + // Row 0: (1*9+2*6+3*3, 1*8+2*5+3*2, 1*7+2*4+3*1) = (30, 24, 18) + c.M11.Should().BeApproximately(30, Tol); + c.M12.Should().BeApproximately(24, Tol); + c.M13.Should().BeApproximately(18, Tol); + // Row 1: (4*9+5*6+6*3, 4*8+5*5+6*2, 4*7+5*4+6*1) = (84, 69, 54) + c.M21.Should().BeApproximately(84, Tol); + c.M22.Should().BeApproximately(69, Tol); + c.M23.Should().BeApproximately(54, Tol); + // Row 2: (7*9+8*6+9*3, 7*8+8*5+9*2, 7*7+8*4+9*1) = (138, 114, 90) + c.M31.Should().BeApproximately(138, Tol); + c.M32.Should().BeApproximately(114, Tol); + c.M33.Should().BeApproximately(90, Tol); + } + + [Fact] + public void Multiplication_Associativity() + { + var a = Matrix3x3.RotationX(Degree.Create(30)); + var b = Matrix3x3.RotationY(Degree.Create(45)); + var c = Matrix3x3.RotationZ(Degree.Create(60)); + var ab_c = (a * b) * c; + var a_bc = a * (b * c); + AssertMatricesEqual(ab_c, a_bc); + } + + #endregion + + #region Transform + + [Fact] + public void Transform_Vector_IdentityPreserves() + { + var v = new Vector3(1, 2, 3); + var r = Matrix3x3.Identity.Transform(v); + r.X.Should().BeApproximately(1, Tol); + r.Y.Should().BeApproximately(2, Tol); + r.Z.Should().BeApproximately(3, Tol); + } + + [Fact] + public void Transform_Vector_RotationX90_RotatesBasisVectors() + { + var m = Matrix3x3.RotationX(Degree.Create(90)); + // Rotating Y axis by 90° around X → Z axis + var ey = new Vector3(0, 1, 0); + var result = m * ey; + result.X.Should().BeApproximately(0, TolLoose); + result.Y.Should().BeApproximately(0, TolLoose); + result.Z.Should().BeApproximately(1, TolLoose); + } + + [Fact] + public void Transform_Point_OperatorSyntax() + { + var m = Matrix3x3.Scale(2, 3, 4); + var p = new Point3(1, 2, 3); + var r = m * p; + r.X.Should().BeApproximately(2, Tol); + r.Y.Should().BeApproximately(6, Tol); + r.Z.Should().BeApproximately(12, Tol); + } + + #endregion + + #region Rotation Factories + + [Fact] + public void RotationX_90_RotatesYToZ() + { + var m = Matrix3x3.RotationX(Degree.Create(90)); + var v = new Vector3(0, 1, 0); + var r = m * v; + r.X.Should().BeApproximately(0, TolLoose); + r.Y.Should().BeApproximately(0, TolLoose); + r.Z.Should().BeApproximately(1, TolLoose); + } + + [Fact] + public void RotationY_90_RotatesZToX() + { + var m = Matrix3x3.RotationY(Degree.Create(90)); + var v = new Vector3(0, 0, 1); + var r = m * v; + r.X.Should().BeApproximately(1, TolLoose); + r.Y.Should().BeApproximately(0, TolLoose); + r.Z.Should().BeApproximately(0, TolLoose); + } + + [Fact] + public void RotationZ_90_RotatesXToY() + { + var m = Matrix3x3.RotationZ(Degree.Create(90)); + var v = new Vector3(1, 0, 0); + var r = m * v; + r.X.Should().BeApproximately(0, TolLoose); + r.Y.Should().BeApproximately(1, TolLoose); + r.Z.Should().BeApproximately(0, TolLoose); + } + + [Fact] + public void RotationZYX_MatchesComposedRotations() + { + var rx = Degree.Create(30d); + var ry = Degree.Create(45d); + var rz = Degree.Create(60d); + var composed = Matrix3x3.RotationZ(rz) * Matrix3x3.RotationY(ry) * Matrix3x3.RotationX(rx); + var direct = Matrix3x3.RotationZYX(rx, ry, rz); + AssertMatricesEqual(composed, direct); + } + + [Fact] + public void RotationMatrix_IsOrthogonal() + { + var m = Matrix3x3.RotationZYX(Degree.Create(25), Degree.Create(35), Degree.Create(45)); + var product = m * m.Transpose(); + AssertIsIdentity(product); + } + + [Fact] + public void RotationMatrix_Determinant_IsOne() + { + var m = Matrix3x3.RotationZYX(Degree.Create(10), Degree.Create(20), Degree.Create(30)); + m.Determinant.Should().BeApproximately(1d, TolLoose); + } + + #endregion + + #region Scale & Construction Factories + + [Fact] + public void Scale_ScalesVector() + { + var m = Matrix3x3.Scale(2, 3, 4); + var v = new Vector3(1, 1, 1); + var r = m * v; + r.X.Should().BeApproximately(2, Tol); + r.Y.Should().BeApproximately(3, Tol); + r.Z.Should().BeApproximately(4, Tol); + } + + [Fact] + public void FromColumns_BuildsCorrectMatrix() + { + var c0 = new Vector3(1, 4, 7); + var c1 = new Vector3(2, 5, 8); + var c2 = new Vector3(3, 6, 9); + var m = Matrix3x3.FromColumns(c0, c1, c2); + m.M11.Should().Be(1); m.M12.Should().Be(2); m.M13.Should().Be(3); + m.M21.Should().Be(4); m.M22.Should().Be(5); m.M23.Should().Be(6); + m.M31.Should().Be(7); m.M32.Should().Be(8); m.M33.Should().Be(9); + } + + [Fact] + public void FromRows_BuildsCorrectMatrix() + { + var r0 = new Vector3(1, 2, 3); + var r1 = new Vector3(4, 5, 6); + var r2 = new Vector3(7, 8, 9); + var m = Matrix3x3.FromRows(r0, r1, r2); + m.M11.Should().Be(1); m.M12.Should().Be(2); m.M13.Should().Be(3); + m.M21.Should().Be(4); m.M22.Should().Be(5); m.M23.Should().Be(6); + m.M31.Should().Be(7); m.M32.Should().Be(8); m.M33.Should().Be(9); + } + + #endregion + + #region Euler Round-trip + + [Theory] + [InlineData(10, 20, 30)] + [InlineData(-15, 45, -60)] + [InlineData(0, 0, 90)] + [InlineData(180, 0, 0)] + [InlineData(5, 10, 15)] + public void RotationZYX_ToEulerZYX_RoundTrip(double rxd, double ryd, double rzd) + { + Degree rx = Degree.Create(rxd), ry = Degree.Create(ryd), rz = Degree.Create(rzd); + var m = Matrix3x3.RotationZYX(rx, ry, rz); + var (erx, ery, erz) = m.ToEulerZYX(); + // Rebuild matrix from extracted angles and compare + var m2 = Matrix3x3.RotationZYX(erx, ery, erz); + AssertMatricesEqual(m, m2); + } + + [Fact] + public void ToEulerZYX_GimbalLock_AtPositive90() + { + var m = Matrix3x3.RotationZYX(Degree.Create(0), Degree.Create(90), Degree.Create(0)); + var (rx, ry, rz) = m.ToEulerZYX(); + // ry should be ~90° + ((double)ry).Should().BeApproximately(90, TolLoose); + // At gimbal lock, rx and rz share DOF; matrix should still reconstruct + var m2 = Matrix3x3.RotationZYX(rx, ry, rz); + AssertMatricesEqual(m, m2, 1e-5); + } + + #endregion + + #region Quaternion Round-trip + + [Theory] + [InlineData(10, 20, 30)] + [InlineData(-30, 45, 60)] + [InlineData(0, 0, 0)] + [InlineData(5, -10, 15)] + public void ToQuaternion_FromQuaternion_RoundTrip(double rxd, double ryd, double rzd) + { + var m = Matrix3x3.RotationZYX(Degree.Create(rxd), Degree.Create(ryd), Degree.Create(rzd)); + var q = Quaternion.FromMatrix3x3(m); + // Convert quaternion back to rotation, then to matrix + var rot = Rotation3.FromQuaternion(q); + var m2 = rot.ToQuaternion().ToMatrix3x3(); + AssertMatricesEqual(m, m2, 1e-9); + } + + [Fact] + public void ToQuaternion_Identity_IsUnitQuaternion() + { + var q = Quaternion.FromMatrix3x3(Matrix3x3.Identity); + q.W.Should().BeApproximately(1, TolLoose); + q.X.Should().BeApproximately(0, TolLoose); + q.Y.Should().BeApproximately(0, TolLoose); + q.Z.Should().BeApproximately(0, TolLoose); + } + + #endregion + + #region Rotation3 Integration + + [Theory] + [InlineData(10, 20, 30)] + [InlineData(-15, 45, -60)] + [InlineData(0, 0, 0)] + public void Rotation3_QuaternionMatrix_RoundTrip(double rxd, double ryd, double rzd) + { + var rot = new Rotation3(rxd, ryd, rzd); + var m = rot.ToQuaternion().ToMatrix3x3(); + var rot2 = Rotation3.FromMatrix3x3(m); + ((double)rot2.Rx).Should().BeApproximately(rxd, TolLoose); + ((double)rot2.Ry).Should().BeApproximately(ryd, TolLoose); + ((double)rot2.Rz).Should().BeApproximately(rzd, TolLoose); + } + + [Fact] + public void Rotation3_QuaternionRotate_MatchesMatrix_Transform() + { + var rot = new Rotation3(30, 45, 60); + var m = Matrix3x3.RotationZYX(rot.Rx, rot.Ry, rot.Rz); + var v = new Vector3(1, 2, 3); + + var rotated = rot.ToQuaternion().Rotate(v); + var transformed = m.Transform(v); + + rotated.X.Should().BeApproximately(transformed.X, TolLoose); + rotated.Y.Should().BeApproximately(transformed.Y, TolLoose); + rotated.Z.Should().BeApproximately(transformed.Z, TolLoose); + } + + #endregion + + #region Scalar & Element-wise + + [Fact] + public void ScalarMultiplication() + { + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var r = m * 2d; + r.M11.Should().Be(2); r.M12.Should().Be(4); r.M13.Should().Be(6); + r.M21.Should().Be(8); r.M22.Should().Be(10); r.M23.Should().Be(12); + r.M31.Should().Be(14); r.M32.Should().Be(16); r.M33.Should().Be(18); + } + + [Fact] + public void ScalarMultiplication_Commutative() + { + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var r1 = m * 3d; + var r2 = 3d * m; + r1.Should().Be(r2); + } + + [Fact] + public void Addition() + { + var a = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var b = new Matrix3x3(9, 8, 7, 6, 5, 4, 3, 2, 1); + var c = a + b; + c.M11.Should().Be(10); c.M12.Should().Be(10); c.M13.Should().Be(10); + c.M21.Should().Be(10); c.M22.Should().Be(10); c.M23.Should().Be(10); + c.M31.Should().Be(10); c.M32.Should().Be(10); c.M33.Should().Be(10); + } + + [Fact] + public void Subtraction() + { + var a = new Matrix3x3(9, 8, 7, 6, 5, 4, 3, 2, 1); + var b = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var c = a - b; + c.M11.Should().Be(8); c.M12.Should().Be(6); c.M13.Should().Be(4); + c.M21.Should().Be(2); c.M22.Should().Be(0); c.M23.Should().Be(-2); + c.M31.Should().Be(-4); c.M32.Should().Be(-6); c.M33.Should().Be(-8); + } + + #endregion + + #region JSON Serialization + + [Fact] + public void Json_Serialize_Double() + { + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var json = JsonSerializer.Serialize(m); + json.Should().Be("[[1,2,3],[4,5,6],[7,8,9]]"); + } + + [Fact] + public void Json_Deserialize_Double() + { + var m = JsonSerializer.Deserialize>("[[1.5,2.5,3.5],[4.5,5.5,6.5],[7.5,8.5,9.5]]"); + m.M11.Should().Be(1.5); m.M12.Should().Be(2.5); m.M13.Should().Be(3.5); + m.M21.Should().Be(4.5); m.M22.Should().Be(5.5); m.M23.Should().Be(6.5); + m.M31.Should().Be(7.5); m.M32.Should().Be(8.5); m.M33.Should().Be(9.5); + } + + [Fact] + public void Json_RoundTrip_Double() + { + var original = new Matrix3x3(1.23, 4.56, 7.89, 0.12, 3.45, 6.78, 9.01, 2.34, 5.67); + var json = JsonSerializer.Serialize(original); + var deserialized = JsonSerializer.Deserialize>(json); + deserialized.Should().Be(original); + } + + [Fact] + public void Json_RoundTrip_Float() + { + var original = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var json = JsonSerializer.Serialize(original); + var deserialized = JsonSerializer.Deserialize>(json); + deserialized.Should().Be(original); + } + + [Fact] + public void Json_Identity_RoundTrip() + { + var original = Matrix3x3.Identity; + var json = JsonSerializer.Serialize(original); + json.Should().Be("[[1,0,0],[0,1,0],[0,0,1]]"); + var deserialized = JsonSerializer.Deserialize>(json); + deserialized.Should().Be(original); + } + + #endregion + + #region ToString + + [Fact] + public void ToString_Format() + { + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + m.ToString().Should().Be("[1, 2, 3; 4, 5, 6; 7, 8, 9]"); + } + + #endregion + + #region Helpers + + #region Quaternion ⇄ matrix bridge (epic-129 Q7: one Euler formula) + + private static Quaternion RandomRotation(Random rng) + { + // Uniform random unit quaternion (Shoemake) + double u1 = rng.NextDouble(), u2 = rng.NextDouble() * 2 * Math.PI, u3 = rng.NextDouble() * 2 * Math.PI; + double a = Math.Sqrt(1 - u1), b = Math.Sqrt(u1); + return new Quaternion(b * Math.Cos(u3), a * Math.Sin(u2), a * Math.Cos(u2), b * Math.Sin(u3)); + } + + [Fact] + public void QuaternionMatrix_RoundTrip_MatrixQuaternionMatrix_10000Random() + { + var rng = new Random(129); + for (int i = 0; i < 10_000; i++) + { + var m = RandomRotation(rng).ToMatrix3x3(); + var back = Quaternion.FromMatrix3x3(m).ToMatrix3x3(); + AssertMatricesEqual(back, m, 1e-12); + } + } + + [Fact] + public void QuaternionMatrix_FromMatrix3x3_AllShepperdBranches() + { + // trace > 0, then each diagonal element largest (rotations by π about X, Y, Z, and near-π mixes) + var cases = new[] + { + Quaternion.FromAxisAngle(new Vector3(1, 2, 3).Normalize(), 0.3), + Quaternion.FromAxisAngle(Vector3.EX, Math.PI), + Quaternion.FromAxisAngle(Vector3.EY, Math.PI), + Quaternion.FromAxisAngle(Vector3.EZ, Math.PI), + Quaternion.FromAxisAngle(new Vector3(1, 1, 0).Normalize(), Math.PI - 1e-9), + }; + foreach (var q in cases) + { + var m = q.ToMatrix3x3(); + var back = Quaternion.FromMatrix3x3(m); + Math.Abs(Quaternion.Dot(back, q)).Should().BeApproximately(1, 1e-12); + AssertMatricesEqual(back.ToMatrix3x3(), m, 1e-12); + } + } + + [Fact] + public void QuaternionMatrix_ToMatrix3x3_ColumnsAreRotatedAxes() + { + var rng = new Random(129); + for (int i = 0; i < 1_000; i++) + { + var q = RandomRotation(rng); + var expected = Matrix3x3.FromColumns(q.Rotate(Vector3.EX), q.Rotate(Vector3.EY), q.Rotate(Vector3.EZ)); + AssertMatricesEqual(q.ToMatrix3x3(), expected, 1e-12); + } + } + + // Master's (1.13) closed-form RotationZYX, kept here as the reference the bridge must not move from. + private static Matrix3x3 MasterRotationZYX(double rxDeg, double ryDeg, double rzDeg) + { + double rx = rxDeg * Math.PI / 180, ry = ryDeg * Math.PI / 180, rz = rzDeg * Math.PI / 180; + double cRx = Math.Cos(rx), sRx = Math.Sin(rx), cRy = Math.Cos(ry), sRy = Math.Sin(ry), cRz = Math.Cos(rz), sRz = Math.Sin(rz); + return new( + cRy * cRz, sRx * sRy * cRz - cRx * sRz, cRx * sRy * cRz + sRx * sRz, + cRy * sRz, sRx * sRy * sRz + cRx * cRz, cRx * sRy * sRz - sRx * cRz, + -sRy, sRx * cRy, cRx * cRy); + } + + [Fact] + public void RotationZYX_EqualsMasterClosedForm_10000Random() + { + var rng = new Random(129); + for (int i = 0; i < 10_000; i++) + { + double rx = rng.NextDouble() * 360 - 180, ry = rng.NextDouble() * 180 - 90, rz = rng.NextDouble() * 360 - 180; + var m = Matrix3x3.RotationZYX(Degree.Create(rx), Degree.Create(ry), Degree.Create(rz)); + AssertMatricesEqual(m, MasterRotationZYX(rx, ry, rz), 1e-12); + } + } + + [Fact] + public void ToEulerZYX_EqualsMasterAngles_OutsideGimbalBand_10000Random() + { + // Master extracted rx = atan2(M32, M33), ry = asin(-M31), rz = atan2(M21, M11) when |cos ry| > 1e-6. + var rng = new Random(129); + for (int i = 0; i < 10_000; i++) + { + double rx = rng.NextDouble() * 358 - 179, ry = rng.NextDouble() * 178 - 89, rz = rng.NextDouble() * 358 - 179; + var m = MasterRotationZYX(rx, ry, rz); + var (erx, ery, erz) = m.ToEulerZYX(); + ((double)erx).Should().BeApproximately(Math.Atan2(m.M32, m.M33) * 180 / Math.PI, 1e-9); + ((double)ery).Should().BeApproximately(Math.Asin(-m.M31) * 180 / Math.PI, 1e-9); + ((double)erz).Should().BeApproximately(Math.Atan2(m.M21, m.M11) * 180 / Math.PI, 1e-9); + } + } + + #endregion + + private static void AssertIsIdentity(Matrix3x3 m, double tol = Tol) + { + m.M11.Should().BeApproximately(1, tol); + m.M12.Should().BeApproximately(0, tol); + m.M13.Should().BeApproximately(0, tol); + m.M21.Should().BeApproximately(0, tol); + m.M22.Should().BeApproximately(1, tol); + m.M23.Should().BeApproximately(0, tol); + m.M31.Should().BeApproximately(0, tol); + m.M32.Should().BeApproximately(0, tol); + m.M33.Should().BeApproximately(1, tol); + } + + private static void AssertMatricesEqual(Matrix3x3 a, Matrix3x3 b, double tol = Tol) + { + a.M11.Should().BeApproximately(b.M11, tol); + a.M12.Should().BeApproximately(b.M12, tol); + a.M13.Should().BeApproximately(b.M13, tol); + a.M21.Should().BeApproximately(b.M21, tol); + a.M22.Should().BeApproximately(b.M22, tol); + a.M23.Should().BeApproximately(b.M23, tol); + a.M31.Should().BeApproximately(b.M31, tol); + a.M32.Should().BeApproximately(b.M32, tol); + a.M33.Should().BeApproximately(b.M33, tol); + } + + #endregion +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/PosePath3BuilderTests.cs b/Sources/ModelingEvolution.Drawing.Tests/PosePath3BuilderTests.cs index 8a8f57c..f843c6f 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/PosePath3BuilderTests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/PosePath3BuilderTests.cs @@ -30,85 +30,45 @@ public void StartAt_CalledTwice_Throws() } [Fact] - public void Delta_BeforeStartAt_Throws() + public void Step_BeforeStartAt_Throws() { var b = new PosePath3Builder(); - Action act = () => b.Delta(new Vector3(1, 0, 0), Rotation3.Identity); + Action act = () => b.Step(new Frame3(new Point3(1, 0, 0), Quaternion.Identity)); act.Should().Throw(); } [Fact] - public void Delta_AppendsTranslationToPreviousPosition() + public void Step_ComposesInThePreviousPoseFrame() { - var start = new Pose3(100, 50, 200, 0, 0, 0); + // Start rotated 90° about Z: a +X step in the pose's frame moves +Y in the world, and the rotations compose. + var step = new Frame3(new Point3(10, 0, 0), Quaternion.FromAxisAngle(Vector3.EZ, Math.PI / 2)); var path = new PosePath3Builder() - .StartAt(start) - .Delta(new Vector3(10, 0, 0), Rotation3.Identity) + .StartAt(new Pose3(100, 50, 200, 0, 0, 90)) + .Step(step) .Build(); path.Count.Should().Be(2); - path[1].X.Should().Be(110); - path[1].Y.Should().Be(50); - path[1].Z.Should().Be(200); + path[1].X.Should().BeApproximately(100, 1e-9); + path[1].Y.Should().BeApproximately(60, 1e-9); + path[1].Z.Should().BeApproximately(200, 1e-9); + ((double)path[1].Rz).Should().BeApproximately(180, 1e-9); } [Fact] - public void Delta_AppendsRotationToPreviousRotation() - { - var start = new Pose3(0, 0, 0, 10, 20, 30); - var path = new PosePath3Builder() - .StartAt(start) - .Delta(Vector3.Zero, new Rotation3(1, 2, 3)) - .Build(); - - path[1].Rx.Should().Be((Degree)11.0); - path[1].Ry.Should().Be((Degree)22.0); - path[1].Rz.Should().Be((Degree)33.0); - } - - [Fact] - public void Chain_OfMultipleDeltas_AccumulatesCorrectly() - { - var path = new PosePath3Builder() - .StartAt(new Pose3(0, 0, 0, 0, 0, 0)) - .Delta([10, 0, 0], [0, 0, 0]) - .Delta([10, 0, 0], [0, 0, 0]) - .Delta([10, 0, 0], [0, 0, 0]) - .Build(); - - path.Count.Should().Be(4); - path[0].Position.Should().Be(new Point3(0, 0, 0)); - path[1].Position.Should().Be(new Point3(10, 0, 0)); - path[2].Position.Should().Be(new Point3(20, 0, 0)); - path[3].Position.Should().Be(new Point3(30, 0, 0)); - } - - [Fact] - public void Construct_WithZeroInitialCapacity_DoesNotInfiniteLoop() + public void Step_Chain_FromZeroCapacityAndLiteralStart_Grows() { // Regression guard: doubling from 0 would never grow; constructor must floor at 1. - var builder = new PosePath3Builder(0); - var path = builder - .StartAt(new Pose3(0, 0, 0, 0, 0, 0)) - .Delta(new Vector3(1, 0, 0), Rotation3.Identity) - .Delta(new Vector3(1, 0, 0), Rotation3.Identity) - .Build(); - - path.Count.Should().Be(3); - } - - [Fact] - public void CollectionLiteralSyntax_CompilesAndProducesCorrectResult() - { - // The headline use case: code-generated programs use [x,y,z] literals - var path = new PosePath3Builder() + var step = new Frame3(new Point3(10, 0, 0), Quaternion.Identity); + var path = new PosePath3Builder(0) .StartAt([120, 50, 200, 180, 0, 0]) - .Delta([10, 0, 0], [0, 0, 0]) - .Delta([10, 0, 0], [0, 0, 0]) + .Step(step) + .Step(step) .Build(); path.Count.Should().Be(3); path[0].Position.Should().Be(new Point3(120, 50, 200)); - path[^1].Position.Should().Be(new Point3(140, 50, 200)); + path[^1].X.Should().BeApproximately(140, 1e-9); + path[^1].Y.Should().BeApproximately(50, 1e-9); + path[^1].Z.Should().BeApproximately(200, 1e-9); } } diff --git a/Sources/ModelingEvolution.Drawing.Tests/Rotation3SurfaceTests.cs b/Sources/ModelingEvolution.Drawing.Tests/Rotation3SurfaceTests.cs new file mode 100644 index 0000000..4aab1c1 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Rotation3SurfaceTests.cs @@ -0,0 +1,80 @@ +using System.Reflection; +using FluentAssertions; + +namespace ModelingEvolution.Drawing.Tests; + +/// +/// Epic-129 Q7: is a wire and edit format only, like . Its rotation +/// algebra is deleted; rotation maths uses or . A new member +/// fails this test until the kept list is changed on purpose. +/// +public class Rotation3SurfaceTests +{ + private const BindingFlags All = BindingFlags.Public | BindingFlags.NonPublic | BindingFlags.Static | BindingFlags.Instance | BindingFlags.DeclaredOnly; + + [Theory] + [InlineData("Combine")] + [InlineData("op_Addition")] + [InlineData("Inverse")] + [InlineData("op_UnaryNegation")] + [InlineData("Slerp")] + [InlineData("Rotate")] + [InlineData("ToMatrix3x3")] + [InlineData("op_Explicit")] + [InlineData("Negate")] + public void DeletedMathsMember_DoesNotExist(string name) + { + typeof(Rotation3).GetMembers(All).Where(m => m.Name == name).Should().BeEmpty(); + } + + [Fact] + public void PublicMembers_AreExactlyTheWireAndEditSet() + { + var actual = typeof(Rotation3) + .GetMembers(BindingFlags.Public | BindingFlags.Static | BindingFlags.Instance | BindingFlags.DeclaredOnly) + .Select(Describe) + .OrderBy(s => s, StringComparer.Ordinal) + .ToArray(); + + string[] kept = + [ + // constructors and named constructors + ".ctor(Degree`1, Degree`1, Degree`1)", + "FromDegrees(Double, Double, Double)", + "FromRadians(Radian`1, Radian`1, Radian`1)", + // identity + "Identity", + "IsIdentity", "get_IsIdentity()", + // angles (init-only for `with`) + "Rx", "get_Rx()", "set_Rx(Degree`1)", + "Ry", "get_Ry()", "set_Ry(Degree`1)", + "Rz", "get_Rz()", "set_Rz(Degree`1)", + "ToRadians()", + // collection-expression literals ([rx, ry, rz]) + "Enumerator", "GetEnumerator()", + // the conversions to and from the maths types (one Euler formula each way) + "ToQuaternion()", + "FromQuaternion(Quaternion`1)", + "FromMatrix3x3(Matrix3x3`1)", + // parsing and formatting + "Parse(String, IFormatProvider)", + "TryParse(String, IFormatProvider, Rotation3`1&)", + "ToString()", + // tuple conversions + "op_Implicit(ValueTuple`3)", + "op_Implicit(Rotation3`1)", + // equality + "Equals(Object)", "Equals(Rotation3`1)", "GetHashCode()", "op_Equality(Rotation3`1, Rotation3`1)", "op_Inequality(Rotation3`1, Rotation3`1)", + // numeric type conversion + "Truncating()", + ]; + + actual.Should().Equal(kept.OrderBy(s => s, StringComparer.Ordinal)); + } + + private static string Describe(MemberInfo m) => m switch + { + MethodBase mb => $"{mb.Name}({string.Join(", ", mb.GetParameters().Select(p => p.ParameterType.Name))})", + _ => m.Name, + }; +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Rotation3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Rotation3Tests.cs index 9cae39d..123a242 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Rotation3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Rotation3Tests.cs @@ -131,12 +131,12 @@ public void Rotation3_QuaternionConversion_RoundTrips() #region Rotation Operations [Fact] - public void Rotation3_Rotate_XAxis90_RotatesYToZ() + public void Rotation3_ToQuaternion_Rotate_XAxis90_RotatesYToZ() { var rotation = new Rotation3(90, 0, 0); // 90 degrees around X var v = new Vector3(0, 1, 0); // Y unit vector - var rotated = rotation.Rotate(v); + var rotated = rotation.ToQuaternion().Rotate(v); rotated.X.Should().BeApproximately(0, Tolerance); rotated.Y.Should().BeApproximately(0, Tolerance); @@ -144,12 +144,12 @@ public void Rotation3_Rotate_XAxis90_RotatesYToZ() } [Fact] - public void Rotation3_Rotate_YAxis90_RotatesZToX() + public void Rotation3_ToQuaternion_Rotate_YAxis90_RotatesZToX() { var rotation = new Rotation3(0, 90, 0); // 90 degrees around Y var v = new Vector3(0, 0, 1); // Z unit vector - var rotated = rotation.Rotate(v); + var rotated = rotation.ToQuaternion().Rotate(v); rotated.X.Should().BeApproximately(1, Tolerance); rotated.Y.Should().BeApproximately(0, Tolerance); @@ -157,12 +157,12 @@ public void Rotation3_Rotate_YAxis90_RotatesZToX() } [Fact] - public void Rotation3_Rotate_ZAxis90_RotatesXToY() + public void Rotation3_ToQuaternion_Rotate_ZAxis90_RotatesXToY() { var rotation = new Rotation3(0, 0, 90); // 90 degrees around Z var v = new Vector3(1, 0, 0); // X unit vector - var rotated = rotation.Rotate(v); + var rotated = rotation.ToQuaternion().Rotate(v); rotated.X.Should().BeApproximately(0, Tolerance); rotated.Y.Should().BeApproximately(1, Tolerance); @@ -170,12 +170,12 @@ public void Rotation3_Rotate_ZAxis90_RotatesXToY() } [Fact] - public void Rotation3_RotatePoint_Works() + public void Rotation3_ToQuaternion_FrameRotatesPoint() { var rotation = new Rotation3(0, 0, 90); var point = new Point3(1, 0, 0); - var rotated = rotation.Rotate(point); + var rotated = new Frame3(Point3.Zero, rotation.ToQuaternion()).TransformPoint(point); rotated.X.Should().BeApproximately(0, Tolerance); rotated.Y.Should().BeApproximately(1, Tolerance); @@ -187,12 +187,12 @@ public void Rotation3_RotatePoint_Works() #region Inverse [Fact] - public void Rotation3_Inverse_ReversesRotation() + public void Rotation3_ToQuaternion_InverseReversesRotation() { var rotation = new Rotation3(30, 45, 60); var v = new Vector3(1, 2, 3); - var rotated = rotation.Rotate(v); + var rotated = rotation.ToQuaternion().Rotate(v); var q = rotation.ToQuaternion(); var qInverse = q.Inverse(); var backToOriginal = qInverse.Rotate(rotated); @@ -207,14 +207,14 @@ public void Rotation3_Inverse_ReversesRotation() #region Combine [Fact] - public void Rotation3_Combine_TwoRotations() + public void Rotation3_ToQuaternion_ProductOfTwoRotations() { var r1 = new Rotation3(90, 0, 0); var r2 = new Rotation3(0, 90, 0); var v = new Vector3(0, 0, 1); - // Combine applies r2 first, then r1 (quaternion convention) - var combined = Rotation3.Combine(r1, r2); + // r1 · r2 applies r2 first, then r1 (quaternion convention) + var combined = r1.ToQuaternion() * r2.ToQuaternion(); var rotated = combined.Rotate(v); // Verify combined rotation produces valid result @@ -226,34 +226,34 @@ public void Rotation3_Combine_TwoRotations() #region Slerp [Fact] - public void Rotation3_Slerp_AtZero_ReturnsFirst() + public void Rotation3_QuaternionSlerp_AtZero_ReturnsFirst() { var a = new Rotation3(0, 0, 0); var b = new Rotation3(90, 0, 0); - var result = Rotation3.Slerp(a, b, 0); + var result = Rotation3.FromQuaternion(Quaternion.Slerp(a.ToQuaternion(), b.ToQuaternion(), 0)); ((double)result.Rx).Should().BeApproximately(0, 1e-6); } [Fact] - public void Rotation3_Slerp_AtOne_ReturnsSecond() + public void Rotation3_QuaternionSlerp_AtOne_ReturnsSecond() { var a = new Rotation3(0, 0, 0); var b = new Rotation3(90, 0, 0); - var result = Rotation3.Slerp(a, b, 1); + var result = Rotation3.FromQuaternion(Quaternion.Slerp(a.ToQuaternion(), b.ToQuaternion(), 1)); ((double)result.Rx).Should().BeApproximately(90, 1e-6); } [Fact] - public void Rotation3_Slerp_AtHalf_ReturnsMidpoint() + public void Rotation3_QuaternionSlerp_AtHalf_ReturnsMidpoint() { var a = new Rotation3(0, 0, 0); var b = new Rotation3(90, 0, 0); - var result = Rotation3.Slerp(a, b, 0.5); + var result = Rotation3.FromQuaternion(Quaternion.Slerp(a.ToQuaternion(), b.ToQuaternion(), 0.5)); ((double)result.Rx).Should().BeApproximately(45, 1e-6); } @@ -274,31 +274,22 @@ public void Rotation3_Equality_Works() } [Fact] - public void Rotation3_UnaryMinus_ReturnsInverse() + public void Rotation3_QuaternionConjugate_IsInverse() { var rotation = new Rotation3(10, 20, 30); - var result = Rotation3.Combine(rotation, -rotation); + var q = rotation.ToQuaternion(); + var result = Rotation3.FromQuaternion(q * q.Conjugate()); ((double)result.Rx).Should().BeApproximately(0, 1e-9); ((double)result.Ry).Should().BeApproximately(0, 1e-9); ((double)result.Rz).Should().BeApproximately(0, 1e-9); } [Fact] - public void Rotation3_Negate_NegatesAngles() - { - var rotation = new Rotation3(10, 20, 30); - var negated = rotation.Negate(); - negated.Rx.Should().Be(-10); - negated.Ry.Should().Be(-20); - negated.Rz.Should().Be(-30); - } - - [Fact] - public void Rotation3_Inverse_CombinesWithOriginalToIdentity() + public void Rotation3_QuaternionInverse_ComposesToIdentity() { var rotation = new Rotation3(30, 45, 60); - var inverse = rotation.Inverse(); - var result = Rotation3.Combine(rotation, inverse); + var q = rotation.ToQuaternion(); + var result = Rotation3.FromQuaternion(q * q.Inverse()); ((double)result.Rx).Should().BeApproximately(0, 1e-9); ((double)result.Ry).Should().BeApproximately(0, 1e-9); ((double)result.Rz).Should().BeApproximately(0, 1e-9); @@ -332,7 +323,7 @@ public void Rotation3_ToVector3_Identity_ReturnsZAxis() { var identity = Rotation3.Identity; - var direction = (Vector3)identity; + var direction = identity.ToQuaternion().Rotate(Vector3.EZ); // Identity rotation applied to +Z should give +Z direction.X.Should().BeApproximately(0, 1e-10); @@ -347,7 +338,7 @@ public void Rotation3_ToVector3_90DegYaw_PointsToY() // Let's use a pitch rotation instead var rotation = new Rotation3(0, 90, 0); // 90 deg pitch around Y - var direction = (Vector3)rotation; + var direction = rotation.ToQuaternion().Rotate(Vector3.EZ); // +Z rotated 90 deg around Y axis goes to +X direction.X.Should().BeApproximately(1, 1e-6); @@ -360,7 +351,7 @@ public void Rotation3_ToVector3_ReturnsUnitVector() { var rotation = new Rotation3(30, 45, 60); - var direction = (Vector3)rotation; + var direction = rotation.ToQuaternion().Rotate(Vector3.EZ); direction.Length.Should().BeApproximately(1, 1e-10); } @@ -372,7 +363,7 @@ public void Rotation3_ToVector3_RoundTrips_WithVector3() var originalDir = new Vector3(1, 1, 1).Normalize(); var rotation = (Rotation3)originalDir; - var backToDir = (Vector3)rotation; + var backToDir = rotation.ToQuaternion().Rotate(Vector3.EZ); backToDir.X.Should().BeApproximately(originalDir.X, 1e-6); backToDir.Y.Should().BeApproximately(originalDir.Y, 1e-6); diff --git a/Sources/ModelingEvolution.Drawing.Tests/TorchRotationTests.cs b/Sources/ModelingEvolution.Drawing.Tests/TorchRotationTests.cs index 7e8d29b..20bd117 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/TorchRotationTests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/TorchRotationTests.cs @@ -7,6 +7,10 @@ public class TorchRotationTests { private const float Tol = 1e-3f; + // Rotation3 is a wire format (2.0): the slerp is a quaternion slerp, converted back for the log. + private static Rotation3 Slerp(Rotation3 a, Rotation3 b, float t) => + Rotation3.FromQuaternion(Quaternion.Slerp(a.ToQuaternion(), b.ToQuaternion(), t)); + /// /// Print what Rotate(EZ) actually produces for various Euler angles. /// This tells us the real convention. @@ -32,7 +36,7 @@ public void PrintForwardDirections() foreach (var (name, rx, ry, rz) in cases) { var rot = new Rotation3(rx, ry, rz); - var fwd = rot.Rotate(Vector3.EZ); + var fwd = rot.ToQuaternion().Rotate(Vector3.EZ); Console.WriteLine($" {name,-20} Rotate(EZ) = ({fwd.X:F4}, {fwd.Y:F4}, {fwd.Z:F4})"); } @@ -44,7 +48,7 @@ public void PrintForwardDirections() for (int ry = -90; ry <= 90; ry += 15) { var rot = new Rotation3(rx, ry, 0); - var fwd = rot.Rotate(Vector3.EZ); + var fwd = rot.ToQuaternion().Rotate(Vector3.EZ); if (MathF.Abs(fwd.X - 0.707f) < 0.1f && MathF.Abs(fwd.Y + 0.707f) < 0.1f && MathF.Abs(fwd.Z) < 0.1f) Console.WriteLine($" Rx={rx}, Ry={ry} → ({fwd.X:F3}, {fwd.Y:F3}, {fwd.Z:F3})"); } @@ -55,7 +59,7 @@ public void PrintForwardDirections() for (int ry = -90; ry <= 90; ry += 15) { var rot = new Rotation3(rx, ry, 0); - var fwd = rot.Rotate(Vector3.EZ); + var fwd = rot.ToQuaternion().Rotate(Vector3.EZ); if (MathF.Abs(fwd.X + 0.707f) < 0.1f && MathF.Abs(fwd.Y + 0.707f) < 0.1f && MathF.Abs(fwd.Z) < 0.1f) Console.WriteLine($" Rx={rx}, Ry={ry} → ({fwd.X:F3}, {fwd.Y:F3}, {fwd.Z:F3})"); } @@ -66,7 +70,7 @@ public void PrintForwardDirections() for (int ry = -90; ry <= 90; ry += 15) { var rot = new Rotation3(rx, ry, 0); - var fwd = rot.Rotate(Vector3.EZ); + var fwd = rot.ToQuaternion().Rotate(Vector3.EZ); if (MathF.Abs(fwd.X) < 0.1f && MathF.Abs(fwd.Y + 0.707f) < 0.1f && MathF.Abs(fwd.Z + 0.707f) < 0.1f) Console.WriteLine($" Rx={rx}, Ry={ry} → ({fwd.X:F3}, {fwd.Y:F3}, {fwd.Z:F3})"); } @@ -76,7 +80,7 @@ public void PrintForwardDirections() public void DownRot_ForwardIsNegativeY() { var rot = new Rotation3(90f, 0f, 0f); - var fwd = rot.Rotate(Vector3.EZ); + var fwd = rot.ToQuaternion().Rotate(Vector3.EZ); fwd.X.Should().BeApproximately(0f, Tol); fwd.Y.Should().BeApproximately(-1f, Tol); fwd.Z.Should().BeApproximately(0f, Tol); @@ -86,7 +90,7 @@ public void DownRot_ForwardIsNegativeY() public void Edge2_PlusZ_ForwardIs_ZDown() { var rot = new Rotation3(45f, 0f, 0f); - var fwd = rot.Rotate(Vector3.EZ); + var fwd = rot.ToQuaternion().Rotate(Vector3.EZ); fwd.X.Should().BeApproximately(0f, Tol); fwd.Y.Should().BeApproximately(-0.707f, Tol); fwd.Z.Should().BeApproximately(0.707f, Tol); @@ -125,9 +129,9 @@ public void SampleLPathTrajectory_PrintForwardAtEachPoint() var downRot = new Rotation3(90f, 0f, 0f); // straight down // Verify the raw rotations first - var e1fwd = edge1Rot.Rotate(Vector3.EZ); - var e2fwd = edge2Rot.Rotate(Vector3.EZ); - var dfwd = downRot.Rotate(Vector3.EZ); + var e1fwd = edge1Rot.ToQuaternion().Rotate(Vector3.EZ); + var e2fwd = edge2Rot.ToQuaternion().Rotate(Vector3.EZ); + var dfwd = downRot.ToQuaternion().Rotate(Vector3.EZ); log.WriteLine($"edge1Rot(45,0,0) fwd=({e1fwd.X:F3}, {e1fwd.Y:F3}, {e1fwd.Z:F3}) // should be (0, -0.707, +0.707)"); log.WriteLine($"edge2Rot(45,-90,0) fwd=({e2fwd.X:F3}, {e2fwd.Y:F3}, {e2fwd.Z:F3}) // should be (-0.707, -0.707, 0)"); log.WriteLine($"downRot(90,0,0) fwd=({dfwd.X:F3}, {dfwd.Y:F3}, {dfwd.Z:F3})"); @@ -153,7 +157,7 @@ public void SampleLPathTrajectory_PrintForwardAtEachPoint() for (int i = 0; i < span.Length; i++) { var wp = span[i]; - var fwd = wp.Pose.Rotation.Rotate(Vector3.EZ); + var fwd = wp.Pose.Rotation.ToQuaternion().Rotate(Vector3.EZ); log.WriteLine($" WP[{i,2}] t={wp.Time:F3}s pos=({wp.Position.X:F1}, {wp.Position.Y:F1}, {wp.Position.Z:F1}) " + $"rot=({wp.Pose.Rotation.Rx:F1}, {wp.Pose.Rotation.Ry:F1}, {wp.Pose.Rotation.Rz:F1}) " + $"fwd=({fwd.X:F3}, {fwd.Y:F3}, {fwd.Z:F3})"); @@ -166,7 +170,7 @@ public void SampleLPathTrajectory_PrintForwardAtEachPoint() for (float t = 0f; t <= duration + 0.01f; t += 0.25f) { var pose = trajectory.AtTime(t0 + t); - var fwd = pose.Rotation.Rotate(Vector3.EZ); + var fwd = pose.Rotation.ToQuaternion().Rotate(Vector3.EZ); float angleFromVert = MathF.Acos(MathF.Min(1f, MathF.Abs(fwd.Y))) * 180f / MathF.PI; log.WriteLine($" t={t:F2}s pos=({pose.X:F1}, {pose.Y:F1}, {pose.Z:F1}) " + $"fwd=({fwd.X:F3}, {fwd.Y:F3}, {fwd.Z:F3}) " + @@ -179,7 +183,7 @@ public void SampleLPathTrajectory_PrintForwardAtEachPoint() var wp2 = span[2]; float midEdge1 = (wp1.Time + wp2.Time) / 2f; var midPose = trajectory.AtTime(midEdge1); - var midFwd = midPose.Rotation.Rotate(Vector3.EZ); + var midFwd = midPose.Rotation.ToQuaternion().Rotate(Vector3.EZ); log.WriteLine($" WP[1].Time={wp1.Time:F3} WP[2].Time={wp2.Time:F3} mid={midEdge1:F3}"); log.WriteLine($" Mid-edge1 pos=({midPose.X:F1}, {midPose.Y:F1}, {midPose.Z:F1}) " + $"fwd=({midFwd.X:F3}, {midFwd.Y:F3}, {midFwd.Z:F3})"); @@ -194,15 +198,15 @@ public void SampleLPathTrajectory_PrintForwardAtEachPoint() // Assert no NaN in any waypoint for (int i = 0; i < span.Length; i++) { - var wpFwd = span[i].Pose.Rotation.Rotate(Vector3.EZ); + var wpFwd = span[i].Pose.Rotation.ToQuaternion().Rotate(Vector3.EZ); float.IsNaN(wpFwd.X).Should().BeFalse($"WP[{i}] should not have NaN rotation"); } // Check: is the Slerp between two identical rotations still the same rotation? log.WriteLine(); log.WriteLine("=== Slerp test: edge1Rot to edge1Rot at t=0.5 ==="); - var slerped = Rotation3.Slerp(edge1Rot, edge1Rot, 0.5f); - var slerpFwd = slerped.Rotate(Vector3.EZ); + var slerped = Slerp(edge1Rot, edge1Rot, 0.5f); + var slerpFwd = slerped.ToQuaternion().Rotate(Vector3.EZ); log.WriteLine($" Slerp result: rot=({slerped.Rx:F1}, {slerped.Ry:F1}, {slerped.Rz:F1}) fwd=({slerpFwd.X:F3}, {slerpFwd.Y:F3}, {slerpFwd.Z:F3})"); // Check: Slerp between downRot and edge1Rot at various t @@ -210,8 +214,8 @@ public void SampleLPathTrajectory_PrintForwardAtEachPoint() log.WriteLine("=== Slerp test: downRot(90,0,0) → edge1Rot(45,90,0) ==="); for (float t = 0f; t <= 1.01f; t += 0.1f) { - var s = Rotation3.Slerp(downRot, edge1Rot, t); - var sf = s.Rotate(Vector3.EZ); + var s = Slerp(downRot, edge1Rot, t); + var sf = s.ToQuaternion().Rotate(Vector3.EZ); log.WriteLine($" t={t:F1} rot=({s.Rx:F1}, {s.Ry:F1}, {s.Rz:F1}) fwd=({sf.X:F3}, {sf.Y:F3}, {sf.Z:F3})"); } @@ -227,10 +231,10 @@ public void SampleLPathTrajectory_PrintForwardAtEachPoint() log.WriteLine($" rotFracIn={rotFracIn:F4}, rotFracOut={rotFracOut:F4}"); - var rotP1 = Rotation3.Slerp(edge1Rot, edge2Rot, rotFracIn); - var rotP2 = Rotation3.Slerp(edge2Rot, edge2Rot, rotFracOut); - var p1fwd = rotP1.Rotate(Vector3.EZ); - var p2fwd = rotP2.Rotate(Vector3.EZ); + var rotP1 = Slerp(edge1Rot, edge2Rot, rotFracIn); + var rotP2 = Slerp(edge2Rot, edge2Rot, rotFracOut); + var p1fwd = rotP1.ToQuaternion().Rotate(Vector3.EZ); + var p2fwd = rotP2.ToQuaternion().Rotate(Vector3.EZ); log.WriteLine($" rotP1 = ({rotP1.Rx:F1}, {rotP1.Ry:F1}, {rotP1.Rz:F1}) fwd=({p1fwd.X:F3}, {p1fwd.Y:F3}, {p1fwd.Z:F3})"); log.WriteLine($" rotP2 = ({rotP2.Rx:F1}, {rotP2.Ry:F1}, {rotP2.Rz:F1}) fwd=({p2fwd.X:F3}, {p2fwd.Y:F3}, {p2fwd.Z:F3})"); @@ -239,8 +243,8 @@ public void SampleLPathTrajectory_PrintForwardAtEachPoint() for (int j = 0; j < arcPointCount; j++) { float at = (float)j / (arcPointCount - 1); - var arcRot = Rotation3.Slerp(rotP1, rotP2, at); - var af = arcRot.Rotate(Vector3.EZ); + var arcRot = Slerp(rotP1, rotP2, at); + var af = arcRot.ToQuaternion().Rotate(Vector3.EZ); log.WriteLine($" arc[{j}] t={at:F3} rot=({arcRot.Rx:F1}, {arcRot.Ry:F1}, {arcRot.Rz:F1}) fwd=({af.X:F3}, {af.Y:F3}, {af.Z:F3})"); } diff --git a/Sources/ModelingEvolution.Drawing.Tests/Vector3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Vector3Tests.cs index 57018a1..210fd28 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Vector3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Vector3Tests.cs @@ -257,7 +257,7 @@ public void Vector3_RotationTo_EZtoEX_Rotates90DegreesAroundY() { var rotation = Vector3.EZ.RotationTo(Vector3.EX); - var result = rotation.Rotate(Vector3.EZ); + var result = rotation.ToQuaternion().Rotate(Vector3.EZ); result.X.Should().BeApproximately(1, 1e-6); result.Y.Should().BeApproximately(0, 1e-6); @@ -269,7 +269,7 @@ public void Vector3_RotationTo_EZtoEY_Rotates90DegreesAroundX() { var rotation = Vector3.EZ.RotationTo(Vector3.EY); - var result = rotation.Rotate(Vector3.EZ); + var result = rotation.ToQuaternion().Rotate(Vector3.EZ); result.X.Should().BeApproximately(0, 1e-6); result.Y.Should().BeApproximately(1, 1e-6); @@ -289,7 +289,7 @@ public void Vector3_RotationTo_AntiParallelVectors_Rotates180Degrees() { var rotation = Vector3.EZ.RotationTo(-Vector3.EZ); - var result = rotation.Rotate(Vector3.EZ); + var result = rotation.ToQuaternion().Rotate(Vector3.EZ); result.X.Should().BeApproximately(0, 1e-6); result.Y.Should().BeApproximately(0, 1e-6); @@ -303,7 +303,7 @@ public void Vector3_RotationTo_ArbitraryVectors_AlignsCorrectly() var to = new Vector3(-2, 1, 0.5).Normalize(); var rotation = from.RotationTo(to); - var result = rotation.Rotate(from); + var result = rotation.ToQuaternion().Rotate(from); result.X.Should().BeApproximately(to.X, 1e-6); result.Y.Should().BeApproximately(to.Y, 1e-6); @@ -317,7 +317,7 @@ public void Vector3_RotationTo_NonUnitVectors_NormalizesFirst() var to = new Vector3(10, 0, 0); // length 10 var rotation = from.RotationTo(to); - var result = rotation.Rotate(Vector3.EZ); + var result = rotation.ToQuaternion().Rotate(Vector3.EZ); result.X.Should().BeApproximately(1, 1e-6); result.Y.Should().BeApproximately(0, 1e-6); @@ -388,7 +388,7 @@ public void Vector3_ToRotation3_ZAxisDirection_ReturnsZeroRotation() // When pointing in +Z, yaw can be any value (atan2(0,0)) but pitch should be 0 // The result should rotate +Z to +Z (identity-like) - var resultDir = (Vector3)rotation; + var resultDir = rotation.ToQuaternion().Rotate(Vector3.EZ); resultDir.Z.Should().BeApproximately(1, 1e-10); } @@ -401,7 +401,7 @@ public void Vector3_ToRotation3_XAxisDirection() var rotation = (Rotation3)x; // Converting back should give +X direction - var resultDir = (Vector3)rotation; + var resultDir = rotation.ToQuaternion().Rotate(Vector3.EZ); resultDir.X.Should().BeApproximately(1, 1e-6); resultDir.Y.Should().BeApproximately(0, 1e-6); resultDir.Z.Should().BeApproximately(0, 1e-6); @@ -416,7 +416,7 @@ public void Vector3_ToRotation3_YAxisDirection() var rotation = (Rotation3)y; // Converting back should give +Y direction - var resultDir = (Vector3)rotation; + var resultDir = rotation.ToQuaternion().Rotate(Vector3.EZ); resultDir.X.Should().BeApproximately(0, 1e-6); resultDir.Y.Should().BeApproximately(1, 1e-6); resultDir.Z.Should().BeApproximately(0, 1e-6); @@ -429,7 +429,7 @@ public void Vector3_ToRotation3_ArbitraryDirection_RoundTrips() var direction = new Vector3(1, 2, 3).Normalize(); var rotation = (Rotation3)direction; - var backToVector = (Vector3)rotation; + var backToVector = rotation.ToQuaternion().Rotate(Vector3.EZ); // Should round-trip back to the same direction backToVector.X.Should().BeApproximately(direction.X, 1e-6); @@ -444,7 +444,7 @@ public void Vector3_ToRotation3_NonUnitVector_NormalizesFirst() var v = new Vector3(3, 4, 0); // length = 5 var rotation = (Rotation3)v; - var resultDir = (Vector3)rotation; + var resultDir = rotation.ToQuaternion().Rotate(Vector3.EZ); // Result should be normalized direction resultDir.Length.Should().BeApproximately(1, 1e-10); diff --git a/Sources/ModelingEvolution.Drawing/Kinematics/KinematicChain.cs b/Sources/ModelingEvolution.Drawing/Kinematics/KinematicChain.cs index 2b651f8..a8e9418 100644 --- a/Sources/ModelingEvolution.Drawing/Kinematics/KinematicChain.cs +++ b/Sources/ModelingEvolution.Drawing/Kinematics/KinematicChain.cs @@ -171,7 +171,7 @@ public void Jacobian(ReadOnlySpan> links, int frame, Span j) { int k = _jointOf[c]; ref readonly var joint = ref Joints.ItemRef(k); - var a = links[c].Rotation.Rotate(joint.Axis); + var a = links[c].TransformVector(joint.Axis); var col = j.Slice(6 * k, 6); if (joint.Type == JointType.Revolute) { diff --git a/Sources/ModelingEvolution.Drawing/Matrix3x3.cs b/Sources/ModelingEvolution.Drawing/Matrix3x3.cs index 89275ed..77700ab 100644 --- a/Sources/ModelingEvolution.Drawing/Matrix3x3.cs +++ b/Sources/ModelingEvolution.Drawing/Matrix3x3.cs @@ -1,340 +1,262 @@ -using System.Numerics; -using System.Runtime.CompilerServices; - -namespace ModelingEvolution.Drawing; - -/// -/// A 3×3 matrix stored as nine fields (row-major). Supports linear algebra -/// operations, 3D rotation matrix construction, and Euler/quaternion conversions. -/// -[Matrix3x3JsonConverter] -public readonly record struct Matrix3x3( - T M11, T M12, T M13, - T M21, T M22, T M23, - T M31, T M32, T M33) - where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, - ISignedNumber, IFloatingPointIeee754, IMinMaxValue -{ - private static readonly T Two = T.CreateTruncating(2); - - /// The 3×3 identity matrix. - public static Matrix3x3 Identity => new( - T.One, T.Zero, T.Zero, - T.Zero, T.One, T.Zero, - T.Zero, T.Zero, T.One); - - /// The 3×3 zero matrix. - public static Matrix3x3 Zero => new( - T.Zero, T.Zero, T.Zero, - T.Zero, T.Zero, T.Zero, - T.Zero, T.Zero, T.Zero); - - /// The determinant of this 3×3 matrix. - public T Determinant - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get => M11 * (M22 * M33 - M23 * M32) - - M12 * (M21 * M33 - M23 * M31) - + M13 * (M21 * M32 - M22 * M31); - } - - /// The trace: M11 + M22 + M33. - public T Trace - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get => M11 + M22 + M33; - } - - /// Returns the transpose of this matrix. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Matrix3x3 Transpose() => new( - M11, M21, M31, - M12, M22, M32, - M13, M23, M33); - - /// - /// Returns the inverse of this matrix via adjugate/determinant. - /// Throws if the matrix is singular (determinant ≈ 0). - /// - public Matrix3x3 Inverse() - { - var det = Determinant; - if (T.Abs(det) < T.CreateTruncating(1e-15)) - throw new InvalidOperationException("Matrix is singular."); - - var invDet = T.One / det; - - return new( - (M22 * M33 - M23 * M32) * invDet, - (M13 * M32 - M12 * M33) * invDet, - (M12 * M23 - M13 * M22) * invDet, - (M23 * M31 - M21 * M33) * invDet, - (M11 * M33 - M13 * M31) * invDet, - (M13 * M21 - M11 * M23) * invDet, - (M21 * M32 - M22 * M31) * invDet, - (M12 * M31 - M11 * M32) * invDet, - (M11 * M22 - M12 * M21) * invDet); - } - - /// Transforms a vector by this matrix: result = M · v. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Vector3 Transform(Vector3 v) => new( - M11 * v.X + M12 * v.Y + M13 * v.Z, - M21 * v.X + M22 * v.Y + M23 * v.Z, - M31 * v.X + M32 * v.Y + M33 * v.Z); - - /// Transforms a point by this matrix: result = M · p. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Point3 Transform(Point3 p) => new( - M11 * p.X + M12 * p.Y + M13 * p.Z, - M21 * p.X + M22 * p.Y + M23 * p.Z, - M31 * p.X + M32 * p.Y + M33 * p.Z); - - #region Operators - - /// Matrix multiplication. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 operator *(in Matrix3x3 a, in Matrix3x3 b) => new( - a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31, - a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32, - a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33, - a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31, - a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32, - a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33, - a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31, - a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32, - a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33); - - /// Matrix-vector multiplication. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 operator *(in Matrix3x3 m, Vector3 v) => m.Transform(v); - - /// Matrix-point multiplication. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Point3 operator *(in Matrix3x3 m, Point3 p) => m.Transform(p); - - /// Scalar multiplication. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 operator *(in Matrix3x3 m, T s) => new( - m.M11 * s, m.M12 * s, m.M13 * s, - m.M21 * s, m.M22 * s, m.M23 * s, - m.M31 * s, m.M32 * s, m.M33 * s); - - /// Scalar multiplication (commutative). - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 operator *(T s, in Matrix3x3 m) => m * s; - - /// Matrix addition. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 operator +(in Matrix3x3 a, in Matrix3x3 b) => new( - a.M11 + b.M11, a.M12 + b.M12, a.M13 + b.M13, - a.M21 + b.M21, a.M22 + b.M22, a.M23 + b.M23, - a.M31 + b.M31, a.M32 + b.M32, a.M33 + b.M33); - - /// Matrix subtraction. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 operator -(in Matrix3x3 a, in Matrix3x3 b) => new( - a.M11 - b.M11, a.M12 - b.M12, a.M13 - b.M13, - a.M21 - b.M21, a.M22 - b.M22, a.M23 - b.M23, - a.M31 - b.M31, a.M32 - b.M32, a.M33 - b.M33); - - #endregion - - #region Rotation Factories - - /// Creates a rotation matrix around the X axis. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 RotationX(Degree angle) - { - var rad = (Radian)angle; - var c = T.Cos((T)rad); - var s = T.Sin((T)rad); - return new( - T.One, T.Zero, T.Zero, - T.Zero, c, -s, - T.Zero, s, c); - } - - /// Creates a rotation matrix around the Y axis. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 RotationY(Degree angle) - { - var rad = (Radian)angle; - var c = T.Cos((T)rad); - var s = T.Sin((T)rad); - return new( - c, T.Zero, s, - T.Zero, T.One, T.Zero, - -s, T.Zero, c); - } - - /// Creates a rotation matrix around the Z axis. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 RotationZ(Degree angle) - { - var rad = (Radian)angle; - var c = T.Cos((T)rad); - var s = T.Sin((T)rad); - return new( - c, -s, T.Zero, - s, c, T.Zero, - T.Zero, T.Zero, T.One); - } - - /// - /// Creates a ZYX Euler rotation matrix: R = Rz(rz) * Ry(ry) * Rx(rx). - /// This matches the convention used by . - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 RotationZYX(Degree rx, Degree ry, Degree rz) - { - var cRx = T.Cos((T)(Radian)rx); var sRx = T.Sin((T)(Radian)rx); - var cRy = T.Cos((T)(Radian)ry); var sRy = T.Sin((T)(Radian)ry); - var cRz = T.Cos((T)(Radian)rz); var sRz = T.Sin((T)(Radian)rz); - - return new( - cRy * cRz, sRx * sRy * cRz - cRx * sRz, cRx * sRy * cRz + sRx * sRz, - cRy * sRz, sRx * sRy * sRz + cRx * cRz, cRx * sRy * sRz - sRx * cRz, - -sRy, sRx * cRy, cRx * cRy); - } - - #endregion - - #region Other Factories - - /// Creates a 3D scaling matrix. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 Scale(T sx, T sy, T sz) => new( - sx, T.Zero, T.Zero, - T.Zero, sy, T.Zero, - T.Zero, T.Zero, sz); - - /// - /// Constructs a similarity matrix (uniform scale × rotation) that maps - /// to when applied as a vector transform. - /// Specifically, M.Transform(source) ≈ target. - /// - /// Source vector (must be non-zero). - /// Target vector (may be any length; zero target yields a degenerate zero matrix). - /// The 3×3 matrix combining rotation that aligns source's direction with target's, scaled by |target|/|source|. - /// Source vector has zero length. - /// - /// Two-point alignment is geometrically under-constrained in 3D (rotation around the source axis is free); - /// this method picks the unique shortest-arc rotation (via ). - /// Use cases include adapting taught polyline paths whose endpoints have shifted. - /// - public static Matrix3x3 SimilarityFromVectors(Vector3 source, Vector3 target) - { - var sourceLength = source.Length; - if (sourceLength == T.Zero) - throw new ArgumentException("Source vector must be non-zero.", nameof(source)); - - var scale = target.Length / sourceLength; - var rotation = source.RotationTo(target); // Rotation3, Euler ZYX - var R = RotationZYX(rotation.Rx, rotation.Ry, rotation.Rz); - return scale * R; - } - - /// Creates a matrix from three column vectors. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 FromColumns(Vector3 c0, Vector3 c1, Vector3 c2) => new( - c0.X, c1.X, c2.X, - c0.Y, c1.Y, c2.Y, - c0.Z, c1.Z, c2.Z); - - /// Creates a matrix from three row vectors. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 FromRows(Vector3 r0, Vector3 r1, Vector3 r2) => new( - r0.X, r0.Y, r0.Z, - r1.X, r1.Y, r1.Z, - r2.X, r2.Y, r2.Z); - - #endregion - - #region Euler & Quaternion Conversions - - /// - /// Extracts ZYX Euler angles (in degrees) from this rotation matrix. - /// Assumes R = Rz(rz) * Ry(ry) * Rx(rx). Handles gimbal lock at ry = ±90°. - /// - public (Degree Rx, Degree Ry, Degree Rz) ToEulerZYX() - { - var epsilon = T.CreateTruncating(1e-6); - var sy = -M31; - - if (sy > T.One) sy = T.One; - if (sy < -T.One) sy = -T.One; - - var ry = T.Asin(sy); - var cy = T.Cos(ry); - - T rx, rz; - if (T.Abs(cy) > epsilon) - { - rx = T.Atan2(M32, M33); - rz = T.Atan2(M21, M11); - } - else - { - // Gimbal lock - rx = T.Atan2(-M23, M22); - rz = T.Zero; - } - - return ( - (Degree)Radian.FromRadian(rx), - (Degree)Radian.FromRadian(ry), - (Degree)Radian.FromRadian(rz)); - } - - /// - /// Converts this rotation matrix to a unit quaternion using Shepperd's method - /// (numerically stable for all orientations). - /// - public Quaternion ToQuaternion() - { - // Shepperd's method: pick the largest diagonal element to avoid division by small numbers - var tr = Trace; - T w, x, y, z; - - if (tr > T.Zero) - { - var s = T.Sqrt(tr + T.One) * Two; - w = s / (T.CreateTruncating(4)); - x = (M32 - M23) / s; - y = (M13 - M31) / s; - z = (M21 - M12) / s; - } - else if (M11 > M22 && M11 > M33) - { - var s = T.Sqrt(T.One + M11 - M22 - M33) * Two; - w = (M32 - M23) / s; - x = s / (T.CreateTruncating(4)); - y = (M12 + M21) / s; - z = (M13 + M31) / s; - } - else if (M22 > M33) - { - var s = T.Sqrt(T.One + M22 - M11 - M33) * Two; - w = (M13 - M31) / s; - x = (M12 + M21) / s; - y = s / (T.CreateTruncating(4)); - z = (M23 + M32) / s; - } - else - { - var s = T.Sqrt(T.One + M33 - M11 - M22) * Two; - w = (M21 - M12) / s; - x = (M13 + M31) / s; - y = (M23 + M32) / s; - z = s / (T.CreateTruncating(4)); - } - - return new Quaternion(w, x, y, z); - } - - #endregion - - public override string ToString() => - $"[{M11}, {M12}, {M13}; {M21}, {M22}, {M23}; {M31}, {M32}, {M33}]"; -} +using System.Numerics; +using System.Runtime.CompilerServices; + +namespace ModelingEvolution.Drawing; + +/// +/// A 3×3 matrix stored as nine fields (row-major). Supports linear algebra +/// operations and 3D rotation matrix construction. Rotation ⇄ matrix goes through +/// (, ); the Euler edge is +/// / . +/// +[Matrix3x3JsonConverter] +public readonly record struct Matrix3x3( + T M11, T M12, T M13, + T M21, T M22, T M23, + T M31, T M32, T M33) + where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, + ISignedNumber, IFloatingPointIeee754, IMinMaxValue +{ + /// The 3×3 identity matrix. + public static Matrix3x3 Identity => new( + T.One, T.Zero, T.Zero, + T.Zero, T.One, T.Zero, + T.Zero, T.Zero, T.One); + + /// The 3×3 zero matrix. + public static Matrix3x3 Zero => new( + T.Zero, T.Zero, T.Zero, + T.Zero, T.Zero, T.Zero, + T.Zero, T.Zero, T.Zero); + + /// The determinant of this 3×3 matrix. + public T Determinant + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get => M11 * (M22 * M33 - M23 * M32) + - M12 * (M21 * M33 - M23 * M31) + + M13 * (M21 * M32 - M22 * M31); + } + + /// The trace: M11 + M22 + M33. + public T Trace + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get => M11 + M22 + M33; + } + + /// Returns the transpose of this matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Matrix3x3 Transpose() => new( + M11, M21, M31, + M12, M22, M32, + M13, M23, M33); + + /// + /// Returns the inverse of this matrix via adjugate/determinant. + /// Throws if the matrix is singular (determinant ≈ 0). + /// + public Matrix3x3 Inverse() + { + var det = Determinant; + if (T.Abs(det) < T.CreateTruncating(1e-15)) + throw new InvalidOperationException("Matrix is singular."); + + var invDet = T.One / det; + + return new( + (M22 * M33 - M23 * M32) * invDet, + (M13 * M32 - M12 * M33) * invDet, + (M12 * M23 - M13 * M22) * invDet, + (M23 * M31 - M21 * M33) * invDet, + (M11 * M33 - M13 * M31) * invDet, + (M13 * M21 - M11 * M23) * invDet, + (M21 * M32 - M22 * M31) * invDet, + (M12 * M31 - M11 * M32) * invDet, + (M11 * M22 - M12 * M21) * invDet); + } + + /// Transforms a vector by this matrix: result = M · v. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Vector3 Transform(Vector3 v) => new( + M11 * v.X + M12 * v.Y + M13 * v.Z, + M21 * v.X + M22 * v.Y + M23 * v.Z, + M31 * v.X + M32 * v.Y + M33 * v.Z); + + /// Transforms a point by this matrix: result = M · p. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Point3 Transform(Point3 p) => new( + M11 * p.X + M12 * p.Y + M13 * p.Z, + M21 * p.X + M22 * p.Y + M23 * p.Z, + M31 * p.X + M32 * p.Y + M33 * p.Z); + + #region Operators + + /// Matrix multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator *(in Matrix3x3 a, in Matrix3x3 b) => new( + a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31, + a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32, + a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33, + a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31, + a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32, + a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33, + a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31, + a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32, + a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33); + + /// Matrix-vector multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 operator *(in Matrix3x3 m, Vector3 v) => m.Transform(v); + + /// Matrix-point multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Point3 operator *(in Matrix3x3 m, Point3 p) => m.Transform(p); + + /// Scalar multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator *(in Matrix3x3 m, T s) => new( + m.M11 * s, m.M12 * s, m.M13 * s, + m.M21 * s, m.M22 * s, m.M23 * s, + m.M31 * s, m.M32 * s, m.M33 * s); + + /// Scalar multiplication (commutative). + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator *(T s, in Matrix3x3 m) => m * s; + + /// Matrix addition. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator +(in Matrix3x3 a, in Matrix3x3 b) => new( + a.M11 + b.M11, a.M12 + b.M12, a.M13 + b.M13, + a.M21 + b.M21, a.M22 + b.M22, a.M23 + b.M23, + a.M31 + b.M31, a.M32 + b.M32, a.M33 + b.M33); + + /// Matrix subtraction. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator -(in Matrix3x3 a, in Matrix3x3 b) => new( + a.M11 - b.M11, a.M12 - b.M12, a.M13 - b.M13, + a.M21 - b.M21, a.M22 - b.M22, a.M23 - b.M23, + a.M31 - b.M31, a.M32 - b.M32, a.M33 - b.M33); + + #endregion + + #region Rotation Factories + + /// Creates a rotation matrix around the X axis. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 RotationX(Degree angle) + { + var rad = (Radian)angle; + var c = T.Cos((T)rad); + var s = T.Sin((T)rad); + return new( + T.One, T.Zero, T.Zero, + T.Zero, c, -s, + T.Zero, s, c); + } + + /// Creates a rotation matrix around the Y axis. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 RotationY(Degree angle) + { + var rad = (Radian)angle; + var c = T.Cos((T)rad); + var s = T.Sin((T)rad); + return new( + c, T.Zero, s, + T.Zero, T.One, T.Zero, + -s, T.Zero, c); + } + + /// Creates a rotation matrix around the Z axis. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 RotationZ(Degree angle) + { + var rad = (Radian)angle; + var c = T.Cos((T)rad); + var s = T.Sin((T)rad); + return new( + c, -s, T.Zero, + s, c, T.Zero, + T.Zero, T.Zero, T.One); + } + + /// + /// Creates a ZYX Euler rotation matrix: R = Rz(rz) * Ry(ry) * Rx(rx), the convention of . + /// The Euler → rotation formula is ; this is its matrix (an edge for + /// matrix-based callers such as solvers and OpenCV). + /// + public static Matrix3x3 RotationZYX(Degree rx, Degree ry, Degree rz) => + Quaternion.FromRpy((T)(Radian)rx, (T)(Radian)ry, (T)(Radian)rz).ToMatrix3x3(); + + #endregion + + #region Other Factories + + /// Creates a 3D scaling matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 Scale(T sx, T sy, T sz) => new( + sx, T.Zero, T.Zero, + T.Zero, sy, T.Zero, + T.Zero, T.Zero, sz); + + /// + /// Constructs a similarity matrix (uniform scale × rotation) that maps + /// to when applied as a vector transform. + /// Specifically, M.Transform(source) ≈ target. + /// + /// Source vector (must be non-zero). + /// Target vector (may be any length; zero target yields a degenerate zero matrix). + /// The 3×3 matrix combining rotation that aligns source's direction with target's, scaled by |target|/|source|. + /// Source vector has zero length. + /// + /// Two-point alignment is geometrically under-constrained in 3D (rotation around the source axis is free); + /// this method picks the unique shortest-arc rotation (via ). + /// Use cases include adapting taught polyline paths whose endpoints have shifted. + /// + public static Matrix3x3 SimilarityFromVectors(Vector3 source, Vector3 target) + { + var sourceLength = source.Length; + if (sourceLength == T.Zero) + throw new ArgumentException("Source vector must be non-zero.", nameof(source)); + + var scale = target.Length / sourceLength; + var rotation = source.RotationTo(target); // Rotation3, Euler ZYX + var R = RotationZYX(rotation.Rx, rotation.Ry, rotation.Rz); + return scale * R; + } + + /// Creates a matrix from three column vectors. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 FromColumns(Vector3 c0, Vector3 c1, Vector3 c2) => new( + c0.X, c1.X, c2.X, + c0.Y, c1.Y, c2.Y, + c0.Z, c1.Z, c2.Z); + + /// Creates a matrix from three row vectors. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 FromRows(Vector3 r0, Vector3 r1, Vector3 r2) => new( + r0.X, r0.Y, r0.Z, + r1.X, r1.Y, r1.Z, + r2.X, r2.Y, r2.Z); + + #endregion + + #region Euler Conversion + + /// + /// Extracts ZYX Euler angles (in degrees) from this rotation matrix, assuming R = Rz(rz) * Ry(ry) * Rx(rx). + /// The matrix goes through and the one rotation → Euler formula, + /// (gimbal lock: rz = 0). + /// + public (Degree Rx, Degree Ry, Degree Rz) ToEulerZYX() + { + var r = Rotation3.FromQuaternion(Quaternion.FromMatrix3x3(this)); + return (r.Rx, r.Ry, r.Rz); + } + + #endregion + + public override string ToString() => + $"[{M11}, {M12}, {M13}; {M21}, {M22}, {M23}; {M31}, {M32}, {M33}]"; +} diff --git a/Sources/ModelingEvolution.Drawing/Polyline3.cs b/Sources/ModelingEvolution.Drawing/Polyline3.cs index c35c988..44a26e3 100644 --- a/Sources/ModelingEvolution.Drawing/Polyline3.cs +++ b/Sources/ModelingEvolution.Drawing/Polyline3.cs @@ -309,19 +309,6 @@ public Polyline3 Simplify(T epsilon) return new Polyline3(mem); } - /// - /// Returns a new polyline with all points rotated by the given rotation. - /// - public Polyline3 Transform(Rotation3 rotation) - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - points[i] = rotation.Rotate(span[i]); - return new Polyline3(mem); - } - /// /// Returns a new polyline with all points transformed by the given frame (rotation + translation). /// diff --git a/Sources/ModelingEvolution.Drawing/PosePath3Builder.cs b/Sources/ModelingEvolution.Drawing/PosePath3Builder.cs index 1f11470..e4663d7 100644 --- a/Sources/ModelingEvolution.Drawing/PosePath3Builder.cs +++ b/Sources/ModelingEvolution.Drawing/PosePath3Builder.cs @@ -5,14 +5,12 @@ namespace ModelingEvolution.Drawing; /// /// Mutable builder for constructing by anchoring at a start pose -/// and appending poses by relative cartesian + Euler-angle deltas. +/// and appending poses by relative steps. /// /// -/// Translation deltas are applied in the world frame (additive). -/// Rotation deltas are applied component-wise in Euler degrees (additive). This is approximate -/// for non-trivial sequences of rotations — for straight-seam path generation with small per-segment -/// angle changes the approximation is adequate. For exact composition use quaternion multiplication -/// directly. +/// A step is a expressed in the previous pose: the next pose is previous · step, +/// composed exactly (epic-129 design, rule 2). The stored poses are wire converted with +/// lengthScale 1, so a step's lengths are in the path's unit. /// public sealed class PosePath3Builder where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, @@ -31,7 +29,7 @@ public PosePath3Builder(int initialCapacity = 16) /// Number of poses added so far. public int Count => _count; - /// Anchors the path at the given absolute pose. Must be called exactly once before any . + /// Anchors the path at the given absolute pose. Must be called exactly once before any . /// When the path is already anchored. [MethodImpl(MethodImplOptions.AggressiveInlining)] public PosePath3Builder StartAt(Pose3 pose) @@ -44,24 +42,16 @@ public PosePath3Builder StartAt(Pose3 pose) } /// - /// Appends a pose by relative cartesian translation and Euler-angle rotation delta from the previous pose. + /// Appends the pose previous · step: is expressed in the previous pose's frame. /// - /// World-frame cartesian offset from the previous pose's position. - /// Euler-angle delta (degrees) added component-wise to the previous pose's rotation. /// When has not been called. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public PosePath3Builder Delta(Vector3 translation, Rotation3 rotation) + public PosePath3Builder Step(in Frame3 step) { if (_count == 0) - throw new InvalidOperationException("StartAt must be called before Delta."); + throw new InvalidOperationException("StartAt must be called before Step."); EnsureCapacity(1); - var last = _buffer[_count - 1]; - var newPos = last.Position + translation; - var newRot = new Rotation3( - last.Rotation.Rx + rotation.Rx, - last.Rotation.Ry + rotation.Ry, - last.Rotation.Rz + rotation.Rz); - _buffer[_count++] = new Pose3(newPos, newRot); + var last = Frame3.FromPose3(_buffer[_count - 1], T.One); + _buffer[_count++] = (last * step).ToPose3(T.One); return this; } diff --git a/Sources/ModelingEvolution.Drawing/Quaternion.cs b/Sources/ModelingEvolution.Drawing/Quaternion.cs index a9bf8bf..35f5a19 100644 --- a/Sources/ModelingEvolution.Drawing/Quaternion.cs +++ b/Sources/ModelingEvolution.Drawing/Quaternion.cs @@ -186,40 +186,60 @@ public static Quaternion FromTo(Vector3 from, Vector3 to) /// : Rotate(EX) = x, Rotate(EY) = y, Rotate(EZ) = z. The axes must form a right-handed /// orthonormal basis (the columns of the rotation matrix). /// - public static Quaternion FromAxes(Vector3 x, Vector3 y, Vector3 z) + public static Quaternion FromAxes(Vector3 x, Vector3 y, Vector3 z) => + FromMatrix3x3(Matrix3x3.FromColumns(x, y, z)); + + /// + /// Creates the unit quaternion of a rotation matrix by Shepperd's method: the branch is picked by the largest of + /// the trace and the diagonal elements, so no division by a small number happens for any rotation. + /// The result is normalized. This is the only matrix → rotation conversion in the library. + /// + /// A proper rotation matrix (orthonormal, determinant +1). + public static Quaternion FromMatrix3x3(in Matrix3x3 m) { - // Shepperd's method on the matrix whose columns are x, y, z. - T m00 = x.X, m10 = x.Y, m20 = x.Z; - T m01 = y.X, m11 = y.Y, m21 = y.Z; - T m02 = z.X, m12 = z.Y, m22 = z.Z; var quarter = T.One / (Two * Two); - var trace = m00 + m11 + m22; + var trace = m.M11 + m.M22 + m.M33; Quaternion q; if (trace > T.Zero) { var s = T.Sqrt(trace + T.One) * Two; - q = new Quaternion(quarter * s, (m21 - m12) / s, (m02 - m20) / s, (m10 - m01) / s); + q = new Quaternion(quarter * s, (m.M32 - m.M23) / s, (m.M13 - m.M31) / s, (m.M21 - m.M12) / s); } - else if (m00 > m11 && m00 > m22) + else if (m.M11 > m.M22 && m.M11 > m.M33) { - var s = T.Sqrt(T.One + m00 - m11 - m22) * Two; - q = new Quaternion((m21 - m12) / s, quarter * s, (m01 + m10) / s, (m02 + m20) / s); + var s = T.Sqrt(T.One + m.M11 - m.M22 - m.M33) * Two; + q = new Quaternion((m.M32 - m.M23) / s, quarter * s, (m.M12 + m.M21) / s, (m.M13 + m.M31) / s); } - else if (m11 > m22) + else if (m.M22 > m.M33) { - var s = T.Sqrt(T.One + m11 - m00 - m22) * Two; - q = new Quaternion((m02 - m20) / s, (m01 + m10) / s, quarter * s, (m12 + m21) / s); + var s = T.Sqrt(T.One + m.M22 - m.M11 - m.M33) * Two; + q = new Quaternion((m.M13 - m.M31) / s, (m.M12 + m.M21) / s, quarter * s, (m.M23 + m.M32) / s); } else { - var s = T.Sqrt(T.One + m22 - m00 - m11) * Two; - q = new Quaternion((m10 - m01) / s, (m02 + m20) / s, (m12 + m21) / s, quarter * s); + var s = T.Sqrt(T.One + m.M33 - m.M11 - m.M22) * Two; + q = new Quaternion((m.M21 - m.M12) / s, (m.M13 + m.M31) / s, (m.M23 + m.M32) / s, quarter * s); } return q.Normalize(); } + /// + /// Returns the rotation matrix of this unit quaternion: its columns are Rotate(EX), Rotate(EY), Rotate(EZ). + /// This is the only rotation → matrix conversion in the library. + /// + public Matrix3x3 ToMatrix3x3() + { + T xx = _x * _x, yy = _y * _y, zz = _z * _z; + T xy = _x * _y, xz = _x * _z, yz = _y * _z; + T wx = _w * _x, wy = _w * _y, wz = _w * _z; + return new Matrix3x3( + T.One - Two * (yy + zz), Two * (xy - wz), Two * (xz + wy), + Two * (xy + wz), T.One - Two * (xx + zz), Two * (yz - wx), + Two * (xz - wy), Two * (yz + wx), T.One - Two * (xx + yy)); + } + /// /// Rotates a vector by this quaternion. /// diff --git a/Sources/ModelingEvolution.Drawing/README.md b/Sources/ModelingEvolution.Drawing/README.md index 65e60b0..fe8cfbb 100644 --- a/Sources/ModelingEvolution.Drawing/README.md +++ b/Sources/ModelingEvolution.Drawing/README.md @@ -1,819 +1,820 @@ -# ModelingEvolution.Drawing - -[![NuGet](https://img.shields.io/nuget/v/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) -[![NuGet Downloads](https://img.shields.io/nuget/dt/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) -[![CI](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml/badge.svg)](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml) -[![License](https://img.shields.io/github/license/modelingevolution/drawing.svg)](https://github.com/modelingevolution/drawing/blob/main/LICENSE) - -A high-performance .NET generic math library for 2D and 3D geometry, coordinate systems, shapes, intersections, trajectories, and drawing primitives. Built on `INumber` constraints from .NET generic math, all types work with `float`, `double`, `decimal`, or any compatible numeric type -- with zero boxing and full type safety. - -## Table of Contents - -- [Why This Library](#why-this-library) -- [Installation](#installation) -- [Quick Start](#quick-start) -- [API Reference](#api-reference) - - [2D Core Types](#2d-core-types) - - [3D Core Types](#3d-core-types) - - [Shapes (2D)](#shapes-2d) - - [Shapes (3D)](#shapes-3d) - - [Lines and Segments](#lines-and-segments) - - [Curves and Paths (2D)](#curves-and-paths-2d) - - [Polylines (2D and 3D)](#polylines-2d-and-3d) - - [Angles and Coordinate Systems](#angles-and-coordinate-systems) - - [Rotations and Orientation](#rotations-and-orientation) - - [Poses and Trajectories](#poses-and-trajectories) - - [Kinematics](#kinematics) - - [Equations](#equations) - - [Colors](#colors) - - [Data Structures and Algorithms](#data-structures-and-algorithms) - - [Interfaces](#interfaces) -- [Key Features](#key-features) - - [Generic Math](#generic-math) - - [IShape Interface Hierarchy](#ishape-interface-hierarchy) - - [Intersections](#intersections) - - [Polygon Boolean Operations](#polygon-boolean-operations) - - [Bezier Path Clipping](#bezier-path-clipping) - - [Pooled Allocation Scopes](#pooled-allocation-scopes) - - [Serialization](#serialization) - - [SVG Export](#svg-export) -- [Code Examples](#code-examples) - - [Points: Distance, Lerp, Rotate](#points-distance-lerp-rotate) - - [3D Points, Vectors, and Rotations](#3d-points-vectors-and-rotations) - - [Spherical Coordinates](#spherical-coordinates) - - [Poses and Trajectories](#poses-and-trajectories-1) - - [Shapes: Area and Perimeter](#shapes-area-and-perimeter) - - [Line-Circle Intersection](#line-circle-intersection) - - [Segment-Rectangle Clipping](#segment-rectangle-clipping) - - [Path-Rectangle Clipping](#path-rectangle-clipping) - - [Triangle Properties](#triangle-properties) - - [Polygon Operations](#polygon-operations) - - [Matrix Transformations (2D and 3D)](#matrix-transformations-2d-and-3d) -- [Requirements](#requirements) -- [License](#license) - -## Why This Library - -- **Generic math** -- write geometry code once; use it with `float`, `double`, or any `IFloatingPointIeee754` type. -- **No boxing** -- all core types are `struct` or `readonly record struct`, avoiding heap allocations. -- **Full 2D and 3D** -- from `Point` and `Vector` to `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Quaternion`, and `Trajectory3`. -- **Rich coordinate systems** -- Cartesian, polar, cylindrical, and spherical with implicit/explicit conversions between them. -- **Rich interface hierarchy** -- `IShape` unifies area, perimeter, centroid, bounding box, containment, rotation, and scaling across all shape types. -- **Comprehensive intersections** -- 21 intersection combinations (line, segment, circle, triangle, rectangle, polygon) with zero-alloc `FirstOf` variants. -- **Polygon booleans** -- union, intersection, difference, and clustering via Clipper2. -- **Exact Bezier clipping** -- `Path.Intersect(Rectangle)` computes analytically exact sub-curves, not polygon approximations. -- **Trajectory planning** -- `Trajectory3` with time-parameterized waypoints, interpolation, resampling, and a real-time `TrajectoryController`. -- **Robotics-ready** -- `Joints6` for 6-DOF joint representations, `Frame3` (position + unit quaternion) for all pose maths, `Frame3.FromSurface()` for plane-based frame construction, and `KinematicChain` for forward kinematics and geometric Jacobians of serial and tree chains. -- **Pooled allocations** -- `AllocationScope` provides arena-style pooled memory for zero-GC geometry pipelines. -- **Serialization built in** -- Protobuf (protobuf-net) attributes on all core types, plus custom JSON converters and SVG exporters. - -## Installation - -```bash -dotnet add package ModelingEvolution.Drawing -``` - -Or via the NuGet Package Manager: - -``` -Install-Package ModelingEvolution.Drawing -``` - -## Quick Start - -```csharp -using ModelingEvolution.Drawing; - -// 2D points and vectors (using double precision) -var a = new Point(1.0, 2.0); -var b = new Point(4.0, 6.0); - -double distance = a.DistanceTo(b); // 5.0 -Point mid = a.Lerp(b, 0.5); // {X=2.5, Y=4} - -// 3D points and rotations -var p = new Point3(1.0, 2.0, 3.0); -var rotation = new Rotation3(45.0, 0.0, 0.0); // 45 deg roll -Point3 rotated = rotation.Rotate(p); - -// Spherical coordinates -var orbit = SphericalPoint.OnEquator(5.0, Degree.Create(30)); -Point3 cartesian = orbit; // implicit conversion - -// Shapes with area and perimeter -var circle = new Circle(Point.Zero, 10.0); -double area = circle.Area(); // ~314.159 -bool inside = circle.Contains(new Point(3.0, 4.0)); // true - -// Polygon boolean operations -var poly1 = new Polygon( - new Point(0, 0), new Point(10, 0), - new Point(10, 10), new Point(0, 10)); -var poly2 = new Polygon( - new Point(5, 5), new Point(15, 5), - new Point(15, 15), new Point(5, 15)); - -Polygon merged = poly1 | poly2; // union -Polygon overlap = poly1 & poly2; // intersection - -// 3D trajectory with time-parameterized waypoints -var trajectory = new Trajectory3( - new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), - new Waypoint3(new Pose3(100, 0, 50, 0, 45, 0), 5.0)); -Pose3 atHalf = trajectory.AtTime(2.5); // interpolated pose -``` - -## API Reference - -### 2D Core Types - -| Type | Description | -|---|---| -| `Point` | 2D Cartesian point. Supports arithmetic with vectors and sizes, distance, lerp, reflect, rotate, clamp, midpoint, matrix transform, tuple conversion, and parsing. | -| `Vector` | 2D vector with magnitude, direction, dot/cross product, normalize, projection, reflect, rotate, perpendicular (CW/CCW), angle between, lerp, and matrix transform. | -| `Size` | Width/height pair with arithmetic operators, component-wise multiply/divide, and parsing. | -| `Matrix` | 3x3 affine transformation matrix. Supports translate, rotate, scale, skew, invert, append/prepend, and point/vector transforms. | -| `Matrix2x2` | 2x2 matrix for linear algebra: determinant, trace, transpose, inverse, eigendecomposition (closed-form for symmetric matrices), and point/vector transforms. | -| `Rectangle` | Axis-aligned rectangle with contains, intersect, inflate, offset, bounds, tiling, diagonal, distance-to-point, and LTRB construction. | -| `Thickness` | Four-sided thickness (top, right, bottom, left) for rectangular borders. | - -### 3D Core Types - -| Type | Description | -|---|---| -| `Point3` | 3D Cartesian point. Distance, lerp, midpoint, random generation, arithmetic with `Vector3`. Conversions to/from `System.Numerics.Vector3` and tuples. Protobuf and JSON serialization. | -| `Vector3` | 3D vector with length, normalize, dot product, cross product, angle between, projection, lerp, random unit vector, and `RotationTo()` for computing the rotation between two directions. Static basis vectors `EX`, `EY`, `EZ`. | -| `Matrix3x3` | 3x3 matrix for 3D transforms. Determinant, trace, transpose, inverse. Rotation factories: `RotationX`, `RotationY`, `RotationZ`, `RotationZYX`. Scale, column/row construction. Euler angle and quaternion extraction. | -| `Quaternion` | Quaternion for 3D rotations. Normalize, conjugate, inverse, from axis-angle, rotate vector, slerp, dot product. Conversions to/from `System.Numerics.Quaternion`. | -| `Segment3` | Bounded line segment in 3D. Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | - -### Shapes (2D) - -All 2D shapes implement `IShape`, providing `Area()`, `Perimeter()`, `Centroid()`, `BoundingBox()`, `Contains(Point)`, `Rotate(Degree, Point)`, and `Scale(T)`. - -| Type | Description | -|---|---| -| `Circle` | Center + radius. Intersects with lines, segments, circles, and rectangles. Tangent point detection. `PointAt(Radian)` for parameterized access. | -| `Triangle` | Three vertices. Incircle, circumcircle, orthocenter, angles, edges. Classification: `IsRight()`, `IsAcute()`, `IsObtuse()`, `IsEquilateral()`, `IsIsosceles()`, `IsScalene()`. Similarity and congruence tests. | -| `Polygon` | Immutable polygon backed by `ReadOnlyMemory>`. Shoelace area, ray-casting containment, convex hull (Graham scan), `IsConvex()`, edges, simplify, skeleton (medial axis). Boolean operations: union, intersect, subtract, cluster. Operators: `\|` (union), `&` (intersect), `-` (subtract). | -| `Rectangle` | Implements `IArea`, `IPerimeter`, `ICentroid`, `IBoundingBox`, `IScalable>`. Rotation returns `Polygon`. | - -### Shapes (3D) - -| Type | Description | -|---|---| -| `Triangle3` | Three vertices in 3D space. Area (cross product), perimeter, centroid, unit normal vector. `ToPose()` creates a coordinate frame from the triangle plane. Translate by `Vector3`. | - -### Lines and Segments - -| Type | Description | -|---|---| -| `Line` | Infinite 2D line (supports vertical lines). From two points, point + direction, or equation. Intersect with lines, segments, circles, triangles, rectangles, polygons. Distance-to-point, angle-between, parallel/perpendicular tests, project point, reflect point. | -| `Segment` | Bounded 2D line segment (start + end). Length, midpoint, direction, lerp, split, reverse. Intersect with segments, lines, circles, rectangles. Distance-to-point, project point, parallel test. Liang-Barsky rectangle clipping. | -| `Segment3` | Bounded 3D line segment (start + end). Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | - -### Curves and Paths (2D) - -| Type | Description | -|---|---| -| `BezierCurve` | Cubic Bezier curve (4 control points). `Evaluate(t)`, `Split(t)`, `SubCurve(t0, t1)` via De Casteljau. Extremum points, linear equation intersection, rectangle edge crossings. | -| `Path` | Immutable sequence of `BezierCurve` segments. Create from points (`FromPoints`) with smoothing, or from SVG path data (`Parse`). `Length()`, `PointAt(t)` with arc-length parameterization. Exact `Intersect(Rectangle)` that splits Bezier curves at crossing points. Transform, rotate, reverse, append. `Close()` and `ToPolygon()` for rasterization. | -| `ComplexCurve` | Composite curve of mixed Bezier and polyline segments packed in a contiguous byte buffer for zero-copy decoding. SVG export and JSON serialization. | -| `PolygonalCurve` | Mutable sequence of connected points that generates smooth Bezier segments via `GetSmoothSegment(i)`. | - -### Polylines (2D and 3D) - -| Type | Description | -|---|---| -| `Polyline` | Immutable open polyline in 2D (non-closing). Length, edges, bounding box, simplification. JSON, Protobuf, and SVG serialization. | -| `Polyline3` | Immutable open polyline in 3D. Length, edges, translate, transform. JSON and Protobuf serialization. | - -### Angles and Coordinate Systems - -| Type | Description | -|---|---| -| `Degree` | Angle in degrees. Implicit conversion from numeric values and from `Radian`. Arithmetic (+, -, *, /), normalize to (-180, 180], abs, comparison operators. | -| `Radian` | Angle in radians. Implicit conversion from `Degree`. Arithmetic (+, -, *, /), normalize to (-pi, pi], abs, comparison operators. Static `Sin`/`Cos` helpers. | -| `PolarPoint` | Polar coordinates (radius, angle). Implicit conversions to/from `Point`, explicit to `Vector2` and `Vector`. | -| `CylindricalPoint` | Cylindrical coordinates (radial distance, angle, height). Factory: `OnPlane(r, angle)`. Builder methods: `WithZ`, `WithRadialDistance`, `WithAngle`. `RotateAngle` for angular displacement. `+`/`-` operators with `Radian` and `Point3`. Conversions to/from `Point3`, `Vector3`, `SphericalPoint`, and `System.Numerics.Vector3`. | -| `SphericalPoint` | Spherical coordinates (radius, azimuth, inclination) with Degree-based storage (ISO convention: inclination from Z-axis). Factories: `OnEquator`, `FromElevation`, `NorthPole`, `SouthPole`. `Normalized`, `WithRadius`, `RotateAzimuth`, `RotateInclination`, `Rotate(az, inc)`. `AngularDistance` (Vincenty formula), `Slerp`, `Lerp`, Euclidean `Distance`. Radius scaling operators (`*`, `/`). `Point3` offset operators (`+`, `-`). Conversions to/from `Point3`, `Vector3`, `CylindricalPoint`, and `System.Numerics.Vector3`. | - -### Rotations and Orientation - -| Type | Description | -|---|---| -| `Rotation3` | Euler angle rotation (Roll/Pitch/Yaw in degrees, ZYX convention). Create from degrees, radians, quaternion, or `Matrix3x3`. `Rotate(Vector3)`, `Rotate(Point3)`. `Combine`, `Inverse`, `Negate`, `Slerp` (via quaternion). Convert to matrix, quaternion, or direction vector. Protobuf and JSON serialization. | -| `Quaternion` | Unit quaternion for 3D rotations. From axis-angle, `FromRpy(roll, pitch, yaw)` (Rz·Ry·Rx, radians; the URDF rpy rule), `FromTo(from, to)` (shortest arc), `FromAxes(x, y, z)` (orthonormal basis), rotate vector, slerp, dot, normalize, conjugate, inverse. Hamilton product operator (`*`). Conversions to/from `System.Numerics.Quaternion`. Protobuf serialization. | -| `Matrix3x3` | Also used as rotation representation. `RotationZYX(rx, ry, rz)` creates ZYX Euler rotation matrix. `ToEulerZYX()` extracts angles (with gimbal-lock handling). `ToQuaternion()` via Shepperd's method. | - -### Poses and Trajectories - -| Type | Description | -|---|---| -| `Frame3` | Rigid frame = position (`Point3`) + unit `Quaternion`. All pose maths: `Multiply` / `*` (compose), `Inverse`, `TransformPoint`, `TransformVector`, `Interpolate` (position lerp + slerp), `Distance` (position error and angle error in radians). `FromSurface(a, b, c)` creates a frame from three coplanar points (right-hand rule); `FromSurface(a, b, c, h)` uses a hint point for Z; `FromSurface(points)` fits a plane. `FromPose3(pose, lengthScale)` / `ToPose3(lengthScale)` convert to and from the wire format (ZYX degrees). | -| `Pose3` | Wire and edit format only (since 2.0): position (`Point3`) + ZYX Euler orientation (`Rotation3`, degrees). Constructors, `Identity`, tuple conversions, deconstruction, parsing, equality, `Truncating()`. Protobuf and JSON serialization. Convert to `Frame3` for any maths. | -| `Waypoint3` | Timestamped `Pose3` -- an element of a trajectory. | -| `PosePath3` | Immutable sequence of `Pose3` (no time). Length, reverse, transform, extract `Polyline3`, convert to `Trajectory3` with uniform timing. | -| `Trajectory3` | Immutable time-parameterized path of `Waypoint3`. `Duration`, `Length`, `AtTime(t)` (binary-search interpolation with position lerp + rotation slerp). `Resample(interval)`, `Reverse`, `Transform`. Convert to `PosePath3` or `Polyline3`. Create a real-time `TrajectoryController` via `Control()`. | -| `TrajectoryController` | Real-time trajectory playback using `Stopwatch` clock. Start, stop, pause, resume, seek. Fires `PoseChanged` events with current pose, time, and progress. | -| `Corner` | Corner blending descriptor: `Sharp` (exact waypoint) or `Round(radius)` (arc blend before/after the waypoint). | - -### Kinematics - -| Type | Description | -|---|---| -| `KinematicChain` | Namespace `ModelingEvolution.Drawing.Kinematics`. A tree of `Link`s moved by revolute/prismatic `Joint`s (origin at q = 0, unit axis, limits) with named `ChainFrame`s; metres and radians. `Forward(q, links)` writes every link pose, `FramePose(links, frame)`, `Jacobian(links, frame, j)` writes the 6 × Dof geometric Jacobian (column-major, v then ω). Spans in, spans out, no allocation; the constructor validates topological order, unit axes, limits and unique names. | -| `Joints6` | 6-DOF joint angles stored as six `Degree` fields (no array allocation). Indexer `[0..5]`, arithmetic operators, `Lerp`, `MaxAbsDelta`, `IsWithin(tolerance)`, `IsBetween(min, max)`. `ToArray()`, `FromSpan()`, `FromArray()`, `CopyTo(Span)`. Protobuf and JSON serialization. | -| `Speed` | Scalar speed in units/second (unitless magnitude). Arithmetic, `TimeFor(distance)`, `DistanceIn(time)`. Parsing. | -| `Velocity3` | 3D velocity vector (direction + magnitude). Create from direction + speed. `Speed`, `Direction`, `DisplacementIn(time)`. Arithmetic operators. | -| `Speed` | **Unit-aware** speed tagged at compile time with its unit (`ISpeedUnit`). Stores the value in `TUnit`; `ToSi()`/`FromSi()`/`To()` normalise via the SI base (m/s). Full arithmetic, comparisons, ratio (`Speed/Speed → T`), `IParsable`. Mixing units (or a non-speed unit) is a compile error. Verified zero-cost vs raw `double`. | -| `ISpeedUnit` | Marker for speed units. Built-ins: `MetrePerSecond` (SI base), `MillimetrePerSecond`, `MetrePerMinute`. Implement to add your own. | -| `AngularSpeed` | **Unit-aware** angular speed tagged at compile time with its unit (`IAngularSpeedUnit`). Stores the value in `TUnit`; `ToSi()`/`FromSi()`/`To()` normalise via the SI base (rad/s). Full arithmetic, signed negation, comparisons, ratio (`AngularSpeed/AngularSpeed → T`), `IParsable`. A separate dimension from `Speed`, so mixing an angular speed with a linear one is a compile error. | -| `IAngularSpeedUnit` | Marker for angular-speed units. Built-ins: `RadianPerSecond` (SI base), `DegreePerSecond`. Implement to add your own. | - -### Equations - -| Type | Description | -|---|---| -| `LinearEquation` | `y = Ax + B`. Create from points, angle, or direction. Compute, zero point, intersect, perpendicular, translate, mirror. | -| `QuadraticEquation` | `ax^2 + bx + c = 0`. Zero points using the discriminant. | -| `CubicEquation` | `ax^3 + bx^2 + cx + d = 0`. Root finding via Newton-Raphson. | -| `CircleEquation` | `(x-cx)^2 + (y-cy)^2 = r^2`. Intersect with `LinearEquation`. | - -### Colors - -| Type | Description | -|---|---| -| `Color` | ARGB color as a 32-bit uint. Parse from hex (`#RRGGBB`, `#AARRGGBB`), `rgba(r,g,b,a)`, or HSV strings. `GetHue()`, `GetSaturation()`, `GetLightness()`, `GetBrightness()`, `MakeTransparent(float)`. Implicit from string or tuple. JSON and Protobuf serialization. | -| `HsvColor` | HSV (hue, saturation, value) color space with optional alpha. Implicit conversions to/from `Color`. Parse from `hsv()`/`hsva()` strings, JSON arrays, or hex. | - -### Data Structures and Algorithms - -| Type | Description | -|---|---| -| `KdTree` | 2D KD-tree for fast nearest-neighbor queries on `Point`. Build from span, add individual points, find nearest neighbor. | -| `Skeleton` | Medial axis approximation of a polygon (interior nodes + edges). Three algorithms: `StraightSkeleton`, `ChordalAxis`, and `Voronoi`. | -| `MovingAverage` | Component-wise moving average over a sliding window of 2D points. O(1) add and query via circular buffer + running sums. | -| `MovingMedian` | Component-wise moving median over a sliding window of 2D points. O(1) median read, O(n) add via maintained sorted arrays. | -| `AllocationScope` | Arena-style pooled allocation scope. Library allocations use `MemoryPool.Shared` when active. Dispose returns all tracked memory at once. Supports `Persist()` to detach individual results. | -| `AlignmentResult` | Result of a rigid 2D alignment (rotation + translation) between two point clouds, with rotation angle and residual error. | - -### Interfaces - -```csharp -// Composable geometry traits -public interface IArea { T Area(); } -public interface IPerimeter { T Perimeter(); } -public interface ICentroid { Point Centroid(); } -public interface IBoundingBox { Rectangle BoundingBox(); } -public interface IRotatable { TSelf Rotate(Degree angle, Point origin = default); } -public interface IScalable { TSelf Scale(T factor); } - -// Unified shape interface -- combines all of the above plus point containment -public interface IShape : IArea, IPerimeter, ICentroid, - IBoundingBox, IRotatable, IScalable -{ - bool Contains(Point point); -} -``` - -Implementing types: `Circle`, `Triangle`, `Polygon`, and `Rectangle` (partial -- no `IRotatable` since rotation produces a `Polygon`). - -## Key Features - -### Generic Math - -All geometry types are parameterized by `T` with constraints like `IFloatingPointIeee754`, `IMinMaxValue`, etc. This means the same code works with any numeric type: - -```csharp -// Single precision -var pf = new Point(1f, 2f); -var cf = new Circle(pf, 5f); - -// Double precision -var pd = new Point(1.0, 2.0); -var cd = new Circle(pd, 5.0); - -// Both compute area with their respective precision -float areaF = cf.Area(); -double areaD = cd.Area(); -``` - -### IShape Interface Hierarchy - -Write generic algorithms over any shape: - -```csharp -void PrintShapeInfo(TShape shape) - where T : INumber, ITrigonometricFunctions, IRootFunctions, - IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue - where TShape : IShape -{ - Console.WriteLine($"Area: {shape.Area()}"); - Console.WriteLine($"Perimeter: {shape.Perimeter()}"); - Console.WriteLine($"Centroid: {shape.Centroid()}"); - Console.WriteLine($"Bounds: {shape.BoundingBox()}"); -} -``` - -### Intersections - -The `Intersections` static class provides 21 combination overloads covering every pair of: `Line`, `Segment`, `Circle`, `Triangle`, `Rectangle`, and `Polygon`. Each combination also has a zero-allocation `FirstOf` variant. - -```csharp -// Line x Line -Point? p = Intersections.Of(line1, line2); - -// Line x Circle -> chord (Segment) or null -Segment? chord = Intersections.Of(line, circle); - -// Tangent detection -Point? tangent = Intersections.TangentPoint(line, circle); - -// Circle x Circle -> radical chord or tangent point -Segment? radicalChord = Intersections.Of(circle1, circle2); - -// Segment x Segment -Point? hit = Intersections.Of(seg1, seg2); - -// Zero-alloc first-hit variants -Segment? first = Intersections.FirstOf(line, polygon); -``` - -Instance methods are also available on each shape for convenience: - -```csharp -Segment? chord = circle.Intersect(line); -Point? hit = segment.Intersect(otherSegment); -Segment? clipped = line.Intersect(rectangle); -``` - -### Polygon Boolean Operations - -Powered by [Clipper2](https://github.com/AngusJohnson/Clipper2): - -```csharp -var a = new Polygon( /* ... */ ); -var b = new Polygon( /* ... */ ); - -// Operators -Polygon union = a | b; -Polygon intersect = a & b; -Polygon difference = a - b; - -// Methods returning multiple result polygons -List> unions = a.Union(b, removeHoles: true); -List> diffs = a.Subtract(b); - -// Batch operations -List> merged = Polygon.Union(polygons); - -// Clustering overlapping polygons -IEnumerable> clusters = Polygon.Cluster(polygons); -``` - -### Bezier Path Clipping - -`Path.Intersect(Rectangle)` performs analytically exact Bezier-rectangle clipping. It finds the precise parameter values where each cubic Bezier segment crosses the rectangle edges, then uses `SubCurve(t0, t1)` via De Casteljau subdivision to return exact sub-paths -- not polygon approximations. - -```csharp -var path = Path.FromPoints(points); -IEnumerable> clipped = path.Intersect(viewport); - -foreach (var subPath in clipped) -{ - // Each sub-path contains only the Bezier segments inside the rectangle, - // with curves split precisely at the rectangle boundaries. -} -``` - -### Pooled Allocation Scopes - -When performing many geometry operations that allocate intermediate results (polygon booleans, skeleton computation, curve corrections), use `AllocationScope` to avoid GC pressure: - -```csharp -using var scope = AllocationScope.Begin(); - -var polygon = somePolygon * new Size(2, 2); // pooled -var skeleton = polygon.Skeleton(); // pooled -var lease = scope.Persist(ref skeleton); // detach from scope - -// scope.Dispose() returns everything except skeleton's memory -// lease.Dispose() returns skeleton's memory when you're done -``` - -### Serialization - -**Protobuf** -- all core types carry `[ProtoContract]` / `[ProtoMember]` attributes for use with protobuf-net: - -```csharp -var point = new Point(1f, 2f); -byte[] bytes = Serializer.Serialize(point); -``` - -**JSON** -- custom `System.Text.Json` converters for `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Polygon`, `Polyline`, `Polyline3`, `PosePath3`, `Trajectory3`, `Path`, `ComplexCurve`, `BezierCurve`, `Matrix2x2`, `Matrix3x3`, `Skeleton`, `Color`, and `HsvColor`: - -```csharp -var pose = new Pose3(100, 200, 300, 45, 90, 0); -string json = JsonSerializer.Serialize(pose); // [100,200,300,45,90,0] - -var joints = new Joints6(10, 20, 30, 40, 50, 60); -string j = JsonSerializer.Serialize(joints); // [10,20,30,40,50,60] -``` - -**String parsing** -- `IParsable` implementations on `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Rectangle`, `Size`, `Path`, `ComplexCurve`, `Speed`, `Color`, and `HsvColor`: - -```csharp -var point = Point.Parse("1.5, 2.5"); -var point3 = Point3.Parse("1.0 2.0 3.0"); -var pose = Pose3.Parse("100, 200, 300, 45, 90, 0"); -var rect = Rectangle.Parse("0 0 100 200"); -var color = Color.Parse("#FF8800"); -var path = Path.Parse("M 0 0 C 1 2, 3 4, 5 6", null); -``` - -### SVG Export - -`Polygon`, `Polyline`, `ComplexCurve`, `Skeleton`, and `Path` support SVG export via the `[SvgExporter]` attribute and the `SvgExporter` infrastructure: - -```csharp -// Path.ToString() produces SVG path data -var path = Path.FromPoints(points); -string svgData = path.ToString(); // "M 0 0 C 1.5 2.3, 3.1 4.2, 5 6 ..." -``` - -## Code Examples - -### Points: Distance, Lerp, Rotate - -```csharp -var a = new Point(0, 0); -var b = new Point(3, 4); - -double dist = a.DistanceTo(b); // 5.0 -Point quarter = a.Lerp(b, 0.25); // {X=0.75, Y=1} -Point reflected = a.Reflect(b); // {X=6, Y=8} - -// Rotate 90 degrees around the origin -Degree angle = 90.0; -Point rotated = b.Rotate(angle); // {X=-4, Y=3} - -// Rotate around a custom center -var center = new Point(1, 1); -Point rotated2 = b.Rotate(angle, center); - -// Clamp to a rectangle -var bounds = new Rectangle(0, 0, 10, 10); -Point clamped = new Point(15, -3).Clamp(bounds); // {X=10, Y=0} -``` - -### 3D Points, Vectors, and Rotations - -```csharp -// 3D point operations -var p1 = new Point3(1, 2, 3); -var p2 = new Point3(4, 6, 3); -double dist3d = Point3.Distance(p1, p2); // 5.0 -Point3 mid3d = Point3.Lerp(p1, p2, 0.5); - -// 3D vector operations -var v = new Vector3(1, 0, 0); -var w = new Vector3(0, 1, 0); -var cross = Vector3.Cross(v, w); // (0, 0, 1) -double dot = Vector3.Dot(v, w); // 0.0 - -// Compute rotation between two directions -Rotation3 rot = v.RotationTo(w); -Vector3 result = rot.Rotate(v); // approximately (0, 1, 0) - -// Euler angle rotation (ZYX convention) -var rotation = Rotation3.FromDegrees(45, 0, 0); -Point3 rotated = rotation.Rotate(p1); - -// Quaternion slerp -var r1 = Rotation3.FromDegrees(0, 0, 0); -var r2 = Rotation3.FromDegrees(0, 0, 90); -var halfway = Rotation3.Slerp(r1, r2, 0.5); - -// Matrix3x3 rotation -var m = Matrix3x3.RotationZ(Degree.Create(45)); -var transformed = m.Transform(new Point3(1, 0, 0)); -``` - -### Spherical Coordinates - -```csharp -// Create spherical points with factory methods -var northPole = SphericalPoint.NorthPole(10.0); // +Z, radius=10 -var equator = SphericalPoint.OnEquator(5.0, Degree.Create(45)); -var elevated = SphericalPoint.FromElevation(5.0, - Degree.Create(0), Degree.Create(30)); - -// Convert to/from Cartesian -Point3 cartesian = equator; // implicit conversion -SphericalPoint spherical = cartesian; // implicit conversion - -// Orbit around a 3D center -var center = new Point3(10, 50, 0); -var points = new List>(); -for (int az = 0; az < 360; az += 30) - points.Add(center + SphericalPoint.OnEquator(5.0, Degree.Create(az))); - -// Rotation (O(1) angle operations) -var sp = SphericalPoint.OnEquator(5.0, Degree.Create(0)); -var rotated = sp.RotateAzimuth(Degree.Create(45)); // rotate around Z - -// Angular distance (Vincenty formula -- numerically stable) -var a = SphericalPoint.NorthPole(1.0); -var b = SphericalPoint.SouthPole(1.0); -Degree angDist = SphericalPoint.AngularDistance(a, b); // 180 degrees - -// Spherical linear interpolation -var mid = SphericalPoint.Slerp(a, b, 0.5); - -// Cylindrical coordinate conversions -CylindricalPoint cyl = sp; // implicit conversion -SphericalPoint back = cyl; // implicit conversion -``` - -### Poses and Trajectories - -```csharp -// Pose = position + orientation -var pose = new Pose3(100, 200, 300, 45, 0, 0); // x,y,z, rx,ry,rz (degrees) - -// Maths composes Frame3 (position + quaternion); Pose3 converts once, here mm -> m -var world = Frame3.FromPose3(new Pose3(10, 0, 0, 0, 0, 0), 0.001); -var local = Frame3.FromPose3(new Pose3(5, 0, 0, 0, 0, 90), 0.001); -var combined = world * local; -Pose3 back = combined.ToPose3(1000); // m -> mm, ZYX degrees - -// Create frame from a surface (3 points define a plane) -var a = new Point3(0, 0, 0); -var b = new Point3(1, 0, 0); -var c = new Point3(0, 1, 0); -var surface = Frame3.FromSurface(a, b, c); // Z-axis via right-hand rule - -// Trajectory with timed waypoints -var trajectory = new Trajectory3( - new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), - new Waypoint3(new Pose3(50, 0, 25, 0, 30, 0), 2.5), - new Waypoint3(new Pose3(100, 0, 0, 0, 0, 0), 5.0)); - -// Interpolate at any time (binary search + lerp/slerp) -Pose3 atT = trajectory.AtTime(1.25); - -// Resample at uniform intervals -var resampled = trajectory.Resample(0.1); // every 0.1 seconds - -// Real-time playback -var controller = trajectory.Control(); -controller.PoseChanged += (_, e) => Console.WriteLine($"t={e.Time} pose={e.Pose}"); -controller.Start(); - -// 6-DOF joint angles for robotics -var home = new Joints6(0, -90, 0, 0, 90, 0); -var target = new Joints6(45, -60, 30, 0, 90, -45); -var halfway = Joints6.Lerp(home, target, 0.5); -bool inRange = target.IsBetween(minLimits, maxLimits); -``` - -### Shapes: Area and Perimeter - -```csharp -// Circle -var circle = new Circle(new Point(5, 5), 3.0); -double cArea = circle.Area(); // ~28.274 -double cPerim = circle.Perimeter(); // ~18.850 - -// Triangle -var triangle = new Triangle( - new Point(0, 0), - new Point(4, 0), - new Point(0, 3)); -double tArea = triangle.Area(); // 6.0 -double tPerim = triangle.Perimeter(); // 12.0 - -// Polygon (shoelace formula) -var poly = new Polygon( - new Point(0, 0), new Point(4, 0), - new Point(4, 3), new Point(0, 3)); -double pArea = poly.Area(); // 12.0 - -// 3D triangle -var t3 = new Triangle3( - new Point3(0, 0, 0), - new Point3(4, 0, 0), - new Point3(0, 3, 0)); -double area3 = t3.Area; // 6.0 -Vector3 normal = t3.Normal; // (0, 0, 1) -``` - -### Line-Circle Intersection - -```csharp -var circle = new Circle(new Point(0, 0), 5.0); -var line = Line.From(new Point(-10, 0), new Point(10, 0)); - -// Secant -- returns the chord as a segment -Segment? chord = circle.Intersect(line); -// chord.Value.Start ~ {X=-5, Y=0}, chord.Value.End ~ {X=5, Y=0} - -// Tangent detection -var tangentLine = Line.Horizontal(5.0); -Point? tangent = circle.TangentPoint(tangentLine); -// tangent.Value ~ {X=0, Y=5} -``` - -### Segment-Rectangle Clipping - -```csharp -var viewport = new Rectangle(0, 0, 100, 100); -var segment = new Segment( - new Point(-20, 50), - new Point(150, 50)); - -// Liang-Barsky clipping -Segment? clipped = segment.Intersect(viewport); -// clipped.Value: Start={X=0, Y=50}, End={X=100, Y=50} -``` - -### Path-Rectangle Clipping - -```csharp -var points = new[] -{ - new Point(10, 50), - new Point(50, 10), - new Point(90, 50), - new Point(130, 90) -}; -var path = Path.FromPoints(points); -var viewport = new Rectangle(20, 20, 60, 60); // 20,20 to 80,80 - -// Exact Bezier clipping -- returns sub-paths with curves split at boundaries -foreach (var subPath in path.Intersect(viewport)) -{ - Console.WriteLine($"Sub-path with {subPath.Count} Bezier segments"); -} -``` - -### Triangle Properties - -```csharp -var t = new Triangle( - new Point(0, 0), - new Point(4, 0), - new Point(2, 3)); - -// Derived circles -Circle incircle = t.Incircle(); // largest circle inside -Circle circumcircle = t.Circumcircle(); // passes through all vertices - -// Center points -Point centroid = t.Centroid(); -Point orthocenter = t.Orthocenter; - -// Classification -bool right = t.IsRight(); -bool equilateral = t.IsEquilateral(); -bool isosceles = t.IsIsosceles(); - -// Similarity and congruence -var t2 = new Triangle( - new Point(0, 0), - new Point(8, 0), - new Point(4, 6)); -bool similar = t.IsSimilarTo(t2); // true (same shape, scaled 2x) -bool congruent = t.IsCongruentTo(t2); // false (different size) - -// Interior angles -var (atA, atB, atC) = t.Angles; -``` - -### Polygon Operations - -```csharp -// Convex hull -var cloud = new Polygon( - new Point(0, 0), new Point(1, 3), - new Point(2, 1), new Point(4, 4), - new Point(3, 0), new Point(5, 2)); -Polygon hull = cloud.ConvexHull(); - -// Point containment (ray casting) -bool inside = hull.Contains(new Point(2, 2)); - -// Boolean operations -var a = new Polygon( /* ... */ ); -var b = new Polygon( /* ... */ ); -List> union = a.Union(b, removeHoles: true); -List> diff = a.Subtract(b); -List> inter = a.Intersect(b); - -// Simplify (remove collinear/close points) -Polygon simplified = hull.Simplify(epsilon: 0.1); - -// Convexity test -bool convex = hull.IsConvex(); - -// Clustering overlapping polygons -var clusters = Polygon.Cluster(polygons); -``` - -### Matrix Transformations (2D and 3D) - -```csharp -// 2D affine transformation -var matrix = Matrix.Identity; -matrix.Translate(10.0, 20.0); -matrix.Rotate(Degree.Create(45.0)); -matrix.Scale(2.0, 2.0); - -Point transformed = matrix.Transform(new Point(1, 0)); -Vector rotatedVec = matrix.Transform(new Vector(1, 0)); - -var combined = matrix1 * matrix2; -matrix.Invert(); - -// 3D rotation matrix -var rx = Matrix3x3.RotationX(Degree.Create(45)); -var ry = Matrix3x3.RotationY(Degree.Create(30)); -var rz = Matrix3x3.RotationZ(Degree.Create(60)); - -// Compose rotations -var rotation = rz * ry * rx; -var p = new Point3(1, 0, 0); -Point3 rotated3 = rotation * p; // or rotation.Transform(p) - -// Extract Euler angles -var (eRx, eRy, eRz) = rotation.ToEulerZYX(); - -// Convert to quaternion -Quaternion q = rotation.ToQuaternion(); -``` - -### Unit-Aware Speed - -```csharp -using ModelingEvolution.Drawing.Units; - -// The unit is part of the type — the value is stored in that unit. -Speed> feed = new(300); // 300 m/min - -// Normalise to the SI base (m/s) or convert to another speed unit. -double si = feed.ToSi(); // 5.0 (m/s) -var inMmPerS = feed.To>(); // 5000 mm/s - -// Same-unit arithmetic, scalar scaling, and a dimensionless ratio. -var faster = feed * 2.0; // 600 m/min -double ratio = faster / feed; // 2.0 - -// Cross-unit add normalises through SI; result stays in the left unit. -var total = inMmPerS.Add(feed); // (5000 + 5000) mm/s - -Console.WriteLine(feed); // "300 m/min" - -// Compile-time safety: -// feed + inMmPerS; // error: different units — use .Add(...) -// feed.To>(); // error: not an ISpeedUnit -``` - -### Unit-Aware Angular Speed - -`AngularSpeed` mirrors `Speed` for rotation, on its own dimension with the SI -base rad/s. Keeping the two dimensions apart is the point: a rotary axis and a linear axis cannot be -commanded with each other's speed. - -```csharp -using ModelingEvolution.Drawing.Units; - -// The unit is part of the type — the value is stored in that unit. -AngularSpeed> tilt = new(5); // 5 °/s - -// Normalise to the SI base (rad/s) or convert to another angular-speed unit. -double si = tilt.ToSi(); // 0.0873 (rad/s) -var inRadPerS = tilt.To>(); - -// Signed: negating reverses the sense of rotation. -var reverse = -tilt; // -5 °/s - -// Scale by a percentage of a maximum, e.g. a jog knob at 20% of the axis limit. -var jog = tilt * new Percentage(20f); // 1 °/s - -Console.WriteLine(tilt); // "5 °/s" - -// Compile-time safety — the dimensions do not mix: -// tilt + new Speed>(20); // error: different types -// tilt.To>(); // error: not an IAngularSpeedUnit -// Speed> wrong; // error: not an ISpeedUnit -``` - -## Requirements - -- **.NET 10.0** or later (uses generic math interfaces from .NET 7+) -- **Dependencies**: [Clipper2](https://www.nuget.org/packages/Clipper2) 1.4.0, [protobuf-net](https://www.nuget.org/packages/protobuf-net) 3.2.45 - -## License - -See [LICENSE](LICENSE) for details. +# ModelingEvolution.Drawing + +[![NuGet](https://img.shields.io/nuget/v/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) +[![NuGet Downloads](https://img.shields.io/nuget/dt/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) +[![CI](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml/badge.svg)](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml) +[![License](https://img.shields.io/github/license/modelingevolution/drawing.svg)](https://github.com/modelingevolution/drawing/blob/main/LICENSE) + +A high-performance .NET generic math library for 2D and 3D geometry, coordinate systems, shapes, intersections, trajectories, and drawing primitives. Built on `INumber` constraints from .NET generic math, all types work with `float`, `double`, `decimal`, or any compatible numeric type -- with zero boxing and full type safety. + +## Table of Contents + +- [Why This Library](#why-this-library) +- [Installation](#installation) +- [Quick Start](#quick-start) +- [API Reference](#api-reference) + - [2D Core Types](#2d-core-types) + - [3D Core Types](#3d-core-types) + - [Shapes (2D)](#shapes-2d) + - [Shapes (3D)](#shapes-3d) + - [Lines and Segments](#lines-and-segments) + - [Curves and Paths (2D)](#curves-and-paths-2d) + - [Polylines (2D and 3D)](#polylines-2d-and-3d) + - [Angles and Coordinate Systems](#angles-and-coordinate-systems) + - [Rotations and Orientation](#rotations-and-orientation) + - [Poses and Trajectories](#poses-and-trajectories) + - [Kinematics](#kinematics) + - [Equations](#equations) + - [Colors](#colors) + - [Data Structures and Algorithms](#data-structures-and-algorithms) + - [Interfaces](#interfaces) +- [Key Features](#key-features) + - [Generic Math](#generic-math) + - [IShape Interface Hierarchy](#ishape-interface-hierarchy) + - [Intersections](#intersections) + - [Polygon Boolean Operations](#polygon-boolean-operations) + - [Bezier Path Clipping](#bezier-path-clipping) + - [Pooled Allocation Scopes](#pooled-allocation-scopes) + - [Serialization](#serialization) + - [SVG Export](#svg-export) +- [Code Examples](#code-examples) + - [Points: Distance, Lerp, Rotate](#points-distance-lerp-rotate) + - [3D Points, Vectors, and Rotations](#3d-points-vectors-and-rotations) + - [Spherical Coordinates](#spherical-coordinates) + - [Poses and Trajectories](#poses-and-trajectories-1) + - [Shapes: Area and Perimeter](#shapes-area-and-perimeter) + - [Line-Circle Intersection](#line-circle-intersection) + - [Segment-Rectangle Clipping](#segment-rectangle-clipping) + - [Path-Rectangle Clipping](#path-rectangle-clipping) + - [Triangle Properties](#triangle-properties) + - [Polygon Operations](#polygon-operations) + - [Matrix Transformations (2D and 3D)](#matrix-transformations-2d-and-3d) +- [Requirements](#requirements) +- [License](#license) + +## Why This Library + +- **Generic math** -- write geometry code once; use it with `float`, `double`, or any `IFloatingPointIeee754` type. +- **No boxing** -- all core types are `struct` or `readonly record struct`, avoiding heap allocations. +- **Full 2D and 3D** -- from `Point` and `Vector` to `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Quaternion`, and `Trajectory3`. +- **Rich coordinate systems** -- Cartesian, polar, cylindrical, and spherical with implicit/explicit conversions between them. +- **Rich interface hierarchy** -- `IShape` unifies area, perimeter, centroid, bounding box, containment, rotation, and scaling across all shape types. +- **Comprehensive intersections** -- 21 intersection combinations (line, segment, circle, triangle, rectangle, polygon) with zero-alloc `FirstOf` variants. +- **Polygon booleans** -- union, intersection, difference, and clustering via Clipper2. +- **Exact Bezier clipping** -- `Path.Intersect(Rectangle)` computes analytically exact sub-curves, not polygon approximations. +- **Trajectory planning** -- `Trajectory3` with time-parameterized waypoints, interpolation, resampling, and a real-time `TrajectoryController`. +- **Robotics-ready** -- `Joints6` for 6-DOF joint representations, `Frame3` (position + unit quaternion) for all pose maths, `Frame3.FromSurface()` for plane-based frame construction, and `KinematicChain` for forward kinematics and geometric Jacobians of serial and tree chains. +- **Pooled allocations** -- `AllocationScope` provides arena-style pooled memory for zero-GC geometry pipelines. +- **Serialization built in** -- Protobuf (protobuf-net) attributes on all core types, plus custom JSON converters and SVG exporters. + +## Installation + +```bash +dotnet add package ModelingEvolution.Drawing +``` + +Or via the NuGet Package Manager: + +``` +Install-Package ModelingEvolution.Drawing +``` + +## Quick Start + +```csharp +using ModelingEvolution.Drawing; + +// 2D points and vectors (using double precision) +var a = new Point(1.0, 2.0); +var b = new Point(4.0, 6.0); + +double distance = a.DistanceTo(b); // 5.0 +Point mid = a.Lerp(b, 0.5); // {X=2.5, Y=4} + +// 3D points and rotations +var p = new Point3(1.0, 2.0, 3.0); +var rotation = new Rotation3(45.0, 0.0, 0.0); // 45 deg roll (wire/edit format) +var rotated = new Frame3(Point3.Zero, rotation.ToQuaternion()).TransformPoint(p); + +// Spherical coordinates +var orbit = SphericalPoint.OnEquator(5.0, Degree.Create(30)); +Point3 cartesian = orbit; // implicit conversion + +// Shapes with area and perimeter +var circle = new Circle(Point.Zero, 10.0); +double area = circle.Area(); // ~314.159 +bool inside = circle.Contains(new Point(3.0, 4.0)); // true + +// Polygon boolean operations +var poly1 = new Polygon( + new Point(0, 0), new Point(10, 0), + new Point(10, 10), new Point(0, 10)); +var poly2 = new Polygon( + new Point(5, 5), new Point(15, 5), + new Point(15, 15), new Point(5, 15)); + +Polygon merged = poly1 | poly2; // union +Polygon overlap = poly1 & poly2; // intersection + +// 3D trajectory with time-parameterized waypoints +var trajectory = new Trajectory3( + new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), + new Waypoint3(new Pose3(100, 0, 50, 0, 45, 0), 5.0)); +Pose3 atHalf = trajectory.AtTime(2.5); // interpolated pose +``` + +## API Reference + +### 2D Core Types + +| Type | Description | +|---|---| +| `Point` | 2D Cartesian point. Supports arithmetic with vectors and sizes, distance, lerp, reflect, rotate, clamp, midpoint, matrix transform, tuple conversion, and parsing. | +| `Vector` | 2D vector with magnitude, direction, dot/cross product, normalize, projection, reflect, rotate, perpendicular (CW/CCW), angle between, lerp, and matrix transform. | +| `Size` | Width/height pair with arithmetic operators, component-wise multiply/divide, and parsing. | +| `Matrix` | 3x3 affine transformation matrix. Supports translate, rotate, scale, skew, invert, append/prepend, and point/vector transforms. | +| `Matrix2x2` | 2x2 matrix for linear algebra: determinant, trace, transpose, inverse, eigendecomposition (closed-form for symmetric matrices), and point/vector transforms. | +| `Rectangle` | Axis-aligned rectangle with contains, intersect, inflate, offset, bounds, tiling, diagonal, distance-to-point, and LTRB construction. | +| `Thickness` | Four-sided thickness (top, right, bottom, left) for rectangular borders. | + +### 3D Core Types + +| Type | Description | +|---|---| +| `Point3` | 3D Cartesian point. Distance, lerp, midpoint, random generation, arithmetic with `Vector3`. Conversions to/from `System.Numerics.Vector3` and tuples. Protobuf and JSON serialization. | +| `Vector3` | 3D vector with length, normalize, dot product, cross product, angle between, projection, lerp, random unit vector, and `RotationTo()` for computing the rotation between two directions. Static basis vectors `EX`, `EY`, `EZ`. | +| `Matrix3x3` | 3x3 matrix for 3D transforms. Determinant, trace, transpose, inverse. Rotation factories: `RotationX`, `RotationY`, `RotationZ`, `RotationZYX`. Scale, column/row construction. Euler angle and quaternion extraction. | +| `Quaternion` | Quaternion for 3D rotations. Normalize, conjugate, inverse, from axis-angle, rotate vector, slerp, dot product. Conversions to/from `System.Numerics.Quaternion`. | +| `Segment3` | Bounded line segment in 3D. Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | + +### Shapes (2D) + +All 2D shapes implement `IShape`, providing `Area()`, `Perimeter()`, `Centroid()`, `BoundingBox()`, `Contains(Point)`, `Rotate(Degree, Point)`, and `Scale(T)`. + +| Type | Description | +|---|---| +| `Circle` | Center + radius. Intersects with lines, segments, circles, and rectangles. Tangent point detection. `PointAt(Radian)` for parameterized access. | +| `Triangle` | Three vertices. Incircle, circumcircle, orthocenter, angles, edges. Classification: `IsRight()`, `IsAcute()`, `IsObtuse()`, `IsEquilateral()`, `IsIsosceles()`, `IsScalene()`. Similarity and congruence tests. | +| `Polygon` | Immutable polygon backed by `ReadOnlyMemory>`. Shoelace area, ray-casting containment, convex hull (Graham scan), `IsConvex()`, edges, simplify, skeleton (medial axis). Boolean operations: union, intersect, subtract, cluster. Operators: `\|` (union), `&` (intersect), `-` (subtract). | +| `Rectangle` | Implements `IArea`, `IPerimeter`, `ICentroid`, `IBoundingBox`, `IScalable>`. Rotation returns `Polygon`. | + +### Shapes (3D) + +| Type | Description | +|---|---| +| `Triangle3` | Three vertices in 3D space. Area (cross product), perimeter, centroid, unit normal vector. `ToPose()` creates a coordinate frame from the triangle plane. Translate by `Vector3`. | + +### Lines and Segments + +| Type | Description | +|---|---| +| `Line` | Infinite 2D line (supports vertical lines). From two points, point + direction, or equation. Intersect with lines, segments, circles, triangles, rectangles, polygons. Distance-to-point, angle-between, parallel/perpendicular tests, project point, reflect point. | +| `Segment` | Bounded 2D line segment (start + end). Length, midpoint, direction, lerp, split, reverse. Intersect with segments, lines, circles, rectangles. Distance-to-point, project point, parallel test. Liang-Barsky rectangle clipping. | +| `Segment3` | Bounded 3D line segment (start + end). Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | + +### Curves and Paths (2D) + +| Type | Description | +|---|---| +| `BezierCurve` | Cubic Bezier curve (4 control points). `Evaluate(t)`, `Split(t)`, `SubCurve(t0, t1)` via De Casteljau. Extremum points, linear equation intersection, rectangle edge crossings. | +| `Path` | Immutable sequence of `BezierCurve` segments. Create from points (`FromPoints`) with smoothing, or from SVG path data (`Parse`). `Length()`, `PointAt(t)` with arc-length parameterization. Exact `Intersect(Rectangle)` that splits Bezier curves at crossing points. Transform, rotate, reverse, append. `Close()` and `ToPolygon()` for rasterization. | +| `ComplexCurve` | Composite curve of mixed Bezier and polyline segments packed in a contiguous byte buffer for zero-copy decoding. SVG export and JSON serialization. | +| `PolygonalCurve` | Mutable sequence of connected points that generates smooth Bezier segments via `GetSmoothSegment(i)`. | + +### Polylines (2D and 3D) + +| Type | Description | +|---|---| +| `Polyline` | Immutable open polyline in 2D (non-closing). Length, edges, bounding box, simplification. JSON, Protobuf, and SVG serialization. | +| `Polyline3` | Immutable open polyline in 3D. Length, edges, translate, transform. JSON and Protobuf serialization. | + +### Angles and Coordinate Systems + +| Type | Description | +|---|---| +| `Degree` | Angle in degrees. Implicit conversion from numeric values and from `Radian`. Arithmetic (+, -, *, /), normalize to (-180, 180], abs, comparison operators. | +| `Radian` | Angle in radians. Implicit conversion from `Degree`. Arithmetic (+, -, *, /), normalize to (-pi, pi], abs, comparison operators. Static `Sin`/`Cos` helpers. | +| `PolarPoint` | Polar coordinates (radius, angle). Implicit conversions to/from `Point`, explicit to `Vector2` and `Vector`. | +| `CylindricalPoint` | Cylindrical coordinates (radial distance, angle, height). Factory: `OnPlane(r, angle)`. Builder methods: `WithZ`, `WithRadialDistance`, `WithAngle`. `RotateAngle` for angular displacement. `+`/`-` operators with `Radian` and `Point3`. Conversions to/from `Point3`, `Vector3`, `SphericalPoint`, and `System.Numerics.Vector3`. | +| `SphericalPoint` | Spherical coordinates (radius, azimuth, inclination) with Degree-based storage (ISO convention: inclination from Z-axis). Factories: `OnEquator`, `FromElevation`, `NorthPole`, `SouthPole`. `Normalized`, `WithRadius`, `RotateAzimuth`, `RotateInclination`, `Rotate(az, inc)`. `AngularDistance` (Vincenty formula), `Slerp`, `Lerp`, Euclidean `Distance`. Radius scaling operators (`*`, `/`). `Point3` offset operators (`+`, `-`). Conversions to/from `Point3`, `Vector3`, `CylindricalPoint`, and `System.Numerics.Vector3`. | + +### Rotations and Orientation + +| Type | Description | +|---|---| +| `Rotation3` | Euler angle rotation (Roll/Pitch/Yaw in degrees, ZYX convention). Wire and edit format only (since 2.0): create from degrees, radians, quaternion (`FromQuaternion`) or `Matrix3x3` (`FromMatrix3x3`); `ToQuaternion()` for maths. No rotation algebra: compose, invert, slerp and rotate with `Quaternion` or `Frame3`. Protobuf and JSON serialization. | +| `Quaternion` | Unit quaternion for 3D rotations. From axis-angle, `FromRpy(roll, pitch, yaw)` (Rz·Ry·Rx, radians; the URDF rpy rule), `FromTo(from, to)` (shortest arc), `FromAxes(x, y, z)` (orthonormal basis), `FromMatrix3x3(m)` (Shepperd's method) and `ToMatrix3x3()`, rotate vector, slerp, dot, normalize, conjugate, inverse. Hamilton product operator (`*`). Conversions to/from `System.Numerics.Quaternion`. Protobuf serialization. | +| `Matrix3x3` | Also used as rotation representation. `RotationZYX(rx, ry, rz)` creates ZYX Euler rotation matrix. `ToEulerZYX()` extracts angles (gimbal lock: rz = 0). Both go through `Quaternion` (`FromRpy`/`ToMatrix3x3`, `FromMatrix3x3`/`Rotation3.FromQuaternion`), so there is one Euler formula each way. | + +### Poses and Trajectories + +| Type | Description | +|---|---| +| `Frame3` | Rigid frame = position (`Point3`) + unit `Quaternion`. All pose maths: `Multiply` / `*` (compose), `Inverse`, `TransformPoint`, `TransformVector`, `Interpolate` (position lerp + slerp), `Distance` (position error and angle error in radians). `FromSurface(a, b, c)` creates a frame from three coplanar points (right-hand rule); `FromSurface(a, b, c, h)` uses a hint point for Z; `FromSurface(points)` fits a plane. `FromPose3(pose, lengthScale)` / `ToPose3(lengthScale)` convert to and from the wire format (ZYX degrees). | +| `Pose3` | Wire and edit format only (since 2.0): position (`Point3`) + ZYX Euler orientation (`Rotation3`, degrees). Constructors, `Identity`, tuple conversions, deconstruction, parsing, equality, `Truncating()`. Protobuf and JSON serialization. Convert to `Frame3` for any maths. | +| `Waypoint3` | Timestamped `Pose3` -- an element of a trajectory. | +| `PosePath3` | Immutable sequence of `Pose3` (no time). Length, reverse, transform, extract `Polyline3`, convert to `Trajectory3` with uniform timing. | +| `Trajectory3` | Immutable time-parameterized path of `Waypoint3`. `Duration`, `Length`, `AtTime(t)` (binary-search interpolation with position lerp + rotation slerp). `Resample(interval)`, `Reverse`, `Transform`. Convert to `PosePath3` or `Polyline3`. Create a real-time `TrajectoryController` via `Control()`. | +| `TrajectoryController` | Real-time trajectory playback using `Stopwatch` clock. Start, stop, pause, resume, seek. Fires `PoseChanged` events with current pose, time, and progress. | +| `Corner` | Corner blending descriptor: `Sharp` (exact waypoint) or `Round(radius)` (arc blend before/after the waypoint). | + +### Kinematics + +| Type | Description | +|---|---| +| `KinematicChain` | Namespace `ModelingEvolution.Drawing.Kinematics`. A tree of `Link`s moved by revolute/prismatic `Joint`s (origin at q = 0, unit axis, limits) with named `ChainFrame`s; metres and radians. `Forward(q, links)` writes every link pose, `FramePose(links, frame)`, `Jacobian(links, frame, j)` writes the 6 × Dof geometric Jacobian (column-major, v then ω). Spans in, spans out, no allocation; the constructor validates topological order, unit axes, limits and unique names. | +| `Joints6` | 6-DOF joint angles stored as six `Degree` fields (no array allocation). Indexer `[0..5]`, arithmetic operators, `Lerp`, `MaxAbsDelta`, `IsWithin(tolerance)`, `IsBetween(min, max)`. `ToArray()`, `FromSpan()`, `FromArray()`, `CopyTo(Span)`. Protobuf and JSON serialization. | +| `Speed` | Scalar speed in units/second (unitless magnitude). Arithmetic, `TimeFor(distance)`, `DistanceIn(time)`. Parsing. | +| `Velocity3` | 3D velocity vector (direction + magnitude). Create from direction + speed. `Speed`, `Direction`, `DisplacementIn(time)`. Arithmetic operators. | +| `Speed` | **Unit-aware** speed tagged at compile time with its unit (`ISpeedUnit`). Stores the value in `TUnit`; `ToSi()`/`FromSi()`/`To()` normalise via the SI base (m/s). Full arithmetic, comparisons, ratio (`Speed/Speed → T`), `IParsable`. Mixing units (or a non-speed unit) is a compile error. Verified zero-cost vs raw `double`. | +| `ISpeedUnit` | Marker for speed units. Built-ins: `MetrePerSecond` (SI base), `MillimetrePerSecond`, `MetrePerMinute`. Implement to add your own. | +| `AngularSpeed` | **Unit-aware** angular speed tagged at compile time with its unit (`IAngularSpeedUnit`). Stores the value in `TUnit`; `ToSi()`/`FromSi()`/`To()` normalise via the SI base (rad/s). Full arithmetic, signed negation, comparisons, ratio (`AngularSpeed/AngularSpeed → T`), `IParsable`. A separate dimension from `Speed`, so mixing an angular speed with a linear one is a compile error. | +| `IAngularSpeedUnit` | Marker for angular-speed units. Built-ins: `RadianPerSecond` (SI base), `DegreePerSecond`. Implement to add your own. | + +### Equations + +| Type | Description | +|---|---| +| `LinearEquation` | `y = Ax + B`. Create from points, angle, or direction. Compute, zero point, intersect, perpendicular, translate, mirror. | +| `QuadraticEquation` | `ax^2 + bx + c = 0`. Zero points using the discriminant. | +| `CubicEquation` | `ax^3 + bx^2 + cx + d = 0`. Root finding via Newton-Raphson. | +| `CircleEquation` | `(x-cx)^2 + (y-cy)^2 = r^2`. Intersect with `LinearEquation`. | + +### Colors + +| Type | Description | +|---|---| +| `Color` | ARGB color as a 32-bit uint. Parse from hex (`#RRGGBB`, `#AARRGGBB`), `rgba(r,g,b,a)`, or HSV strings. `GetHue()`, `GetSaturation()`, `GetLightness()`, `GetBrightness()`, `MakeTransparent(float)`. Implicit from string or tuple. JSON and Protobuf serialization. | +| `HsvColor` | HSV (hue, saturation, value) color space with optional alpha. Implicit conversions to/from `Color`. Parse from `hsv()`/`hsva()` strings, JSON arrays, or hex. | + +### Data Structures and Algorithms + +| Type | Description | +|---|---| +| `KdTree` | 2D KD-tree for fast nearest-neighbor queries on `Point`. Build from span, add individual points, find nearest neighbor. | +| `Skeleton` | Medial axis approximation of a polygon (interior nodes + edges). Three algorithms: `StraightSkeleton`, `ChordalAxis`, and `Voronoi`. | +| `MovingAverage` | Component-wise moving average over a sliding window of 2D points. O(1) add and query via circular buffer + running sums. | +| `MovingMedian` | Component-wise moving median over a sliding window of 2D points. O(1) median read, O(n) add via maintained sorted arrays. | +| `AllocationScope` | Arena-style pooled allocation scope. Library allocations use `MemoryPool.Shared` when active. Dispose returns all tracked memory at once. Supports `Persist()` to detach individual results. | +| `AlignmentResult` | Result of a rigid 2D alignment (rotation + translation) between two point clouds, with rotation angle and residual error. | + +### Interfaces + +```csharp +// Composable geometry traits +public interface IArea { T Area(); } +public interface IPerimeter { T Perimeter(); } +public interface ICentroid { Point Centroid(); } +public interface IBoundingBox { Rectangle BoundingBox(); } +public interface IRotatable { TSelf Rotate(Degree angle, Point origin = default); } +public interface IScalable { TSelf Scale(T factor); } + +// Unified shape interface -- combines all of the above plus point containment +public interface IShape : IArea, IPerimeter, ICentroid, + IBoundingBox, IRotatable, IScalable +{ + bool Contains(Point point); +} +``` + +Implementing types: `Circle`, `Triangle`, `Polygon`, and `Rectangle` (partial -- no `IRotatable` since rotation produces a `Polygon`). + +## Key Features + +### Generic Math + +All geometry types are parameterized by `T` with constraints like `IFloatingPointIeee754`, `IMinMaxValue`, etc. This means the same code works with any numeric type: + +```csharp +// Single precision +var pf = new Point(1f, 2f); +var cf = new Circle(pf, 5f); + +// Double precision +var pd = new Point(1.0, 2.0); +var cd = new Circle(pd, 5.0); + +// Both compute area with their respective precision +float areaF = cf.Area(); +double areaD = cd.Area(); +``` + +### IShape Interface Hierarchy + +Write generic algorithms over any shape: + +```csharp +void PrintShapeInfo(TShape shape) + where T : INumber, ITrigonometricFunctions, IRootFunctions, + IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue + where TShape : IShape +{ + Console.WriteLine($"Area: {shape.Area()}"); + Console.WriteLine($"Perimeter: {shape.Perimeter()}"); + Console.WriteLine($"Centroid: {shape.Centroid()}"); + Console.WriteLine($"Bounds: {shape.BoundingBox()}"); +} +``` + +### Intersections + +The `Intersections` static class provides 21 combination overloads covering every pair of: `Line`, `Segment`, `Circle`, `Triangle`, `Rectangle`, and `Polygon`. Each combination also has a zero-allocation `FirstOf` variant. + +```csharp +// Line x Line +Point? p = Intersections.Of(line1, line2); + +// Line x Circle -> chord (Segment) or null +Segment? chord = Intersections.Of(line, circle); + +// Tangent detection +Point? tangent = Intersections.TangentPoint(line, circle); + +// Circle x Circle -> radical chord or tangent point +Segment? radicalChord = Intersections.Of(circle1, circle2); + +// Segment x Segment +Point? hit = Intersections.Of(seg1, seg2); + +// Zero-alloc first-hit variants +Segment? first = Intersections.FirstOf(line, polygon); +``` + +Instance methods are also available on each shape for convenience: + +```csharp +Segment? chord = circle.Intersect(line); +Point? hit = segment.Intersect(otherSegment); +Segment? clipped = line.Intersect(rectangle); +``` + +### Polygon Boolean Operations + +Powered by [Clipper2](https://github.com/AngusJohnson/Clipper2): + +```csharp +var a = new Polygon( /* ... */ ); +var b = new Polygon( /* ... */ ); + +// Operators +Polygon union = a | b; +Polygon intersect = a & b; +Polygon difference = a - b; + +// Methods returning multiple result polygons +List> unions = a.Union(b, removeHoles: true); +List> diffs = a.Subtract(b); + +// Batch operations +List> merged = Polygon.Union(polygons); + +// Clustering overlapping polygons +IEnumerable> clusters = Polygon.Cluster(polygons); +``` + +### Bezier Path Clipping + +`Path.Intersect(Rectangle)` performs analytically exact Bezier-rectangle clipping. It finds the precise parameter values where each cubic Bezier segment crosses the rectangle edges, then uses `SubCurve(t0, t1)` via De Casteljau subdivision to return exact sub-paths -- not polygon approximations. + +```csharp +var path = Path.FromPoints(points); +IEnumerable> clipped = path.Intersect(viewport); + +foreach (var subPath in clipped) +{ + // Each sub-path contains only the Bezier segments inside the rectangle, + // with curves split precisely at the rectangle boundaries. +} +``` + +### Pooled Allocation Scopes + +When performing many geometry operations that allocate intermediate results (polygon booleans, skeleton computation, curve corrections), use `AllocationScope` to avoid GC pressure: + +```csharp +using var scope = AllocationScope.Begin(); + +var polygon = somePolygon * new Size(2, 2); // pooled +var skeleton = polygon.Skeleton(); // pooled +var lease = scope.Persist(ref skeleton); // detach from scope + +// scope.Dispose() returns everything except skeleton's memory +// lease.Dispose() returns skeleton's memory when you're done +``` + +### Serialization + +**Protobuf** -- all core types carry `[ProtoContract]` / `[ProtoMember]` attributes for use with protobuf-net: + +```csharp +var point = new Point(1f, 2f); +byte[] bytes = Serializer.Serialize(point); +``` + +**JSON** -- custom `System.Text.Json` converters for `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Polygon`, `Polyline`, `Polyline3`, `PosePath3`, `Trajectory3`, `Path`, `ComplexCurve`, `BezierCurve`, `Matrix2x2`, `Matrix3x3`, `Skeleton`, `Color`, and `HsvColor`: + +```csharp +var pose = new Pose3(100, 200, 300, 45, 90, 0); +string json = JsonSerializer.Serialize(pose); // [100,200,300,45,90,0] + +var joints = new Joints6(10, 20, 30, 40, 50, 60); +string j = JsonSerializer.Serialize(joints); // [10,20,30,40,50,60] +``` + +**String parsing** -- `IParsable` implementations on `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Rectangle`, `Size`, `Path`, `ComplexCurve`, `Speed`, `Color`, and `HsvColor`: + +```csharp +var point = Point.Parse("1.5, 2.5"); +var point3 = Point3.Parse("1.0 2.0 3.0"); +var pose = Pose3.Parse("100, 200, 300, 45, 90, 0"); +var rect = Rectangle.Parse("0 0 100 200"); +var color = Color.Parse("#FF8800"); +var path = Path.Parse("M 0 0 C 1 2, 3 4, 5 6", null); +``` + +### SVG Export + +`Polygon`, `Polyline`, `ComplexCurve`, `Skeleton`, and `Path` support SVG export via the `[SvgExporter]` attribute and the `SvgExporter` infrastructure: + +```csharp +// Path.ToString() produces SVG path data +var path = Path.FromPoints(points); +string svgData = path.ToString(); // "M 0 0 C 1.5 2.3, 3.1 4.2, 5 6 ..." +``` + +## Code Examples + +### Points: Distance, Lerp, Rotate + +```csharp +var a = new Point(0, 0); +var b = new Point(3, 4); + +double dist = a.DistanceTo(b); // 5.0 +Point quarter = a.Lerp(b, 0.25); // {X=0.75, Y=1} +Point reflected = a.Reflect(b); // {X=6, Y=8} + +// Rotate 90 degrees around the origin +Degree angle = 90.0; +Point rotated = b.Rotate(angle); // {X=-4, Y=3} + +// Rotate around a custom center +var center = new Point(1, 1); +Point rotated2 = b.Rotate(angle, center); + +// Clamp to a rectangle +var bounds = new Rectangle(0, 0, 10, 10); +Point clamped = new Point(15, -3).Clamp(bounds); // {X=10, Y=0} +``` + +### 3D Points, Vectors, and Rotations + +```csharp +// 3D point operations +var p1 = new Point3(1, 2, 3); +var p2 = new Point3(4, 6, 3); +double dist3d = Point3.Distance(p1, p2); // 5.0 +Point3 mid3d = Point3.Lerp(p1, p2, 0.5); + +// 3D vector operations +var v = new Vector3(1, 0, 0); +var w = new Vector3(0, 1, 0); +var cross = Vector3.Cross(v, w); // (0, 0, 1) +double dot = Vector3.Dot(v, w); // 0.0 + +// Compute rotation between two directions +var rot = Quaternion.FromTo(v, w); +Vector3 result = rot.Rotate(v); // approximately (0, 1, 0) + +// Euler angles (ZYX convention) are a wire format: convert once, then use the quaternion +var rotation = Rotation3.FromDegrees(45, 0, 0).ToQuaternion(); +Vector3 rotated = rotation.Rotate((Vector3)p1); + +// Quaternion slerp +var r1 = Quaternion.Identity; +var r2 = Quaternion.FromAxisAngle(Vector3.EZ, Math.PI / 2); +var halfway = Quaternion.Slerp(r1, r2, 0.5); + +// Matrix3x3 rotation +var m = Matrix3x3.RotationZ(Degree.Create(45)); +var transformed = m.Transform(new Point3(1, 0, 0)); +``` + +### Spherical Coordinates + +```csharp +// Create spherical points with factory methods +var northPole = SphericalPoint.NorthPole(10.0); // +Z, radius=10 +var equator = SphericalPoint.OnEquator(5.0, Degree.Create(45)); +var elevated = SphericalPoint.FromElevation(5.0, + Degree.Create(0), Degree.Create(30)); + +// Convert to/from Cartesian +Point3 cartesian = equator; // implicit conversion +SphericalPoint spherical = cartesian; // implicit conversion + +// Orbit around a 3D center +var center = new Point3(10, 50, 0); +var points = new List>(); +for (int az = 0; az < 360; az += 30) + points.Add(center + SphericalPoint.OnEquator(5.0, Degree.Create(az))); + +// Rotation (O(1) angle operations) +var sp = SphericalPoint.OnEquator(5.0, Degree.Create(0)); +var rotated = sp.RotateAzimuth(Degree.Create(45)); // rotate around Z + +// Angular distance (Vincenty formula -- numerically stable) +var a = SphericalPoint.NorthPole(1.0); +var b = SphericalPoint.SouthPole(1.0); +Degree angDist = SphericalPoint.AngularDistance(a, b); // 180 degrees + +// Spherical linear interpolation +var mid = SphericalPoint.Slerp(a, b, 0.5); + +// Cylindrical coordinate conversions +CylindricalPoint cyl = sp; // implicit conversion +SphericalPoint back = cyl; // implicit conversion +``` + +### Poses and Trajectories + +```csharp +// Pose = position + orientation +var pose = new Pose3(100, 200, 300, 45, 0, 0); // x,y,z, rx,ry,rz (degrees) + +// Maths composes Frame3 (position + quaternion); Pose3 converts once, here mm -> m +var world = Frame3.FromPose3(new Pose3(10, 0, 0, 0, 0, 0), 0.001); +var local = Frame3.FromPose3(new Pose3(5, 0, 0, 0, 0, 90), 0.001); +var combined = world * local; +Pose3 back = combined.ToPose3(1000); // m -> mm, ZYX degrees + +// Create frame from a surface (3 points define a plane) +var a = new Point3(0, 0, 0); +var b = new Point3(1, 0, 0); +var c = new Point3(0, 1, 0); +var surface = Frame3.FromSurface(a, b, c); // Z-axis via right-hand rule + +// Trajectory with timed waypoints +var trajectory = new Trajectory3( + new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), + new Waypoint3(new Pose3(50, 0, 25, 0, 30, 0), 2.5), + new Waypoint3(new Pose3(100, 0, 0, 0, 0, 0), 5.0)); + +// Interpolate at any time (binary search + lerp/slerp) +Pose3 atT = trajectory.AtTime(1.25); + +// Resample at uniform intervals +var resampled = trajectory.Resample(0.1); // every 0.1 seconds + +// Real-time playback +var controller = trajectory.Control(); +controller.PoseChanged += (_, e) => Console.WriteLine($"t={e.Time} pose={e.Pose}"); +controller.Start(); + +// 6-DOF joint angles for robotics +var home = new Joints6(0, -90, 0, 0, 90, 0); +var target = new Joints6(45, -60, 30, 0, 90, -45); +var halfway = Joints6.Lerp(home, target, 0.5); +bool inRange = target.IsBetween(minLimits, maxLimits); +``` + +### Shapes: Area and Perimeter + +```csharp +// Circle +var circle = new Circle(new Point(5, 5), 3.0); +double cArea = circle.Area(); // ~28.274 +double cPerim = circle.Perimeter(); // ~18.850 + +// Triangle +var triangle = new Triangle( + new Point(0, 0), + new Point(4, 0), + new Point(0, 3)); +double tArea = triangle.Area(); // 6.0 +double tPerim = triangle.Perimeter(); // 12.0 + +// Polygon (shoelace formula) +var poly = new Polygon( + new Point(0, 0), new Point(4, 0), + new Point(4, 3), new Point(0, 3)); +double pArea = poly.Area(); // 12.0 + +// 3D triangle +var t3 = new Triangle3( + new Point3(0, 0, 0), + new Point3(4, 0, 0), + new Point3(0, 3, 0)); +double area3 = t3.Area; // 6.0 +Vector3 normal = t3.Normal; // (0, 0, 1) +``` + +### Line-Circle Intersection + +```csharp +var circle = new Circle(new Point(0, 0), 5.0); +var line = Line.From(new Point(-10, 0), new Point(10, 0)); + +// Secant -- returns the chord as a segment +Segment? chord = circle.Intersect(line); +// chord.Value.Start ~ {X=-5, Y=0}, chord.Value.End ~ {X=5, Y=0} + +// Tangent detection +var tangentLine = Line.Horizontal(5.0); +Point? tangent = circle.TangentPoint(tangentLine); +// tangent.Value ~ {X=0, Y=5} +``` + +### Segment-Rectangle Clipping + +```csharp +var viewport = new Rectangle(0, 0, 100, 100); +var segment = new Segment( + new Point(-20, 50), + new Point(150, 50)); + +// Liang-Barsky clipping +Segment? clipped = segment.Intersect(viewport); +// clipped.Value: Start={X=0, Y=50}, End={X=100, Y=50} +``` + +### Path-Rectangle Clipping + +```csharp +var points = new[] +{ + new Point(10, 50), + new Point(50, 10), + new Point(90, 50), + new Point(130, 90) +}; +var path = Path.FromPoints(points); +var viewport = new Rectangle(20, 20, 60, 60); // 20,20 to 80,80 + +// Exact Bezier clipping -- returns sub-paths with curves split at boundaries +foreach (var subPath in path.Intersect(viewport)) +{ + Console.WriteLine($"Sub-path with {subPath.Count} Bezier segments"); +} +``` + +### Triangle Properties + +```csharp +var t = new Triangle( + new Point(0, 0), + new Point(4, 0), + new Point(2, 3)); + +// Derived circles +Circle incircle = t.Incircle(); // largest circle inside +Circle circumcircle = t.Circumcircle(); // passes through all vertices + +// Center points +Point centroid = t.Centroid(); +Point orthocenter = t.Orthocenter; + +// Classification +bool right = t.IsRight(); +bool equilateral = t.IsEquilateral(); +bool isosceles = t.IsIsosceles(); + +// Similarity and congruence +var t2 = new Triangle( + new Point(0, 0), + new Point(8, 0), + new Point(4, 6)); +bool similar = t.IsSimilarTo(t2); // true (same shape, scaled 2x) +bool congruent = t.IsCongruentTo(t2); // false (different size) + +// Interior angles +var (atA, atB, atC) = t.Angles; +``` + +### Polygon Operations + +```csharp +// Convex hull +var cloud = new Polygon( + new Point(0, 0), new Point(1, 3), + new Point(2, 1), new Point(4, 4), + new Point(3, 0), new Point(5, 2)); +Polygon hull = cloud.ConvexHull(); + +// Point containment (ray casting) +bool inside = hull.Contains(new Point(2, 2)); + +// Boolean operations +var a = new Polygon( /* ... */ ); +var b = new Polygon( /* ... */ ); +List> union = a.Union(b, removeHoles: true); +List> diff = a.Subtract(b); +List> inter = a.Intersect(b); + +// Simplify (remove collinear/close points) +Polygon simplified = hull.Simplify(epsilon: 0.1); + +// Convexity test +bool convex = hull.IsConvex(); + +// Clustering overlapping polygons +var clusters = Polygon.Cluster(polygons); +``` + +### Matrix Transformations (2D and 3D) + +```csharp +// 2D affine transformation +var matrix = Matrix.Identity; +matrix.Translate(10.0, 20.0); +matrix.Rotate(Degree.Create(45.0)); +matrix.Scale(2.0, 2.0); + +Point transformed = matrix.Transform(new Point(1, 0)); +Vector rotatedVec = matrix.Transform(new Vector(1, 0)); + +var combined = matrix1 * matrix2; +matrix.Invert(); + +// 3D rotation matrix +var rx = Matrix3x3.RotationX(Degree.Create(45)); +var ry = Matrix3x3.RotationY(Degree.Create(30)); +var rz = Matrix3x3.RotationZ(Degree.Create(60)); + +// Compose rotations +var rotation = rz * ry * rx; +var p = new Point3(1, 0, 0); +Point3 rotated3 = rotation * p; // or rotation.Transform(p) + +// Extract Euler angles +var (eRx, eRy, eRz) = rotation.ToEulerZYX(); + +// Convert to quaternion and back +Quaternion q = Quaternion.FromMatrix3x3(rotation); +Matrix3x3 back = q.ToMatrix3x3(); +``` + +### Unit-Aware Speed + +```csharp +using ModelingEvolution.Drawing.Units; + +// The unit is part of the type — the value is stored in that unit. +Speed> feed = new(300); // 300 m/min + +// Normalise to the SI base (m/s) or convert to another speed unit. +double si = feed.ToSi(); // 5.0 (m/s) +var inMmPerS = feed.To>(); // 5000 mm/s + +// Same-unit arithmetic, scalar scaling, and a dimensionless ratio. +var faster = feed * 2.0; // 600 m/min +double ratio = faster / feed; // 2.0 + +// Cross-unit add normalises through SI; result stays in the left unit. +var total = inMmPerS.Add(feed); // (5000 + 5000) mm/s + +Console.WriteLine(feed); // "300 m/min" + +// Compile-time safety: +// feed + inMmPerS; // error: different units — use .Add(...) +// feed.To>(); // error: not an ISpeedUnit +``` + +### Unit-Aware Angular Speed + +`AngularSpeed` mirrors `Speed` for rotation, on its own dimension with the SI +base rad/s. Keeping the two dimensions apart is the point: a rotary axis and a linear axis cannot be +commanded with each other's speed. + +```csharp +using ModelingEvolution.Drawing.Units; + +// The unit is part of the type — the value is stored in that unit. +AngularSpeed> tilt = new(5); // 5 °/s + +// Normalise to the SI base (rad/s) or convert to another angular-speed unit. +double si = tilt.ToSi(); // 0.0873 (rad/s) +var inRadPerS = tilt.To>(); + +// Signed: negating reverses the sense of rotation. +var reverse = -tilt; // -5 °/s + +// Scale by a percentage of a maximum, e.g. a jog knob at 20% of the axis limit. +var jog = tilt * new Percentage(20f); // 1 °/s + +Console.WriteLine(tilt); // "5 °/s" + +// Compile-time safety — the dimensions do not mix: +// tilt + new Speed>(20); // error: different types +// tilt.To>(); // error: not an IAngularSpeedUnit +// Speed> wrong; // error: not an ISpeedUnit +``` + +## Requirements + +- **.NET 10.0** or later (uses generic math interfaces from .NET 7+) +- **Dependencies**: [Clipper2](https://www.nuget.org/packages/Clipper2) 1.4.0, [protobuf-net](https://www.nuget.org/packages/protobuf-net) 3.2.45 + +## License + +See [LICENSE](LICENSE) for details. diff --git a/Sources/ModelingEvolution.Drawing/Rotation3.cs b/Sources/ModelingEvolution.Drawing/Rotation3.cs index 68a4c88..78015e9 100644 --- a/Sources/ModelingEvolution.Drawing/Rotation3.cs +++ b/Sources/ModelingEvolution.Drawing/Rotation3.cs @@ -9,6 +9,8 @@ namespace ModelingEvolution.Drawing; /// /// Represents a 3D rotation using Euler angles (Roll, Pitch, Yaw) in degrees. /// Convention: Rotation order is Z-Y-X (Yaw, Pitch, Roll) - also known as Tait-Bryan angles. +/// Since 2.0 this is a wire and edit format only, like : rotation maths uses +/// () or (epic-129 design, rule 2). /// /// The numeric type used for angles. [ProtoContract] @@ -188,64 +190,6 @@ public static Rotation3 FromQuaternion(Quaternion q) (Degree)Radian.FromRadian(rz)); } - /// - /// Rotates a vector by this rotation. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Vector3 Rotate(Vector3 v) - { - var q = ToQuaternion(); - return q.Rotate(v); - } - - /// - /// Rotates a point by this rotation around the origin. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Point3 Rotate(Point3 p) - { - var v = Rotate((Vector3)p); - return (Point3)v; - } - - /// - /// Returns a rotation with all angles negated. - /// Note: This is NOT the mathematical inverse for ZYX Euler angles. - /// Use for the true inverse rotation. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Rotation3 Negate() => new(-_rx, -_ry, -_rz); - - /// - /// Returns the inverse rotation (R such that Combine(this, R) ≈ Identity). - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Rotation3 Inverse() => FromQuaternion(ToQuaternion().Conjugate()); - - /// - /// Combines two rotations. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Rotation3 Combine(Rotation3 a, Rotation3 b) - { - var qa = a.ToQuaternion(); - var qb = b.ToQuaternion(); - var combined = qa * qb; - return FromQuaternion(combined); - } - - /// - /// Linearly interpolates between two rotations (spherical interpolation via quaternions). - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Rotation3 Slerp(Rotation3 a, Rotation3 b, T t) - { - var qa = a.ToQuaternion(); - var qb = b.ToQuaternion(); - var result = Quaternion.Slerp(qa, qb, t); - return FromQuaternion(result); - } - /// /// Converts this rotation to a different numeric type. /// @@ -258,10 +202,6 @@ public Rotation3 Truncating() #region Operators - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Rotation3 operator +(Rotation3 a, Rotation3 b) => Combine(a, b); - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Rotation3 operator -(Rotation3 r) => r.Inverse(); [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool operator ==(Rotation3 a, Rotation3 b) => a._rx == b._rx && a._ry == b._ry && a._rz == b._rz; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -270,20 +210,11 @@ public Rotation3 Truncating() #endregion /// - /// Converts this Euler rotation to a 3x3 rotation matrix (ZYX convention). + /// Creates a rotation from a 3x3 rotation matrix: , then + /// (the one rotation → Euler formula). /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Matrix3x3 ToMatrix3x3() => Matrix3x3.RotationZYX(_rx, _ry, _rz); - - /// - /// Creates a rotation from a 3x3 rotation matrix by extracting ZYX Euler angles. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Rotation3 FromMatrix3x3(Matrix3x3 m) - { - var (rx, ry, rz) = m.ToEulerZYX(); - return new Rotation3(rx, ry, rz); - } + public static Rotation3 FromMatrix3x3(Matrix3x3 m) => FromQuaternion(Quaternion.FromMatrix3x3(m)); #region Conversions @@ -292,13 +223,6 @@ public static Rotation3 FromMatrix3x3(Matrix3x3 m) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static implicit operator (T rx, T ry, T rz)(Rotation3 r) => ((T)r._rx, (T)r._ry, (T)r._rz); - /// - /// Converts a rotation to a unit vector (versor) representing the direction - /// that the +Z axis would point after applying this rotation. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static explicit operator Vector3(Rotation3 r) => r.Rotate(Vector3.EZ); - #endregion #region Equality & Formatting diff --git a/Sources/ModelingEvolution.Drawing/Trajectory3Builder.cs b/Sources/ModelingEvolution.Drawing/Trajectory3Builder.cs index 48da1aa..9979f79 100644 --- a/Sources/ModelingEvolution.Drawing/Trajectory3Builder.cs +++ b/Sources/ModelingEvolution.Drawing/Trajectory3Builder.cs @@ -1,429 +1,433 @@ -using System.Numerics; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; - -namespace ModelingEvolution.Drawing; - -/// -/// Fluent builder for constructing by appending waypoints. -/// Tracks current time and last pose for speed-based operations. -/// Supports corner blending via to create smooth arcs at waypoints. -/// -public sealed class Trajectory3Builder - where T : INumber, ITrigonometricFunctions, IRootFunctions, - IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue, IParsable -{ - private Waypoint3[] _buffer; - private Corner[] _corners; - private Speed[] _cornerSpeeds; - private int _count; - private T _currentTime; - private bool _hasCorners; - - private static readonly T Two = T.CreateTruncating(2); - private static readonly T Epsilon = T.CreateTruncating(1e-9); - - public Trajectory3Builder(int initialCapacity = 64) - { - _buffer = new Waypoint3[initialCapacity]; - _corners = new Corner[initialCapacity]; - _cornerSpeeds = new Speed[initialCapacity]; - _count = 0; - _currentTime = T.Zero; - } - - /// Gets the number of waypoints added so far. - public int Count => _count; - - /// Gets the current accumulated time. - public T CurrentTime => _currentTime; - - /// - /// Number of arc subdivision points per 90-degree turn. Default is 8. - /// Higher values produce smoother arcs. - /// - public int ArcResolution { get; set; } = 8; - - #region Add methods - - /// - /// Adds a waypoint with the given pose at the current accumulated time. - /// If this is the first waypoint, time starts at zero. - /// - public Trajectory3Builder Add(Pose3 pose) - { - EnsureCapacity(1); - _buffer[_count++] = new Waypoint3(pose, _currentTime); - return this; - } - - /// - /// Adds a waypoint with the given pose at an explicit time. - /// Updates the current accumulated time. - /// - public Trajectory3Builder Add(Pose3 pose, T time) - { - EnsureCapacity(1); - _buffer[_count++] = new Waypoint3(pose, time); - _currentTime = time; - return this; - } - - /// - /// Adds a waypoint directly. Updates the current accumulated time. - /// - public Trajectory3Builder Add(Waypoint3 waypoint) - { - EnsureCapacity(1); - _buffer[_count++] = waypoint; - _currentTime = waypoint.Time; - return this; - } - - /// - /// Adds a waypoint from raw position, rotation, and time values. - /// - public Trajectory3Builder Add(T x, T y, T z, T rx, T ry, T rz, T time) - { - return Add(new Pose3(x, y, z, rx, ry, rz), time); - } - - #endregion - - #region MoveTo methods - - /// - /// Moves to the given pose at the specified speed. - /// The time delta is computed as distance / speed from the last waypoint's position. - /// - public Trajectory3Builder MoveTo(Pose3 pose, Speed speed) - { - if (_count == 0) - return Add(pose); - - var lastPose = _buffer[_count - 1].Pose; - var distance = Point3.Distance(lastPose.Position, pose.Position); - var dt = distance / (T)speed; - _currentTime += dt; - - EnsureCapacity(1); - _buffer[_count++] = new Waypoint3(pose, _currentTime); - return this; - } - - /// - /// Moves to the given pose at the specified speed with a corner blend. - /// - public Trajectory3Builder MoveTo(Pose3 pose, Speed speed, Corner corner) - { - if (_count == 0) - return Add(pose); - - var lastPose = _buffer[_count - 1].Pose; - var distance = Point3.Distance(lastPose.Position, pose.Position); - var dt = distance / (T)speed; - _currentTime += dt; - - EnsureCapacity(1); - _corners[_count] = corner; - _hasCorners = _hasCorners || !corner.IsSharp; - _buffer[_count++] = new Waypoint3(pose, _currentTime); - return this; - } - - /// - /// Moves to the given pose at the specified speed with a corner blend and explicit corner speed. - /// - public Trajectory3Builder MoveTo(Pose3 pose, Speed speed, Corner corner, Speed cornerSpeed) - { - if (_count == 0) - return Add(pose); - - var lastPose = _buffer[_count - 1].Pose; - var distance = Point3.Distance(lastPose.Position, pose.Position); - var dt = distance / (T)speed; - _currentTime += dt; - - EnsureCapacity(1); - _corners[_count] = corner; - _cornerSpeeds[_count] = cornerSpeed; - _hasCorners = _hasCorners || !corner.IsSharp; - _buffer[_count++] = new Waypoint3(pose, _currentTime); - return this; - } - - /// - /// Moves to the given position and rotation at the specified speed. - /// - public Trajectory3Builder MoveTo(Point3 position, Rotation3 rotation, Speed speed) - => MoveTo(new Pose3(position, rotation), speed); - - /// - /// Moves to the given position and rotation at the specified speed with a corner blend. - /// - public Trajectory3Builder MoveTo(Point3 position, Rotation3 rotation, Speed speed, Corner corner) - => MoveTo(new Pose3(position, rotation), speed, corner); - - /// - /// Moves to the given position and rotation at the specified speed with a corner blend and explicit corner speed. - /// - public Trajectory3Builder MoveTo(Point3 position, Rotation3 rotation, Speed speed, Corner corner, Speed cornerSpeed) - => MoveTo(new Pose3(position, rotation), speed, corner, cornerSpeed); - - #endregion - - #region Append methods - - /// - /// Appends a span of waypoints. Updates the current accumulated time to the last waypoint's time. - /// - public Trajectory3Builder AddRange(ReadOnlySpan> waypoints) - { - EnsureCapacity(waypoints.Length); - waypoints.CopyTo(_buffer.AsSpan(_count)); - _count += waypoints.Length; - if (waypoints.Length > 0) - _currentTime = waypoints[^1].Time; - return this; - } - - /// - /// Appends all waypoints from an existing trajectory, shifting timestamps so the first - /// appended waypoint starts at the current accumulated time. - /// - public Trajectory3Builder AddTrajectory(in Trajectory3 trajectory) - { - var span = trajectory.AsSpan(); - if (span.Length == 0) return this; - - var offset = _currentTime - span[0].Time; - EnsureCapacity(span.Length); - for (int i = 0; i < span.Length; i++) - _buffer[_count++] = new Waypoint3(span[i].Pose, span[i].Time + offset); - _currentTime = span[^1].Time + offset; - return this; - } - - /// - /// Appends a pose path with uniformly distributed timestamps between startTime and endTime. - /// - public Trajectory3Builder AddPath(in PosePath3 path, T startTime, T endTime) - { - var span = path.AsSpan(); - if (span.Length == 0) return this; - if (span.Length == 1) - return Add(span[0], startTime); - - var n = T.CreateTruncating(span.Length - 1); - EnsureCapacity(span.Length); - for (int i = 0; i < span.Length; i++) - { - var t = startTime + (endTime - startTime) * T.CreateTruncating(i) / n; - _buffer[_count++] = new Waypoint3(span[i], t); - } - _currentTime = endTime; - return this; - } - - #endregion - - #region Build - - /// - /// Builds an immutable from the accumulated waypoints. - /// Corners with are expanded into smooth arcs. - /// Uses to respect active . - /// - public Trajectory3 Build() - { - if (_count == 0) return new Trajectory3(); - - // Fast path: no corners to expand - if (!_hasCorners) - { - var mem = Alloc.Memory>(_count); - _buffer.AsSpan(0, _count).CopyTo(mem.Span); - return new Trajectory3(mem); - } - - // Slow path: expand corners into arc waypoints - var output = new List>(_count * 2); - var timeShift = T.Zero; - - for (int i = 0; i < _count; i++) - { - // First and last waypoints cannot have corners - if (i == 0 || i == _count - 1 || _corners[i].IsSharp) - { - output.Add(new Waypoint3(_buffer[i].Pose, _buffer[i].Time + timeShift)); - continue; - } - - ExpandCorner(output, i, ref timeShift); - } - - var result = Alloc.Memory>(output.Count); - CollectionsMarshal.AsSpan(output).CopyTo(result.Span); - return new Trajectory3(result); - } - - private void ExpandCorner(List> output, int index, ref T timeShift) - { - var prev = _buffer[index - 1]; - var curr = _buffer[index]; - var next = _buffer[index + 1]; - var corner = _corners[index]; - var cornerSpeed = _cornerSpeeds[index]; - - var prevPos = prev.Position; - var currPos = curr.Position; - var nextPos = next.Position; - - // Directions from corner toward adjacent waypoints - var dIn = prevPos - currPos; // Vector3: from curr toward prev - var dOut = nextPos - currPos; // Vector3: from curr toward next - var distIn = dIn.Length; - var distOut = dOut.Length; - - // Clamp radius to half the shorter adjacent segment (prevents overlap) - var maxR = T.Min(distIn, distOut) / Two; - var r = T.Min(corner.Radius, maxR); - - if (distIn < Epsilon || distOut < Epsilon || r < Epsilon) - { - output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); - return; - } - - var dInNorm = dIn / distIn; - var dOutNorm = dOut / distOut; - - // Angle between the two directions from the corner vertex (interior angle) - var cosTheta = T.Clamp(Vector3.Dot(dInNorm, dOutNorm), -T.One, T.One); - var theta = T.Acos(cosTheta); - - // Deflection angle (how much the path actually turns) - var alpha = T.Pi - theta; - - // If nearly straight, skip blending - if (alpha < T.CreateTruncating(0.01)) - { - output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); - return; - } - - // Blend start/end points - var p1 = currPos + dInNorm * r; // on incoming segment, r from corner - var p2 = currPos + dOutNorm * r; // on outgoing segment, r from corner - - // Arc geometry: inscribed circle tangent to both segments at P1 and P2 - var halfTheta = theta / Two; - var sinHalf = T.Sin(halfTheta); - var cosHalf = T.Cos(halfTheta); - - if (T.Abs(cosHalf) < Epsilon) - { - // Degenerate case (U-turn), fall back to sharp corner - output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); - return; - } - - var arcRadius = r * sinHalf / cosHalf; // R = r * tan(theta/2) - var bisector = dInNorm + dOutNorm; - var bisectorLen = bisector.Length; - - if (bisectorLen < Epsilon) - { - output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); - return; - } - - bisector = bisector / bisectorLen; - var centerDist = r / cosHalf; - var arcCenter = currPos + bisector * centerDist; - - // Radius vectors from arc center to blend points - var v1 = p1 - arcCenter; - var v2 = p2 - arcCenter; - - // Rotation axis: cross(v1, v2) gives correct direction for P1→P2 arc - var arcNormal = Vector3.Cross(v1, v2); - var arcNormalLen = arcNormal.Length; - - if (arcNormalLen < Epsilon) - { - output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); - return; - } - - arcNormal = arcNormal / arcNormalLen; - - // Timestamps (with accumulated shift from previous corners) - var prevTime = prev.Time + timeShift; - var currTime = curr.Time + timeShift; - var nextTime = next.Time + timeShift; - - // P1 time: proportional position on incoming segment - var t_P1 = prevTime + (currTime - prevTime) * (distIn - r) / distIn; - // P2 natural time: proportional position on outgoing segment - var t_P2_natural = currTime + (nextTime - currTime) * r / distOut; - - // Arc length and duration - var arcLen = arcRadius * alpha; - var naturalDuration = t_P2_natural - t_P1; - T arcDuration; - - if (cornerSpeed._val > T.Zero) - arcDuration = arcLen / cornerSpeed._val; - else - arcDuration = naturalDuration; - - var t_P2 = t_P1 + arcDuration; - - // Update cumulative time shift - timeShift += arcDuration - naturalDuration; - - // Rotation interpolation: compute rotations at blend start/end - var rotFracIn = (distIn - r) / distIn; // how far P1 is along prev→curr (0..1) - var rotFracOut = r / distOut; // how far P2 is along curr→next (0..1) - var rotP1 = Rotation3.Slerp(prev.Rotation, curr.Rotation, rotFracIn); - var rotP2 = Rotation3.Slerp(curr.Rotation, next.Rotation, rotFracOut); - - // Number of arc points (proportional to turn angle) - int arcPointCount = Math.Max(3, - (int)Math.Ceiling(double.CreateTruncating(alpha) / (Math.PI / 2.0) * ArcResolution)); - - // Generate arc points via Rodrigues rotation - for (int j = 0; j < arcPointCount; j++) - { - var t = T.CreateTruncating(j) / T.CreateTruncating(arcPointCount - 1); - - // Rotate v1 around arcNormal by angle (alpha * t) - var angle = alpha * t; - var cosA = T.Cos(angle); - var sinA = T.Sin(angle); - var crossNV1 = Vector3.Cross(arcNormal, v1); - var vRot = v1 * cosA + crossNV1 * sinA; - - var arcPos = arcCenter + vRot; - var arcTime = t_P1 + arcDuration * t; - var arcRot = Rotation3.Slerp(rotP1, rotP2, t); - - output.Add(new Waypoint3(new Pose3(arcPos, arcRot), arcTime)); - } - } - - #endregion - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private void EnsureCapacity(int additional) - { - var required = _count + additional; - if (required <= _buffer.Length) return; - var newSize = _buffer.Length; - while (newSize < required) newSize *= 2; - Array.Resize(ref _buffer, newSize); - Array.Resize(ref _corners, newSize); - Array.Resize(ref _cornerSpeeds, newSize); - } -} +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +namespace ModelingEvolution.Drawing; + +/// +/// Fluent builder for constructing by appending waypoints. +/// Tracks current time and last pose for speed-based operations. +/// Supports corner blending via to create smooth arcs at waypoints. +/// +public sealed class Trajectory3Builder + where T : INumber, ITrigonometricFunctions, IRootFunctions, + IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue, IParsable +{ + private Waypoint3[] _buffer; + private Corner[] _corners; + private Speed[] _cornerSpeeds; + private int _count; + private T _currentTime; + private bool _hasCorners; + + private static readonly T Two = T.CreateTruncating(2); + private static readonly T Epsilon = T.CreateTruncating(1e-9); + + public Trajectory3Builder(int initialCapacity = 64) + { + _buffer = new Waypoint3[initialCapacity]; + _corners = new Corner[initialCapacity]; + _cornerSpeeds = new Speed[initialCapacity]; + _count = 0; + _currentTime = T.Zero; + } + + /// Gets the number of waypoints added so far. + public int Count => _count; + + /// Gets the current accumulated time. + public T CurrentTime => _currentTime; + + /// + /// Number of arc subdivision points per 90-degree turn. Default is 8. + /// Higher values produce smoother arcs. + /// + public int ArcResolution { get; set; } = 8; + + #region Add methods + + /// + /// Adds a waypoint with the given pose at the current accumulated time. + /// If this is the first waypoint, time starts at zero. + /// + public Trajectory3Builder Add(Pose3 pose) + { + EnsureCapacity(1); + _buffer[_count++] = new Waypoint3(pose, _currentTime); + return this; + } + + /// + /// Adds a waypoint with the given pose at an explicit time. + /// Updates the current accumulated time. + /// + public Trajectory3Builder Add(Pose3 pose, T time) + { + EnsureCapacity(1); + _buffer[_count++] = new Waypoint3(pose, time); + _currentTime = time; + return this; + } + + /// + /// Adds a waypoint directly. Updates the current accumulated time. + /// + public Trajectory3Builder Add(Waypoint3 waypoint) + { + EnsureCapacity(1); + _buffer[_count++] = waypoint; + _currentTime = waypoint.Time; + return this; + } + + /// + /// Adds a waypoint from raw position, rotation, and time values. + /// + public Trajectory3Builder Add(T x, T y, T z, T rx, T ry, T rz, T time) + { + return Add(new Pose3(x, y, z, rx, ry, rz), time); + } + + #endregion + + #region MoveTo methods + + /// + /// Moves to the given pose at the specified speed. + /// The time delta is computed as distance / speed from the last waypoint's position. + /// + public Trajectory3Builder MoveTo(Pose3 pose, Speed speed) + { + if (_count == 0) + return Add(pose); + + var lastPose = _buffer[_count - 1].Pose; + var distance = Point3.Distance(lastPose.Position, pose.Position); + var dt = distance / (T)speed; + _currentTime += dt; + + EnsureCapacity(1); + _buffer[_count++] = new Waypoint3(pose, _currentTime); + return this; + } + + /// + /// Moves to the given pose at the specified speed with a corner blend. + /// + public Trajectory3Builder MoveTo(Pose3 pose, Speed speed, Corner corner) + { + if (_count == 0) + return Add(pose); + + var lastPose = _buffer[_count - 1].Pose; + var distance = Point3.Distance(lastPose.Position, pose.Position); + var dt = distance / (T)speed; + _currentTime += dt; + + EnsureCapacity(1); + _corners[_count] = corner; + _hasCorners = _hasCorners || !corner.IsSharp; + _buffer[_count++] = new Waypoint3(pose, _currentTime); + return this; + } + + /// + /// Moves to the given pose at the specified speed with a corner blend and explicit corner speed. + /// + public Trajectory3Builder MoveTo(Pose3 pose, Speed speed, Corner corner, Speed cornerSpeed) + { + if (_count == 0) + return Add(pose); + + var lastPose = _buffer[_count - 1].Pose; + var distance = Point3.Distance(lastPose.Position, pose.Position); + var dt = distance / (T)speed; + _currentTime += dt; + + EnsureCapacity(1); + _corners[_count] = corner; + _cornerSpeeds[_count] = cornerSpeed; + _hasCorners = _hasCorners || !corner.IsSharp; + _buffer[_count++] = new Waypoint3(pose, _currentTime); + return this; + } + + /// + /// Moves to the given position and rotation at the specified speed. + /// + public Trajectory3Builder MoveTo(Point3 position, Rotation3 rotation, Speed speed) + => MoveTo(new Pose3(position, rotation), speed); + + /// + /// Moves to the given position and rotation at the specified speed with a corner blend. + /// + public Trajectory3Builder MoveTo(Point3 position, Rotation3 rotation, Speed speed, Corner corner) + => MoveTo(new Pose3(position, rotation), speed, corner); + + /// + /// Moves to the given position and rotation at the specified speed with a corner blend and explicit corner speed. + /// + public Trajectory3Builder MoveTo(Point3 position, Rotation3 rotation, Speed speed, Corner corner, Speed cornerSpeed) + => MoveTo(new Pose3(position, rotation), speed, corner, cornerSpeed); + + #endregion + + #region Append methods + + /// + /// Appends a span of waypoints. Updates the current accumulated time to the last waypoint's time. + /// + public Trajectory3Builder AddRange(ReadOnlySpan> waypoints) + { + EnsureCapacity(waypoints.Length); + waypoints.CopyTo(_buffer.AsSpan(_count)); + _count += waypoints.Length; + if (waypoints.Length > 0) + _currentTime = waypoints[^1].Time; + return this; + } + + /// + /// Appends all waypoints from an existing trajectory, shifting timestamps so the first + /// appended waypoint starts at the current accumulated time. + /// + public Trajectory3Builder AddTrajectory(in Trajectory3 trajectory) + { + var span = trajectory.AsSpan(); + if (span.Length == 0) return this; + + var offset = _currentTime - span[0].Time; + EnsureCapacity(span.Length); + for (int i = 0; i < span.Length; i++) + _buffer[_count++] = new Waypoint3(span[i].Pose, span[i].Time + offset); + _currentTime = span[^1].Time + offset; + return this; + } + + /// + /// Appends a pose path with uniformly distributed timestamps between startTime and endTime. + /// + public Trajectory3Builder AddPath(in PosePath3 path, T startTime, T endTime) + { + var span = path.AsSpan(); + if (span.Length == 0) return this; + if (span.Length == 1) + return Add(span[0], startTime); + + var n = T.CreateTruncating(span.Length - 1); + EnsureCapacity(span.Length); + for (int i = 0; i < span.Length; i++) + { + var t = startTime + (endTime - startTime) * T.CreateTruncating(i) / n; + _buffer[_count++] = new Waypoint3(span[i], t); + } + _currentTime = endTime; + return this; + } + + #endregion + + #region Build + + /// + /// Builds an immutable from the accumulated waypoints. + /// Corners with are expanded into smooth arcs. + /// Uses to respect active . + /// + public Trajectory3 Build() + { + if (_count == 0) return new Trajectory3(); + + // Fast path: no corners to expand + if (!_hasCorners) + { + var mem = Alloc.Memory>(_count); + _buffer.AsSpan(0, _count).CopyTo(mem.Span); + return new Trajectory3(mem); + } + + // Slow path: expand corners into arc waypoints + var output = new List>(_count * 2); + var timeShift = T.Zero; + + for (int i = 0; i < _count; i++) + { + // First and last waypoints cannot have corners + if (i == 0 || i == _count - 1 || _corners[i].IsSharp) + { + output.Add(new Waypoint3(_buffer[i].Pose, _buffer[i].Time + timeShift)); + continue; + } + + ExpandCorner(output, i, ref timeShift); + } + + var result = Alloc.Memory>(output.Count); + CollectionsMarshal.AsSpan(output).CopyTo(result.Span); + return new Trajectory3(result); + } + + private void ExpandCorner(List> output, int index, ref T timeShift) + { + var prev = _buffer[index - 1]; + var curr = _buffer[index]; + var next = _buffer[index + 1]; + var corner = _corners[index]; + var cornerSpeed = _cornerSpeeds[index]; + + var prevPos = prev.Position; + var currPos = curr.Position; + var nextPos = next.Position; + + // Directions from corner toward adjacent waypoints + var dIn = prevPos - currPos; // Vector3: from curr toward prev + var dOut = nextPos - currPos; // Vector3: from curr toward next + var distIn = dIn.Length; + var distOut = dOut.Length; + + // Clamp radius to half the shorter adjacent segment (prevents overlap) + var maxR = T.Min(distIn, distOut) / Two; + var r = T.Min(corner.Radius, maxR); + + if (distIn < Epsilon || distOut < Epsilon || r < Epsilon) + { + output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); + return; + } + + var dInNorm = dIn / distIn; + var dOutNorm = dOut / distOut; + + // Angle between the two directions from the corner vertex (interior angle) + var cosTheta = T.Clamp(Vector3.Dot(dInNorm, dOutNorm), -T.One, T.One); + var theta = T.Acos(cosTheta); + + // Deflection angle (how much the path actually turns) + var alpha = T.Pi - theta; + + // If nearly straight, skip blending + if (alpha < T.CreateTruncating(0.01)) + { + output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); + return; + } + + // Blend start/end points + var p1 = currPos + dInNorm * r; // on incoming segment, r from corner + var p2 = currPos + dOutNorm * r; // on outgoing segment, r from corner + + // Arc geometry: inscribed circle tangent to both segments at P1 and P2 + var halfTheta = theta / Two; + var sinHalf = T.Sin(halfTheta); + var cosHalf = T.Cos(halfTheta); + + if (T.Abs(cosHalf) < Epsilon) + { + // Degenerate case (U-turn), fall back to sharp corner + output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); + return; + } + + var arcRadius = r * sinHalf / cosHalf; // R = r * tan(theta/2) + var bisector = dInNorm + dOutNorm; + var bisectorLen = bisector.Length; + + if (bisectorLen < Epsilon) + { + output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); + return; + } + + bisector = bisector / bisectorLen; + var centerDist = r / cosHalf; + var arcCenter = currPos + bisector * centerDist; + + // Radius vectors from arc center to blend points + var v1 = p1 - arcCenter; + var v2 = p2 - arcCenter; + + // Rotation axis: cross(v1, v2) gives correct direction for P1→P2 arc + var arcNormal = Vector3.Cross(v1, v2); + var arcNormalLen = arcNormal.Length; + + if (arcNormalLen < Epsilon) + { + output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); + return; + } + + arcNormal = arcNormal / arcNormalLen; + + // Timestamps (with accumulated shift from previous corners) + var prevTime = prev.Time + timeShift; + var currTime = curr.Time + timeShift; + var nextTime = next.Time + timeShift; + + // P1 time: proportional position on incoming segment + var t_P1 = prevTime + (currTime - prevTime) * (distIn - r) / distIn; + // P2 natural time: proportional position on outgoing segment + var t_P2_natural = currTime + (nextTime - currTime) * r / distOut; + + // Arc length and duration + var arcLen = arcRadius * alpha; + var naturalDuration = t_P2_natural - t_P1; + T arcDuration; + + if (cornerSpeed._val > T.Zero) + arcDuration = arcLen / cornerSpeed._val; + else + arcDuration = naturalDuration; + + var t_P2 = t_P1 + arcDuration; + + // Update cumulative time shift + timeShift += arcDuration - naturalDuration; + + // Rotation interpolation: compute rotations at blend start/end + var rotFracIn = (distIn - r) / distIn; // how far P1 is along prev→curr (0..1) + var rotFracOut = r / distOut; // how far P2 is along curr→next (0..1) + // Quaternion slerp on the stored wire poses (lengthScale 1: only the rotations are used) + var qPrev = Frame3.FromPose3(prev.Pose, T.One).Rotation; + var qCurr = Frame3.FromPose3(curr.Pose, T.One).Rotation; + var qNext = Frame3.FromPose3(next.Pose, T.One).Rotation; + var rotP1 = Quaternion.Slerp(qPrev, qCurr, rotFracIn); + var rotP2 = Quaternion.Slerp(qCurr, qNext, rotFracOut); + + // Number of arc points (proportional to turn angle) + int arcPointCount = Math.Max(3, + (int)Math.Ceiling(double.CreateTruncating(alpha) / (Math.PI / 2.0) * ArcResolution)); + + // Generate arc points via Rodrigues rotation + for (int j = 0; j < arcPointCount; j++) + { + var t = T.CreateTruncating(j) / T.CreateTruncating(arcPointCount - 1); + + // Rotate v1 around arcNormal by angle (alpha * t) + var angle = alpha * t; + var cosA = T.Cos(angle); + var sinA = T.Sin(angle); + var crossNV1 = Vector3.Cross(arcNormal, v1); + var vRot = v1 * cosA + crossNV1 * sinA; + + var arcPos = arcCenter + vRot; + var arcTime = t_P1 + arcDuration * t; + var arcRot = Quaternion.Slerp(rotP1, rotP2, t); + + output.Add(new Waypoint3(new Frame3(arcPos, arcRot).ToPose3(T.One), arcTime)); + } + } + + #endregion + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private void EnsureCapacity(int additional) + { + var required = _count + additional; + if (required <= _buffer.Length) return; + var newSize = _buffer.Length; + while (newSize < required) newSize *= 2; + Array.Resize(ref _buffer, newSize); + Array.Resize(ref _corners, newSize); + Array.Resize(ref _cornerSpeeds, newSize); + } +} diff --git a/Sources/ModelingEvolution.Drawing/Vector3.cs b/Sources/ModelingEvolution.Drawing/Vector3.cs index 5ce7397..284ba78 100644 --- a/Sources/ModelingEvolution.Drawing/Vector3.cs +++ b/Sources/ModelingEvolution.Drawing/Vector3.cs @@ -194,7 +194,7 @@ public static T AngleBetween(Vector3 a, Vector3 b) /// Computes the rotation that transforms this vector to align with the target vector. /// /// The target direction to rotate towards. - /// A rotation R such that R.Rotate(this.Normalize()) ≈ target.Normalize(). + /// A rotation R such that R.ToQuaternion().Rotate(this.Normalize()) ≈ target.Normalize(). /// /// When vectors are parallel, returns identity. When anti-parallel, rotation axis is chosen arbitrarily. /// @@ -231,42 +231,11 @@ public Rotation3 RotationTo(Vector3 target) } /// - /// Creates a Rotation3 from axis-angle representation. + /// Creates a Rotation3 from axis-angle representation, through and the + /// one rotation → Euler formula, . /// - private static Rotation3 RotationFromAxisAngle(Vector3 axis, T angleRadians) - { - var halfAngle = angleRadians / T.CreateTruncating(2); - var s = T.Sin(halfAngle); - var c = T.Cos(halfAngle); - - // Quaternion components - var qw = c; - var qx = axis.X * s; - var qy = axis.Y * s; - var qz = axis.Z * s; - - // Convert quaternion to Euler angles (ZYX convention) - // Roll (X) - var sinRoll = T.CreateTruncating(2) * (qw * qx + qy * qz); - var cosRoll = T.One - T.CreateTruncating(2) * (qx * qx + qy * qy); - var rx = T.Atan2(sinRoll, cosRoll); - - // Pitch (Y) - var sinPitch = T.CreateTruncating(2) * (qw * qy - qz * qx); - sinPitch = T.Clamp(sinPitch, -T.One, T.One); - var ry = T.Asin(sinPitch); - - // Yaw (Z) - var sinYaw = T.CreateTruncating(2) * (qw * qz + qx * qy); - var cosYaw = T.One - T.CreateTruncating(2) * (qy * qy + qz * qz); - var rz = T.Atan2(sinYaw, cosYaw); - - // rx, ry, rz are in radians — convert via Radian → Degree - return new Rotation3( - (Degree)Radian.FromRadian(rx), - (Degree)Radian.FromRadian(ry), - (Degree)Radian.FromRadian(rz)); - } + private static Rotation3 RotationFromAxisAngle(Vector3 axis, T angleRadians) => + Rotation3.FromQuaternion(Quaternion.FromAxisAngle(axis, angleRadians)); /// /// Calculates the projection of this vector onto another vector. From 30642b82cd65ee82d0e02c51d90e8c34c11e9dc8 Mon Sep 17 00:00:00 2001 From: Rafal Maciag Date: Sat, 24 Oct 2026 18:54:26 +0000 Subject: [PATCH 4/6] =?UTF-8?q?S3:=20delete=20the=20last=20direction=20?= =?UTF-8?q?=E2=86=92=20Euler=20shortcuts=20on=20Vector3=20(epic-129)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Vector3.RotationTo, the explicit Vector3 → Rotation3 cast (a second direction → Euler formula) and operator +(Vector3, Rotation3) → Pose3 are deleted; the rotation between two directions is Quaternion.FromTo. - Matrix3x3.SimilarityFromVectors = |t|/|s| · Quaternion.FromTo(s, t).ToMatrix3x3(). - EulerSurfaceTests: Vector3 and Matrix3x3 take or return no Rotation3/Pose3. - grep gate G-19d also rejects \bRotationTo\s*\( in Sources/. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_014wH5aL3TZhJNvLqa8YTku8 --- .github/scripts/grep-gate.sh | 5 +- README.md | 2 +- .../EulerSurfaceTests.cs | 47 +++++ .../Matrix3x3SimilarityTests.cs | 28 ++- .../Rotation3Tests.cs | 6 +- .../Vector3Tests.cs | 185 ------------------ .../ModelingEvolution.Drawing/Matrix3x3.cs | 11 +- Sources/ModelingEvolution.Drawing/README.md | 2 +- Sources/ModelingEvolution.Drawing/Vector3.cs | 88 --------- 9 files changed, 82 insertions(+), 292 deletions(-) create mode 100644 Sources/ModelingEvolution.Drawing.Tests/EulerSurfaceTests.cs diff --git a/.github/scripts/grep-gate.sh b/.github/scripts/grep-gate.sh index 20605ed..1ae46b0 100755 --- a/.github/scripts/grep-gate.sh +++ b/.github/scripts/grep-gate.sh @@ -7,8 +7,9 @@ cd "$(git rev-parse --show-toplevel)" ROWS=( $'G-17\t\\b(TransformPoint|TransformVector|Inverse|Multiply|Lerp|Distance|FromSurface)\\s*[<(]|operator\\s*[-+*]\tSources/ModelingEvolution.Drawing/Pose3.cs' $'G-18\tPose3(<[^>]*>)?\\.(Lerp|Distance|FromSurface)\\b\tSources/' - # G-19d (epic-129 Q7; the lead renumbers it in the docs): no Euler rotation algebra on Rotation3 or a Pose3's rotation. - $'G-19d\tRotation3(<[^>]*>)?\\.(Combine|Slerp)\\b|\\.Rotation\\.(Rotate|Inverse|Combine)\\(\tSources/' + # G-19d (epic-129 Q7; the lead renumbers it in the docs): no Euler rotation algebra on Rotation3 or a Pose3's rotation, + # and no direction → Euler shortcut (Vector3.RotationTo is deleted; use Quaternion.FromTo). + $'G-19d\tRotation3(<[^>]*>)?\\.(Combine|Slerp)\\b|\\.Rotation\\.(Rotate|Inverse|Combine)\\(|\\bRotationTo\\s*\\(\tSources/' ) failed=0 diff --git a/README.md b/README.md index 20bb0f0..4ebe804 100644 --- a/README.md +++ b/README.md @@ -142,7 +142,7 @@ Pose3 atHalf = trajectory.AtTime(2.5); // interpolated pose | Type | Description | |---|---| | `Point3` | 3D Cartesian point. Distance, lerp, midpoint, random generation, arithmetic with `Vector3`. Conversions to/from `System.Numerics.Vector3` and tuples. Protobuf and JSON serialization. | -| `Vector3` | 3D vector with length, normalize, dot product, cross product, angle between, projection, lerp, random unit vector, and `RotationTo()` for computing the rotation between two directions. Static basis vectors `EX`, `EY`, `EZ`. | +| `Vector3` | 3D vector with length, normalize, dot product, cross product, angle between, projection, lerp, random unit vector. The rotation between two directions is `Quaternion.FromTo(from, to)`. Static basis vectors `EX`, `EY`, `EZ`. | | `Matrix3x3` | 3x3 matrix for 3D transforms. Determinant, trace, transpose, inverse. Rotation factories: `RotationX`, `RotationY`, `RotationZ`, `RotationZYX`. Scale, column/row construction. Euler angle and quaternion extraction. | | `Quaternion` | Quaternion for 3D rotations. Normalize, conjugate, inverse, from axis-angle, rotate vector, slerp, dot product. Conversions to/from `System.Numerics.Quaternion`. | | `Segment3` | Bounded line segment in 3D. Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | diff --git a/Sources/ModelingEvolution.Drawing.Tests/EulerSurfaceTests.cs b/Sources/ModelingEvolution.Drawing.Tests/EulerSurfaceTests.cs new file mode 100644 index 0000000..d7425d2 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/EulerSurfaceTests.cs @@ -0,0 +1,47 @@ +using System.Reflection; +using FluentAssertions; + +namespace ModelingEvolution.Drawing.Tests; + +/// +/// Epic-129 Q7: and are wire and edit formats. The vector and matrix +/// maths types neither take nor return them; direction → rotation is . +/// +public class EulerSurfaceTests +{ + private const BindingFlags All = BindingFlags.Public | BindingFlags.NonPublic | BindingFlags.Static | BindingFlags.Instance | BindingFlags.DeclaredOnly; + + private static bool IsWireType(Type t) + { + if (t.IsByRef) t = t.GetElementType()!; + return t.IsGenericType && (t.GetGenericTypeDefinition() == typeof(Rotation3<>) || t.GetGenericTypeDefinition() == typeof(Pose3<>)); + } + + private static IEnumerable MembersTouchingWireTypes(Type type) + { + foreach (var m in type.GetMethods(All)) + if (IsWireType(m.ReturnType) || m.GetParameters().Any(p => IsWireType(p.ParameterType))) + yield return m.ToString()!; + foreach (var c in type.GetConstructors(All)) + if (c.GetParameters().Any(p => IsWireType(p.ParameterType))) + yield return c.ToString()!; + foreach (var p in type.GetProperties(All)) + if (IsWireType(p.PropertyType)) + yield return p.ToString()!; + foreach (var f in type.GetFields(All)) + if (IsWireType(f.FieldType)) + yield return f.ToString()!; + } + + [Fact] + public void Vector3_HasNoMemberTakingOrReturningRotation3OrPose3() + { + MembersTouchingWireTypes(typeof(Vector3)).Should().BeEmpty(); + } + + [Fact] + public void Matrix3x3_HasNoMemberTakingOrReturningRotation3OrPose3() + { + MembersTouchingWireTypes(typeof(Matrix3x3)).Should().BeEmpty(); + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3SimilarityTests.cs b/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3SimilarityTests.cs index 49e42e3..60972c0 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3SimilarityTests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3SimilarityTests.cs @@ -70,18 +70,34 @@ public void ZeroSource_Throws() } [Fact] - public void AntiParallel_HandledByRotationTo() + public void ZeroTarget_Throws() { - // source = +X, target = -X — 180° rotation around some perpendicular axis var source = new Vector3(1, 0, 0); - var target = new Vector3(-1, 0, 0); + Action act = () => Matrix3x3.SimilarityFromVectors(source, Vector3.Zero); + act.Should().Throw(); + } + + [Theory] + [InlineData(1, 0, 0)] + [InlineData(0, 0, 1)] + [InlineData(0.3, -0.4, 0.5)] + public void AntiParallel_IsAHalfTurnMappingSourceToTarget(double x, double y, double z) + { + // Opposite vectors: the rotation is Quaternion.FromTo's half-turn. Its axis is normalize(EX × source), or + // normalize(EY × source) when source is parallel to EX (so +X → -X turns about -Z). The test asserts only + // what every choice satisfies: source maps to target and the rotation is a half-turn (trace = -1). + var source = new Vector3(x, y, z); + var target = -2 * source; var M = Matrix3x3.SimilarityFromVectors(source, target); var result = M.Transform(source); - result.X.Should().BeApproximately(target.X, 1e-6); - result.Y.Should().BeApproximately(target.Y, 1e-6); - result.Z.Should().BeApproximately(target.Z, 1e-6); + result.X.Should().BeApproximately(target.X, 1e-12); + result.Y.Should().BeApproximately(target.Y, 1e-12); + result.Z.Should().BeApproximately(target.Z, 1e-12); + var R = 0.5 * M; + (R.M11 + R.M22 + R.M33).Should().BeApproximately(-1, 1e-12); + R.Determinant.Should().BeApproximately(1, 1e-12); } [Fact] diff --git a/Sources/ModelingEvolution.Drawing.Tests/Rotation3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Rotation3Tests.cs index 123a242..0866aa6 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Rotation3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Rotation3Tests.cs @@ -357,12 +357,12 @@ public void Rotation3_ToVector3_ReturnsUnitVector() } [Fact] - public void Rotation3_ToVector3_RoundTrips_WithVector3() + public void Rotation3_FromTo_RoundTrips_WithVector3() { - // Start with a direction, convert to rotation, convert back + // Start with a direction, take the rotation that turns +Z onto it, store it as Euler (wire) and turn +Z back var originalDir = new Vector3(1, 1, 1).Normalize(); - var rotation = (Rotation3)originalDir; + var rotation = Rotation3.FromQuaternion(Quaternion.FromTo(Vector3.EZ, originalDir)); var backToDir = rotation.ToQuaternion().Rotate(Vector3.EZ); backToDir.X.Should().BeApproximately(originalDir.X, 1e-6); diff --git a/Sources/ModelingEvolution.Drawing.Tests/Vector3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Vector3Tests.cs index 210fd28..c30f1f2 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Vector3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Vector3Tests.cs @@ -250,82 +250,6 @@ public void Vector3_ProjectOnto_Works() #endregion - #region RotationTo - - [Fact] - public void Vector3_RotationTo_EZtoEX_Rotates90DegreesAroundY() - { - var rotation = Vector3.EZ.RotationTo(Vector3.EX); - - var result = rotation.ToQuaternion().Rotate(Vector3.EZ); - - result.X.Should().BeApproximately(1, 1e-6); - result.Y.Should().BeApproximately(0, 1e-6); - result.Z.Should().BeApproximately(0, 1e-6); - } - - [Fact] - public void Vector3_RotationTo_EZtoEY_Rotates90DegreesAroundX() - { - var rotation = Vector3.EZ.RotationTo(Vector3.EY); - - var result = rotation.ToQuaternion().Rotate(Vector3.EZ); - - result.X.Should().BeApproximately(0, 1e-6); - result.Y.Should().BeApproximately(1, 1e-6); - result.Z.Should().BeApproximately(0, 1e-6); - } - - [Fact] - public void Vector3_RotationTo_ParallelVectors_ReturnsIdentity() - { - var rotation = Vector3.EZ.RotationTo(Vector3.EZ); - - rotation.Should().Be(Rotation3.Identity); - } - - [Fact] - public void Vector3_RotationTo_AntiParallelVectors_Rotates180Degrees() - { - var rotation = Vector3.EZ.RotationTo(-Vector3.EZ); - - var result = rotation.ToQuaternion().Rotate(Vector3.EZ); - - result.X.Should().BeApproximately(0, 1e-6); - result.Y.Should().BeApproximately(0, 1e-6); - result.Z.Should().BeApproximately(-1, 1e-6); - } - - [Fact] - public void Vector3_RotationTo_ArbitraryVectors_AlignsCorrectly() - { - var from = new Vector3(1, 2, 3).Normalize(); - var to = new Vector3(-2, 1, 0.5).Normalize(); - - var rotation = from.RotationTo(to); - var result = rotation.ToQuaternion().Rotate(from); - - result.X.Should().BeApproximately(to.X, 1e-6); - result.Y.Should().BeApproximately(to.Y, 1e-6); - result.Z.Should().BeApproximately(to.Z, 1e-6); - } - - [Fact] - public void Vector3_RotationTo_NonUnitVectors_NormalizesFirst() - { - var from = new Vector3(0, 0, 5); // length 5 - var to = new Vector3(10, 0, 0); // length 10 - - var rotation = from.RotationTo(to); - var result = rotation.ToQuaternion().Rotate(Vector3.EZ); - - result.X.Should().BeApproximately(1, 1e-6); - result.Y.Should().BeApproximately(0, 1e-6); - result.Z.Should().BeApproximately(0, 1e-6); - } - - #endregion - #region Interpolation [Fact] @@ -356,115 +280,6 @@ public void Vector3_UnitVectors_AreCorrect() #endregion - #region Pose3 Operators - - [Fact] - public void Vector3_AddRotation_ReturnsPose3() - { - var vector = new Vector3(10, 20, 30); - var rotation = new Rotation3(45, 90, -30); - - Pose3 pose = vector + rotation; - - pose.X.Should().Be(10); - pose.Y.Should().Be(20); - pose.Z.Should().Be(30); - pose.Rx.Should().Be(45); - pose.Ry.Should().Be(90); - pose.Rz.Should().Be(-30); - } - - #endregion - - #region Vector3 to Rotation3 Conversion - - [Fact] - public void Vector3_ToRotation3_ZAxisDirection_ReturnsZeroRotation() - { - // +Z direction should result in identity rotation (no rotation needed) - var z = Vector3.EZ; - - var rotation = (Rotation3)z; - - // When pointing in +Z, yaw can be any value (atan2(0,0)) but pitch should be 0 - // The result should rotate +Z to +Z (identity-like) - var resultDir = rotation.ToQuaternion().Rotate(Vector3.EZ); - resultDir.Z.Should().BeApproximately(1, 1e-10); - } - - [Fact] - public void Vector3_ToRotation3_XAxisDirection() - { - // +X direction - var x = Vector3.EX; - - var rotation = (Rotation3)x; - - // Converting back should give +X direction - var resultDir = rotation.ToQuaternion().Rotate(Vector3.EZ); - resultDir.X.Should().BeApproximately(1, 1e-6); - resultDir.Y.Should().BeApproximately(0, 1e-6); - resultDir.Z.Should().BeApproximately(0, 1e-6); - } - - [Fact] - public void Vector3_ToRotation3_YAxisDirection() - { - // +Y direction - var y = Vector3.EY; - - var rotation = (Rotation3)y; - - // Converting back should give +Y direction - var resultDir = rotation.ToQuaternion().Rotate(Vector3.EZ); - resultDir.X.Should().BeApproximately(0, 1e-6); - resultDir.Y.Should().BeApproximately(1, 1e-6); - resultDir.Z.Should().BeApproximately(0, 1e-6); - } - - [Fact] - public void Vector3_ToRotation3_ArbitraryDirection_RoundTrips() - { - // Arbitrary normalized direction - var direction = new Vector3(1, 2, 3).Normalize(); - - var rotation = (Rotation3)direction; - var backToVector = rotation.ToQuaternion().Rotate(Vector3.EZ); - - // Should round-trip back to the same direction - backToVector.X.Should().BeApproximately(direction.X, 1e-6); - backToVector.Y.Should().BeApproximately(direction.Y, 1e-6); - backToVector.Z.Should().BeApproximately(direction.Z, 1e-6); - } - - [Fact] - public void Vector3_ToRotation3_NonUnitVector_NormalizesFirst() - { - // Non-unit vector should be normalized before conversion - var v = new Vector3(3, 4, 0); // length = 5 - - var rotation = (Rotation3)v; - var resultDir = rotation.ToQuaternion().Rotate(Vector3.EZ); - - // Result should be normalized direction - resultDir.Length.Should().BeApproximately(1, 1e-10); - resultDir.X.Should().BeApproximately(0.6, 1e-6); - resultDir.Y.Should().BeApproximately(0.8, 1e-6); - resultDir.Z.Should().BeApproximately(0, 1e-6); - } - - [Fact] - public void Vector3_ToRotation3_ZeroVector_ReturnsIdentity() - { - var zero = Vector3.Zero; - - var rotation = (Rotation3)zero; - - rotation.Should().Be(Rotation3.Identity); - } - - #endregion - #region Parsing [Fact] diff --git a/Sources/ModelingEvolution.Drawing/Matrix3x3.cs b/Sources/ModelingEvolution.Drawing/Matrix3x3.cs index 77700ab..4ebd992 100644 --- a/Sources/ModelingEvolution.Drawing/Matrix3x3.cs +++ b/Sources/ModelingEvolution.Drawing/Matrix3x3.cs @@ -206,12 +206,13 @@ public static Matrix3x3 RotationZYX(Degree rx, Degree ry, Degree rz) /// Specifically, M.Transform(source) ≈ target. /// /// Source vector (must be non-zero). - /// Target vector (may be any length; zero target yields a degenerate zero matrix). + /// Target vector (must be non-zero). /// The 3×3 matrix combining rotation that aligns source's direction with target's, scaled by |target|/|source|. - /// Source vector has zero length. + /// Source or target vector has zero length. /// /// Two-point alignment is geometrically under-constrained in 3D (rotation around the source axis is free); - /// this method picks the unique shortest-arc rotation (via ). + /// this method picks the shortest-arc rotation, (for opposite vectors a + /// half-turn about the axis FromTo picks). /// Use cases include adapting taught polyline paths whose endpoints have shifted. /// public static Matrix3x3 SimilarityFromVectors(Vector3 source, Vector3 target) @@ -221,9 +222,7 @@ public static Matrix3x3 SimilarityFromVectors(Vector3 source, Vector3 t throw new ArgumentException("Source vector must be non-zero.", nameof(source)); var scale = target.Length / sourceLength; - var rotation = source.RotationTo(target); // Rotation3, Euler ZYX - var R = RotationZYX(rotation.Rx, rotation.Ry, rotation.Rz); - return scale * R; + return scale * Quaternion.FromTo(source, target).ToMatrix3x3(); } /// Creates a matrix from three column vectors. diff --git a/Sources/ModelingEvolution.Drawing/README.md b/Sources/ModelingEvolution.Drawing/README.md index fe8cfbb..ae354d8 100644 --- a/Sources/ModelingEvolution.Drawing/README.md +++ b/Sources/ModelingEvolution.Drawing/README.md @@ -142,7 +142,7 @@ Pose3 atHalf = trajectory.AtTime(2.5); // interpolated pose | Type | Description | |---|---| | `Point3` | 3D Cartesian point. Distance, lerp, midpoint, random generation, arithmetic with `Vector3`. Conversions to/from `System.Numerics.Vector3` and tuples. Protobuf and JSON serialization. | -| `Vector3` | 3D vector with length, normalize, dot product, cross product, angle between, projection, lerp, random unit vector, and `RotationTo()` for computing the rotation between two directions. Static basis vectors `EX`, `EY`, `EZ`. | +| `Vector3` | 3D vector with length, normalize, dot product, cross product, angle between, projection, lerp, random unit vector. The rotation between two directions is `Quaternion.FromTo(from, to)`. Static basis vectors `EX`, `EY`, `EZ`. | | `Matrix3x3` | 3x3 matrix for 3D transforms. Determinant, trace, transpose, inverse. Rotation factories: `RotationX`, `RotationY`, `RotationZ`, `RotationZYX`. Scale, column/row construction. Euler angle and quaternion extraction. | | `Quaternion` | Quaternion for 3D rotations. Normalize, conjugate, inverse, from axis-angle, rotate vector, slerp, dot product. Conversions to/from `System.Numerics.Quaternion`. | | `Segment3` | Bounded line segment in 3D. Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | diff --git a/Sources/ModelingEvolution.Drawing/Vector3.cs b/Sources/ModelingEvolution.Drawing/Vector3.cs index 284ba78..c5afcd8 100644 --- a/Sources/ModelingEvolution.Drawing/Vector3.cs +++ b/Sources/ModelingEvolution.Drawing/Vector3.cs @@ -190,53 +190,6 @@ public static T AngleBetween(Vector3 a, Vector3 b) return T.Acos(T.Clamp(cosTheta, -T.One, T.One)); } - /// - /// Computes the rotation that transforms this vector to align with the target vector. - /// - /// The target direction to rotate towards. - /// A rotation R such that R.ToQuaternion().Rotate(this.Normalize()) ≈ target.Normalize(). - /// - /// When vectors are parallel, returns identity. When anti-parallel, rotation axis is chosen arbitrarily. - /// - public Rotation3 RotationTo(Vector3 target) - { - var epsilon = T.CreateTruncating(1e-6); - - var from = Normalize(); - var to = target.Normalize(); - - var dot = Dot(from, to); - - // Parallel vectors (same direction) - if (dot > T.One - epsilon) - return Rotation3.Identity; - - // Anti-parallel vectors (opposite direction) - if (dot < -T.One + epsilon) - { - // Find a perpendicular axis - var axis = T.Abs(from.X) < T.CreateTruncating(0.9) - ? Cross(from, EX).Normalize() - : Cross(from, EY).Normalize(); - - // 180 degree rotation around the perpendicular axis - return RotationFromAxisAngle(axis, T.Pi); - } - - // General case: rotation around cross product axis - var rotationAxis = Cross(from, to).Normalize(); - var angle = T.Acos(T.Clamp(dot, -T.One, T.One)); - - return RotationFromAxisAngle(rotationAxis, angle); - } - - /// - /// Creates a Rotation3 from axis-angle representation, through and the - /// one rotation → Euler formula, . - /// - private static Rotation3 RotationFromAxisAngle(Vector3 axis, T angleRadians) => - Rotation3.FromQuaternion(Quaternion.FromAxisAngle(axis, angleRadians)); - /// /// Calculates the projection of this vector onto another vector. /// @@ -301,11 +254,6 @@ public Vector3 Truncating() [MethodImpl(MethodImplOptions.AggressiveInlining)] public static T operator *(Vector3 a, Vector3 b) => Dot(a, b); /// - /// Combines a 3D vector (as position) with a rotation to create a pose. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Pose3 operator +(Vector3 v, Rotation3 r) => new((Point3)v, r); - /// /// Determines whether two 3D vectors are equal. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -345,42 +293,6 @@ public static explicit operator System.Numerics.Vector3(Vector3 v) => public static explicit operator Vector3(System.Numerics.Vector3 v) => new(T.CreateTruncating(v.X), T.CreateTruncating(v.Y), T.CreateTruncating(v.Z)); - /// - /// Converts a direction vector to a rotation that would rotate +Z axis to point in this direction. - /// Uses ZYX Euler angle convention where rotation order is Rx first, then Ry, then Rz. - /// - public static explicit operator Rotation3(Vector3 v) - { - var len = v.Length; - if (len == T.Zero) - return Rotation3.Identity; - - // Normalize the vector - var x = v._x / len; - var y = v._y / len; - var z = v._z / len; - - // For ZYX quaternion order, rotation is applied as: Rz(Ry(Rx(v))) - // Starting with +Z = (0, 0, 1), after Rx then Ry then Rz (with rz=0): - // x = cos(rx)*sin(ry) - // y = -sin(rx) - // z = cos(rx)*cos(ry) - // - // Therefore: rx = -asin(y), ry = atan2(x, z) - - // Roll (rotation around X) - determines Y component (result in radians) - var rx = T.Asin(T.Clamp(-y, -T.One, T.One)); - - // Pitch (rotation around Y) - determines X/Z ratio (result in radians) - var ry = T.Atan2(x, z); - - // Yaw (rotation around Z) - not needed for direction, set to 0 - return new Rotation3( - (Degree)Radian.FromRadian(rx), - (Degree)Radian.FromRadian(ry), - Degree.Zero); - } - #endregion #region Equality & Formatting From 3f3b72012f0564ce46bdb19caee6041e274ec5cf Mon Sep 17 00:00:00 2001 From: Rafal Maciag Date: Sat, 24 Oct 2026 19:00:05 +0000 Subject: [PATCH 5/6] S3: restore CRLF line endings master had on 11 files (epic-129) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Content unchanged (git diff --ignore-cr-at-eol is empty); undoes the CRLF → LF conversion so blame and the PR diff show only real changes. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_014wH5aL3TZhJNvLqa8YTku8 --- README.md | 1576 ++++++++-------- .../Matrix3x3Tests.cs | 1224 ++++++------ .../Polyline3Tests.cs | 948 +++++----- .../PosePath3Tests.cs | 334 ++-- .../Trajectory3Tests.cs | 480 ++--- .../ModelingEvolution.Drawing/Matrix3x3.cs | 522 +++--- .../ModelingEvolution.Drawing/Polyline3.cs | 846 ++++----- .../ModelingEvolution.Drawing/PosePath3.cs | 524 +++--- Sources/ModelingEvolution.Drawing/README.md | 1640 ++++++++--------- .../ModelingEvolution.Drawing/Trajectory3.cs | 680 +++---- .../Trajectory3Builder.cs | 866 ++++----- 11 files changed, 4820 insertions(+), 4820 deletions(-) diff --git a/README.md b/README.md index 4ebe804..60ea306 100644 --- a/README.md +++ b/README.md @@ -1,788 +1,788 @@ -# ModelingEvolution.Drawing - -[![NuGet](https://img.shields.io/nuget/v/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) -[![NuGet Downloads](https://img.shields.io/nuget/dt/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) -[![CI](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml/badge.svg)](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml) -[![License](https://img.shields.io/github/license/modelingevolution/drawing.svg)](https://github.com/modelingevolution/drawing/blob/main/LICENSE) - -A high-performance .NET generic math library for 2D and 3D geometry, coordinate systems, shapes, intersections, trajectories, and drawing primitives. Built on `INumber` constraints from .NET generic math, all types work with `float`, `double`, `decimal`, or any compatible numeric type -- with zero boxing and full type safety. - -## Table of Contents - -- [Why This Library](#why-this-library) -- [Installation](#installation) -- [Quick Start](#quick-start) -- [API Reference](#api-reference) - - [2D Core Types](#2d-core-types) - - [3D Core Types](#3d-core-types) - - [Shapes (2D)](#shapes-2d) - - [Shapes (3D)](#shapes-3d) - - [Lines and Segments](#lines-and-segments) - - [Curves and Paths (2D)](#curves-and-paths-2d) - - [Polylines (2D and 3D)](#polylines-2d-and-3d) - - [Angles and Coordinate Systems](#angles-and-coordinate-systems) - - [Rotations and Orientation](#rotations-and-orientation) - - [Poses and Trajectories](#poses-and-trajectories) - - [Kinematics](#kinematics) - - [Equations](#equations) - - [Colors](#colors) - - [Data Structures and Algorithms](#data-structures-and-algorithms) - - [Interfaces](#interfaces) -- [Key Features](#key-features) - - [Generic Math](#generic-math) - - [IShape Interface Hierarchy](#ishape-interface-hierarchy) - - [Intersections](#intersections) - - [Polygon Boolean Operations](#polygon-boolean-operations) - - [Bezier Path Clipping](#bezier-path-clipping) - - [Pooled Allocation Scopes](#pooled-allocation-scopes) - - [Serialization](#serialization) - - [SVG Export](#svg-export) -- [Code Examples](#code-examples) - - [Points: Distance, Lerp, Rotate](#points-distance-lerp-rotate) - - [3D Points, Vectors, and Rotations](#3d-points-vectors-and-rotations) - - [Spherical Coordinates](#spherical-coordinates) - - [Poses and Trajectories](#poses-and-trajectories-1) - - [Shapes: Area and Perimeter](#shapes-area-and-perimeter) - - [Line-Circle Intersection](#line-circle-intersection) - - [Segment-Rectangle Clipping](#segment-rectangle-clipping) - - [Path-Rectangle Clipping](#path-rectangle-clipping) - - [Triangle Properties](#triangle-properties) - - [Polygon Operations](#polygon-operations) - - [Matrix Transformations (2D and 3D)](#matrix-transformations-2d-and-3d) -- [Requirements](#requirements) -- [License](#license) - -## Why This Library - -- **Generic math** -- write geometry code once; use it with `float`, `double`, or any `IFloatingPointIeee754` type. -- **No boxing** -- all core types are `struct` or `readonly record struct`, avoiding heap allocations. -- **Full 2D and 3D** -- from `Point` and `Vector` to `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Quaternion`, and `Trajectory3`. -- **Rich coordinate systems** -- Cartesian, polar, cylindrical, and spherical with implicit/explicit conversions between them. -- **Rich interface hierarchy** -- `IShape` unifies area, perimeter, centroid, bounding box, containment, rotation, and scaling across all shape types. -- **Comprehensive intersections** -- 21 intersection combinations (line, segment, circle, triangle, rectangle, polygon) with zero-alloc `FirstOf` variants. -- **Polygon booleans** -- union, intersection, difference, and clustering via Clipper2. -- **Exact Bezier clipping** -- `Path.Intersect(Rectangle)` computes analytically exact sub-curves, not polygon approximations. -- **Trajectory planning** -- `Trajectory3` with time-parameterized waypoints, interpolation, resampling, and a real-time `TrajectoryController`. -- **Robotics-ready** -- `Joints6` for 6-DOF joint representations, `Frame3` (position + unit quaternion) for all pose maths, `Frame3.FromSurface()` for plane-based frame construction, and `KinematicChain` for forward kinematics and geometric Jacobians of serial and tree chains. -- **Pooled allocations** -- `AllocationScope` provides arena-style pooled memory for zero-GC geometry pipelines. -- **Serialization built in** -- Protobuf (protobuf-net) attributes on all core types, plus custom JSON converters and SVG exporters. - -## Installation - -```bash -dotnet add package ModelingEvolution.Drawing -``` - -Or via the NuGet Package Manager: - -``` -Install-Package ModelingEvolution.Drawing -``` - -## Quick Start - -```csharp -using ModelingEvolution.Drawing; - -// 2D points and vectors (using double precision) -var a = new Point(1.0, 2.0); -var b = new Point(4.0, 6.0); - -double distance = a.DistanceTo(b); // 5.0 -Point mid = a.Lerp(b, 0.5); // {X=2.5, Y=4} - -// 3D points and rotations -var p = new Point3(1.0, 2.0, 3.0); -var rotation = new Rotation3(45.0, 0.0, 0.0); // 45 deg roll (wire/edit format) -var rotated = new Frame3(Point3.Zero, rotation.ToQuaternion()).TransformPoint(p); - -// Spherical coordinates -var orbit = SphericalPoint.OnEquator(5.0, Degree.Create(30)); -Point3 cartesian = orbit; // implicit conversion - -// Shapes with area and perimeter -var circle = new Circle(Point.Zero, 10.0); -double area = circle.Area(); // ~314.159 -bool inside = circle.Contains(new Point(3.0, 4.0)); // true - -// Polygon boolean operations -var poly1 = new Polygon( - new Point(0, 0), new Point(10, 0), - new Point(10, 10), new Point(0, 10)); -var poly2 = new Polygon( - new Point(5, 5), new Point(15, 5), - new Point(15, 15), new Point(5, 15)); - -Polygon merged = poly1 | poly2; // union -Polygon overlap = poly1 & poly2; // intersection - -// 3D trajectory with time-parameterized waypoints -var trajectory = new Trajectory3( - new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), - new Waypoint3(new Pose3(100, 0, 50, 0, 45, 0), 5.0)); -Pose3 atHalf = trajectory.AtTime(2.5); // interpolated pose -``` - -## API Reference - -### 2D Core Types - -| Type | Description | -|---|---| -| `Point` | 2D Cartesian point. Supports arithmetic with vectors and sizes, distance, lerp, reflect, rotate, clamp, midpoint, matrix transform, tuple conversion, and parsing. | -| `Vector` | 2D vector with magnitude, direction, dot/cross product, normalize, projection, reflect, rotate, perpendicular (CW/CCW), angle between, lerp, and matrix transform. | -| `Size` | Width/height pair with arithmetic operators, component-wise multiply/divide, and parsing. | -| `Matrix` | 3x3 affine transformation matrix. Supports translate, rotate, scale, skew, invert, append/prepend, and point/vector transforms. | -| `Matrix2x2` | 2x2 matrix for linear algebra: determinant, trace, transpose, inverse, eigendecomposition (closed-form for symmetric matrices), and point/vector transforms. | -| `Rectangle` | Axis-aligned rectangle with contains, intersect, inflate, offset, bounds, tiling, diagonal, distance-to-point, and LTRB construction. | -| `Thickness` | Four-sided thickness (top, right, bottom, left) for rectangular borders. | - -### 3D Core Types - -| Type | Description | -|---|---| -| `Point3` | 3D Cartesian point. Distance, lerp, midpoint, random generation, arithmetic with `Vector3`. Conversions to/from `System.Numerics.Vector3` and tuples. Protobuf and JSON serialization. | -| `Vector3` | 3D vector with length, normalize, dot product, cross product, angle between, projection, lerp, random unit vector. The rotation between two directions is `Quaternion.FromTo(from, to)`. Static basis vectors `EX`, `EY`, `EZ`. | -| `Matrix3x3` | 3x3 matrix for 3D transforms. Determinant, trace, transpose, inverse. Rotation factories: `RotationX`, `RotationY`, `RotationZ`, `RotationZYX`. Scale, column/row construction. Euler angle and quaternion extraction. | -| `Quaternion` | Quaternion for 3D rotations. Normalize, conjugate, inverse, from axis-angle, rotate vector, slerp, dot product. Conversions to/from `System.Numerics.Quaternion`. | -| `Segment3` | Bounded line segment in 3D. Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | - -### Shapes (2D) - -All 2D shapes implement `IShape`, providing `Area()`, `Perimeter()`, `Centroid()`, `BoundingBox()`, `Contains(Point)`, `Rotate(Degree, Point)`, and `Scale(T)`. - -| Type | Description | -|---|---| -| `Circle` | Center + radius. Intersects with lines, segments, circles, and rectangles. Tangent point detection. `PointAt(Radian)` for parameterized access. | -| `Triangle` | Three vertices. Incircle, circumcircle, orthocenter, angles, edges. Classification: `IsRight()`, `IsAcute()`, `IsObtuse()`, `IsEquilateral()`, `IsIsosceles()`, `IsScalene()`. Similarity and congruence tests. | -| `Polygon` | Immutable polygon backed by `ReadOnlyMemory>`. Shoelace area, ray-casting containment, convex hull (Graham scan), `IsConvex()`, edges, simplify, skeleton (medial axis). Boolean operations: union, intersect, subtract, cluster. Operators: `\|` (union), `&` (intersect), `-` (subtract). | -| `Rectangle` | Implements `IArea`, `IPerimeter`, `ICentroid`, `IBoundingBox`, `IScalable>`. Rotation returns `Polygon`. | - -### Shapes (3D) - -| Type | Description | -|---|---| -| `Triangle3` | Three vertices in 3D space. Area (cross product), perimeter, centroid, unit normal vector. `ToPose()` creates a coordinate frame from the triangle plane. Translate by `Vector3`. | - -### Lines and Segments - -| Type | Description | -|---|---| -| `Line` | Infinite 2D line (supports vertical lines). From two points, point + direction, or equation. Intersect with lines, segments, circles, triangles, rectangles, polygons. Distance-to-point, angle-between, parallel/perpendicular tests, project point, reflect point. | -| `Segment` | Bounded 2D line segment (start + end). Length, midpoint, direction, lerp, split, reverse. Intersect with segments, lines, circles, rectangles. Distance-to-point, project point, parallel test. Liang-Barsky rectangle clipping. | -| `Segment3` | Bounded 3D line segment (start + end). Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | - -### Curves and Paths (2D) - -| Type | Description | -|---|---| -| `BezierCurve` | Cubic Bezier curve (4 control points). `Evaluate(t)`, `Split(t)`, `SubCurve(t0, t1)` via De Casteljau. Extremum points, linear equation intersection, rectangle edge crossings. | -| `Path` | Immutable sequence of `BezierCurve` segments. Create from points (`FromPoints`) with smoothing, or from SVG path data (`Parse`). `Length()`, `PointAt(t)` with arc-length parameterization. Exact `Intersect(Rectangle)` that splits Bezier curves at crossing points. Transform, rotate, reverse, append. `Close()` and `ToPolygon()` for rasterization. | -| `ComplexCurve` | Composite curve of mixed Bezier and polyline segments packed in a contiguous byte buffer for zero-copy decoding. SVG export and JSON serialization. | -| `PolygonalCurve` | Mutable sequence of connected points that generates smooth Bezier segments via `GetSmoothSegment(i)`. | - -### Polylines (2D and 3D) - -| Type | Description | -|---|---| -| `Polyline` | Immutable open polyline in 2D (non-closing). Length, edges, bounding box, simplification. JSON, Protobuf, and SVG serialization. | -| `Polyline3` | Immutable open polyline in 3D. Length, edges, translate, transform. JSON and Protobuf serialization. | - -### Angles and Coordinate Systems - -| Type | Description | -|---|---| -| `Degree` | Angle in degrees. Implicit conversion from numeric values and from `Radian`. Arithmetic (+, -, *, /), normalize to (-180, 180], abs, comparison operators. | -| `Radian` | Angle in radians. Implicit conversion from `Degree`. Arithmetic (+, -, *, /), normalize to (-pi, pi], abs, comparison operators. Static `Sin`/`Cos` helpers. | -| `PolarPoint` | Polar coordinates (radius, angle). Implicit conversions to/from `Point`, explicit to `Vector2` and `Vector`. | -| `CylindricalPoint` | Cylindrical coordinates (radial distance, angle, height). Factory: `OnPlane(r, angle)`. Builder methods: `WithZ`, `WithRadialDistance`, `WithAngle`. `RotateAngle` for angular displacement. `+`/`-` operators with `Radian` and `Point3`. Conversions to/from `Point3`, `Vector3`, `SphericalPoint`, and `System.Numerics.Vector3`. | -| `SphericalPoint` | Spherical coordinates (radius, azimuth, inclination) with Degree-based storage (ISO convention: inclination from Z-axis). Factories: `OnEquator`, `FromElevation`, `NorthPole`, `SouthPole`. `Normalized`, `WithRadius`, `RotateAzimuth`, `RotateInclination`, `Rotate(az, inc)`. `AngularDistance` (Vincenty formula), `Slerp`, `Lerp`, Euclidean `Distance`. Radius scaling operators (`*`, `/`). `Point3` offset operators (`+`, `-`). Conversions to/from `Point3`, `Vector3`, `CylindricalPoint`, and `System.Numerics.Vector3`. | - -### Rotations and Orientation - -| Type | Description | -|---|---| -| `Rotation3` | Euler angle rotation (Roll/Pitch/Yaw in degrees, ZYX convention). Wire and edit format only (since 2.0): create from degrees, radians, quaternion (`FromQuaternion`) or `Matrix3x3` (`FromMatrix3x3`); `ToQuaternion()` for maths. No rotation algebra: compose, invert, slerp and rotate with `Quaternion` or `Frame3`. Protobuf and JSON serialization. | -| `Quaternion` | Unit quaternion for 3D rotations. From axis-angle, `FromRpy(roll, pitch, yaw)` (Rz·Ry·Rx, radians; the URDF rpy rule), `FromTo(from, to)` (shortest arc), `FromAxes(x, y, z)` (orthonormal basis), `FromMatrix3x3(m)` (Shepperd's method) and `ToMatrix3x3()`, rotate vector, slerp, dot, normalize, conjugate, inverse. Hamilton product operator (`*`). Conversions to/from `System.Numerics.Quaternion`. Protobuf serialization. | -| `Matrix3x3` | Also used as rotation representation. `RotationZYX(rx, ry, rz)` creates ZYX Euler rotation matrix. `ToEulerZYX()` extracts angles (gimbal lock: rz = 0). Both go through `Quaternion` (`FromRpy`/`ToMatrix3x3`, `FromMatrix3x3`/`Rotation3.FromQuaternion`), so there is one Euler formula each way. | - -### Poses and Trajectories - -| Type | Description | -|---|---| -| `Frame3` | Rigid frame = position (`Point3`) + unit `Quaternion`. All pose maths: `Multiply` / `*` (compose), `Inverse`, `TransformPoint`, `TransformVector`, `Interpolate` (position lerp + slerp), `Distance` (position error and angle error in radians). `FromSurface(a, b, c)` creates a frame from three coplanar points (right-hand rule); `FromSurface(a, b, c, h)` uses a hint point for Z; `FromSurface(points)` fits a plane. `FromPose3(pose, lengthScale)` / `ToPose3(lengthScale)` convert to and from the wire format (ZYX degrees). | -| `Pose3` | Wire and edit format only (since 2.0): position (`Point3`) + ZYX Euler orientation (`Rotation3`, degrees). Constructors, `Identity`, tuple conversions, deconstruction, parsing, equality, `Truncating()`. Protobuf and JSON serialization. Convert to `Frame3` for any maths. | -| `Waypoint3` | Timestamped `Pose3` -- an element of a trajectory. | -| `PosePath3` | Immutable sequence of `Pose3` (no time). Length, reverse, transform, extract `Polyline3`, convert to `Trajectory3` with uniform timing. | -| `Trajectory3` | Immutable time-parameterized path of `Waypoint3`. `Duration`, `Length`, `AtTime(t)` (binary-search interpolation with position lerp + rotation slerp). `Resample(interval)`, `Reverse`, `Transform`. Convert to `PosePath3` or `Polyline3`. Create a real-time `TrajectoryController` via `Control()`. | -| `TrajectoryController` | Real-time trajectory playback using `Stopwatch` clock. Start, stop, pause, resume, seek. Fires `PoseChanged` events with current pose, time, and progress. | -| `Corner` | Corner blending descriptor: `Sharp` (exact waypoint) or `Round(radius)` (arc blend before/after the waypoint). | - -### Kinematics - -| Type | Description | -|---|---| -| `KinematicChain` | Namespace `ModelingEvolution.Drawing.Kinematics`. A tree of `Link`s moved by revolute/prismatic `Joint`s (origin at q = 0, unit axis, limits) with named `ChainFrame`s; metres and radians. `Forward(q, links)` writes every link pose, `FramePose(links, frame)`, `Jacobian(links, frame, j)` writes the 6 × Dof geometric Jacobian (column-major, v then ω). Spans in, spans out, no allocation; the constructor validates topological order, unit axes, limits and unique names. | -| `Joints6` | 6-DOF joint angles stored as six `Degree` fields (no array allocation). Indexer `[0..5]`, arithmetic operators, `Lerp`, `MaxAbsDelta`, `IsWithin(tolerance)`, `IsBetween(min, max)`. `ToArray()`, `FromSpan()`, `FromArray()`, `CopyTo(Span)`. Protobuf and JSON serialization. | -| `Speed` | Scalar speed in units/second (unitless magnitude). Arithmetic, `TimeFor(distance)`, `DistanceIn(time)`. Parsing. | -| `Velocity3` | 3D velocity vector (direction + magnitude). Create from direction + speed. `Speed`, `Direction`, `DisplacementIn(time)`. Arithmetic operators. | -| `Speed` | **Unit-aware** speed tagged at compile time with its unit (`ISpeedUnit`). Stores the value in `TUnit`; `ToSi()`/`FromSi()`/`To()` normalise via the SI base (m/s). Full arithmetic, comparisons, ratio (`Speed/Speed → T`), `IParsable`. Mixing units (or a non-speed unit) is a compile error. Verified zero-cost vs raw `double`. | -| `ISpeedUnit` | Marker for speed units. Built-ins: `MetrePerSecond` (SI base), `MillimetrePerSecond`, `MetrePerMinute`. Implement to add your own. | - -### Equations - -| Type | Description | -|---|---| -| `LinearEquation` | `y = Ax + B`. Create from points, angle, or direction. Compute, zero point, intersect, perpendicular, translate, mirror. | -| `QuadraticEquation` | `ax^2 + bx + c = 0`. Zero points using the discriminant. | -| `CubicEquation` | `ax^3 + bx^2 + cx + d = 0`. Root finding via Newton-Raphson. | -| `CircleEquation` | `(x-cx)^2 + (y-cy)^2 = r^2`. Intersect with `LinearEquation`. | - -### Colors - -| Type | Description | -|---|---| -| `Color` | ARGB color as a 32-bit uint. Parse from hex (`#RRGGBB`, `#AARRGGBB`), `rgba(r,g,b,a)`, or HSV strings. `GetHue()`, `GetSaturation()`, `GetLightness()`, `GetBrightness()`, `MakeTransparent(float)`. Implicit from string or tuple. JSON and Protobuf serialization. | -| `HsvColor` | HSV (hue, saturation, value) color space with optional alpha. Implicit conversions to/from `Color`. Parse from `hsv()`/`hsva()` strings, JSON arrays, or hex. | - -### Data Structures and Algorithms - -| Type | Description | -|---|---| -| `KdTree` | 2D KD-tree for fast nearest-neighbor queries on `Point`. Build from span, add individual points, find nearest neighbor. | -| `Skeleton` | Medial axis approximation of a polygon (interior nodes + edges). Three algorithms: `StraightSkeleton`, `ChordalAxis`, and `Voronoi`. | -| `MovingAverage` | Component-wise moving average over a sliding window of 2D points. O(1) add and query via circular buffer + running sums. | -| `MovingMedian` | Component-wise moving median over a sliding window of 2D points. O(1) median read, O(n) add via maintained sorted arrays. | -| `AllocationScope` | Arena-style pooled allocation scope. Library allocations use `MemoryPool.Shared` when active. Dispose returns all tracked memory at once. Supports `Persist()` to detach individual results. | -| `AlignmentResult` | Result of a rigid 2D alignment (rotation + translation) between two point clouds, with rotation angle and residual error. | - -### Interfaces - -```csharp -// Composable geometry traits -public interface IArea { T Area(); } -public interface IPerimeter { T Perimeter(); } -public interface ICentroid { Point Centroid(); } -public interface IBoundingBox { Rectangle BoundingBox(); } -public interface IRotatable { TSelf Rotate(Degree angle, Point origin = default); } -public interface IScalable { TSelf Scale(T factor); } - -// Unified shape interface -- combines all of the above plus point containment -public interface IShape : IArea, IPerimeter, ICentroid, - IBoundingBox, IRotatable, IScalable -{ - bool Contains(Point point); -} -``` - -Implementing types: `Circle`, `Triangle`, `Polygon`, and `Rectangle` (partial -- no `IRotatable` since rotation produces a `Polygon`). - -## Key Features - -### Generic Math - -All geometry types are parameterized by `T` with constraints like `IFloatingPointIeee754`, `IMinMaxValue`, etc. This means the same code works with any numeric type: - -```csharp -// Single precision -var pf = new Point(1f, 2f); -var cf = new Circle(pf, 5f); - -// Double precision -var pd = new Point(1.0, 2.0); -var cd = new Circle(pd, 5.0); - -// Both compute area with their respective precision -float areaF = cf.Area(); -double areaD = cd.Area(); -``` - -### IShape Interface Hierarchy - -Write generic algorithms over any shape: - -```csharp -void PrintShapeInfo(TShape shape) - where T : INumber, ITrigonometricFunctions, IRootFunctions, - IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue - where TShape : IShape -{ - Console.WriteLine($"Area: {shape.Area()}"); - Console.WriteLine($"Perimeter: {shape.Perimeter()}"); - Console.WriteLine($"Centroid: {shape.Centroid()}"); - Console.WriteLine($"Bounds: {shape.BoundingBox()}"); -} -``` - -### Intersections - -The `Intersections` static class provides 21 combination overloads covering every pair of: `Line`, `Segment`, `Circle`, `Triangle`, `Rectangle`, and `Polygon`. Each combination also has a zero-allocation `FirstOf` variant. - -```csharp -// Line x Line -Point? p = Intersections.Of(line1, line2); - -// Line x Circle -> chord (Segment) or null -Segment? chord = Intersections.Of(line, circle); - -// Tangent detection -Point? tangent = Intersections.TangentPoint(line, circle); - -// Circle x Circle -> radical chord or tangent point -Segment? radicalChord = Intersections.Of(circle1, circle2); - -// Segment x Segment -Point? hit = Intersections.Of(seg1, seg2); - -// Zero-alloc first-hit variants -Segment? first = Intersections.FirstOf(line, polygon); -``` - -Instance methods are also available on each shape for convenience: - -```csharp -Segment? chord = circle.Intersect(line); -Point? hit = segment.Intersect(otherSegment); -Segment? clipped = line.Intersect(rectangle); -``` - -### Polygon Boolean Operations - -Powered by [Clipper2](https://github.com/AngusJohnson/Clipper2): - -```csharp -var a = new Polygon( /* ... */ ); -var b = new Polygon( /* ... */ ); - -// Operators -Polygon union = a | b; -Polygon intersect = a & b; -Polygon difference = a - b; - -// Methods returning multiple result polygons -List> unions = a.Union(b, removeHoles: true); -List> diffs = a.Subtract(b); - -// Batch operations -List> merged = Polygon.Union(polygons); - -// Clustering overlapping polygons -IEnumerable> clusters = Polygon.Cluster(polygons); -``` - -### Bezier Path Clipping - -`Path.Intersect(Rectangle)` performs analytically exact Bezier-rectangle clipping. It finds the precise parameter values where each cubic Bezier segment crosses the rectangle edges, then uses `SubCurve(t0, t1)` via De Casteljau subdivision to return exact sub-paths -- not polygon approximations. - -```csharp -var path = Path.FromPoints(points); -IEnumerable> clipped = path.Intersect(viewport); - -foreach (var subPath in clipped) -{ - // Each sub-path contains only the Bezier segments inside the rectangle, - // with curves split precisely at the rectangle boundaries. -} -``` - -### Pooled Allocation Scopes - -When performing many geometry operations that allocate intermediate results (polygon booleans, skeleton computation, curve corrections), use `AllocationScope` to avoid GC pressure: - -```csharp -using var scope = AllocationScope.Begin(); - -var polygon = somePolygon * new Size(2, 2); // pooled -var skeleton = polygon.Skeleton(); // pooled -var lease = scope.Persist(ref skeleton); // detach from scope - -// scope.Dispose() returns everything except skeleton's memory -// lease.Dispose() returns skeleton's memory when you're done -``` - -### Serialization - -**Protobuf** -- all core types carry `[ProtoContract]` / `[ProtoMember]` attributes for use with protobuf-net: - -```csharp -var point = new Point(1f, 2f); -byte[] bytes = Serializer.Serialize(point); -``` - -**JSON** -- custom `System.Text.Json` converters for `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Polygon`, `Polyline`, `Polyline3`, `PosePath3`, `Trajectory3`, `Path`, `ComplexCurve`, `BezierCurve`, `Matrix2x2`, `Matrix3x3`, `Skeleton`, `Color`, and `HsvColor`: - -```csharp -var pose = new Pose3(100, 200, 300, 45, 90, 0); -string json = JsonSerializer.Serialize(pose); // [100,200,300,45,90,0] - -var joints = new Joints6(10, 20, 30, 40, 50, 60); -string j = JsonSerializer.Serialize(joints); // [10,20,30,40,50,60] -``` - -**String parsing** -- `IParsable` implementations on `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Rectangle`, `Size`, `Path`, `ComplexCurve`, `Speed`, `Color`, and `HsvColor`: - -```csharp -var point = Point.Parse("1.5, 2.5"); -var point3 = Point3.Parse("1.0 2.0 3.0"); -var pose = Pose3.Parse("100, 200, 300, 45, 90, 0"); -var rect = Rectangle.Parse("0 0 100 200"); -var color = Color.Parse("#FF8800"); -var path = Path.Parse("M 0 0 C 1 2, 3 4, 5 6", null); -``` - -### SVG Export - -`Polygon`, `Polyline`, `ComplexCurve`, `Skeleton`, and `Path` support SVG export via the `[SvgExporter]` attribute and the `SvgExporter` infrastructure: - -```csharp -// Path.ToString() produces SVG path data -var path = Path.FromPoints(points); -string svgData = path.ToString(); // "M 0 0 C 1.5 2.3, 3.1 4.2, 5 6 ..." -``` - -## Code Examples - -### Points: Distance, Lerp, Rotate - -```csharp -var a = new Point(0, 0); -var b = new Point(3, 4); - -double dist = a.DistanceTo(b); // 5.0 -Point quarter = a.Lerp(b, 0.25); // {X=0.75, Y=1} -Point reflected = a.Reflect(b); // {X=6, Y=8} - -// Rotate 90 degrees around the origin -Degree angle = 90.0; -Point rotated = b.Rotate(angle); // {X=-4, Y=3} - -// Rotate around a custom center -var center = new Point(1, 1); -Point rotated2 = b.Rotate(angle, center); - -// Clamp to a rectangle -var bounds = new Rectangle(0, 0, 10, 10); -Point clamped = new Point(15, -3).Clamp(bounds); // {X=10, Y=0} -``` - -### 3D Points, Vectors, and Rotations - -```csharp -// 3D point operations -var p1 = new Point3(1, 2, 3); -var p2 = new Point3(4, 6, 3); -double dist3d = Point3.Distance(p1, p2); // 5.0 -Point3 mid3d = Point3.Lerp(p1, p2, 0.5); - -// 3D vector operations -var v = new Vector3(1, 0, 0); -var w = new Vector3(0, 1, 0); -var cross = Vector3.Cross(v, w); // (0, 0, 1) -double dot = Vector3.Dot(v, w); // 0.0 - -// Compute rotation between two directions -var rot = Quaternion.FromTo(v, w); -Vector3 result = rot.Rotate(v); // approximately (0, 1, 0) - -// Euler angles (ZYX convention) are a wire format: convert once, then use the quaternion -var rotation = Rotation3.FromDegrees(45, 0, 0).ToQuaternion(); -Vector3 rotated = rotation.Rotate((Vector3)p1); - -// Quaternion slerp -var r1 = Quaternion.Identity; -var r2 = Quaternion.FromAxisAngle(Vector3.EZ, Math.PI / 2); -var halfway = Quaternion.Slerp(r1, r2, 0.5); - -// Matrix3x3 rotation -var m = Matrix3x3.RotationZ(Degree.Create(45)); -var transformed = m.Transform(new Point3(1, 0, 0)); -``` - -### Spherical Coordinates - -```csharp -// Create spherical points with factory methods -var northPole = SphericalPoint.NorthPole(10.0); // +Z, radius=10 -var equator = SphericalPoint.OnEquator(5.0, Degree.Create(45)); -var elevated = SphericalPoint.FromElevation(5.0, - Degree.Create(0), Degree.Create(30)); - -// Convert to/from Cartesian -Point3 cartesian = equator; // implicit conversion -SphericalPoint spherical = cartesian; // implicit conversion - -// Orbit around a 3D center -var center = new Point3(10, 50, 0); -var points = new List>(); -for (int az = 0; az < 360; az += 30) - points.Add(center + SphericalPoint.OnEquator(5.0, Degree.Create(az))); - -// Rotation (O(1) angle operations) -var sp = SphericalPoint.OnEquator(5.0, Degree.Create(0)); -var rotated = sp.RotateAzimuth(Degree.Create(45)); // rotate around Z - -// Angular distance (Vincenty formula -- numerically stable) -var a = SphericalPoint.NorthPole(1.0); -var b = SphericalPoint.SouthPole(1.0); -Degree angDist = SphericalPoint.AngularDistance(a, b); // 180 degrees - -// Spherical linear interpolation -var mid = SphericalPoint.Slerp(a, b, 0.5); - -// Cylindrical coordinate conversions -CylindricalPoint cyl = sp; // implicit conversion -SphericalPoint back = cyl; // implicit conversion -``` - -### Poses and Trajectories - -```csharp -// Pose = position + orientation -var pose = new Pose3(100, 200, 300, 45, 0, 0); // x,y,z, rx,ry,rz (degrees) - -// Maths composes Frame3 (position + quaternion); Pose3 converts once, here mm -> m -var world = Frame3.FromPose3(new Pose3(10, 0, 0, 0, 0, 0), 0.001); -var local = Frame3.FromPose3(new Pose3(5, 0, 0, 0, 0, 90), 0.001); -var combined = world * local; -Pose3 back = combined.ToPose3(1000); // m -> mm, ZYX degrees - -// Create frame from a surface (3 points define a plane) -var a = new Point3(0, 0, 0); -var b = new Point3(1, 0, 0); -var c = new Point3(0, 1, 0); -var surface = Frame3.FromSurface(a, b, c); // Z-axis via right-hand rule - -// Trajectory with timed waypoints -var trajectory = new Trajectory3( - new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), - new Waypoint3(new Pose3(50, 0, 25, 0, 30, 0), 2.5), - new Waypoint3(new Pose3(100, 0, 0, 0, 0, 0), 5.0)); - -// Interpolate at any time (binary search + lerp/slerp) -Pose3 atT = trajectory.AtTime(1.25); - -// Resample at uniform intervals -var resampled = trajectory.Resample(0.1); // every 0.1 seconds - -// Real-time playback -var controller = trajectory.Control(); -controller.PoseChanged += (_, e) => Console.WriteLine($"t={e.Time} pose={e.Pose}"); -controller.Start(); - -// 6-DOF joint angles for robotics -var home = new Joints6(0, -90, 0, 0, 90, 0); -var target = new Joints6(45, -60, 30, 0, 90, -45); -var halfway = Joints6.Lerp(home, target, 0.5); -bool inRange = target.IsBetween(minLimits, maxLimits); -``` - -### Shapes: Area and Perimeter - -```csharp -// Circle -var circle = new Circle(new Point(5, 5), 3.0); -double cArea = circle.Area(); // ~28.274 -double cPerim = circle.Perimeter(); // ~18.850 - -// Triangle -var triangle = new Triangle( - new Point(0, 0), - new Point(4, 0), - new Point(0, 3)); -double tArea = triangle.Area(); // 6.0 -double tPerim = triangle.Perimeter(); // 12.0 - -// Polygon (shoelace formula) -var poly = new Polygon( - new Point(0, 0), new Point(4, 0), - new Point(4, 3), new Point(0, 3)); -double pArea = poly.Area(); // 12.0 - -// 3D triangle -var t3 = new Triangle3( - new Point3(0, 0, 0), - new Point3(4, 0, 0), - new Point3(0, 3, 0)); -double area3 = t3.Area; // 6.0 -Vector3 normal = t3.Normal; // (0, 0, 1) -``` - -### Line-Circle Intersection - -```csharp -var circle = new Circle(new Point(0, 0), 5.0); -var line = Line.From(new Point(-10, 0), new Point(10, 0)); - -// Secant -- returns the chord as a segment -Segment? chord = circle.Intersect(line); -// chord.Value.Start ~ {X=-5, Y=0}, chord.Value.End ~ {X=5, Y=0} - -// Tangent detection -var tangentLine = Line.Horizontal(5.0); -Point? tangent = circle.TangentPoint(tangentLine); -// tangent.Value ~ {X=0, Y=5} -``` - -### Segment-Rectangle Clipping - -```csharp -var viewport = new Rectangle(0, 0, 100, 100); -var segment = new Segment( - new Point(-20, 50), - new Point(150, 50)); - -// Liang-Barsky clipping -Segment? clipped = segment.Intersect(viewport); -// clipped.Value: Start={X=0, Y=50}, End={X=100, Y=50} -``` - -### Path-Rectangle Clipping - -```csharp -var points = new[] -{ - new Point(10, 50), - new Point(50, 10), - new Point(90, 50), - new Point(130, 90) -}; -var path = Path.FromPoints(points); -var viewport = new Rectangle(20, 20, 60, 60); // 20,20 to 80,80 - -// Exact Bezier clipping -- returns sub-paths with curves split at boundaries -foreach (var subPath in path.Intersect(viewport)) -{ - Console.WriteLine($"Sub-path with {subPath.Count} Bezier segments"); -} -``` - -### Triangle Properties - -```csharp -var t = new Triangle( - new Point(0, 0), - new Point(4, 0), - new Point(2, 3)); - -// Derived circles -Circle incircle = t.Incircle(); // largest circle inside -Circle circumcircle = t.Circumcircle(); // passes through all vertices - -// Center points -Point centroid = t.Centroid(); -Point orthocenter = t.Orthocenter; - -// Classification -bool right = t.IsRight(); -bool equilateral = t.IsEquilateral(); -bool isosceles = t.IsIsosceles(); - -// Similarity and congruence -var t2 = new Triangle( - new Point(0, 0), - new Point(8, 0), - new Point(4, 6)); -bool similar = t.IsSimilarTo(t2); // true (same shape, scaled 2x) -bool congruent = t.IsCongruentTo(t2); // false (different size) - -// Interior angles -var (atA, atB, atC) = t.Angles; -``` - -### Polygon Operations - -```csharp -// Convex hull -var cloud = new Polygon( - new Point(0, 0), new Point(1, 3), - new Point(2, 1), new Point(4, 4), - new Point(3, 0), new Point(5, 2)); -Polygon hull = cloud.ConvexHull(); - -// Point containment (ray casting) -bool inside = hull.Contains(new Point(2, 2)); - -// Boolean operations -var a = new Polygon( /* ... */ ); -var b = new Polygon( /* ... */ ); -List> union = a.Union(b, removeHoles: true); -List> diff = a.Subtract(b); -List> inter = a.Intersect(b); - -// Simplify (remove collinear/close points) -Polygon simplified = hull.Simplify(epsilon: 0.1); - -// Convexity test -bool convex = hull.IsConvex(); - -// Clustering overlapping polygons -var clusters = Polygon.Cluster(polygons); -``` - -### Matrix Transformations (2D and 3D) - -```csharp -// 2D affine transformation -var matrix = Matrix.Identity; -matrix.Translate(10.0, 20.0); -matrix.Rotate(Degree.Create(45.0)); -matrix.Scale(2.0, 2.0); - -Point transformed = matrix.Transform(new Point(1, 0)); -Vector rotatedVec = matrix.Transform(new Vector(1, 0)); - -var combined = matrix1 * matrix2; -matrix.Invert(); - -// 3D rotation matrix -var rx = Matrix3x3.RotationX(Degree.Create(45)); -var ry = Matrix3x3.RotationY(Degree.Create(30)); -var rz = Matrix3x3.RotationZ(Degree.Create(60)); - -// Compose rotations -var rotation = rz * ry * rx; -var p = new Point3(1, 0, 0); -Point3 rotated3 = rotation * p; // or rotation.Transform(p) - -// Extract Euler angles -var (eRx, eRy, eRz) = rotation.ToEulerZYX(); - -// Convert to quaternion and back -Quaternion q = Quaternion.FromMatrix3x3(rotation); -Matrix3x3 back = q.ToMatrix3x3(); -``` - -### Unit-Aware Speed - -```csharp -using ModelingEvolution.Drawing.Units; - -// The unit is part of the type — the value is stored in that unit. -Speed> feed = new(300); // 300 m/min - -// Normalise to the SI base (m/s) or convert to another speed unit. -double si = feed.ToSi(); // 5.0 (m/s) -var inMmPerS = feed.To>(); // 5000 mm/s - -// Same-unit arithmetic, scalar scaling, and a dimensionless ratio. -var faster = feed * 2.0; // 600 m/min -double ratio = faster / feed; // 2.0 - -// Cross-unit add normalises through SI; result stays in the left unit. -var total = inMmPerS.Add(feed); // (5000 + 5000) mm/s - -Console.WriteLine(feed); // "300 m/min" - -// Compile-time safety: -// feed + inMmPerS; // error: different units — use .Add(...) -// feed.To>(); // error: not an ISpeedUnit -``` - -## Requirements - -- **.NET 10.0** or later (uses generic math interfaces from .NET 7+) -- **Dependencies**: [Clipper2](https://www.nuget.org/packages/Clipper2) 1.4.0, [protobuf-net](https://www.nuget.org/packages/protobuf-net) 3.2.45 - -## License - -See [LICENSE](LICENSE) for details. +# ModelingEvolution.Drawing + +[![NuGet](https://img.shields.io/nuget/v/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) +[![NuGet Downloads](https://img.shields.io/nuget/dt/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) +[![CI](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml/badge.svg)](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml) +[![License](https://img.shields.io/github/license/modelingevolution/drawing.svg)](https://github.com/modelingevolution/drawing/blob/main/LICENSE) + +A high-performance .NET generic math library for 2D and 3D geometry, coordinate systems, shapes, intersections, trajectories, and drawing primitives. Built on `INumber` constraints from .NET generic math, all types work with `float`, `double`, `decimal`, or any compatible numeric type -- with zero boxing and full type safety. + +## Table of Contents + +- [Why This Library](#why-this-library) +- [Installation](#installation) +- [Quick Start](#quick-start) +- [API Reference](#api-reference) + - [2D Core Types](#2d-core-types) + - [3D Core Types](#3d-core-types) + - [Shapes (2D)](#shapes-2d) + - [Shapes (3D)](#shapes-3d) + - [Lines and Segments](#lines-and-segments) + - [Curves and Paths (2D)](#curves-and-paths-2d) + - [Polylines (2D and 3D)](#polylines-2d-and-3d) + - [Angles and Coordinate Systems](#angles-and-coordinate-systems) + - [Rotations and Orientation](#rotations-and-orientation) + - [Poses and Trajectories](#poses-and-trajectories) + - [Kinematics](#kinematics) + - [Equations](#equations) + - [Colors](#colors) + - [Data Structures and Algorithms](#data-structures-and-algorithms) + - [Interfaces](#interfaces) +- [Key Features](#key-features) + - [Generic Math](#generic-math) + - [IShape Interface Hierarchy](#ishape-interface-hierarchy) + - [Intersections](#intersections) + - [Polygon Boolean Operations](#polygon-boolean-operations) + - [Bezier Path Clipping](#bezier-path-clipping) + - [Pooled Allocation Scopes](#pooled-allocation-scopes) + - [Serialization](#serialization) + - [SVG Export](#svg-export) +- [Code Examples](#code-examples) + - [Points: Distance, Lerp, Rotate](#points-distance-lerp-rotate) + - [3D Points, Vectors, and Rotations](#3d-points-vectors-and-rotations) + - [Spherical Coordinates](#spherical-coordinates) + - [Poses and Trajectories](#poses-and-trajectories-1) + - [Shapes: Area and Perimeter](#shapes-area-and-perimeter) + - [Line-Circle Intersection](#line-circle-intersection) + - [Segment-Rectangle Clipping](#segment-rectangle-clipping) + - [Path-Rectangle Clipping](#path-rectangle-clipping) + - [Triangle Properties](#triangle-properties) + - [Polygon Operations](#polygon-operations) + - [Matrix Transformations (2D and 3D)](#matrix-transformations-2d-and-3d) +- [Requirements](#requirements) +- [License](#license) + +## Why This Library + +- **Generic math** -- write geometry code once; use it with `float`, `double`, or any `IFloatingPointIeee754` type. +- **No boxing** -- all core types are `struct` or `readonly record struct`, avoiding heap allocations. +- **Full 2D and 3D** -- from `Point` and `Vector` to `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Quaternion`, and `Trajectory3`. +- **Rich coordinate systems** -- Cartesian, polar, cylindrical, and spherical with implicit/explicit conversions between them. +- **Rich interface hierarchy** -- `IShape` unifies area, perimeter, centroid, bounding box, containment, rotation, and scaling across all shape types. +- **Comprehensive intersections** -- 21 intersection combinations (line, segment, circle, triangle, rectangle, polygon) with zero-alloc `FirstOf` variants. +- **Polygon booleans** -- union, intersection, difference, and clustering via Clipper2. +- **Exact Bezier clipping** -- `Path.Intersect(Rectangle)` computes analytically exact sub-curves, not polygon approximations. +- **Trajectory planning** -- `Trajectory3` with time-parameterized waypoints, interpolation, resampling, and a real-time `TrajectoryController`. +- **Robotics-ready** -- `Joints6` for 6-DOF joint representations, `Frame3` (position + unit quaternion) for all pose maths, `Frame3.FromSurface()` for plane-based frame construction, and `KinematicChain` for forward kinematics and geometric Jacobians of serial and tree chains. +- **Pooled allocations** -- `AllocationScope` provides arena-style pooled memory for zero-GC geometry pipelines. +- **Serialization built in** -- Protobuf (protobuf-net) attributes on all core types, plus custom JSON converters and SVG exporters. + +## Installation + +```bash +dotnet add package ModelingEvolution.Drawing +``` + +Or via the NuGet Package Manager: + +``` +Install-Package ModelingEvolution.Drawing +``` + +## Quick Start + +```csharp +using ModelingEvolution.Drawing; + +// 2D points and vectors (using double precision) +var a = new Point(1.0, 2.0); +var b = new Point(4.0, 6.0); + +double distance = a.DistanceTo(b); // 5.0 +Point mid = a.Lerp(b, 0.5); // {X=2.5, Y=4} + +// 3D points and rotations +var p = new Point3(1.0, 2.0, 3.0); +var rotation = new Rotation3(45.0, 0.0, 0.0); // 45 deg roll (wire/edit format) +var rotated = new Frame3(Point3.Zero, rotation.ToQuaternion()).TransformPoint(p); + +// Spherical coordinates +var orbit = SphericalPoint.OnEquator(5.0, Degree.Create(30)); +Point3 cartesian = orbit; // implicit conversion + +// Shapes with area and perimeter +var circle = new Circle(Point.Zero, 10.0); +double area = circle.Area(); // ~314.159 +bool inside = circle.Contains(new Point(3.0, 4.0)); // true + +// Polygon boolean operations +var poly1 = new Polygon( + new Point(0, 0), new Point(10, 0), + new Point(10, 10), new Point(0, 10)); +var poly2 = new Polygon( + new Point(5, 5), new Point(15, 5), + new Point(15, 15), new Point(5, 15)); + +Polygon merged = poly1 | poly2; // union +Polygon overlap = poly1 & poly2; // intersection + +// 3D trajectory with time-parameterized waypoints +var trajectory = new Trajectory3( + new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), + new Waypoint3(new Pose3(100, 0, 50, 0, 45, 0), 5.0)); +Pose3 atHalf = trajectory.AtTime(2.5); // interpolated pose +``` + +## API Reference + +### 2D Core Types + +| Type | Description | +|---|---| +| `Point` | 2D Cartesian point. Supports arithmetic with vectors and sizes, distance, lerp, reflect, rotate, clamp, midpoint, matrix transform, tuple conversion, and parsing. | +| `Vector` | 2D vector with magnitude, direction, dot/cross product, normalize, projection, reflect, rotate, perpendicular (CW/CCW), angle between, lerp, and matrix transform. | +| `Size` | Width/height pair with arithmetic operators, component-wise multiply/divide, and parsing. | +| `Matrix` | 3x3 affine transformation matrix. Supports translate, rotate, scale, skew, invert, append/prepend, and point/vector transforms. | +| `Matrix2x2` | 2x2 matrix for linear algebra: determinant, trace, transpose, inverse, eigendecomposition (closed-form for symmetric matrices), and point/vector transforms. | +| `Rectangle` | Axis-aligned rectangle with contains, intersect, inflate, offset, bounds, tiling, diagonal, distance-to-point, and LTRB construction. | +| `Thickness` | Four-sided thickness (top, right, bottom, left) for rectangular borders. | + +### 3D Core Types + +| Type | Description | +|---|---| +| `Point3` | 3D Cartesian point. Distance, lerp, midpoint, random generation, arithmetic with `Vector3`. Conversions to/from `System.Numerics.Vector3` and tuples. Protobuf and JSON serialization. | +| `Vector3` | 3D vector with length, normalize, dot product, cross product, angle between, projection, lerp, random unit vector. The rotation between two directions is `Quaternion.FromTo(from, to)`. Static basis vectors `EX`, `EY`, `EZ`. | +| `Matrix3x3` | 3x3 matrix for 3D transforms. Determinant, trace, transpose, inverse. Rotation factories: `RotationX`, `RotationY`, `RotationZ`, `RotationZYX`. Scale, column/row construction. Euler angle and quaternion extraction. | +| `Quaternion` | Quaternion for 3D rotations. Normalize, conjugate, inverse, from axis-angle, rotate vector, slerp, dot product. Conversions to/from `System.Numerics.Quaternion`. | +| `Segment3` | Bounded line segment in 3D. Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | + +### Shapes (2D) + +All 2D shapes implement `IShape`, providing `Area()`, `Perimeter()`, `Centroid()`, `BoundingBox()`, `Contains(Point)`, `Rotate(Degree, Point)`, and `Scale(T)`. + +| Type | Description | +|---|---| +| `Circle` | Center + radius. Intersects with lines, segments, circles, and rectangles. Tangent point detection. `PointAt(Radian)` for parameterized access. | +| `Triangle` | Three vertices. Incircle, circumcircle, orthocenter, angles, edges. Classification: `IsRight()`, `IsAcute()`, `IsObtuse()`, `IsEquilateral()`, `IsIsosceles()`, `IsScalene()`. Similarity and congruence tests. | +| `Polygon` | Immutable polygon backed by `ReadOnlyMemory>`. Shoelace area, ray-casting containment, convex hull (Graham scan), `IsConvex()`, edges, simplify, skeleton (medial axis). Boolean operations: union, intersect, subtract, cluster. Operators: `\|` (union), `&` (intersect), `-` (subtract). | +| `Rectangle` | Implements `IArea`, `IPerimeter`, `ICentroid`, `IBoundingBox`, `IScalable>`. Rotation returns `Polygon`. | + +### Shapes (3D) + +| Type | Description | +|---|---| +| `Triangle3` | Three vertices in 3D space. Area (cross product), perimeter, centroid, unit normal vector. `ToPose()` creates a coordinate frame from the triangle plane. Translate by `Vector3`. | + +### Lines and Segments + +| Type | Description | +|---|---| +| `Line` | Infinite 2D line (supports vertical lines). From two points, point + direction, or equation. Intersect with lines, segments, circles, triangles, rectangles, polygons. Distance-to-point, angle-between, parallel/perpendicular tests, project point, reflect point. | +| `Segment` | Bounded 2D line segment (start + end). Length, midpoint, direction, lerp, split, reverse. Intersect with segments, lines, circles, rectangles. Distance-to-point, project point, parallel test. Liang-Barsky rectangle clipping. | +| `Segment3` | Bounded 3D line segment (start + end). Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | + +### Curves and Paths (2D) + +| Type | Description | +|---|---| +| `BezierCurve` | Cubic Bezier curve (4 control points). `Evaluate(t)`, `Split(t)`, `SubCurve(t0, t1)` via De Casteljau. Extremum points, linear equation intersection, rectangle edge crossings. | +| `Path` | Immutable sequence of `BezierCurve` segments. Create from points (`FromPoints`) with smoothing, or from SVG path data (`Parse`). `Length()`, `PointAt(t)` with arc-length parameterization. Exact `Intersect(Rectangle)` that splits Bezier curves at crossing points. Transform, rotate, reverse, append. `Close()` and `ToPolygon()` for rasterization. | +| `ComplexCurve` | Composite curve of mixed Bezier and polyline segments packed in a contiguous byte buffer for zero-copy decoding. SVG export and JSON serialization. | +| `PolygonalCurve` | Mutable sequence of connected points that generates smooth Bezier segments via `GetSmoothSegment(i)`. | + +### Polylines (2D and 3D) + +| Type | Description | +|---|---| +| `Polyline` | Immutable open polyline in 2D (non-closing). Length, edges, bounding box, simplification. JSON, Protobuf, and SVG serialization. | +| `Polyline3` | Immutable open polyline in 3D. Length, edges, translate, transform. JSON and Protobuf serialization. | + +### Angles and Coordinate Systems + +| Type | Description | +|---|---| +| `Degree` | Angle in degrees. Implicit conversion from numeric values and from `Radian`. Arithmetic (+, -, *, /), normalize to (-180, 180], abs, comparison operators. | +| `Radian` | Angle in radians. Implicit conversion from `Degree`. Arithmetic (+, -, *, /), normalize to (-pi, pi], abs, comparison operators. Static `Sin`/`Cos` helpers. | +| `PolarPoint` | Polar coordinates (radius, angle). Implicit conversions to/from `Point`, explicit to `Vector2` and `Vector`. | +| `CylindricalPoint` | Cylindrical coordinates (radial distance, angle, height). Factory: `OnPlane(r, angle)`. Builder methods: `WithZ`, `WithRadialDistance`, `WithAngle`. `RotateAngle` for angular displacement. `+`/`-` operators with `Radian` and `Point3`. Conversions to/from `Point3`, `Vector3`, `SphericalPoint`, and `System.Numerics.Vector3`. | +| `SphericalPoint` | Spherical coordinates (radius, azimuth, inclination) with Degree-based storage (ISO convention: inclination from Z-axis). Factories: `OnEquator`, `FromElevation`, `NorthPole`, `SouthPole`. `Normalized`, `WithRadius`, `RotateAzimuth`, `RotateInclination`, `Rotate(az, inc)`. `AngularDistance` (Vincenty formula), `Slerp`, `Lerp`, Euclidean `Distance`. Radius scaling operators (`*`, `/`). `Point3` offset operators (`+`, `-`). Conversions to/from `Point3`, `Vector3`, `CylindricalPoint`, and `System.Numerics.Vector3`. | + +### Rotations and Orientation + +| Type | Description | +|---|---| +| `Rotation3` | Euler angle rotation (Roll/Pitch/Yaw in degrees, ZYX convention). Wire and edit format only (since 2.0): create from degrees, radians, quaternion (`FromQuaternion`) or `Matrix3x3` (`FromMatrix3x3`); `ToQuaternion()` for maths. No rotation algebra: compose, invert, slerp and rotate with `Quaternion` or `Frame3`. Protobuf and JSON serialization. | +| `Quaternion` | Unit quaternion for 3D rotations. From axis-angle, `FromRpy(roll, pitch, yaw)` (Rz·Ry·Rx, radians; the URDF rpy rule), `FromTo(from, to)` (shortest arc), `FromAxes(x, y, z)` (orthonormal basis), `FromMatrix3x3(m)` (Shepperd's method) and `ToMatrix3x3()`, rotate vector, slerp, dot, normalize, conjugate, inverse. Hamilton product operator (`*`). Conversions to/from `System.Numerics.Quaternion`. Protobuf serialization. | +| `Matrix3x3` | Also used as rotation representation. `RotationZYX(rx, ry, rz)` creates ZYX Euler rotation matrix. `ToEulerZYX()` extracts angles (gimbal lock: rz = 0). Both go through `Quaternion` (`FromRpy`/`ToMatrix3x3`, `FromMatrix3x3`/`Rotation3.FromQuaternion`), so there is one Euler formula each way. | + +### Poses and Trajectories + +| Type | Description | +|---|---| +| `Frame3` | Rigid frame = position (`Point3`) + unit `Quaternion`. All pose maths: `Multiply` / `*` (compose), `Inverse`, `TransformPoint`, `TransformVector`, `Interpolate` (position lerp + slerp), `Distance` (position error and angle error in radians). `FromSurface(a, b, c)` creates a frame from three coplanar points (right-hand rule); `FromSurface(a, b, c, h)` uses a hint point for Z; `FromSurface(points)` fits a plane. `FromPose3(pose, lengthScale)` / `ToPose3(lengthScale)` convert to and from the wire format (ZYX degrees). | +| `Pose3` | Wire and edit format only (since 2.0): position (`Point3`) + ZYX Euler orientation (`Rotation3`, degrees). Constructors, `Identity`, tuple conversions, deconstruction, parsing, equality, `Truncating()`. Protobuf and JSON serialization. Convert to `Frame3` for any maths. | +| `Waypoint3` | Timestamped `Pose3` -- an element of a trajectory. | +| `PosePath3` | Immutable sequence of `Pose3` (no time). Length, reverse, transform, extract `Polyline3`, convert to `Trajectory3` with uniform timing. | +| `Trajectory3` | Immutable time-parameterized path of `Waypoint3`. `Duration`, `Length`, `AtTime(t)` (binary-search interpolation with position lerp + rotation slerp). `Resample(interval)`, `Reverse`, `Transform`. Convert to `PosePath3` or `Polyline3`. Create a real-time `TrajectoryController` via `Control()`. | +| `TrajectoryController` | Real-time trajectory playback using `Stopwatch` clock. Start, stop, pause, resume, seek. Fires `PoseChanged` events with current pose, time, and progress. | +| `Corner` | Corner blending descriptor: `Sharp` (exact waypoint) or `Round(radius)` (arc blend before/after the waypoint). | + +### Kinematics + +| Type | Description | +|---|---| +| `KinematicChain` | Namespace `ModelingEvolution.Drawing.Kinematics`. A tree of `Link`s moved by revolute/prismatic `Joint`s (origin at q = 0, unit axis, limits) with named `ChainFrame`s; metres and radians. `Forward(q, links)` writes every link pose, `FramePose(links, frame)`, `Jacobian(links, frame, j)` writes the 6 × Dof geometric Jacobian (column-major, v then ω). Spans in, spans out, no allocation; the constructor validates topological order, unit axes, limits and unique names. | +| `Joints6` | 6-DOF joint angles stored as six `Degree` fields (no array allocation). Indexer `[0..5]`, arithmetic operators, `Lerp`, `MaxAbsDelta`, `IsWithin(tolerance)`, `IsBetween(min, max)`. `ToArray()`, `FromSpan()`, `FromArray()`, `CopyTo(Span)`. Protobuf and JSON serialization. | +| `Speed` | Scalar speed in units/second (unitless magnitude). Arithmetic, `TimeFor(distance)`, `DistanceIn(time)`. Parsing. | +| `Velocity3` | 3D velocity vector (direction + magnitude). Create from direction + speed. `Speed`, `Direction`, `DisplacementIn(time)`. Arithmetic operators. | +| `Speed` | **Unit-aware** speed tagged at compile time with its unit (`ISpeedUnit`). Stores the value in `TUnit`; `ToSi()`/`FromSi()`/`To()` normalise via the SI base (m/s). Full arithmetic, comparisons, ratio (`Speed/Speed → T`), `IParsable`. Mixing units (or a non-speed unit) is a compile error. Verified zero-cost vs raw `double`. | +| `ISpeedUnit` | Marker for speed units. Built-ins: `MetrePerSecond` (SI base), `MillimetrePerSecond`, `MetrePerMinute`. Implement to add your own. | + +### Equations + +| Type | Description | +|---|---| +| `LinearEquation` | `y = Ax + B`. Create from points, angle, or direction. Compute, zero point, intersect, perpendicular, translate, mirror. | +| `QuadraticEquation` | `ax^2 + bx + c = 0`. Zero points using the discriminant. | +| `CubicEquation` | `ax^3 + bx^2 + cx + d = 0`. Root finding via Newton-Raphson. | +| `CircleEquation` | `(x-cx)^2 + (y-cy)^2 = r^2`. Intersect with `LinearEquation`. | + +### Colors + +| Type | Description | +|---|---| +| `Color` | ARGB color as a 32-bit uint. Parse from hex (`#RRGGBB`, `#AARRGGBB`), `rgba(r,g,b,a)`, or HSV strings. `GetHue()`, `GetSaturation()`, `GetLightness()`, `GetBrightness()`, `MakeTransparent(float)`. Implicit from string or tuple. JSON and Protobuf serialization. | +| `HsvColor` | HSV (hue, saturation, value) color space with optional alpha. Implicit conversions to/from `Color`. Parse from `hsv()`/`hsva()` strings, JSON arrays, or hex. | + +### Data Structures and Algorithms + +| Type | Description | +|---|---| +| `KdTree` | 2D KD-tree for fast nearest-neighbor queries on `Point`. Build from span, add individual points, find nearest neighbor. | +| `Skeleton` | Medial axis approximation of a polygon (interior nodes + edges). Three algorithms: `StraightSkeleton`, `ChordalAxis`, and `Voronoi`. | +| `MovingAverage` | Component-wise moving average over a sliding window of 2D points. O(1) add and query via circular buffer + running sums. | +| `MovingMedian` | Component-wise moving median over a sliding window of 2D points. O(1) median read, O(n) add via maintained sorted arrays. | +| `AllocationScope` | Arena-style pooled allocation scope. Library allocations use `MemoryPool.Shared` when active. Dispose returns all tracked memory at once. Supports `Persist()` to detach individual results. | +| `AlignmentResult` | Result of a rigid 2D alignment (rotation + translation) between two point clouds, with rotation angle and residual error. | + +### Interfaces + +```csharp +// Composable geometry traits +public interface IArea { T Area(); } +public interface IPerimeter { T Perimeter(); } +public interface ICentroid { Point Centroid(); } +public interface IBoundingBox { Rectangle BoundingBox(); } +public interface IRotatable { TSelf Rotate(Degree angle, Point origin = default); } +public interface IScalable { TSelf Scale(T factor); } + +// Unified shape interface -- combines all of the above plus point containment +public interface IShape : IArea, IPerimeter, ICentroid, + IBoundingBox, IRotatable, IScalable +{ + bool Contains(Point point); +} +``` + +Implementing types: `Circle`, `Triangle`, `Polygon`, and `Rectangle` (partial -- no `IRotatable` since rotation produces a `Polygon`). + +## Key Features + +### Generic Math + +All geometry types are parameterized by `T` with constraints like `IFloatingPointIeee754`, `IMinMaxValue`, etc. This means the same code works with any numeric type: + +```csharp +// Single precision +var pf = new Point(1f, 2f); +var cf = new Circle(pf, 5f); + +// Double precision +var pd = new Point(1.0, 2.0); +var cd = new Circle(pd, 5.0); + +// Both compute area with their respective precision +float areaF = cf.Area(); +double areaD = cd.Area(); +``` + +### IShape Interface Hierarchy + +Write generic algorithms over any shape: + +```csharp +void PrintShapeInfo(TShape shape) + where T : INumber, ITrigonometricFunctions, IRootFunctions, + IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue + where TShape : IShape +{ + Console.WriteLine($"Area: {shape.Area()}"); + Console.WriteLine($"Perimeter: {shape.Perimeter()}"); + Console.WriteLine($"Centroid: {shape.Centroid()}"); + Console.WriteLine($"Bounds: {shape.BoundingBox()}"); +} +``` + +### Intersections + +The `Intersections` static class provides 21 combination overloads covering every pair of: `Line`, `Segment`, `Circle`, `Triangle`, `Rectangle`, and `Polygon`. Each combination also has a zero-allocation `FirstOf` variant. + +```csharp +// Line x Line +Point? p = Intersections.Of(line1, line2); + +// Line x Circle -> chord (Segment) or null +Segment? chord = Intersections.Of(line, circle); + +// Tangent detection +Point? tangent = Intersections.TangentPoint(line, circle); + +// Circle x Circle -> radical chord or tangent point +Segment? radicalChord = Intersections.Of(circle1, circle2); + +// Segment x Segment +Point? hit = Intersections.Of(seg1, seg2); + +// Zero-alloc first-hit variants +Segment? first = Intersections.FirstOf(line, polygon); +``` + +Instance methods are also available on each shape for convenience: + +```csharp +Segment? chord = circle.Intersect(line); +Point? hit = segment.Intersect(otherSegment); +Segment? clipped = line.Intersect(rectangle); +``` + +### Polygon Boolean Operations + +Powered by [Clipper2](https://github.com/AngusJohnson/Clipper2): + +```csharp +var a = new Polygon( /* ... */ ); +var b = new Polygon( /* ... */ ); + +// Operators +Polygon union = a | b; +Polygon intersect = a & b; +Polygon difference = a - b; + +// Methods returning multiple result polygons +List> unions = a.Union(b, removeHoles: true); +List> diffs = a.Subtract(b); + +// Batch operations +List> merged = Polygon.Union(polygons); + +// Clustering overlapping polygons +IEnumerable> clusters = Polygon.Cluster(polygons); +``` + +### Bezier Path Clipping + +`Path.Intersect(Rectangle)` performs analytically exact Bezier-rectangle clipping. It finds the precise parameter values where each cubic Bezier segment crosses the rectangle edges, then uses `SubCurve(t0, t1)` via De Casteljau subdivision to return exact sub-paths -- not polygon approximations. + +```csharp +var path = Path.FromPoints(points); +IEnumerable> clipped = path.Intersect(viewport); + +foreach (var subPath in clipped) +{ + // Each sub-path contains only the Bezier segments inside the rectangle, + // with curves split precisely at the rectangle boundaries. +} +``` + +### Pooled Allocation Scopes + +When performing many geometry operations that allocate intermediate results (polygon booleans, skeleton computation, curve corrections), use `AllocationScope` to avoid GC pressure: + +```csharp +using var scope = AllocationScope.Begin(); + +var polygon = somePolygon * new Size(2, 2); // pooled +var skeleton = polygon.Skeleton(); // pooled +var lease = scope.Persist(ref skeleton); // detach from scope + +// scope.Dispose() returns everything except skeleton's memory +// lease.Dispose() returns skeleton's memory when you're done +``` + +### Serialization + +**Protobuf** -- all core types carry `[ProtoContract]` / `[ProtoMember]` attributes for use with protobuf-net: + +```csharp +var point = new Point(1f, 2f); +byte[] bytes = Serializer.Serialize(point); +``` + +**JSON** -- custom `System.Text.Json` converters for `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Polygon`, `Polyline`, `Polyline3`, `PosePath3`, `Trajectory3`, `Path`, `ComplexCurve`, `BezierCurve`, `Matrix2x2`, `Matrix3x3`, `Skeleton`, `Color`, and `HsvColor`: + +```csharp +var pose = new Pose3(100, 200, 300, 45, 90, 0); +string json = JsonSerializer.Serialize(pose); // [100,200,300,45,90,0] + +var joints = new Joints6(10, 20, 30, 40, 50, 60); +string j = JsonSerializer.Serialize(joints); // [10,20,30,40,50,60] +``` + +**String parsing** -- `IParsable` implementations on `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Rectangle`, `Size`, `Path`, `ComplexCurve`, `Speed`, `Color`, and `HsvColor`: + +```csharp +var point = Point.Parse("1.5, 2.5"); +var point3 = Point3.Parse("1.0 2.0 3.0"); +var pose = Pose3.Parse("100, 200, 300, 45, 90, 0"); +var rect = Rectangle.Parse("0 0 100 200"); +var color = Color.Parse("#FF8800"); +var path = Path.Parse("M 0 0 C 1 2, 3 4, 5 6", null); +``` + +### SVG Export + +`Polygon`, `Polyline`, `ComplexCurve`, `Skeleton`, and `Path` support SVG export via the `[SvgExporter]` attribute and the `SvgExporter` infrastructure: + +```csharp +// Path.ToString() produces SVG path data +var path = Path.FromPoints(points); +string svgData = path.ToString(); // "M 0 0 C 1.5 2.3, 3.1 4.2, 5 6 ..." +``` + +## Code Examples + +### Points: Distance, Lerp, Rotate + +```csharp +var a = new Point(0, 0); +var b = new Point(3, 4); + +double dist = a.DistanceTo(b); // 5.0 +Point quarter = a.Lerp(b, 0.25); // {X=0.75, Y=1} +Point reflected = a.Reflect(b); // {X=6, Y=8} + +// Rotate 90 degrees around the origin +Degree angle = 90.0; +Point rotated = b.Rotate(angle); // {X=-4, Y=3} + +// Rotate around a custom center +var center = new Point(1, 1); +Point rotated2 = b.Rotate(angle, center); + +// Clamp to a rectangle +var bounds = new Rectangle(0, 0, 10, 10); +Point clamped = new Point(15, -3).Clamp(bounds); // {X=10, Y=0} +``` + +### 3D Points, Vectors, and Rotations + +```csharp +// 3D point operations +var p1 = new Point3(1, 2, 3); +var p2 = new Point3(4, 6, 3); +double dist3d = Point3.Distance(p1, p2); // 5.0 +Point3 mid3d = Point3.Lerp(p1, p2, 0.5); + +// 3D vector operations +var v = new Vector3(1, 0, 0); +var w = new Vector3(0, 1, 0); +var cross = Vector3.Cross(v, w); // (0, 0, 1) +double dot = Vector3.Dot(v, w); // 0.0 + +// Compute rotation between two directions +var rot = Quaternion.FromTo(v, w); +Vector3 result = rot.Rotate(v); // approximately (0, 1, 0) + +// Euler angles (ZYX convention) are a wire format: convert once, then use the quaternion +var rotation = Rotation3.FromDegrees(45, 0, 0).ToQuaternion(); +Vector3 rotated = rotation.Rotate((Vector3)p1); + +// Quaternion slerp +var r1 = Quaternion.Identity; +var r2 = Quaternion.FromAxisAngle(Vector3.EZ, Math.PI / 2); +var halfway = Quaternion.Slerp(r1, r2, 0.5); + +// Matrix3x3 rotation +var m = Matrix3x3.RotationZ(Degree.Create(45)); +var transformed = m.Transform(new Point3(1, 0, 0)); +``` + +### Spherical Coordinates + +```csharp +// Create spherical points with factory methods +var northPole = SphericalPoint.NorthPole(10.0); // +Z, radius=10 +var equator = SphericalPoint.OnEquator(5.0, Degree.Create(45)); +var elevated = SphericalPoint.FromElevation(5.0, + Degree.Create(0), Degree.Create(30)); + +// Convert to/from Cartesian +Point3 cartesian = equator; // implicit conversion +SphericalPoint spherical = cartesian; // implicit conversion + +// Orbit around a 3D center +var center = new Point3(10, 50, 0); +var points = new List>(); +for (int az = 0; az < 360; az += 30) + points.Add(center + SphericalPoint.OnEquator(5.0, Degree.Create(az))); + +// Rotation (O(1) angle operations) +var sp = SphericalPoint.OnEquator(5.0, Degree.Create(0)); +var rotated = sp.RotateAzimuth(Degree.Create(45)); // rotate around Z + +// Angular distance (Vincenty formula -- numerically stable) +var a = SphericalPoint.NorthPole(1.0); +var b = SphericalPoint.SouthPole(1.0); +Degree angDist = SphericalPoint.AngularDistance(a, b); // 180 degrees + +// Spherical linear interpolation +var mid = SphericalPoint.Slerp(a, b, 0.5); + +// Cylindrical coordinate conversions +CylindricalPoint cyl = sp; // implicit conversion +SphericalPoint back = cyl; // implicit conversion +``` + +### Poses and Trajectories + +```csharp +// Pose = position + orientation +var pose = new Pose3(100, 200, 300, 45, 0, 0); // x,y,z, rx,ry,rz (degrees) + +// Maths composes Frame3 (position + quaternion); Pose3 converts once, here mm -> m +var world = Frame3.FromPose3(new Pose3(10, 0, 0, 0, 0, 0), 0.001); +var local = Frame3.FromPose3(new Pose3(5, 0, 0, 0, 0, 90), 0.001); +var combined = world * local; +Pose3 back = combined.ToPose3(1000); // m -> mm, ZYX degrees + +// Create frame from a surface (3 points define a plane) +var a = new Point3(0, 0, 0); +var b = new Point3(1, 0, 0); +var c = new Point3(0, 1, 0); +var surface = Frame3.FromSurface(a, b, c); // Z-axis via right-hand rule + +// Trajectory with timed waypoints +var trajectory = new Trajectory3( + new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), + new Waypoint3(new Pose3(50, 0, 25, 0, 30, 0), 2.5), + new Waypoint3(new Pose3(100, 0, 0, 0, 0, 0), 5.0)); + +// Interpolate at any time (binary search + lerp/slerp) +Pose3 atT = trajectory.AtTime(1.25); + +// Resample at uniform intervals +var resampled = trajectory.Resample(0.1); // every 0.1 seconds + +// Real-time playback +var controller = trajectory.Control(); +controller.PoseChanged += (_, e) => Console.WriteLine($"t={e.Time} pose={e.Pose}"); +controller.Start(); + +// 6-DOF joint angles for robotics +var home = new Joints6(0, -90, 0, 0, 90, 0); +var target = new Joints6(45, -60, 30, 0, 90, -45); +var halfway = Joints6.Lerp(home, target, 0.5); +bool inRange = target.IsBetween(minLimits, maxLimits); +``` + +### Shapes: Area and Perimeter + +```csharp +// Circle +var circle = new Circle(new Point(5, 5), 3.0); +double cArea = circle.Area(); // ~28.274 +double cPerim = circle.Perimeter(); // ~18.850 + +// Triangle +var triangle = new Triangle( + new Point(0, 0), + new Point(4, 0), + new Point(0, 3)); +double tArea = triangle.Area(); // 6.0 +double tPerim = triangle.Perimeter(); // 12.0 + +// Polygon (shoelace formula) +var poly = new Polygon( + new Point(0, 0), new Point(4, 0), + new Point(4, 3), new Point(0, 3)); +double pArea = poly.Area(); // 12.0 + +// 3D triangle +var t3 = new Triangle3( + new Point3(0, 0, 0), + new Point3(4, 0, 0), + new Point3(0, 3, 0)); +double area3 = t3.Area; // 6.0 +Vector3 normal = t3.Normal; // (0, 0, 1) +``` + +### Line-Circle Intersection + +```csharp +var circle = new Circle(new Point(0, 0), 5.0); +var line = Line.From(new Point(-10, 0), new Point(10, 0)); + +// Secant -- returns the chord as a segment +Segment? chord = circle.Intersect(line); +// chord.Value.Start ~ {X=-5, Y=0}, chord.Value.End ~ {X=5, Y=0} + +// Tangent detection +var tangentLine = Line.Horizontal(5.0); +Point? tangent = circle.TangentPoint(tangentLine); +// tangent.Value ~ {X=0, Y=5} +``` + +### Segment-Rectangle Clipping + +```csharp +var viewport = new Rectangle(0, 0, 100, 100); +var segment = new Segment( + new Point(-20, 50), + new Point(150, 50)); + +// Liang-Barsky clipping +Segment? clipped = segment.Intersect(viewport); +// clipped.Value: Start={X=0, Y=50}, End={X=100, Y=50} +``` + +### Path-Rectangle Clipping + +```csharp +var points = new[] +{ + new Point(10, 50), + new Point(50, 10), + new Point(90, 50), + new Point(130, 90) +}; +var path = Path.FromPoints(points); +var viewport = new Rectangle(20, 20, 60, 60); // 20,20 to 80,80 + +// Exact Bezier clipping -- returns sub-paths with curves split at boundaries +foreach (var subPath in path.Intersect(viewport)) +{ + Console.WriteLine($"Sub-path with {subPath.Count} Bezier segments"); +} +``` + +### Triangle Properties + +```csharp +var t = new Triangle( + new Point(0, 0), + new Point(4, 0), + new Point(2, 3)); + +// Derived circles +Circle incircle = t.Incircle(); // largest circle inside +Circle circumcircle = t.Circumcircle(); // passes through all vertices + +// Center points +Point centroid = t.Centroid(); +Point orthocenter = t.Orthocenter; + +// Classification +bool right = t.IsRight(); +bool equilateral = t.IsEquilateral(); +bool isosceles = t.IsIsosceles(); + +// Similarity and congruence +var t2 = new Triangle( + new Point(0, 0), + new Point(8, 0), + new Point(4, 6)); +bool similar = t.IsSimilarTo(t2); // true (same shape, scaled 2x) +bool congruent = t.IsCongruentTo(t2); // false (different size) + +// Interior angles +var (atA, atB, atC) = t.Angles; +``` + +### Polygon Operations + +```csharp +// Convex hull +var cloud = new Polygon( + new Point(0, 0), new Point(1, 3), + new Point(2, 1), new Point(4, 4), + new Point(3, 0), new Point(5, 2)); +Polygon hull = cloud.ConvexHull(); + +// Point containment (ray casting) +bool inside = hull.Contains(new Point(2, 2)); + +// Boolean operations +var a = new Polygon( /* ... */ ); +var b = new Polygon( /* ... */ ); +List> union = a.Union(b, removeHoles: true); +List> diff = a.Subtract(b); +List> inter = a.Intersect(b); + +// Simplify (remove collinear/close points) +Polygon simplified = hull.Simplify(epsilon: 0.1); + +// Convexity test +bool convex = hull.IsConvex(); + +// Clustering overlapping polygons +var clusters = Polygon.Cluster(polygons); +``` + +### Matrix Transformations (2D and 3D) + +```csharp +// 2D affine transformation +var matrix = Matrix.Identity; +matrix.Translate(10.0, 20.0); +matrix.Rotate(Degree.Create(45.0)); +matrix.Scale(2.0, 2.0); + +Point transformed = matrix.Transform(new Point(1, 0)); +Vector rotatedVec = matrix.Transform(new Vector(1, 0)); + +var combined = matrix1 * matrix2; +matrix.Invert(); + +// 3D rotation matrix +var rx = Matrix3x3.RotationX(Degree.Create(45)); +var ry = Matrix3x3.RotationY(Degree.Create(30)); +var rz = Matrix3x3.RotationZ(Degree.Create(60)); + +// Compose rotations +var rotation = rz * ry * rx; +var p = new Point3(1, 0, 0); +Point3 rotated3 = rotation * p; // or rotation.Transform(p) + +// Extract Euler angles +var (eRx, eRy, eRz) = rotation.ToEulerZYX(); + +// Convert to quaternion and back +Quaternion q = Quaternion.FromMatrix3x3(rotation); +Matrix3x3 back = q.ToMatrix3x3(); +``` + +### Unit-Aware Speed + +```csharp +using ModelingEvolution.Drawing.Units; + +// The unit is part of the type — the value is stored in that unit. +Speed> feed = new(300); // 300 m/min + +// Normalise to the SI base (m/s) or convert to another speed unit. +double si = feed.ToSi(); // 5.0 (m/s) +var inMmPerS = feed.To>(); // 5000 mm/s + +// Same-unit arithmetic, scalar scaling, and a dimensionless ratio. +var faster = feed * 2.0; // 600 m/min +double ratio = faster / feed; // 2.0 + +// Cross-unit add normalises through SI; result stays in the left unit. +var total = inMmPerS.Add(feed); // (5000 + 5000) mm/s + +Console.WriteLine(feed); // "300 m/min" + +// Compile-time safety: +// feed + inMmPerS; // error: different units — use .Add(...) +// feed.To>(); // error: not an ISpeedUnit +``` + +## Requirements + +- **.NET 10.0** or later (uses generic math interfaces from .NET 7+) +- **Dependencies**: [Clipper2](https://www.nuget.org/packages/Clipper2) 1.4.0, [protobuf-net](https://www.nuget.org/packages/protobuf-net) 3.2.45 + +## License + +See [LICENSE](LICENSE) for details. diff --git a/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs index 12aaed8..e3aaa34 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs @@ -1,612 +1,612 @@ -using System.Text.Json; -using FluentAssertions; -using ModelingEvolution.Drawing; - -namespace ModelingEvolution.Drawing.Tests; - -public class Matrix3x3Tests -{ - private const double Tol = 1e-10; - private const double TolLoose = 1e-6; - - #region Identity & Properties - - [Fact] - public void Identity_HasCorrectValues() - { - var m = Matrix3x3.Identity; - m.M11.Should().Be(1d); m.M12.Should().Be(0d); m.M13.Should().Be(0d); - m.M21.Should().Be(0d); m.M22.Should().Be(1d); m.M23.Should().Be(0d); - m.M31.Should().Be(0d); m.M32.Should().Be(0d); m.M33.Should().Be(1d); - } - - [Fact] - public void Zero_HasCorrectValues() - { - var m = Matrix3x3.Zero; - m.M11.Should().Be(0d); m.M12.Should().Be(0d); m.M13.Should().Be(0d); - m.M21.Should().Be(0d); m.M22.Should().Be(0d); m.M23.Should().Be(0d); - m.M31.Should().Be(0d); m.M32.Should().Be(0d); m.M33.Should().Be(0d); - } - - [Fact] - public void Determinant_Identity_IsOne() - { - Matrix3x3.Identity.Determinant.Should().Be(1d); - } - - [Fact] - public void Determinant_KnownMatrix() - { - // |1 2 3; 0 1 4; 5 6 0| = 1*(0-24) - 2*(0-20) + 3*(0-5) = -24+40-15 = 1 - var m = new Matrix3x3(1, 2, 3, 0, 1, 4, 5, 6, 0); - m.Determinant.Should().BeApproximately(1d, Tol); - } - - [Fact] - public void Trace_KnownMatrix() - { - var m = new Matrix3x3(3, 0, 0, 0, 5, 0, 0, 0, 7); - m.Trace.Should().BeApproximately(15d, Tol); - } - - #endregion - - #region Transpose - - [Fact] - public void Transpose_General() - { - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var t = m.Transpose(); - t.M11.Should().Be(1d); t.M12.Should().Be(4d); t.M13.Should().Be(7d); - t.M21.Should().Be(2d); t.M22.Should().Be(5d); t.M23.Should().Be(8d); - t.M31.Should().Be(3d); t.M32.Should().Be(6d); t.M33.Should().Be(9d); - } - - [Fact] - public void Transpose_RotationMatrix_IsInverse() - { - var r = Matrix3x3.RotationX(Degree.Create(30)); - var product = r * r.Transpose(); - AssertIsIdentity(product); - } - - #endregion - - #region Inverse - - [Fact] - public void Inverse_Identity() - { - var inv = Matrix3x3.Identity.Inverse(); - AssertIsIdentity(inv); - } - - [Fact] - public void Inverse_KnownMatrix() - { - var m = new Matrix3x3(1, 2, 3, 0, 1, 4, 5, 6, 0); - var inv = m.Inverse(); - var product = m * inv; - AssertIsIdentity(product); - } - - [Fact] - public void Inverse_Singular_Throws() - { - // Third row = first row → det = 0 - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 1, 2, 3); - var act = () => m.Inverse(); - act.Should().Throw(); - } - - #endregion - - #region Multiplication - - [Fact] - public void Multiplication_IdentityIsNeutral() - { - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var r = m * Matrix3x3.Identity; - AssertMatricesEqual(r, m); - } - - [Fact] - public void Multiplication_KnownResult() - { - var a = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var b = new Matrix3x3(9, 8, 7, 6, 5, 4, 3, 2, 1); - var c = a * b; - // Row 0: (1*9+2*6+3*3, 1*8+2*5+3*2, 1*7+2*4+3*1) = (30, 24, 18) - c.M11.Should().BeApproximately(30, Tol); - c.M12.Should().BeApproximately(24, Tol); - c.M13.Should().BeApproximately(18, Tol); - // Row 1: (4*9+5*6+6*3, 4*8+5*5+6*2, 4*7+5*4+6*1) = (84, 69, 54) - c.M21.Should().BeApproximately(84, Tol); - c.M22.Should().BeApproximately(69, Tol); - c.M23.Should().BeApproximately(54, Tol); - // Row 2: (7*9+8*6+9*3, 7*8+8*5+9*2, 7*7+8*4+9*1) = (138, 114, 90) - c.M31.Should().BeApproximately(138, Tol); - c.M32.Should().BeApproximately(114, Tol); - c.M33.Should().BeApproximately(90, Tol); - } - - [Fact] - public void Multiplication_Associativity() - { - var a = Matrix3x3.RotationX(Degree.Create(30)); - var b = Matrix3x3.RotationY(Degree.Create(45)); - var c = Matrix3x3.RotationZ(Degree.Create(60)); - var ab_c = (a * b) * c; - var a_bc = a * (b * c); - AssertMatricesEqual(ab_c, a_bc); - } - - #endregion - - #region Transform - - [Fact] - public void Transform_Vector_IdentityPreserves() - { - var v = new Vector3(1, 2, 3); - var r = Matrix3x3.Identity.Transform(v); - r.X.Should().BeApproximately(1, Tol); - r.Y.Should().BeApproximately(2, Tol); - r.Z.Should().BeApproximately(3, Tol); - } - - [Fact] - public void Transform_Vector_RotationX90_RotatesBasisVectors() - { - var m = Matrix3x3.RotationX(Degree.Create(90)); - // Rotating Y axis by 90° around X → Z axis - var ey = new Vector3(0, 1, 0); - var result = m * ey; - result.X.Should().BeApproximately(0, TolLoose); - result.Y.Should().BeApproximately(0, TolLoose); - result.Z.Should().BeApproximately(1, TolLoose); - } - - [Fact] - public void Transform_Point_OperatorSyntax() - { - var m = Matrix3x3.Scale(2, 3, 4); - var p = new Point3(1, 2, 3); - var r = m * p; - r.X.Should().BeApproximately(2, Tol); - r.Y.Should().BeApproximately(6, Tol); - r.Z.Should().BeApproximately(12, Tol); - } - - #endregion - - #region Rotation Factories - - [Fact] - public void RotationX_90_RotatesYToZ() - { - var m = Matrix3x3.RotationX(Degree.Create(90)); - var v = new Vector3(0, 1, 0); - var r = m * v; - r.X.Should().BeApproximately(0, TolLoose); - r.Y.Should().BeApproximately(0, TolLoose); - r.Z.Should().BeApproximately(1, TolLoose); - } - - [Fact] - public void RotationY_90_RotatesZToX() - { - var m = Matrix3x3.RotationY(Degree.Create(90)); - var v = new Vector3(0, 0, 1); - var r = m * v; - r.X.Should().BeApproximately(1, TolLoose); - r.Y.Should().BeApproximately(0, TolLoose); - r.Z.Should().BeApproximately(0, TolLoose); - } - - [Fact] - public void RotationZ_90_RotatesXToY() - { - var m = Matrix3x3.RotationZ(Degree.Create(90)); - var v = new Vector3(1, 0, 0); - var r = m * v; - r.X.Should().BeApproximately(0, TolLoose); - r.Y.Should().BeApproximately(1, TolLoose); - r.Z.Should().BeApproximately(0, TolLoose); - } - - [Fact] - public void RotationZYX_MatchesComposedRotations() - { - var rx = Degree.Create(30d); - var ry = Degree.Create(45d); - var rz = Degree.Create(60d); - var composed = Matrix3x3.RotationZ(rz) * Matrix3x3.RotationY(ry) * Matrix3x3.RotationX(rx); - var direct = Matrix3x3.RotationZYX(rx, ry, rz); - AssertMatricesEqual(composed, direct); - } - - [Fact] - public void RotationMatrix_IsOrthogonal() - { - var m = Matrix3x3.RotationZYX(Degree.Create(25), Degree.Create(35), Degree.Create(45)); - var product = m * m.Transpose(); - AssertIsIdentity(product); - } - - [Fact] - public void RotationMatrix_Determinant_IsOne() - { - var m = Matrix3x3.RotationZYX(Degree.Create(10), Degree.Create(20), Degree.Create(30)); - m.Determinant.Should().BeApproximately(1d, TolLoose); - } - - #endregion - - #region Scale & Construction Factories - - [Fact] - public void Scale_ScalesVector() - { - var m = Matrix3x3.Scale(2, 3, 4); - var v = new Vector3(1, 1, 1); - var r = m * v; - r.X.Should().BeApproximately(2, Tol); - r.Y.Should().BeApproximately(3, Tol); - r.Z.Should().BeApproximately(4, Tol); - } - - [Fact] - public void FromColumns_BuildsCorrectMatrix() - { - var c0 = new Vector3(1, 4, 7); - var c1 = new Vector3(2, 5, 8); - var c2 = new Vector3(3, 6, 9); - var m = Matrix3x3.FromColumns(c0, c1, c2); - m.M11.Should().Be(1); m.M12.Should().Be(2); m.M13.Should().Be(3); - m.M21.Should().Be(4); m.M22.Should().Be(5); m.M23.Should().Be(6); - m.M31.Should().Be(7); m.M32.Should().Be(8); m.M33.Should().Be(9); - } - - [Fact] - public void FromRows_BuildsCorrectMatrix() - { - var r0 = new Vector3(1, 2, 3); - var r1 = new Vector3(4, 5, 6); - var r2 = new Vector3(7, 8, 9); - var m = Matrix3x3.FromRows(r0, r1, r2); - m.M11.Should().Be(1); m.M12.Should().Be(2); m.M13.Should().Be(3); - m.M21.Should().Be(4); m.M22.Should().Be(5); m.M23.Should().Be(6); - m.M31.Should().Be(7); m.M32.Should().Be(8); m.M33.Should().Be(9); - } - - #endregion - - #region Euler Round-trip - - [Theory] - [InlineData(10, 20, 30)] - [InlineData(-15, 45, -60)] - [InlineData(0, 0, 90)] - [InlineData(180, 0, 0)] - [InlineData(5, 10, 15)] - public void RotationZYX_ToEulerZYX_RoundTrip(double rxd, double ryd, double rzd) - { - Degree rx = Degree.Create(rxd), ry = Degree.Create(ryd), rz = Degree.Create(rzd); - var m = Matrix3x3.RotationZYX(rx, ry, rz); - var (erx, ery, erz) = m.ToEulerZYX(); - // Rebuild matrix from extracted angles and compare - var m2 = Matrix3x3.RotationZYX(erx, ery, erz); - AssertMatricesEqual(m, m2); - } - - [Fact] - public void ToEulerZYX_GimbalLock_AtPositive90() - { - var m = Matrix3x3.RotationZYX(Degree.Create(0), Degree.Create(90), Degree.Create(0)); - var (rx, ry, rz) = m.ToEulerZYX(); - // ry should be ~90° - ((double)ry).Should().BeApproximately(90, TolLoose); - // At gimbal lock, rx and rz share DOF; matrix should still reconstruct - var m2 = Matrix3x3.RotationZYX(rx, ry, rz); - AssertMatricesEqual(m, m2, 1e-5); - } - - #endregion - - #region Quaternion Round-trip - - [Theory] - [InlineData(10, 20, 30)] - [InlineData(-30, 45, 60)] - [InlineData(0, 0, 0)] - [InlineData(5, -10, 15)] - public void ToQuaternion_FromQuaternion_RoundTrip(double rxd, double ryd, double rzd) - { - var m = Matrix3x3.RotationZYX(Degree.Create(rxd), Degree.Create(ryd), Degree.Create(rzd)); - var q = Quaternion.FromMatrix3x3(m); - // Convert quaternion back to rotation, then to matrix - var rot = Rotation3.FromQuaternion(q); - var m2 = rot.ToQuaternion().ToMatrix3x3(); - AssertMatricesEqual(m, m2, 1e-9); - } - - [Fact] - public void ToQuaternion_Identity_IsUnitQuaternion() - { - var q = Quaternion.FromMatrix3x3(Matrix3x3.Identity); - q.W.Should().BeApproximately(1, TolLoose); - q.X.Should().BeApproximately(0, TolLoose); - q.Y.Should().BeApproximately(0, TolLoose); - q.Z.Should().BeApproximately(0, TolLoose); - } - - #endregion - - #region Rotation3 Integration - - [Theory] - [InlineData(10, 20, 30)] - [InlineData(-15, 45, -60)] - [InlineData(0, 0, 0)] - public void Rotation3_QuaternionMatrix_RoundTrip(double rxd, double ryd, double rzd) - { - var rot = new Rotation3(rxd, ryd, rzd); - var m = rot.ToQuaternion().ToMatrix3x3(); - var rot2 = Rotation3.FromMatrix3x3(m); - ((double)rot2.Rx).Should().BeApproximately(rxd, TolLoose); - ((double)rot2.Ry).Should().BeApproximately(ryd, TolLoose); - ((double)rot2.Rz).Should().BeApproximately(rzd, TolLoose); - } - - [Fact] - public void Rotation3_QuaternionRotate_MatchesMatrix_Transform() - { - var rot = new Rotation3(30, 45, 60); - var m = Matrix3x3.RotationZYX(rot.Rx, rot.Ry, rot.Rz); - var v = new Vector3(1, 2, 3); - - var rotated = rot.ToQuaternion().Rotate(v); - var transformed = m.Transform(v); - - rotated.X.Should().BeApproximately(transformed.X, TolLoose); - rotated.Y.Should().BeApproximately(transformed.Y, TolLoose); - rotated.Z.Should().BeApproximately(transformed.Z, TolLoose); - } - - #endregion - - #region Scalar & Element-wise - - [Fact] - public void ScalarMultiplication() - { - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var r = m * 2d; - r.M11.Should().Be(2); r.M12.Should().Be(4); r.M13.Should().Be(6); - r.M21.Should().Be(8); r.M22.Should().Be(10); r.M23.Should().Be(12); - r.M31.Should().Be(14); r.M32.Should().Be(16); r.M33.Should().Be(18); - } - - [Fact] - public void ScalarMultiplication_Commutative() - { - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var r1 = m * 3d; - var r2 = 3d * m; - r1.Should().Be(r2); - } - - [Fact] - public void Addition() - { - var a = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var b = new Matrix3x3(9, 8, 7, 6, 5, 4, 3, 2, 1); - var c = a + b; - c.M11.Should().Be(10); c.M12.Should().Be(10); c.M13.Should().Be(10); - c.M21.Should().Be(10); c.M22.Should().Be(10); c.M23.Should().Be(10); - c.M31.Should().Be(10); c.M32.Should().Be(10); c.M33.Should().Be(10); - } - - [Fact] - public void Subtraction() - { - var a = new Matrix3x3(9, 8, 7, 6, 5, 4, 3, 2, 1); - var b = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var c = a - b; - c.M11.Should().Be(8); c.M12.Should().Be(6); c.M13.Should().Be(4); - c.M21.Should().Be(2); c.M22.Should().Be(0); c.M23.Should().Be(-2); - c.M31.Should().Be(-4); c.M32.Should().Be(-6); c.M33.Should().Be(-8); - } - - #endregion - - #region JSON Serialization - - [Fact] - public void Json_Serialize_Double() - { - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var json = JsonSerializer.Serialize(m); - json.Should().Be("[[1,2,3],[4,5,6],[7,8,9]]"); - } - - [Fact] - public void Json_Deserialize_Double() - { - var m = JsonSerializer.Deserialize>("[[1.5,2.5,3.5],[4.5,5.5,6.5],[7.5,8.5,9.5]]"); - m.M11.Should().Be(1.5); m.M12.Should().Be(2.5); m.M13.Should().Be(3.5); - m.M21.Should().Be(4.5); m.M22.Should().Be(5.5); m.M23.Should().Be(6.5); - m.M31.Should().Be(7.5); m.M32.Should().Be(8.5); m.M33.Should().Be(9.5); - } - - [Fact] - public void Json_RoundTrip_Double() - { - var original = new Matrix3x3(1.23, 4.56, 7.89, 0.12, 3.45, 6.78, 9.01, 2.34, 5.67); - var json = JsonSerializer.Serialize(original); - var deserialized = JsonSerializer.Deserialize>(json); - deserialized.Should().Be(original); - } - - [Fact] - public void Json_RoundTrip_Float() - { - var original = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - var json = JsonSerializer.Serialize(original); - var deserialized = JsonSerializer.Deserialize>(json); - deserialized.Should().Be(original); - } - - [Fact] - public void Json_Identity_RoundTrip() - { - var original = Matrix3x3.Identity; - var json = JsonSerializer.Serialize(original); - json.Should().Be("[[1,0,0],[0,1,0],[0,0,1]]"); - var deserialized = JsonSerializer.Deserialize>(json); - deserialized.Should().Be(original); - } - - #endregion - - #region ToString - - [Fact] - public void ToString_Format() - { - var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); - m.ToString().Should().Be("[1, 2, 3; 4, 5, 6; 7, 8, 9]"); - } - - #endregion - - #region Helpers - - #region Quaternion ⇄ matrix bridge (epic-129 Q7: one Euler formula) - - private static Quaternion RandomRotation(Random rng) - { - // Uniform random unit quaternion (Shoemake) - double u1 = rng.NextDouble(), u2 = rng.NextDouble() * 2 * Math.PI, u3 = rng.NextDouble() * 2 * Math.PI; - double a = Math.Sqrt(1 - u1), b = Math.Sqrt(u1); - return new Quaternion(b * Math.Cos(u3), a * Math.Sin(u2), a * Math.Cos(u2), b * Math.Sin(u3)); - } - - [Fact] - public void QuaternionMatrix_RoundTrip_MatrixQuaternionMatrix_10000Random() - { - var rng = new Random(129); - for (int i = 0; i < 10_000; i++) - { - var m = RandomRotation(rng).ToMatrix3x3(); - var back = Quaternion.FromMatrix3x3(m).ToMatrix3x3(); - AssertMatricesEqual(back, m, 1e-12); - } - } - - [Fact] - public void QuaternionMatrix_FromMatrix3x3_AllShepperdBranches() - { - // trace > 0, then each diagonal element largest (rotations by π about X, Y, Z, and near-π mixes) - var cases = new[] - { - Quaternion.FromAxisAngle(new Vector3(1, 2, 3).Normalize(), 0.3), - Quaternion.FromAxisAngle(Vector3.EX, Math.PI), - Quaternion.FromAxisAngle(Vector3.EY, Math.PI), - Quaternion.FromAxisAngle(Vector3.EZ, Math.PI), - Quaternion.FromAxisAngle(new Vector3(1, 1, 0).Normalize(), Math.PI - 1e-9), - }; - foreach (var q in cases) - { - var m = q.ToMatrix3x3(); - var back = Quaternion.FromMatrix3x3(m); - Math.Abs(Quaternion.Dot(back, q)).Should().BeApproximately(1, 1e-12); - AssertMatricesEqual(back.ToMatrix3x3(), m, 1e-12); - } - } - - [Fact] - public void QuaternionMatrix_ToMatrix3x3_ColumnsAreRotatedAxes() - { - var rng = new Random(129); - for (int i = 0; i < 1_000; i++) - { - var q = RandomRotation(rng); - var expected = Matrix3x3.FromColumns(q.Rotate(Vector3.EX), q.Rotate(Vector3.EY), q.Rotate(Vector3.EZ)); - AssertMatricesEqual(q.ToMatrix3x3(), expected, 1e-12); - } - } - - // Master's (1.13) closed-form RotationZYX, kept here as the reference the bridge must not move from. - private static Matrix3x3 MasterRotationZYX(double rxDeg, double ryDeg, double rzDeg) - { - double rx = rxDeg * Math.PI / 180, ry = ryDeg * Math.PI / 180, rz = rzDeg * Math.PI / 180; - double cRx = Math.Cos(rx), sRx = Math.Sin(rx), cRy = Math.Cos(ry), sRy = Math.Sin(ry), cRz = Math.Cos(rz), sRz = Math.Sin(rz); - return new( - cRy * cRz, sRx * sRy * cRz - cRx * sRz, cRx * sRy * cRz + sRx * sRz, - cRy * sRz, sRx * sRy * sRz + cRx * cRz, cRx * sRy * sRz - sRx * cRz, - -sRy, sRx * cRy, cRx * cRy); - } - - [Fact] - public void RotationZYX_EqualsMasterClosedForm_10000Random() - { - var rng = new Random(129); - for (int i = 0; i < 10_000; i++) - { - double rx = rng.NextDouble() * 360 - 180, ry = rng.NextDouble() * 180 - 90, rz = rng.NextDouble() * 360 - 180; - var m = Matrix3x3.RotationZYX(Degree.Create(rx), Degree.Create(ry), Degree.Create(rz)); - AssertMatricesEqual(m, MasterRotationZYX(rx, ry, rz), 1e-12); - } - } - - [Fact] - public void ToEulerZYX_EqualsMasterAngles_OutsideGimbalBand_10000Random() - { - // Master extracted rx = atan2(M32, M33), ry = asin(-M31), rz = atan2(M21, M11) when |cos ry| > 1e-6. - var rng = new Random(129); - for (int i = 0; i < 10_000; i++) - { - double rx = rng.NextDouble() * 358 - 179, ry = rng.NextDouble() * 178 - 89, rz = rng.NextDouble() * 358 - 179; - var m = MasterRotationZYX(rx, ry, rz); - var (erx, ery, erz) = m.ToEulerZYX(); - ((double)erx).Should().BeApproximately(Math.Atan2(m.M32, m.M33) * 180 / Math.PI, 1e-9); - ((double)ery).Should().BeApproximately(Math.Asin(-m.M31) * 180 / Math.PI, 1e-9); - ((double)erz).Should().BeApproximately(Math.Atan2(m.M21, m.M11) * 180 / Math.PI, 1e-9); - } - } - - #endregion - - private static void AssertIsIdentity(Matrix3x3 m, double tol = Tol) - { - m.M11.Should().BeApproximately(1, tol); - m.M12.Should().BeApproximately(0, tol); - m.M13.Should().BeApproximately(0, tol); - m.M21.Should().BeApproximately(0, tol); - m.M22.Should().BeApproximately(1, tol); - m.M23.Should().BeApproximately(0, tol); - m.M31.Should().BeApproximately(0, tol); - m.M32.Should().BeApproximately(0, tol); - m.M33.Should().BeApproximately(1, tol); - } - - private static void AssertMatricesEqual(Matrix3x3 a, Matrix3x3 b, double tol = Tol) - { - a.M11.Should().BeApproximately(b.M11, tol); - a.M12.Should().BeApproximately(b.M12, tol); - a.M13.Should().BeApproximately(b.M13, tol); - a.M21.Should().BeApproximately(b.M21, tol); - a.M22.Should().BeApproximately(b.M22, tol); - a.M23.Should().BeApproximately(b.M23, tol); - a.M31.Should().BeApproximately(b.M31, tol); - a.M32.Should().BeApproximately(b.M32, tol); - a.M33.Should().BeApproximately(b.M33, tol); - } - - #endregion -} +using System.Text.Json; +using FluentAssertions; +using ModelingEvolution.Drawing; + +namespace ModelingEvolution.Drawing.Tests; + +public class Matrix3x3Tests +{ + private const double Tol = 1e-10; + private const double TolLoose = 1e-6; + + #region Identity & Properties + + [Fact] + public void Identity_HasCorrectValues() + { + var m = Matrix3x3.Identity; + m.M11.Should().Be(1d); m.M12.Should().Be(0d); m.M13.Should().Be(0d); + m.M21.Should().Be(0d); m.M22.Should().Be(1d); m.M23.Should().Be(0d); + m.M31.Should().Be(0d); m.M32.Should().Be(0d); m.M33.Should().Be(1d); + } + + [Fact] + public void Zero_HasCorrectValues() + { + var m = Matrix3x3.Zero; + m.M11.Should().Be(0d); m.M12.Should().Be(0d); m.M13.Should().Be(0d); + m.M21.Should().Be(0d); m.M22.Should().Be(0d); m.M23.Should().Be(0d); + m.M31.Should().Be(0d); m.M32.Should().Be(0d); m.M33.Should().Be(0d); + } + + [Fact] + public void Determinant_Identity_IsOne() + { + Matrix3x3.Identity.Determinant.Should().Be(1d); + } + + [Fact] + public void Determinant_KnownMatrix() + { + // |1 2 3; 0 1 4; 5 6 0| = 1*(0-24) - 2*(0-20) + 3*(0-5) = -24+40-15 = 1 + var m = new Matrix3x3(1, 2, 3, 0, 1, 4, 5, 6, 0); + m.Determinant.Should().BeApproximately(1d, Tol); + } + + [Fact] + public void Trace_KnownMatrix() + { + var m = new Matrix3x3(3, 0, 0, 0, 5, 0, 0, 0, 7); + m.Trace.Should().BeApproximately(15d, Tol); + } + + #endregion + + #region Transpose + + [Fact] + public void Transpose_General() + { + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var t = m.Transpose(); + t.M11.Should().Be(1d); t.M12.Should().Be(4d); t.M13.Should().Be(7d); + t.M21.Should().Be(2d); t.M22.Should().Be(5d); t.M23.Should().Be(8d); + t.M31.Should().Be(3d); t.M32.Should().Be(6d); t.M33.Should().Be(9d); + } + + [Fact] + public void Transpose_RotationMatrix_IsInverse() + { + var r = Matrix3x3.RotationX(Degree.Create(30)); + var product = r * r.Transpose(); + AssertIsIdentity(product); + } + + #endregion + + #region Inverse + + [Fact] + public void Inverse_Identity() + { + var inv = Matrix3x3.Identity.Inverse(); + AssertIsIdentity(inv); + } + + [Fact] + public void Inverse_KnownMatrix() + { + var m = new Matrix3x3(1, 2, 3, 0, 1, 4, 5, 6, 0); + var inv = m.Inverse(); + var product = m * inv; + AssertIsIdentity(product); + } + + [Fact] + public void Inverse_Singular_Throws() + { + // Third row = first row → det = 0 + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 1, 2, 3); + var act = () => m.Inverse(); + act.Should().Throw(); + } + + #endregion + + #region Multiplication + + [Fact] + public void Multiplication_IdentityIsNeutral() + { + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var r = m * Matrix3x3.Identity; + AssertMatricesEqual(r, m); + } + + [Fact] + public void Multiplication_KnownResult() + { + var a = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var b = new Matrix3x3(9, 8, 7, 6, 5, 4, 3, 2, 1); + var c = a * b; + // Row 0: (1*9+2*6+3*3, 1*8+2*5+3*2, 1*7+2*4+3*1) = (30, 24, 18) + c.M11.Should().BeApproximately(30, Tol); + c.M12.Should().BeApproximately(24, Tol); + c.M13.Should().BeApproximately(18, Tol); + // Row 1: (4*9+5*6+6*3, 4*8+5*5+6*2, 4*7+5*4+6*1) = (84, 69, 54) + c.M21.Should().BeApproximately(84, Tol); + c.M22.Should().BeApproximately(69, Tol); + c.M23.Should().BeApproximately(54, Tol); + // Row 2: (7*9+8*6+9*3, 7*8+8*5+9*2, 7*7+8*4+9*1) = (138, 114, 90) + c.M31.Should().BeApproximately(138, Tol); + c.M32.Should().BeApproximately(114, Tol); + c.M33.Should().BeApproximately(90, Tol); + } + + [Fact] + public void Multiplication_Associativity() + { + var a = Matrix3x3.RotationX(Degree.Create(30)); + var b = Matrix3x3.RotationY(Degree.Create(45)); + var c = Matrix3x3.RotationZ(Degree.Create(60)); + var ab_c = (a * b) * c; + var a_bc = a * (b * c); + AssertMatricesEqual(ab_c, a_bc); + } + + #endregion + + #region Transform + + [Fact] + public void Transform_Vector_IdentityPreserves() + { + var v = new Vector3(1, 2, 3); + var r = Matrix3x3.Identity.Transform(v); + r.X.Should().BeApproximately(1, Tol); + r.Y.Should().BeApproximately(2, Tol); + r.Z.Should().BeApproximately(3, Tol); + } + + [Fact] + public void Transform_Vector_RotationX90_RotatesBasisVectors() + { + var m = Matrix3x3.RotationX(Degree.Create(90)); + // Rotating Y axis by 90° around X → Z axis + var ey = new Vector3(0, 1, 0); + var result = m * ey; + result.X.Should().BeApproximately(0, TolLoose); + result.Y.Should().BeApproximately(0, TolLoose); + result.Z.Should().BeApproximately(1, TolLoose); + } + + [Fact] + public void Transform_Point_OperatorSyntax() + { + var m = Matrix3x3.Scale(2, 3, 4); + var p = new Point3(1, 2, 3); + var r = m * p; + r.X.Should().BeApproximately(2, Tol); + r.Y.Should().BeApproximately(6, Tol); + r.Z.Should().BeApproximately(12, Tol); + } + + #endregion + + #region Rotation Factories + + [Fact] + public void RotationX_90_RotatesYToZ() + { + var m = Matrix3x3.RotationX(Degree.Create(90)); + var v = new Vector3(0, 1, 0); + var r = m * v; + r.X.Should().BeApproximately(0, TolLoose); + r.Y.Should().BeApproximately(0, TolLoose); + r.Z.Should().BeApproximately(1, TolLoose); + } + + [Fact] + public void RotationY_90_RotatesZToX() + { + var m = Matrix3x3.RotationY(Degree.Create(90)); + var v = new Vector3(0, 0, 1); + var r = m * v; + r.X.Should().BeApproximately(1, TolLoose); + r.Y.Should().BeApproximately(0, TolLoose); + r.Z.Should().BeApproximately(0, TolLoose); + } + + [Fact] + public void RotationZ_90_RotatesXToY() + { + var m = Matrix3x3.RotationZ(Degree.Create(90)); + var v = new Vector3(1, 0, 0); + var r = m * v; + r.X.Should().BeApproximately(0, TolLoose); + r.Y.Should().BeApproximately(1, TolLoose); + r.Z.Should().BeApproximately(0, TolLoose); + } + + [Fact] + public void RotationZYX_MatchesComposedRotations() + { + var rx = Degree.Create(30d); + var ry = Degree.Create(45d); + var rz = Degree.Create(60d); + var composed = Matrix3x3.RotationZ(rz) * Matrix3x3.RotationY(ry) * Matrix3x3.RotationX(rx); + var direct = Matrix3x3.RotationZYX(rx, ry, rz); + AssertMatricesEqual(composed, direct); + } + + [Fact] + public void RotationMatrix_IsOrthogonal() + { + var m = Matrix3x3.RotationZYX(Degree.Create(25), Degree.Create(35), Degree.Create(45)); + var product = m * m.Transpose(); + AssertIsIdentity(product); + } + + [Fact] + public void RotationMatrix_Determinant_IsOne() + { + var m = Matrix3x3.RotationZYX(Degree.Create(10), Degree.Create(20), Degree.Create(30)); + m.Determinant.Should().BeApproximately(1d, TolLoose); + } + + #endregion + + #region Scale & Construction Factories + + [Fact] + public void Scale_ScalesVector() + { + var m = Matrix3x3.Scale(2, 3, 4); + var v = new Vector3(1, 1, 1); + var r = m * v; + r.X.Should().BeApproximately(2, Tol); + r.Y.Should().BeApproximately(3, Tol); + r.Z.Should().BeApproximately(4, Tol); + } + + [Fact] + public void FromColumns_BuildsCorrectMatrix() + { + var c0 = new Vector3(1, 4, 7); + var c1 = new Vector3(2, 5, 8); + var c2 = new Vector3(3, 6, 9); + var m = Matrix3x3.FromColumns(c0, c1, c2); + m.M11.Should().Be(1); m.M12.Should().Be(2); m.M13.Should().Be(3); + m.M21.Should().Be(4); m.M22.Should().Be(5); m.M23.Should().Be(6); + m.M31.Should().Be(7); m.M32.Should().Be(8); m.M33.Should().Be(9); + } + + [Fact] + public void FromRows_BuildsCorrectMatrix() + { + var r0 = new Vector3(1, 2, 3); + var r1 = new Vector3(4, 5, 6); + var r2 = new Vector3(7, 8, 9); + var m = Matrix3x3.FromRows(r0, r1, r2); + m.M11.Should().Be(1); m.M12.Should().Be(2); m.M13.Should().Be(3); + m.M21.Should().Be(4); m.M22.Should().Be(5); m.M23.Should().Be(6); + m.M31.Should().Be(7); m.M32.Should().Be(8); m.M33.Should().Be(9); + } + + #endregion + + #region Euler Round-trip + + [Theory] + [InlineData(10, 20, 30)] + [InlineData(-15, 45, -60)] + [InlineData(0, 0, 90)] + [InlineData(180, 0, 0)] + [InlineData(5, 10, 15)] + public void RotationZYX_ToEulerZYX_RoundTrip(double rxd, double ryd, double rzd) + { + Degree rx = Degree.Create(rxd), ry = Degree.Create(ryd), rz = Degree.Create(rzd); + var m = Matrix3x3.RotationZYX(rx, ry, rz); + var (erx, ery, erz) = m.ToEulerZYX(); + // Rebuild matrix from extracted angles and compare + var m2 = Matrix3x3.RotationZYX(erx, ery, erz); + AssertMatricesEqual(m, m2); + } + + [Fact] + public void ToEulerZYX_GimbalLock_AtPositive90() + { + var m = Matrix3x3.RotationZYX(Degree.Create(0), Degree.Create(90), Degree.Create(0)); + var (rx, ry, rz) = m.ToEulerZYX(); + // ry should be ~90° + ((double)ry).Should().BeApproximately(90, TolLoose); + // At gimbal lock, rx and rz share DOF; matrix should still reconstruct + var m2 = Matrix3x3.RotationZYX(rx, ry, rz); + AssertMatricesEqual(m, m2, 1e-5); + } + + #endregion + + #region Quaternion Round-trip + + [Theory] + [InlineData(10, 20, 30)] + [InlineData(-30, 45, 60)] + [InlineData(0, 0, 0)] + [InlineData(5, -10, 15)] + public void ToQuaternion_FromQuaternion_RoundTrip(double rxd, double ryd, double rzd) + { + var m = Matrix3x3.RotationZYX(Degree.Create(rxd), Degree.Create(ryd), Degree.Create(rzd)); + var q = Quaternion.FromMatrix3x3(m); + // Convert quaternion back to rotation, then to matrix + var rot = Rotation3.FromQuaternion(q); + var m2 = rot.ToQuaternion().ToMatrix3x3(); + AssertMatricesEqual(m, m2, 1e-9); + } + + [Fact] + public void ToQuaternion_Identity_IsUnitQuaternion() + { + var q = Quaternion.FromMatrix3x3(Matrix3x3.Identity); + q.W.Should().BeApproximately(1, TolLoose); + q.X.Should().BeApproximately(0, TolLoose); + q.Y.Should().BeApproximately(0, TolLoose); + q.Z.Should().BeApproximately(0, TolLoose); + } + + #endregion + + #region Rotation3 Integration + + [Theory] + [InlineData(10, 20, 30)] + [InlineData(-15, 45, -60)] + [InlineData(0, 0, 0)] + public void Rotation3_QuaternionMatrix_RoundTrip(double rxd, double ryd, double rzd) + { + var rot = new Rotation3(rxd, ryd, rzd); + var m = rot.ToQuaternion().ToMatrix3x3(); + var rot2 = Rotation3.FromMatrix3x3(m); + ((double)rot2.Rx).Should().BeApproximately(rxd, TolLoose); + ((double)rot2.Ry).Should().BeApproximately(ryd, TolLoose); + ((double)rot2.Rz).Should().BeApproximately(rzd, TolLoose); + } + + [Fact] + public void Rotation3_QuaternionRotate_MatchesMatrix_Transform() + { + var rot = new Rotation3(30, 45, 60); + var m = Matrix3x3.RotationZYX(rot.Rx, rot.Ry, rot.Rz); + var v = new Vector3(1, 2, 3); + + var rotated = rot.ToQuaternion().Rotate(v); + var transformed = m.Transform(v); + + rotated.X.Should().BeApproximately(transformed.X, TolLoose); + rotated.Y.Should().BeApproximately(transformed.Y, TolLoose); + rotated.Z.Should().BeApproximately(transformed.Z, TolLoose); + } + + #endregion + + #region Scalar & Element-wise + + [Fact] + public void ScalarMultiplication() + { + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var r = m * 2d; + r.M11.Should().Be(2); r.M12.Should().Be(4); r.M13.Should().Be(6); + r.M21.Should().Be(8); r.M22.Should().Be(10); r.M23.Should().Be(12); + r.M31.Should().Be(14); r.M32.Should().Be(16); r.M33.Should().Be(18); + } + + [Fact] + public void ScalarMultiplication_Commutative() + { + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var r1 = m * 3d; + var r2 = 3d * m; + r1.Should().Be(r2); + } + + [Fact] + public void Addition() + { + var a = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var b = new Matrix3x3(9, 8, 7, 6, 5, 4, 3, 2, 1); + var c = a + b; + c.M11.Should().Be(10); c.M12.Should().Be(10); c.M13.Should().Be(10); + c.M21.Should().Be(10); c.M22.Should().Be(10); c.M23.Should().Be(10); + c.M31.Should().Be(10); c.M32.Should().Be(10); c.M33.Should().Be(10); + } + + [Fact] + public void Subtraction() + { + var a = new Matrix3x3(9, 8, 7, 6, 5, 4, 3, 2, 1); + var b = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var c = a - b; + c.M11.Should().Be(8); c.M12.Should().Be(6); c.M13.Should().Be(4); + c.M21.Should().Be(2); c.M22.Should().Be(0); c.M23.Should().Be(-2); + c.M31.Should().Be(-4); c.M32.Should().Be(-6); c.M33.Should().Be(-8); + } + + #endregion + + #region JSON Serialization + + [Fact] + public void Json_Serialize_Double() + { + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var json = JsonSerializer.Serialize(m); + json.Should().Be("[[1,2,3],[4,5,6],[7,8,9]]"); + } + + [Fact] + public void Json_Deserialize_Double() + { + var m = JsonSerializer.Deserialize>("[[1.5,2.5,3.5],[4.5,5.5,6.5],[7.5,8.5,9.5]]"); + m.M11.Should().Be(1.5); m.M12.Should().Be(2.5); m.M13.Should().Be(3.5); + m.M21.Should().Be(4.5); m.M22.Should().Be(5.5); m.M23.Should().Be(6.5); + m.M31.Should().Be(7.5); m.M32.Should().Be(8.5); m.M33.Should().Be(9.5); + } + + [Fact] + public void Json_RoundTrip_Double() + { + var original = new Matrix3x3(1.23, 4.56, 7.89, 0.12, 3.45, 6.78, 9.01, 2.34, 5.67); + var json = JsonSerializer.Serialize(original); + var deserialized = JsonSerializer.Deserialize>(json); + deserialized.Should().Be(original); + } + + [Fact] + public void Json_RoundTrip_Float() + { + var original = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + var json = JsonSerializer.Serialize(original); + var deserialized = JsonSerializer.Deserialize>(json); + deserialized.Should().Be(original); + } + + [Fact] + public void Json_Identity_RoundTrip() + { + var original = Matrix3x3.Identity; + var json = JsonSerializer.Serialize(original); + json.Should().Be("[[1,0,0],[0,1,0],[0,0,1]]"); + var deserialized = JsonSerializer.Deserialize>(json); + deserialized.Should().Be(original); + } + + #endregion + + #region ToString + + [Fact] + public void ToString_Format() + { + var m = new Matrix3x3(1, 2, 3, 4, 5, 6, 7, 8, 9); + m.ToString().Should().Be("[1, 2, 3; 4, 5, 6; 7, 8, 9]"); + } + + #endregion + + #region Helpers + + #region Quaternion ⇄ matrix bridge (epic-129 Q7: one Euler formula) + + private static Quaternion RandomRotation(Random rng) + { + // Uniform random unit quaternion (Shoemake) + double u1 = rng.NextDouble(), u2 = rng.NextDouble() * 2 * Math.PI, u3 = rng.NextDouble() * 2 * Math.PI; + double a = Math.Sqrt(1 - u1), b = Math.Sqrt(u1); + return new Quaternion(b * Math.Cos(u3), a * Math.Sin(u2), a * Math.Cos(u2), b * Math.Sin(u3)); + } + + [Fact] + public void QuaternionMatrix_RoundTrip_MatrixQuaternionMatrix_10000Random() + { + var rng = new Random(129); + for (int i = 0; i < 10_000; i++) + { + var m = RandomRotation(rng).ToMatrix3x3(); + var back = Quaternion.FromMatrix3x3(m).ToMatrix3x3(); + AssertMatricesEqual(back, m, 1e-12); + } + } + + [Fact] + public void QuaternionMatrix_FromMatrix3x3_AllShepperdBranches() + { + // trace > 0, then each diagonal element largest (rotations by π about X, Y, Z, and near-π mixes) + var cases = new[] + { + Quaternion.FromAxisAngle(new Vector3(1, 2, 3).Normalize(), 0.3), + Quaternion.FromAxisAngle(Vector3.EX, Math.PI), + Quaternion.FromAxisAngle(Vector3.EY, Math.PI), + Quaternion.FromAxisAngle(Vector3.EZ, Math.PI), + Quaternion.FromAxisAngle(new Vector3(1, 1, 0).Normalize(), Math.PI - 1e-9), + }; + foreach (var q in cases) + { + var m = q.ToMatrix3x3(); + var back = Quaternion.FromMatrix3x3(m); + Math.Abs(Quaternion.Dot(back, q)).Should().BeApproximately(1, 1e-12); + AssertMatricesEqual(back.ToMatrix3x3(), m, 1e-12); + } + } + + [Fact] + public void QuaternionMatrix_ToMatrix3x3_ColumnsAreRotatedAxes() + { + var rng = new Random(129); + for (int i = 0; i < 1_000; i++) + { + var q = RandomRotation(rng); + var expected = Matrix3x3.FromColumns(q.Rotate(Vector3.EX), q.Rotate(Vector3.EY), q.Rotate(Vector3.EZ)); + AssertMatricesEqual(q.ToMatrix3x3(), expected, 1e-12); + } + } + + // Master's (1.13) closed-form RotationZYX, kept here as the reference the bridge must not move from. + private static Matrix3x3 MasterRotationZYX(double rxDeg, double ryDeg, double rzDeg) + { + double rx = rxDeg * Math.PI / 180, ry = ryDeg * Math.PI / 180, rz = rzDeg * Math.PI / 180; + double cRx = Math.Cos(rx), sRx = Math.Sin(rx), cRy = Math.Cos(ry), sRy = Math.Sin(ry), cRz = Math.Cos(rz), sRz = Math.Sin(rz); + return new( + cRy * cRz, sRx * sRy * cRz - cRx * sRz, cRx * sRy * cRz + sRx * sRz, + cRy * sRz, sRx * sRy * sRz + cRx * cRz, cRx * sRy * sRz - sRx * cRz, + -sRy, sRx * cRy, cRx * cRy); + } + + [Fact] + public void RotationZYX_EqualsMasterClosedForm_10000Random() + { + var rng = new Random(129); + for (int i = 0; i < 10_000; i++) + { + double rx = rng.NextDouble() * 360 - 180, ry = rng.NextDouble() * 180 - 90, rz = rng.NextDouble() * 360 - 180; + var m = Matrix3x3.RotationZYX(Degree.Create(rx), Degree.Create(ry), Degree.Create(rz)); + AssertMatricesEqual(m, MasterRotationZYX(rx, ry, rz), 1e-12); + } + } + + [Fact] + public void ToEulerZYX_EqualsMasterAngles_OutsideGimbalBand_10000Random() + { + // Master extracted rx = atan2(M32, M33), ry = asin(-M31), rz = atan2(M21, M11) when |cos ry| > 1e-6. + var rng = new Random(129); + for (int i = 0; i < 10_000; i++) + { + double rx = rng.NextDouble() * 358 - 179, ry = rng.NextDouble() * 178 - 89, rz = rng.NextDouble() * 358 - 179; + var m = MasterRotationZYX(rx, ry, rz); + var (erx, ery, erz) = m.ToEulerZYX(); + ((double)erx).Should().BeApproximately(Math.Atan2(m.M32, m.M33) * 180 / Math.PI, 1e-9); + ((double)ery).Should().BeApproximately(Math.Asin(-m.M31) * 180 / Math.PI, 1e-9); + ((double)erz).Should().BeApproximately(Math.Atan2(m.M21, m.M11) * 180 / Math.PI, 1e-9); + } + } + + #endregion + + private static void AssertIsIdentity(Matrix3x3 m, double tol = Tol) + { + m.M11.Should().BeApproximately(1, tol); + m.M12.Should().BeApproximately(0, tol); + m.M13.Should().BeApproximately(0, tol); + m.M21.Should().BeApproximately(0, tol); + m.M22.Should().BeApproximately(1, tol); + m.M23.Should().BeApproximately(0, tol); + m.M31.Should().BeApproximately(0, tol); + m.M32.Should().BeApproximately(0, tol); + m.M33.Should().BeApproximately(1, tol); + } + + private static void AssertMatricesEqual(Matrix3x3 a, Matrix3x3 b, double tol = Tol) + { + a.M11.Should().BeApproximately(b.M11, tol); + a.M12.Should().BeApproximately(b.M12, tol); + a.M13.Should().BeApproximately(b.M13, tol); + a.M21.Should().BeApproximately(b.M21, tol); + a.M22.Should().BeApproximately(b.M22, tol); + a.M23.Should().BeApproximately(b.M23, tol); + a.M31.Should().BeApproximately(b.M31, tol); + a.M32.Should().BeApproximately(b.M32, tol); + a.M33.Should().BeApproximately(b.M33, tol); + } + + #endregion +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Polyline3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Polyline3Tests.cs index cbff910..5367a66 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Polyline3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Polyline3Tests.cs @@ -1,474 +1,474 @@ -using FluentAssertions; -using ModelingEvolution.Drawing; - -namespace ModelingEvolution.Drawing.Tests; - -public class Polyline3Tests -{ - private const float Tol = 1e-4f; - - // ───────────────────────────────────────────── - // Construction - // ───────────────────────────────────────────── - - [Fact] - public void DefaultCtor_Empty() - { - var pl = new Polyline3(); - pl.Count.Should().Be(0); - pl.AsSpan().Length.Should().Be(0); - } - - [Fact] - public void ParamsCtor_StoresPoints() - { - var p1 = new Point3(0, 0, 0); - var p2 = new Point3(1, 0, 0); - var p3 = new Point3(1, 1, 0); - var pl = new Polyline3(p1, p2, p3); - pl.Count.Should().Be(3); - pl[0].Should().Be(p1); - pl[1].Should().Be(p2); - pl[2].Should().Be(p3); - } - - [Fact] - public void ListCtor_CopiesPoints() - { - var list = new List> - { - new(0, 0, 0), new(5, 5, 5), new(10, 0, 0) - }; - var pl = new Polyline3(list); - pl.Count.Should().Be(3); - pl[2].Should().Be(new Point3(10, 0, 0)); - } - - [Fact] - public void CoordsCtor_ParsesTriplets() - { - var coords = new float[] { 0, 0, 0, 10, 0, 0, 10, 10, 0 }; - var pl = new Polyline3(coords); - pl.Count.Should().Be(3); - pl[1].Should().Be(new Point3(10, 0, 0)); - } - - [Fact] - public void MemoryCtor_ZeroCopy() - { - var arr = new Point3[] { new(1, 2, 3), new(4, 5, 6), new(7, 8, 9) }; - var mem = new ReadOnlyMemory>(arr); - var pl = new Polyline3(mem); - pl.Count.Should().Be(3); - pl[0].Should().Be(new Point3(1, 2, 3)); - } - - // ───────────────────────────────────────────── - // Start / End - // ───────────────────────────────────────────── - - [Fact] - public void Start_ReturnsFirstPoint() - { - var pl = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); - pl.Start.Should().Be(new Point3(1, 2, 3)); - } - - [Fact] - public void End_ReturnsLastPoint() - { - var pl = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); - pl.End.Should().Be(new Point3(4, 5, 6)); - } - - // ───────────────────────────────────────────── - // Length - // ───────────────────────────────────────────── - - [Fact] - public void Length_StraightLine() - { - var pl = new Polyline3(new Point3(0, 0, 0), new Point3(10, 0, 0)); - pl.Length().Should().BeApproximately(10f, Tol); - } - - [Fact] - public void Length_MultiSegment() - { - // L-shaped in 3D: (0,0,0)->(10,0,0)->(10,5,0) - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0), - new Point3(10, 5, 0)); - pl.Length().Should().BeApproximately(15f, Tol); - } - - [Fact] - public void Length_Diagonal3D() - { - // Diagonal from origin to (1,1,1), length = sqrt(3) - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(1, 1, 1)); - pl.Length().Should().BeApproximately(MathF.Sqrt(3f), Tol); - } - - [Fact] - public void Length_SinglePoint_ReturnsZero() - { - var pl = new Polyline3(new Point3(5, 5, 5)); - pl.Length().Should().Be(0f); - } - - // ───────────────────────────────────────────── - // Centroid - // ───────────────────────────────────────────── - - [Fact] - public void Centroid_SinglePoint_ReturnsThatPoint() - { - var pl = new Polyline3(new Point3(3, 4, 5)); - var c = pl.Centroid(); - c.X.Should().BeApproximately(3f, Tol); - c.Y.Should().BeApproximately(4f, Tol); - c.Z.Should().BeApproximately(5f, Tol); - } - - [Fact] - public void Centroid_TwoPoints_IsMidpoint() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0)); - var c = pl.Centroid(); - c.X.Should().BeApproximately(5f, Tol); - c.Y.Should().BeApproximately(0f, Tol); - c.Z.Should().BeApproximately(0f, Tol); - } - - [Fact] - public void Centroid_EqualSegments_IsGeometricCenter() - { - // Three equal-length segments along X: 0->10->20->30 - // Segment midpoints: 5, 15, 25 — all with equal length - // Centroid X = (5+15+25)/3 = 15 - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0), - new Point3(20, 0, 0), - new Point3(30, 0, 0)); - var c = pl.Centroid(); - c.X.Should().BeApproximately(15f, Tol); - c.Y.Should().BeApproximately(0f, Tol); - c.Z.Should().BeApproximately(0f, Tol); - } - - [Fact] - public void Centroid_UnequalSegments_WeightedByLength() - { - // Segment 1: (0,0,0)->(10,0,0) length=10 midpoint=(5,0,0) - // Segment 2: (10,0,0)->(11,0,0) length=1 midpoint=(10.5,0,0) - // Centroid X = (5*10 + 10.5*1) / 11 = 60.5/11 ≈ 5.5 - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0), - new Point3(11, 0, 0)); - var c = pl.Centroid(); - c.X.Should().BeApproximately(60.5f / 11f, Tol); - } - - [Fact] - public void Centroid_3D_AllAxes() - { - // Segment along Z axis: (0,0,0)->(0,0,10) - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(0, 0, 10)); - var c = pl.Centroid(); - c.X.Should().BeApproximately(0f, Tol); - c.Y.Should().BeApproximately(0f, Tol); - c.Z.Should().BeApproximately(5f, Tol); - } - - // ───────────────────────────────────────────── - // Reverse - // ───────────────────────────────────────────── - - [Fact] - public void Reverse_FlipsOrder() - { - var p1 = new Point3(0, 0, 0); - var p2 = new Point3(1, 0, 0); - var p3 = new Point3(2, 0, 0); - var pl = new Polyline3(p1, p2, p3).Reverse(); - pl[0].Should().Be(p3); - pl[1].Should().Be(p2); - pl[2].Should().Be(p1); - } - - // ───────────────────────────────────────────── - // Edges (open — n-1 segments, no wrap-around) - // ───────────────────────────────────────────── - - [Fact] - public void Edges_ReturnsNMinus1Segments() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0), - new Point3(10, 10, 0)); - var edges = pl.Edges(); - edges.Length.Should().Be(2); - var edgesSpan = edges.Span; - edgesSpan[0].Start.Should().Be(new Point3(0, 0, 0)); - edgesSpan[0].End.Should().Be(new Point3(10, 0, 0)); - edgesSpan[1].Start.Should().Be(new Point3(10, 0, 0)); - edgesSpan[1].End.Should().Be(new Point3(10, 10, 0)); - } - - [Fact] - public void Edges_SinglePoint_Empty() - { - var pl = new Polyline3(new Point3(5, 5, 5)); - pl.Edges().Span.ToArray().Should().BeEmpty(); - } - - // ───────────────────────────────────────────── - // Operators - // ───────────────────────────────────────────── - - [Fact] - public void PlusVector_TranslatesAll() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0)); - var moved = pl + new Vector3(5, 3, 1); - moved[0].Should().Be(new Point3(5, 3, 1)); - moved[1].Should().Be(new Point3(15, 3, 1)); - } - - [Fact] - public void MinusVector_TranslatesAll() - { - var pl = new Polyline3( - new Point3(5, 3, 1), - new Point3(15, 3, 1)); - var moved = pl - new Vector3(5, 3, 1); - moved[0].X.Should().BeApproximately(0f, Tol); - moved[0].Y.Should().BeApproximately(0f, Tol); - moved[0].Z.Should().BeApproximately(0f, Tol); - } - - [Fact] - public void MultiplyScalar_ScalesAll() - { - var pl = new Polyline3( - new Point3(1, 2, 3), - new Point3(4, 5, 6)); - var scaled = pl * 2f; - scaled[0].Should().Be(new Point3(2, 4, 6)); - scaled[1].Should().Be(new Point3(8, 10, 12)); - } - - [Fact] - public void DivideScalar_DividesAll() - { - var pl = new Polyline3( - new Point3(2, 4, 6), - new Point3(8, 10, 12)); - var scaled = pl / 2f; - scaled[0].Should().Be(new Point3(1, 2, 3)); - scaled[1].Should().Be(new Point3(4, 5, 6)); - } - - // ───────────────────────────────────────────── - // Equality - // ───────────────────────────────────────────── - - [Fact] - public void Equality_SamePoints_AreEqual() - { - var a = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); - var b = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); - a.Should().Be(b); - a.GetHashCode().Should().Be(b.GetHashCode()); - } - - [Fact] - public void Equality_DifferentPoints_AreNotEqual() - { - var a = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); - var b = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 7)); - a.Should().NotBe(b); - } - - // ───────────────────────────────────────────── - // Simplify (Ramer-Douglas-Peucker 3D) - // ───────────────────────────────────────────── - - [Fact] - public void Simplify_StraightLine_RemovesCollinearPoints() - { - // 5 collinear points along X → should reduce to 2 endpoints - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(2, 0, 0), - new Point3(5, 0, 0), - new Point3(7, 0, 0), - new Point3(10, 0, 0)); - var simplified = pl.Simplify(0.1f); - simplified.Count.Should().Be(2); - simplified[0].Should().Be(new Point3(0, 0, 0)); - simplified[1].Should().Be(new Point3(10, 0, 0)); - } - - [Fact] - public void Simplify_CollinearAlongDiagonal3D() - { - // Points along (1,1,1) direction — all collinear - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(1, 1, 1), - new Point3(2, 2, 2), - new Point3(3, 3, 3)); - var simplified = pl.Simplify(0.1f); - simplified.Count.Should().Be(2); - } - - [Fact] - public void Simplify_LShape_KeepsCorner() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0), - new Point3(10, 10, 0)); - var simplified = pl.Simplify(1f); - simplified.Count.Should().Be(3); - } - - [Fact] - public void Simplify_3DDeviation() - { - // Point deviates in Z only - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(5, 0, 2f), // 2 units off the line in Z - new Point3(10, 0, 0)); - pl.Simplify(1f).Count.Should().Be(3); // 2 > 1 → kept - pl.Simplify(3f).Count.Should().Be(2); // 2 < 3 → removed - } - - [Fact] - public void Simplify_TwoPoints_ReturnsSelf() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0)); - var simplified = pl.Simplify(1f); - simplified.Count.Should().Be(2); - } - - // ───────────────────────────────────────────── - // Scale - // ───────────────────────────────────────────── - - [Fact] - public void Scale_AroundCentroid_PreservesCentroid() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0)); - var centroid = pl.Centroid(); - var scaled = pl.Scale(2f); - var scaledCentroid = scaled.Centroid(); - scaledCentroid.X.Should().BeApproximately(centroid.X, Tol); - scaledCentroid.Y.Should().BeApproximately(centroid.Y, Tol); - scaledCentroid.Z.Should().BeApproximately(centroid.Z, Tol); - } - - [Fact] - public void Scale_DoublesLength() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(10, 0, 0)); - var scaled = pl.Scale(2f); - scaled.Length().Should().BeApproximately(20f, Tol); - } - - // ───────────────────────────────────────────── - // Points list - // ───────────────────────────────────────────── - - [Fact] - public void Points_ReturnsReadOnlyList() - { - var pl = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); - pl.Points.Count.Should().Be(2); - pl.Points[0].Should().Be(new Point3(1, 2, 3)); - } - - // ───────────────────────────────────────────── - // ToString - // ───────────────────────────────────────────── - - [Fact] - public void ToString_ShowsCount() - { - var pl = new Polyline3( - new Point3(0, 0, 0), - new Point3(1, 1, 1)); - pl.ToString().Should().Be("Polyline3(2 points)"); - } - - // ───────────────────────────────────────────── - // JSON round-trip - // ───────────────────────────────────────────── - - [Fact] - public void Json_RoundTrip_Float() - { - var pl = new Polyline3( - new Point3(1, 2, 3), - new Point3(4, 5, 6)); - var json = System.Text.Json.JsonSerializer.Serialize(pl); - json.Should().Be("[1,2,3,4,5,6]"); - var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); - deserialized.Should().Be(pl); - } - - [Fact] - public void Json_RoundTrip_Double() - { - var pl = new Polyline3( - new Point3(1.5, 2.5, 3.5), - new Point3(4.5, 5.5, 6.5)); - var json = System.Text.Json.JsonSerializer.Serialize(pl); - var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); - deserialized.Should().Be(pl); - } - - [Fact] - public void Json_Empty() - { - var pl = new Polyline3(); - var json = System.Text.Json.JsonSerializer.Serialize(pl); - json.Should().Be("[]"); - var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); - deserialized.Count.Should().Be(0); - } - - [Fact] - public void Transform_Frame_RotatesThenTranslatesEveryPoint() - { - var pl = new Polyline3(new Point3(1, 0, 0), new Point3(0, 2, 0)); - var frame = new Frame3(new Point3(10, 0, 0), Quaternion.FromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2)); - - var r = pl.Transform(frame); - - r.Count.Should().Be(2); - r[0].X.Should().BeApproximately(10, Tol); r[0].Y.Should().BeApproximately(1, Tol); r[0].Z.Should().BeApproximately(0, Tol); - r[1].X.Should().BeApproximately(8, Tol); r[1].Y.Should().BeApproximately(0, Tol); r[1].Z.Should().BeApproximately(0, Tol); - } -} +using FluentAssertions; +using ModelingEvolution.Drawing; + +namespace ModelingEvolution.Drawing.Tests; + +public class Polyline3Tests +{ + private const float Tol = 1e-4f; + + // ───────────────────────────────────────────── + // Construction + // ───────────────────────────────────────────── + + [Fact] + public void DefaultCtor_Empty() + { + var pl = new Polyline3(); + pl.Count.Should().Be(0); + pl.AsSpan().Length.Should().Be(0); + } + + [Fact] + public void ParamsCtor_StoresPoints() + { + var p1 = new Point3(0, 0, 0); + var p2 = new Point3(1, 0, 0); + var p3 = new Point3(1, 1, 0); + var pl = new Polyline3(p1, p2, p3); + pl.Count.Should().Be(3); + pl[0].Should().Be(p1); + pl[1].Should().Be(p2); + pl[2].Should().Be(p3); + } + + [Fact] + public void ListCtor_CopiesPoints() + { + var list = new List> + { + new(0, 0, 0), new(5, 5, 5), new(10, 0, 0) + }; + var pl = new Polyline3(list); + pl.Count.Should().Be(3); + pl[2].Should().Be(new Point3(10, 0, 0)); + } + + [Fact] + public void CoordsCtor_ParsesTriplets() + { + var coords = new float[] { 0, 0, 0, 10, 0, 0, 10, 10, 0 }; + var pl = new Polyline3(coords); + pl.Count.Should().Be(3); + pl[1].Should().Be(new Point3(10, 0, 0)); + } + + [Fact] + public void MemoryCtor_ZeroCopy() + { + var arr = new Point3[] { new(1, 2, 3), new(4, 5, 6), new(7, 8, 9) }; + var mem = new ReadOnlyMemory>(arr); + var pl = new Polyline3(mem); + pl.Count.Should().Be(3); + pl[0].Should().Be(new Point3(1, 2, 3)); + } + + // ───────────────────────────────────────────── + // Start / End + // ───────────────────────────────────────────── + + [Fact] + public void Start_ReturnsFirstPoint() + { + var pl = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); + pl.Start.Should().Be(new Point3(1, 2, 3)); + } + + [Fact] + public void End_ReturnsLastPoint() + { + var pl = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); + pl.End.Should().Be(new Point3(4, 5, 6)); + } + + // ───────────────────────────────────────────── + // Length + // ───────────────────────────────────────────── + + [Fact] + public void Length_StraightLine() + { + var pl = new Polyline3(new Point3(0, 0, 0), new Point3(10, 0, 0)); + pl.Length().Should().BeApproximately(10f, Tol); + } + + [Fact] + public void Length_MultiSegment() + { + // L-shaped in 3D: (0,0,0)->(10,0,0)->(10,5,0) + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0), + new Point3(10, 5, 0)); + pl.Length().Should().BeApproximately(15f, Tol); + } + + [Fact] + public void Length_Diagonal3D() + { + // Diagonal from origin to (1,1,1), length = sqrt(3) + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(1, 1, 1)); + pl.Length().Should().BeApproximately(MathF.Sqrt(3f), Tol); + } + + [Fact] + public void Length_SinglePoint_ReturnsZero() + { + var pl = new Polyline3(new Point3(5, 5, 5)); + pl.Length().Should().Be(0f); + } + + // ───────────────────────────────────────────── + // Centroid + // ───────────────────────────────────────────── + + [Fact] + public void Centroid_SinglePoint_ReturnsThatPoint() + { + var pl = new Polyline3(new Point3(3, 4, 5)); + var c = pl.Centroid(); + c.X.Should().BeApproximately(3f, Tol); + c.Y.Should().BeApproximately(4f, Tol); + c.Z.Should().BeApproximately(5f, Tol); + } + + [Fact] + public void Centroid_TwoPoints_IsMidpoint() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0)); + var c = pl.Centroid(); + c.X.Should().BeApproximately(5f, Tol); + c.Y.Should().BeApproximately(0f, Tol); + c.Z.Should().BeApproximately(0f, Tol); + } + + [Fact] + public void Centroid_EqualSegments_IsGeometricCenter() + { + // Three equal-length segments along X: 0->10->20->30 + // Segment midpoints: 5, 15, 25 — all with equal length + // Centroid X = (5+15+25)/3 = 15 + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0), + new Point3(20, 0, 0), + new Point3(30, 0, 0)); + var c = pl.Centroid(); + c.X.Should().BeApproximately(15f, Tol); + c.Y.Should().BeApproximately(0f, Tol); + c.Z.Should().BeApproximately(0f, Tol); + } + + [Fact] + public void Centroid_UnequalSegments_WeightedByLength() + { + // Segment 1: (0,0,0)->(10,0,0) length=10 midpoint=(5,0,0) + // Segment 2: (10,0,0)->(11,0,0) length=1 midpoint=(10.5,0,0) + // Centroid X = (5*10 + 10.5*1) / 11 = 60.5/11 ≈ 5.5 + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0), + new Point3(11, 0, 0)); + var c = pl.Centroid(); + c.X.Should().BeApproximately(60.5f / 11f, Tol); + } + + [Fact] + public void Centroid_3D_AllAxes() + { + // Segment along Z axis: (0,0,0)->(0,0,10) + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(0, 0, 10)); + var c = pl.Centroid(); + c.X.Should().BeApproximately(0f, Tol); + c.Y.Should().BeApproximately(0f, Tol); + c.Z.Should().BeApproximately(5f, Tol); + } + + // ───────────────────────────────────────────── + // Reverse + // ───────────────────────────────────────────── + + [Fact] + public void Reverse_FlipsOrder() + { + var p1 = new Point3(0, 0, 0); + var p2 = new Point3(1, 0, 0); + var p3 = new Point3(2, 0, 0); + var pl = new Polyline3(p1, p2, p3).Reverse(); + pl[0].Should().Be(p3); + pl[1].Should().Be(p2); + pl[2].Should().Be(p1); + } + + // ───────────────────────────────────────────── + // Edges (open — n-1 segments, no wrap-around) + // ───────────────────────────────────────────── + + [Fact] + public void Edges_ReturnsNMinus1Segments() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0), + new Point3(10, 10, 0)); + var edges = pl.Edges(); + edges.Length.Should().Be(2); + var edgesSpan = edges.Span; + edgesSpan[0].Start.Should().Be(new Point3(0, 0, 0)); + edgesSpan[0].End.Should().Be(new Point3(10, 0, 0)); + edgesSpan[1].Start.Should().Be(new Point3(10, 0, 0)); + edgesSpan[1].End.Should().Be(new Point3(10, 10, 0)); + } + + [Fact] + public void Edges_SinglePoint_Empty() + { + var pl = new Polyline3(new Point3(5, 5, 5)); + pl.Edges().Span.ToArray().Should().BeEmpty(); + } + + // ───────────────────────────────────────────── + // Operators + // ───────────────────────────────────────────── + + [Fact] + public void PlusVector_TranslatesAll() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0)); + var moved = pl + new Vector3(5, 3, 1); + moved[0].Should().Be(new Point3(5, 3, 1)); + moved[1].Should().Be(new Point3(15, 3, 1)); + } + + [Fact] + public void MinusVector_TranslatesAll() + { + var pl = new Polyline3( + new Point3(5, 3, 1), + new Point3(15, 3, 1)); + var moved = pl - new Vector3(5, 3, 1); + moved[0].X.Should().BeApproximately(0f, Tol); + moved[0].Y.Should().BeApproximately(0f, Tol); + moved[0].Z.Should().BeApproximately(0f, Tol); + } + + [Fact] + public void MultiplyScalar_ScalesAll() + { + var pl = new Polyline3( + new Point3(1, 2, 3), + new Point3(4, 5, 6)); + var scaled = pl * 2f; + scaled[0].Should().Be(new Point3(2, 4, 6)); + scaled[1].Should().Be(new Point3(8, 10, 12)); + } + + [Fact] + public void DivideScalar_DividesAll() + { + var pl = new Polyline3( + new Point3(2, 4, 6), + new Point3(8, 10, 12)); + var scaled = pl / 2f; + scaled[0].Should().Be(new Point3(1, 2, 3)); + scaled[1].Should().Be(new Point3(4, 5, 6)); + } + + // ───────────────────────────────────────────── + // Equality + // ───────────────────────────────────────────── + + [Fact] + public void Equality_SamePoints_AreEqual() + { + var a = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); + var b = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); + a.Should().Be(b); + a.GetHashCode().Should().Be(b.GetHashCode()); + } + + [Fact] + public void Equality_DifferentPoints_AreNotEqual() + { + var a = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); + var b = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 7)); + a.Should().NotBe(b); + } + + // ───────────────────────────────────────────── + // Simplify (Ramer-Douglas-Peucker 3D) + // ───────────────────────────────────────────── + + [Fact] + public void Simplify_StraightLine_RemovesCollinearPoints() + { + // 5 collinear points along X → should reduce to 2 endpoints + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(2, 0, 0), + new Point3(5, 0, 0), + new Point3(7, 0, 0), + new Point3(10, 0, 0)); + var simplified = pl.Simplify(0.1f); + simplified.Count.Should().Be(2); + simplified[0].Should().Be(new Point3(0, 0, 0)); + simplified[1].Should().Be(new Point3(10, 0, 0)); + } + + [Fact] + public void Simplify_CollinearAlongDiagonal3D() + { + // Points along (1,1,1) direction — all collinear + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(1, 1, 1), + new Point3(2, 2, 2), + new Point3(3, 3, 3)); + var simplified = pl.Simplify(0.1f); + simplified.Count.Should().Be(2); + } + + [Fact] + public void Simplify_LShape_KeepsCorner() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0), + new Point3(10, 10, 0)); + var simplified = pl.Simplify(1f); + simplified.Count.Should().Be(3); + } + + [Fact] + public void Simplify_3DDeviation() + { + // Point deviates in Z only + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(5, 0, 2f), // 2 units off the line in Z + new Point3(10, 0, 0)); + pl.Simplify(1f).Count.Should().Be(3); // 2 > 1 → kept + pl.Simplify(3f).Count.Should().Be(2); // 2 < 3 → removed + } + + [Fact] + public void Simplify_TwoPoints_ReturnsSelf() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0)); + var simplified = pl.Simplify(1f); + simplified.Count.Should().Be(2); + } + + // ───────────────────────────────────────────── + // Scale + // ───────────────────────────────────────────── + + [Fact] + public void Scale_AroundCentroid_PreservesCentroid() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0)); + var centroid = pl.Centroid(); + var scaled = pl.Scale(2f); + var scaledCentroid = scaled.Centroid(); + scaledCentroid.X.Should().BeApproximately(centroid.X, Tol); + scaledCentroid.Y.Should().BeApproximately(centroid.Y, Tol); + scaledCentroid.Z.Should().BeApproximately(centroid.Z, Tol); + } + + [Fact] + public void Scale_DoublesLength() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(10, 0, 0)); + var scaled = pl.Scale(2f); + scaled.Length().Should().BeApproximately(20f, Tol); + } + + // ───────────────────────────────────────────── + // Points list + // ───────────────────────────────────────────── + + [Fact] + public void Points_ReturnsReadOnlyList() + { + var pl = new Polyline3(new Point3(1, 2, 3), new Point3(4, 5, 6)); + pl.Points.Count.Should().Be(2); + pl.Points[0].Should().Be(new Point3(1, 2, 3)); + } + + // ───────────────────────────────────────────── + // ToString + // ───────────────────────────────────────────── + + [Fact] + public void ToString_ShowsCount() + { + var pl = new Polyline3( + new Point3(0, 0, 0), + new Point3(1, 1, 1)); + pl.ToString().Should().Be("Polyline3(2 points)"); + } + + // ───────────────────────────────────────────── + // JSON round-trip + // ───────────────────────────────────────────── + + [Fact] + public void Json_RoundTrip_Float() + { + var pl = new Polyline3( + new Point3(1, 2, 3), + new Point3(4, 5, 6)); + var json = System.Text.Json.JsonSerializer.Serialize(pl); + json.Should().Be("[1,2,3,4,5,6]"); + var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); + deserialized.Should().Be(pl); + } + + [Fact] + public void Json_RoundTrip_Double() + { + var pl = new Polyline3( + new Point3(1.5, 2.5, 3.5), + new Point3(4.5, 5.5, 6.5)); + var json = System.Text.Json.JsonSerializer.Serialize(pl); + var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); + deserialized.Should().Be(pl); + } + + [Fact] + public void Json_Empty() + { + var pl = new Polyline3(); + var json = System.Text.Json.JsonSerializer.Serialize(pl); + json.Should().Be("[]"); + var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); + deserialized.Count.Should().Be(0); + } + + [Fact] + public void Transform_Frame_RotatesThenTranslatesEveryPoint() + { + var pl = new Polyline3(new Point3(1, 0, 0), new Point3(0, 2, 0)); + var frame = new Frame3(new Point3(10, 0, 0), Quaternion.FromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2)); + + var r = pl.Transform(frame); + + r.Count.Should().Be(2); + r[0].X.Should().BeApproximately(10, Tol); r[0].Y.Should().BeApproximately(1, Tol); r[0].Z.Should().BeApproximately(0, Tol); + r[1].X.Should().BeApproximately(8, Tol); r[1].Y.Should().BeApproximately(0, Tol); r[1].Z.Should().BeApproximately(0, Tol); + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/PosePath3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/PosePath3Tests.cs index 5777332..c66b581 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/PosePath3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/PosePath3Tests.cs @@ -1,167 +1,167 @@ -using FluentAssertions; -using ModelingEvolution.Drawing; - -namespace ModelingEvolution.Drawing.Tests; - -public class PosePath3Tests -{ - private const float Tol = 1e-3f; - - private static Pose3 P(float x, float y, float z) => new(x, y, z, 0, 0, 0); - - // ───────────────────────────────────────────── - // Construction - // ───────────────────────────────────────────── - - [Fact] - public void DefaultCtor_Empty() - { - var path = new PosePath3(); - path.Count.Should().Be(0); - } - - [Fact] - public void ParamsCtor_StoresPoses() - { - var path = new PosePath3(P(0, 0, 0), P(10, 0, 0)); - path.Count.Should().Be(2); - path[0].Should().Be(P(0, 0, 0)); - path[1].Should().Be(P(10, 0, 0)); - } - - [Fact] - public void Start_End() - { - var path = new PosePath3(P(1, 2, 3), P(4, 5, 6)); - path.Start.Should().Be(P(1, 2, 3)); - path.End.Should().Be(P(4, 5, 6)); - } - - // ───────────────────────────────────────────── - // Length - // ───────────────────────────────────────────── - - [Fact] - public void Length_StraightLine() - { - var path = new PosePath3(P(0, 0, 0), P(10, 0, 0)); - path.Length().Should().BeApproximately(10f, Tol); - } - - [Fact] - public void Length_MultiSegment() - { - var path = new PosePath3(P(0, 0, 0), P(10, 0, 0), P(10, 5, 0)); - path.Length().Should().BeApproximately(15f, Tol); - } - - // ───────────────────────────────────────────── - // Reverse - // ───────────────────────────────────────────── - - [Fact] - public void Reverse_FlipsOrder() - { - var path = new PosePath3(P(0, 0, 0), P(10, 0, 0), P(20, 0, 0)); - var rev = path.Reverse(); - rev[0].Should().Be(P(20, 0, 0)); - rev[1].Should().Be(P(10, 0, 0)); - rev[2].Should().Be(P(0, 0, 0)); - } - - // ───────────────────────────────────────────── - // ToPolyline3 - // ───────────────────────────────────────────── - - [Fact] - public void ToPolyline3_ExtractsPositions() - { - var path = new PosePath3(P(1, 2, 3), P(4, 5, 6)); - var polyline = path.ToPolyline3(); - polyline.Count.Should().Be(2); - polyline[0].Should().Be(new Point3(1, 2, 3)); - polyline[1].Should().Be(new Point3(4, 5, 6)); - } - - // ───────────────────────────────────────────── - // ToTrajectory - // ───────────────────────────────────────────── - - [Fact] - public void ToTrajectory_AssignsUniformTimestamps() - { - var path = new PosePath3(P(0, 0, 0), P(5, 0, 0), P(10, 0, 0)); - var traj = path.ToTrajectory(4f); - traj.Count.Should().Be(3); - traj[0].Time.Should().BeApproximately(0f, Tol); - traj[1].Time.Should().BeApproximately(2f, Tol); - traj[2].Time.Should().BeApproximately(4f, Tol); - } - - // ───────────────────────────────────────────── - // Operators - // ───────────────────────────────────────────── - - [Fact] - public void PlusVector_TranslatesPositions() - { - var path = new PosePath3(P(0, 0, 0), P(10, 0, 0)); - var moved = path + new Vector3(1, 2, 3); - moved[0].X.Should().BeApproximately(1f, Tol); - moved[0].Y.Should().BeApproximately(2f, Tol); - moved[0].Z.Should().BeApproximately(3f, Tol); - } - - // ───────────────────────────────────────────── - // Equality - // ───────────────────────────────────────────── - - [Fact] - public void Equality_SamePoses_AreEqual() - { - var a = new PosePath3(P(1, 2, 3), P(4, 5, 6)); - var b = new PosePath3(P(1, 2, 3), P(4, 5, 6)); - a.Should().Be(b); - a.GetHashCode().Should().Be(b.GetHashCode()); - } - - // ───────────────────────────────────────────── - // JSON round-trip - // ───────────────────────────────────────────── - - [Fact] - public void Json_RoundTrip() - { - var path = new PosePath3(P(1, 2, 3), P(4, 5, 6)); - var json = System.Text.Json.JsonSerializer.Serialize(path); - json.Should().Be("[1,2,3,0,0,0,4,5,6,0,0,0]"); - var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); - deserialized.Should().Be(path); - } - - // ───────────────────────────────────────────── - // ToString - // ───────────────────────────────────────────── - - [Fact] - public void ToString_ShowsCount() - { - var path = new PosePath3(P(0, 0, 0), P(1, 1, 1)); - path.ToString().Should().Be("PosePath3(2 poses)"); - } - - [Fact] - public void Transform_Frame_ComposesFrameTimesEachPose() - { - var path = new PosePath3(P(1, 0, 0), P(0, 2, 0)); - var frame = new Frame3(new Point3(10, 0, 0), Quaternion.FromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2)); - - var r = path.Transform(frame); - - r.Count.Should().Be(2); - r[0].X.Should().BeApproximately(10, Tol); r[0].Y.Should().BeApproximately(1, Tol); r[0].Z.Should().BeApproximately(0, Tol); - ((float)r[0].Rz).Should().BeApproximately(90, Tol); - r[1].X.Should().BeApproximately(8, Tol); r[1].Y.Should().BeApproximately(0, Tol); - ((float)r[1].Rz).Should().BeApproximately(90, Tol); - } -} +using FluentAssertions; +using ModelingEvolution.Drawing; + +namespace ModelingEvolution.Drawing.Tests; + +public class PosePath3Tests +{ + private const float Tol = 1e-3f; + + private static Pose3 P(float x, float y, float z) => new(x, y, z, 0, 0, 0); + + // ───────────────────────────────────────────── + // Construction + // ───────────────────────────────────────────── + + [Fact] + public void DefaultCtor_Empty() + { + var path = new PosePath3(); + path.Count.Should().Be(0); + } + + [Fact] + public void ParamsCtor_StoresPoses() + { + var path = new PosePath3(P(0, 0, 0), P(10, 0, 0)); + path.Count.Should().Be(2); + path[0].Should().Be(P(0, 0, 0)); + path[1].Should().Be(P(10, 0, 0)); + } + + [Fact] + public void Start_End() + { + var path = new PosePath3(P(1, 2, 3), P(4, 5, 6)); + path.Start.Should().Be(P(1, 2, 3)); + path.End.Should().Be(P(4, 5, 6)); + } + + // ───────────────────────────────────────────── + // Length + // ───────────────────────────────────────────── + + [Fact] + public void Length_StraightLine() + { + var path = new PosePath3(P(0, 0, 0), P(10, 0, 0)); + path.Length().Should().BeApproximately(10f, Tol); + } + + [Fact] + public void Length_MultiSegment() + { + var path = new PosePath3(P(0, 0, 0), P(10, 0, 0), P(10, 5, 0)); + path.Length().Should().BeApproximately(15f, Tol); + } + + // ───────────────────────────────────────────── + // Reverse + // ───────────────────────────────────────────── + + [Fact] + public void Reverse_FlipsOrder() + { + var path = new PosePath3(P(0, 0, 0), P(10, 0, 0), P(20, 0, 0)); + var rev = path.Reverse(); + rev[0].Should().Be(P(20, 0, 0)); + rev[1].Should().Be(P(10, 0, 0)); + rev[2].Should().Be(P(0, 0, 0)); + } + + // ───────────────────────────────────────────── + // ToPolyline3 + // ───────────────────────────────────────────── + + [Fact] + public void ToPolyline3_ExtractsPositions() + { + var path = new PosePath3(P(1, 2, 3), P(4, 5, 6)); + var polyline = path.ToPolyline3(); + polyline.Count.Should().Be(2); + polyline[0].Should().Be(new Point3(1, 2, 3)); + polyline[1].Should().Be(new Point3(4, 5, 6)); + } + + // ───────────────────────────────────────────── + // ToTrajectory + // ───────────────────────────────────────────── + + [Fact] + public void ToTrajectory_AssignsUniformTimestamps() + { + var path = new PosePath3(P(0, 0, 0), P(5, 0, 0), P(10, 0, 0)); + var traj = path.ToTrajectory(4f); + traj.Count.Should().Be(3); + traj[0].Time.Should().BeApproximately(0f, Tol); + traj[1].Time.Should().BeApproximately(2f, Tol); + traj[2].Time.Should().BeApproximately(4f, Tol); + } + + // ───────────────────────────────────────────── + // Operators + // ───────────────────────────────────────────── + + [Fact] + public void PlusVector_TranslatesPositions() + { + var path = new PosePath3(P(0, 0, 0), P(10, 0, 0)); + var moved = path + new Vector3(1, 2, 3); + moved[0].X.Should().BeApproximately(1f, Tol); + moved[0].Y.Should().BeApproximately(2f, Tol); + moved[0].Z.Should().BeApproximately(3f, Tol); + } + + // ───────────────────────────────────────────── + // Equality + // ───────────────────────────────────────────── + + [Fact] + public void Equality_SamePoses_AreEqual() + { + var a = new PosePath3(P(1, 2, 3), P(4, 5, 6)); + var b = new PosePath3(P(1, 2, 3), P(4, 5, 6)); + a.Should().Be(b); + a.GetHashCode().Should().Be(b.GetHashCode()); + } + + // ───────────────────────────────────────────── + // JSON round-trip + // ───────────────────────────────────────────── + + [Fact] + public void Json_RoundTrip() + { + var path = new PosePath3(P(1, 2, 3), P(4, 5, 6)); + var json = System.Text.Json.JsonSerializer.Serialize(path); + json.Should().Be("[1,2,3,0,0,0,4,5,6,0,0,0]"); + var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); + deserialized.Should().Be(path); + } + + // ───────────────────────────────────────────── + // ToString + // ───────────────────────────────────────────── + + [Fact] + public void ToString_ShowsCount() + { + var path = new PosePath3(P(0, 0, 0), P(1, 1, 1)); + path.ToString().Should().Be("PosePath3(2 poses)"); + } + + [Fact] + public void Transform_Frame_ComposesFrameTimesEachPose() + { + var path = new PosePath3(P(1, 0, 0), P(0, 2, 0)); + var frame = new Frame3(new Point3(10, 0, 0), Quaternion.FromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2)); + + var r = path.Transform(frame); + + r.Count.Should().Be(2); + r[0].X.Should().BeApproximately(10, Tol); r[0].Y.Should().BeApproximately(1, Tol); r[0].Z.Should().BeApproximately(0, Tol); + ((float)r[0].Rz).Should().BeApproximately(90, Tol); + r[1].X.Should().BeApproximately(8, Tol); r[1].Y.Should().BeApproximately(0, Tol); + ((float)r[1].Rz).Should().BeApproximately(90, Tol); + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Trajectory3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Trajectory3Tests.cs index 0ea71fd..d22056e 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Trajectory3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Trajectory3Tests.cs @@ -1,240 +1,240 @@ -using FluentAssertions; -using ModelingEvolution.Drawing; - -namespace ModelingEvolution.Drawing.Tests; - -public class Trajectory3Tests -{ - private const float Tol = 1e-3f; - - private static Pose3 P(float x, float y, float z) => new(x, y, z, 0, 0, 0); - private static Waypoint3 W(float x, float y, float z, float t) => new(P(x, y, z), t); - - // ───────────────────────────────────────────── - // Construction - // ───────────────────────────────────────────── - - [Fact] - public void DefaultCtor_Empty() - { - var traj = new Trajectory3(); - traj.Count.Should().Be(0); - } - - [Fact] - public void ParamsCtor_StoresWaypoints() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1)); - traj.Count.Should().Be(2); - traj[0].Position.Should().Be(new Point3(0, 0, 0)); - traj[0].Time.Should().Be(0f); - traj[1].Time.Should().Be(1f); - } - - [Fact] - public void Start_End() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 5)); - traj.Start.Time.Should().Be(0f); - traj.End.Time.Should().Be(5f); - } - - // ───────────────────────────────────────────── - // Duration & Length - // ───────────────────────────────────────────── - - [Fact] - public void Duration_IsLastMinusFirst() - { - var traj = new Trajectory3(W(0, 0, 0, 1), W(10, 0, 0, 4)); - traj.Duration().Should().BeApproximately(3f, Tol); - } - - [Fact] - public void Length_PositionalDistance() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1)); - traj.Length().Should().BeApproximately(10f, Tol); - } - - // ───────────────────────────────────────────── - // AtTime — interpolation - // ───────────────────────────────────────────── - - [Fact] - public void AtTime_AtStart_ReturnsFirstPose() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); - var pose = traj.AtTime(0f); - pose.X.Should().BeApproximately(0f, Tol); - } - - [Fact] - public void AtTime_AtEnd_ReturnsLastPose() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); - var pose = traj.AtTime(2f); - pose.X.Should().BeApproximately(10f, Tol); - } - - [Fact] - public void AtTime_AtMidpoint_InterpolatesPosition() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); - var pose = traj.AtTime(1f); - pose.X.Should().BeApproximately(5f, Tol); - pose.Y.Should().BeApproximately(0f, Tol); - pose.Z.Should().BeApproximately(0f, Tol); - } - - [Fact] - public void AtTime_BeforeStart_ClampsToFirst() - { - var traj = new Trajectory3(W(0, 0, 0, 1), W(10, 0, 0, 3)); - var pose = traj.AtTime(0f); - pose.X.Should().BeApproximately(0f, Tol); - } - - [Fact] - public void AtTime_AfterEnd_ClampsToLast() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); - var pose = traj.AtTime(100f); - pose.X.Should().BeApproximately(10f, Tol); - } - - [Fact] - public void AtTime_MultiSegment() - { - // 0->10 over [0,2], 10->30 over [2,4] - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2), W(30, 0, 0, 4)); - traj.AtTime(1f).X.Should().BeApproximately(5f, Tol); // midway first segment - traj.AtTime(3f).X.Should().BeApproximately(20f, Tol); // midway second segment - } - - // ───────────────────────────────────────────── - // Reverse - // ───────────────────────────────────────────── - - [Fact] - public void Reverse_FlipsOrderPreservesTimingIntervals() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1), W(30, 0, 0, 3)); - var rev = traj.Reverse(); - rev.Count.Should().Be(3); - // Original intervals: [0-1], [1-3] → reversed order with same intervals: [0-2], [2-3] - rev[0].Position.X.Should().BeApproximately(30f, Tol); - rev[0].Time.Should().BeApproximately(0f, Tol); - rev[1].Position.X.Should().BeApproximately(10f, Tol); - rev[1].Time.Should().BeApproximately(2f, Tol); - rev[2].Position.X.Should().BeApproximately(0f, Tol); - rev[2].Time.Should().BeApproximately(3f, Tol); - } - - // ───────────────────────────────────────────── - // ToPosePath & ToPolyline3 - // ───────────────────────────────────────────── - - [Fact] - public void ToPosePath_StripsTimestamps() - { - var traj = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); - var path = traj.ToPosePath(); - path.Count.Should().Be(2); - path[0].X.Should().BeApproximately(1f, Tol); - path[1].X.Should().BeApproximately(4f, Tol); - } - - [Fact] - public void ToPolyline3_ExtractsPositions() - { - var traj = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); - var poly = traj.ToPolyline3(); - poly.Count.Should().Be(2); - poly[0].Should().Be(new Point3(1, 2, 3)); - } - - // ───────────────────────────────────────────── - // Resample - // ───────────────────────────────────────────── - - [Fact] - public void Resample_UniformInterval() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 10)); - var resampled = traj.Resample(5f); - resampled.Count.Should().Be(3); // t=0, t=5, t=10 - resampled[0].Time.Should().BeApproximately(0f, Tol); - resampled[1].Time.Should().BeApproximately(5f, Tol); - resampled[2].Time.Should().BeApproximately(10f, Tol); - resampled[1].Position.X.Should().BeApproximately(5f, Tol); - } - - // ───────────────────────────────────────────── - // Operators - // ───────────────────────────────────────────── - - [Fact] - public void PlusVector_TranslatesPositions() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1)); - var moved = traj + new Vector3(1, 2, 3); - moved[0].Position.X.Should().BeApproximately(1f, Tol); - moved[0].Position.Y.Should().BeApproximately(2f, Tol); - moved[0].Time.Should().Be(0f); // time unchanged - } - - // ───────────────────────────────────────────── - // Equality - // ───────────────────────────────────────────── - - [Fact] - public void Equality_SameWaypoints_AreEqual() - { - var a = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); - var b = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); - a.Should().Be(b); - a.GetHashCode().Should().Be(b.GetHashCode()); - } - - // ───────────────────────────────────────────── - // JSON round-trip - // ───────────────────────────────────────────── - - [Fact] - public void Json_RoundTrip() - { - var traj = new Trajectory3(W(1, 2, 3, 0.5f), W(4, 5, 6, 1.5f)); - var json = System.Text.Json.JsonSerializer.Serialize(traj); - json.Should().Be("[1,2,3,0,0,0,0.5,4,5,6,0,0,0,1.5]"); - var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); - deserialized.Should().Be(traj); - } - - // ───────────────────────────────────────────── - // ToString - // ───────────────────────────────────────────── - - [Fact] - public void ToString_ShowsCount() - { - var traj = new Trajectory3(W(0, 0, 0, 0), W(1, 1, 1, 1)); - traj.ToString().Should().Be("Trajectory3(2 waypoints)"); - } - - [Fact] - public void Transform_Frame_ComposesFrameTimesEachPose_PreservesTime() - { - var traj = new Trajectory3(W(1, 0, 0, 0), W(0, 2, 0, 0.5f)); - var frame = new Frame3(new Point3(10, 0, 0), Quaternion.FromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2)); - - var r = traj.Transform(frame); - - r.Count.Should().Be(2); - r[0].Pose.X.Should().BeApproximately(10, Tol); r[0].Pose.Y.Should().BeApproximately(1, Tol); - ((float)r[0].Pose.Rz).Should().BeApproximately(90, Tol); - r[0].Time.Should().Be(0); - r[1].Pose.X.Should().BeApproximately(8, Tol); r[1].Pose.Y.Should().BeApproximately(0, Tol); - r[1].Time.Should().Be(0.5f); - } -} +using FluentAssertions; +using ModelingEvolution.Drawing; + +namespace ModelingEvolution.Drawing.Tests; + +public class Trajectory3Tests +{ + private const float Tol = 1e-3f; + + private static Pose3 P(float x, float y, float z) => new(x, y, z, 0, 0, 0); + private static Waypoint3 W(float x, float y, float z, float t) => new(P(x, y, z), t); + + // ───────────────────────────────────────────── + // Construction + // ───────────────────────────────────────────── + + [Fact] + public void DefaultCtor_Empty() + { + var traj = new Trajectory3(); + traj.Count.Should().Be(0); + } + + [Fact] + public void ParamsCtor_StoresWaypoints() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1)); + traj.Count.Should().Be(2); + traj[0].Position.Should().Be(new Point3(0, 0, 0)); + traj[0].Time.Should().Be(0f); + traj[1].Time.Should().Be(1f); + } + + [Fact] + public void Start_End() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 5)); + traj.Start.Time.Should().Be(0f); + traj.End.Time.Should().Be(5f); + } + + // ───────────────────────────────────────────── + // Duration & Length + // ───────────────────────────────────────────── + + [Fact] + public void Duration_IsLastMinusFirst() + { + var traj = new Trajectory3(W(0, 0, 0, 1), W(10, 0, 0, 4)); + traj.Duration().Should().BeApproximately(3f, Tol); + } + + [Fact] + public void Length_PositionalDistance() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1)); + traj.Length().Should().BeApproximately(10f, Tol); + } + + // ───────────────────────────────────────────── + // AtTime — interpolation + // ───────────────────────────────────────────── + + [Fact] + public void AtTime_AtStart_ReturnsFirstPose() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); + var pose = traj.AtTime(0f); + pose.X.Should().BeApproximately(0f, Tol); + } + + [Fact] + public void AtTime_AtEnd_ReturnsLastPose() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); + var pose = traj.AtTime(2f); + pose.X.Should().BeApproximately(10f, Tol); + } + + [Fact] + public void AtTime_AtMidpoint_InterpolatesPosition() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); + var pose = traj.AtTime(1f); + pose.X.Should().BeApproximately(5f, Tol); + pose.Y.Should().BeApproximately(0f, Tol); + pose.Z.Should().BeApproximately(0f, Tol); + } + + [Fact] + public void AtTime_BeforeStart_ClampsToFirst() + { + var traj = new Trajectory3(W(0, 0, 0, 1), W(10, 0, 0, 3)); + var pose = traj.AtTime(0f); + pose.X.Should().BeApproximately(0f, Tol); + } + + [Fact] + public void AtTime_AfterEnd_ClampsToLast() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2)); + var pose = traj.AtTime(100f); + pose.X.Should().BeApproximately(10f, Tol); + } + + [Fact] + public void AtTime_MultiSegment() + { + // 0->10 over [0,2], 10->30 over [2,4] + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 2), W(30, 0, 0, 4)); + traj.AtTime(1f).X.Should().BeApproximately(5f, Tol); // midway first segment + traj.AtTime(3f).X.Should().BeApproximately(20f, Tol); // midway second segment + } + + // ───────────────────────────────────────────── + // Reverse + // ───────────────────────────────────────────── + + [Fact] + public void Reverse_FlipsOrderPreservesTimingIntervals() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1), W(30, 0, 0, 3)); + var rev = traj.Reverse(); + rev.Count.Should().Be(3); + // Original intervals: [0-1], [1-3] → reversed order with same intervals: [0-2], [2-3] + rev[0].Position.X.Should().BeApproximately(30f, Tol); + rev[0].Time.Should().BeApproximately(0f, Tol); + rev[1].Position.X.Should().BeApproximately(10f, Tol); + rev[1].Time.Should().BeApproximately(2f, Tol); + rev[2].Position.X.Should().BeApproximately(0f, Tol); + rev[2].Time.Should().BeApproximately(3f, Tol); + } + + // ───────────────────────────────────────────── + // ToPosePath & ToPolyline3 + // ───────────────────────────────────────────── + + [Fact] + public void ToPosePath_StripsTimestamps() + { + var traj = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); + var path = traj.ToPosePath(); + path.Count.Should().Be(2); + path[0].X.Should().BeApproximately(1f, Tol); + path[1].X.Should().BeApproximately(4f, Tol); + } + + [Fact] + public void ToPolyline3_ExtractsPositions() + { + var traj = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); + var poly = traj.ToPolyline3(); + poly.Count.Should().Be(2); + poly[0].Should().Be(new Point3(1, 2, 3)); + } + + // ───────────────────────────────────────────── + // Resample + // ───────────────────────────────────────────── + + [Fact] + public void Resample_UniformInterval() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 10)); + var resampled = traj.Resample(5f); + resampled.Count.Should().Be(3); // t=0, t=5, t=10 + resampled[0].Time.Should().BeApproximately(0f, Tol); + resampled[1].Time.Should().BeApproximately(5f, Tol); + resampled[2].Time.Should().BeApproximately(10f, Tol); + resampled[1].Position.X.Should().BeApproximately(5f, Tol); + } + + // ───────────────────────────────────────────── + // Operators + // ───────────────────────────────────────────── + + [Fact] + public void PlusVector_TranslatesPositions() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(10, 0, 0, 1)); + var moved = traj + new Vector3(1, 2, 3); + moved[0].Position.X.Should().BeApproximately(1f, Tol); + moved[0].Position.Y.Should().BeApproximately(2f, Tol); + moved[0].Time.Should().Be(0f); // time unchanged + } + + // ───────────────────────────────────────────── + // Equality + // ───────────────────────────────────────────── + + [Fact] + public void Equality_SameWaypoints_AreEqual() + { + var a = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); + var b = new Trajectory3(W(1, 2, 3, 0), W(4, 5, 6, 1)); + a.Should().Be(b); + a.GetHashCode().Should().Be(b.GetHashCode()); + } + + // ───────────────────────────────────────────── + // JSON round-trip + // ───────────────────────────────────────────── + + [Fact] + public void Json_RoundTrip() + { + var traj = new Trajectory3(W(1, 2, 3, 0.5f), W(4, 5, 6, 1.5f)); + var json = System.Text.Json.JsonSerializer.Serialize(traj); + json.Should().Be("[1,2,3,0,0,0,0.5,4,5,6,0,0,0,1.5]"); + var deserialized = System.Text.Json.JsonSerializer.Deserialize>(json); + deserialized.Should().Be(traj); + } + + // ───────────────────────────────────────────── + // ToString + // ───────────────────────────────────────────── + + [Fact] + public void ToString_ShowsCount() + { + var traj = new Trajectory3(W(0, 0, 0, 0), W(1, 1, 1, 1)); + traj.ToString().Should().Be("Trajectory3(2 waypoints)"); + } + + [Fact] + public void Transform_Frame_ComposesFrameTimesEachPose_PreservesTime() + { + var traj = new Trajectory3(W(1, 0, 0, 0), W(0, 2, 0, 0.5f)); + var frame = new Frame3(new Point3(10, 0, 0), Quaternion.FromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2)); + + var r = traj.Transform(frame); + + r.Count.Should().Be(2); + r[0].Pose.X.Should().BeApproximately(10, Tol); r[0].Pose.Y.Should().BeApproximately(1, Tol); + ((float)r[0].Pose.Rz).Should().BeApproximately(90, Tol); + r[0].Time.Should().Be(0); + r[1].Pose.X.Should().BeApproximately(8, Tol); r[1].Pose.Y.Should().BeApproximately(0, Tol); + r[1].Time.Should().Be(0.5f); + } +} diff --git a/Sources/ModelingEvolution.Drawing/Matrix3x3.cs b/Sources/ModelingEvolution.Drawing/Matrix3x3.cs index 4ebd992..5606798 100644 --- a/Sources/ModelingEvolution.Drawing/Matrix3x3.cs +++ b/Sources/ModelingEvolution.Drawing/Matrix3x3.cs @@ -1,261 +1,261 @@ -using System.Numerics; -using System.Runtime.CompilerServices; - -namespace ModelingEvolution.Drawing; - -/// -/// A 3×3 matrix stored as nine fields (row-major). Supports linear algebra -/// operations and 3D rotation matrix construction. Rotation ⇄ matrix goes through -/// (, ); the Euler edge is -/// / . -/// -[Matrix3x3JsonConverter] -public readonly record struct Matrix3x3( - T M11, T M12, T M13, - T M21, T M22, T M23, - T M31, T M32, T M33) - where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, - ISignedNumber, IFloatingPointIeee754, IMinMaxValue -{ - /// The 3×3 identity matrix. - public static Matrix3x3 Identity => new( - T.One, T.Zero, T.Zero, - T.Zero, T.One, T.Zero, - T.Zero, T.Zero, T.One); - - /// The 3×3 zero matrix. - public static Matrix3x3 Zero => new( - T.Zero, T.Zero, T.Zero, - T.Zero, T.Zero, T.Zero, - T.Zero, T.Zero, T.Zero); - - /// The determinant of this 3×3 matrix. - public T Determinant - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get => M11 * (M22 * M33 - M23 * M32) - - M12 * (M21 * M33 - M23 * M31) - + M13 * (M21 * M32 - M22 * M31); - } - - /// The trace: M11 + M22 + M33. - public T Trace - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get => M11 + M22 + M33; - } - - /// Returns the transpose of this matrix. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Matrix3x3 Transpose() => new( - M11, M21, M31, - M12, M22, M32, - M13, M23, M33); - - /// - /// Returns the inverse of this matrix via adjugate/determinant. - /// Throws if the matrix is singular (determinant ≈ 0). - /// - public Matrix3x3 Inverse() - { - var det = Determinant; - if (T.Abs(det) < T.CreateTruncating(1e-15)) - throw new InvalidOperationException("Matrix is singular."); - - var invDet = T.One / det; - - return new( - (M22 * M33 - M23 * M32) * invDet, - (M13 * M32 - M12 * M33) * invDet, - (M12 * M23 - M13 * M22) * invDet, - (M23 * M31 - M21 * M33) * invDet, - (M11 * M33 - M13 * M31) * invDet, - (M13 * M21 - M11 * M23) * invDet, - (M21 * M32 - M22 * M31) * invDet, - (M12 * M31 - M11 * M32) * invDet, - (M11 * M22 - M12 * M21) * invDet); - } - - /// Transforms a vector by this matrix: result = M · v. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Vector3 Transform(Vector3 v) => new( - M11 * v.X + M12 * v.Y + M13 * v.Z, - M21 * v.X + M22 * v.Y + M23 * v.Z, - M31 * v.X + M32 * v.Y + M33 * v.Z); - - /// Transforms a point by this matrix: result = M · p. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Point3 Transform(Point3 p) => new( - M11 * p.X + M12 * p.Y + M13 * p.Z, - M21 * p.X + M22 * p.Y + M23 * p.Z, - M31 * p.X + M32 * p.Y + M33 * p.Z); - - #region Operators - - /// Matrix multiplication. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 operator *(in Matrix3x3 a, in Matrix3x3 b) => new( - a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31, - a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32, - a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33, - a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31, - a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32, - a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33, - a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31, - a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32, - a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33); - - /// Matrix-vector multiplication. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 operator *(in Matrix3x3 m, Vector3 v) => m.Transform(v); - - /// Matrix-point multiplication. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Point3 operator *(in Matrix3x3 m, Point3 p) => m.Transform(p); - - /// Scalar multiplication. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 operator *(in Matrix3x3 m, T s) => new( - m.M11 * s, m.M12 * s, m.M13 * s, - m.M21 * s, m.M22 * s, m.M23 * s, - m.M31 * s, m.M32 * s, m.M33 * s); - - /// Scalar multiplication (commutative). - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 operator *(T s, in Matrix3x3 m) => m * s; - - /// Matrix addition. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 operator +(in Matrix3x3 a, in Matrix3x3 b) => new( - a.M11 + b.M11, a.M12 + b.M12, a.M13 + b.M13, - a.M21 + b.M21, a.M22 + b.M22, a.M23 + b.M23, - a.M31 + b.M31, a.M32 + b.M32, a.M33 + b.M33); - - /// Matrix subtraction. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 operator -(in Matrix3x3 a, in Matrix3x3 b) => new( - a.M11 - b.M11, a.M12 - b.M12, a.M13 - b.M13, - a.M21 - b.M21, a.M22 - b.M22, a.M23 - b.M23, - a.M31 - b.M31, a.M32 - b.M32, a.M33 - b.M33); - - #endregion - - #region Rotation Factories - - /// Creates a rotation matrix around the X axis. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 RotationX(Degree angle) - { - var rad = (Radian)angle; - var c = T.Cos((T)rad); - var s = T.Sin((T)rad); - return new( - T.One, T.Zero, T.Zero, - T.Zero, c, -s, - T.Zero, s, c); - } - - /// Creates a rotation matrix around the Y axis. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 RotationY(Degree angle) - { - var rad = (Radian)angle; - var c = T.Cos((T)rad); - var s = T.Sin((T)rad); - return new( - c, T.Zero, s, - T.Zero, T.One, T.Zero, - -s, T.Zero, c); - } - - /// Creates a rotation matrix around the Z axis. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 RotationZ(Degree angle) - { - var rad = (Radian)angle; - var c = T.Cos((T)rad); - var s = T.Sin((T)rad); - return new( - c, -s, T.Zero, - s, c, T.Zero, - T.Zero, T.Zero, T.One); - } - - /// - /// Creates a ZYX Euler rotation matrix: R = Rz(rz) * Ry(ry) * Rx(rx), the convention of . - /// The Euler → rotation formula is ; this is its matrix (an edge for - /// matrix-based callers such as solvers and OpenCV). - /// - public static Matrix3x3 RotationZYX(Degree rx, Degree ry, Degree rz) => - Quaternion.FromRpy((T)(Radian)rx, (T)(Radian)ry, (T)(Radian)rz).ToMatrix3x3(); - - #endregion - - #region Other Factories - - /// Creates a 3D scaling matrix. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 Scale(T sx, T sy, T sz) => new( - sx, T.Zero, T.Zero, - T.Zero, sy, T.Zero, - T.Zero, T.Zero, sz); - - /// - /// Constructs a similarity matrix (uniform scale × rotation) that maps - /// to when applied as a vector transform. - /// Specifically, M.Transform(source) ≈ target. - /// - /// Source vector (must be non-zero). - /// Target vector (must be non-zero). - /// The 3×3 matrix combining rotation that aligns source's direction with target's, scaled by |target|/|source|. - /// Source or target vector has zero length. - /// - /// Two-point alignment is geometrically under-constrained in 3D (rotation around the source axis is free); - /// this method picks the shortest-arc rotation, (for opposite vectors a - /// half-turn about the axis FromTo picks). - /// Use cases include adapting taught polyline paths whose endpoints have shifted. - /// - public static Matrix3x3 SimilarityFromVectors(Vector3 source, Vector3 target) - { - var sourceLength = source.Length; - if (sourceLength == T.Zero) - throw new ArgumentException("Source vector must be non-zero.", nameof(source)); - - var scale = target.Length / sourceLength; - return scale * Quaternion.FromTo(source, target).ToMatrix3x3(); - } - - /// Creates a matrix from three column vectors. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 FromColumns(Vector3 c0, Vector3 c1, Vector3 c2) => new( - c0.X, c1.X, c2.X, - c0.Y, c1.Y, c2.Y, - c0.Z, c1.Z, c2.Z); - - /// Creates a matrix from three row vectors. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 FromRows(Vector3 r0, Vector3 r1, Vector3 r2) => new( - r0.X, r0.Y, r0.Z, - r1.X, r1.Y, r1.Z, - r2.X, r2.Y, r2.Z); - - #endregion - - #region Euler Conversion - - /// - /// Extracts ZYX Euler angles (in degrees) from this rotation matrix, assuming R = Rz(rz) * Ry(ry) * Rx(rx). - /// The matrix goes through and the one rotation → Euler formula, - /// (gimbal lock: rz = 0). - /// - public (Degree Rx, Degree Ry, Degree Rz) ToEulerZYX() - { - var r = Rotation3.FromQuaternion(Quaternion.FromMatrix3x3(this)); - return (r.Rx, r.Ry, r.Rz); - } - - #endregion - - public override string ToString() => - $"[{M11}, {M12}, {M13}; {M21}, {M22}, {M23}; {M31}, {M32}, {M33}]"; -} +using System.Numerics; +using System.Runtime.CompilerServices; + +namespace ModelingEvolution.Drawing; + +/// +/// A 3×3 matrix stored as nine fields (row-major). Supports linear algebra +/// operations and 3D rotation matrix construction. Rotation ⇄ matrix goes through +/// (, ); the Euler edge is +/// / . +/// +[Matrix3x3JsonConverter] +public readonly record struct Matrix3x3( + T M11, T M12, T M13, + T M21, T M22, T M23, + T M31, T M32, T M33) + where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, + ISignedNumber, IFloatingPointIeee754, IMinMaxValue +{ + /// The 3×3 identity matrix. + public static Matrix3x3 Identity => new( + T.One, T.Zero, T.Zero, + T.Zero, T.One, T.Zero, + T.Zero, T.Zero, T.One); + + /// The 3×3 zero matrix. + public static Matrix3x3 Zero => new( + T.Zero, T.Zero, T.Zero, + T.Zero, T.Zero, T.Zero, + T.Zero, T.Zero, T.Zero); + + /// The determinant of this 3×3 matrix. + public T Determinant + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get => M11 * (M22 * M33 - M23 * M32) + - M12 * (M21 * M33 - M23 * M31) + + M13 * (M21 * M32 - M22 * M31); + } + + /// The trace: M11 + M22 + M33. + public T Trace + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get => M11 + M22 + M33; + } + + /// Returns the transpose of this matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Matrix3x3 Transpose() => new( + M11, M21, M31, + M12, M22, M32, + M13, M23, M33); + + /// + /// Returns the inverse of this matrix via adjugate/determinant. + /// Throws if the matrix is singular (determinant ≈ 0). + /// + public Matrix3x3 Inverse() + { + var det = Determinant; + if (T.Abs(det) < T.CreateTruncating(1e-15)) + throw new InvalidOperationException("Matrix is singular."); + + var invDet = T.One / det; + + return new( + (M22 * M33 - M23 * M32) * invDet, + (M13 * M32 - M12 * M33) * invDet, + (M12 * M23 - M13 * M22) * invDet, + (M23 * M31 - M21 * M33) * invDet, + (M11 * M33 - M13 * M31) * invDet, + (M13 * M21 - M11 * M23) * invDet, + (M21 * M32 - M22 * M31) * invDet, + (M12 * M31 - M11 * M32) * invDet, + (M11 * M22 - M12 * M21) * invDet); + } + + /// Transforms a vector by this matrix: result = M · v. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Vector3 Transform(Vector3 v) => new( + M11 * v.X + M12 * v.Y + M13 * v.Z, + M21 * v.X + M22 * v.Y + M23 * v.Z, + M31 * v.X + M32 * v.Y + M33 * v.Z); + + /// Transforms a point by this matrix: result = M · p. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Point3 Transform(Point3 p) => new( + M11 * p.X + M12 * p.Y + M13 * p.Z, + M21 * p.X + M22 * p.Y + M23 * p.Z, + M31 * p.X + M32 * p.Y + M33 * p.Z); + + #region Operators + + /// Matrix multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator *(in Matrix3x3 a, in Matrix3x3 b) => new( + a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31, + a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32, + a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33, + a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31, + a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32, + a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33, + a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31, + a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32, + a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33); + + /// Matrix-vector multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 operator *(in Matrix3x3 m, Vector3 v) => m.Transform(v); + + /// Matrix-point multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Point3 operator *(in Matrix3x3 m, Point3 p) => m.Transform(p); + + /// Scalar multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator *(in Matrix3x3 m, T s) => new( + m.M11 * s, m.M12 * s, m.M13 * s, + m.M21 * s, m.M22 * s, m.M23 * s, + m.M31 * s, m.M32 * s, m.M33 * s); + + /// Scalar multiplication (commutative). + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator *(T s, in Matrix3x3 m) => m * s; + + /// Matrix addition. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator +(in Matrix3x3 a, in Matrix3x3 b) => new( + a.M11 + b.M11, a.M12 + b.M12, a.M13 + b.M13, + a.M21 + b.M21, a.M22 + b.M22, a.M23 + b.M23, + a.M31 + b.M31, a.M32 + b.M32, a.M33 + b.M33); + + /// Matrix subtraction. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator -(in Matrix3x3 a, in Matrix3x3 b) => new( + a.M11 - b.M11, a.M12 - b.M12, a.M13 - b.M13, + a.M21 - b.M21, a.M22 - b.M22, a.M23 - b.M23, + a.M31 - b.M31, a.M32 - b.M32, a.M33 - b.M33); + + #endregion + + #region Rotation Factories + + /// Creates a rotation matrix around the X axis. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 RotationX(Degree angle) + { + var rad = (Radian)angle; + var c = T.Cos((T)rad); + var s = T.Sin((T)rad); + return new( + T.One, T.Zero, T.Zero, + T.Zero, c, -s, + T.Zero, s, c); + } + + /// Creates a rotation matrix around the Y axis. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 RotationY(Degree angle) + { + var rad = (Radian)angle; + var c = T.Cos((T)rad); + var s = T.Sin((T)rad); + return new( + c, T.Zero, s, + T.Zero, T.One, T.Zero, + -s, T.Zero, c); + } + + /// Creates a rotation matrix around the Z axis. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 RotationZ(Degree angle) + { + var rad = (Radian)angle; + var c = T.Cos((T)rad); + var s = T.Sin((T)rad); + return new( + c, -s, T.Zero, + s, c, T.Zero, + T.Zero, T.Zero, T.One); + } + + /// + /// Creates a ZYX Euler rotation matrix: R = Rz(rz) * Ry(ry) * Rx(rx), the convention of . + /// The Euler → rotation formula is ; this is its matrix (an edge for + /// matrix-based callers such as solvers and OpenCV). + /// + public static Matrix3x3 RotationZYX(Degree rx, Degree ry, Degree rz) => + Quaternion.FromRpy((T)(Radian)rx, (T)(Radian)ry, (T)(Radian)rz).ToMatrix3x3(); + + #endregion + + #region Other Factories + + /// Creates a 3D scaling matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 Scale(T sx, T sy, T sz) => new( + sx, T.Zero, T.Zero, + T.Zero, sy, T.Zero, + T.Zero, T.Zero, sz); + + /// + /// Constructs a similarity matrix (uniform scale × rotation) that maps + /// to when applied as a vector transform. + /// Specifically, M.Transform(source) ≈ target. + /// + /// Source vector (must be non-zero). + /// Target vector (must be non-zero). + /// The 3×3 matrix combining rotation that aligns source's direction with target's, scaled by |target|/|source|. + /// Source or target vector has zero length. + /// + /// Two-point alignment is geometrically under-constrained in 3D (rotation around the source axis is free); + /// this method picks the shortest-arc rotation, (for opposite vectors a + /// half-turn about the axis FromTo picks). + /// Use cases include adapting taught polyline paths whose endpoints have shifted. + /// + public static Matrix3x3 SimilarityFromVectors(Vector3 source, Vector3 target) + { + var sourceLength = source.Length; + if (sourceLength == T.Zero) + throw new ArgumentException("Source vector must be non-zero.", nameof(source)); + + var scale = target.Length / sourceLength; + return scale * Quaternion.FromTo(source, target).ToMatrix3x3(); + } + + /// Creates a matrix from three column vectors. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 FromColumns(Vector3 c0, Vector3 c1, Vector3 c2) => new( + c0.X, c1.X, c2.X, + c0.Y, c1.Y, c2.Y, + c0.Z, c1.Z, c2.Z); + + /// Creates a matrix from three row vectors. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 FromRows(Vector3 r0, Vector3 r1, Vector3 r2) => new( + r0.X, r0.Y, r0.Z, + r1.X, r1.Y, r1.Z, + r2.X, r2.Y, r2.Z); + + #endregion + + #region Euler Conversion + + /// + /// Extracts ZYX Euler angles (in degrees) from this rotation matrix, assuming R = Rz(rz) * Ry(ry) * Rx(rx). + /// The matrix goes through and the one rotation → Euler formula, + /// (gimbal lock: rz = 0). + /// + public (Degree Rx, Degree Ry, Degree Rz) ToEulerZYX() + { + var r = Rotation3.FromQuaternion(Quaternion.FromMatrix3x3(this)); + return (r.Rx, r.Ry, r.Rz); + } + + #endregion + + public override string ToString() => + $"[{M11}, {M12}, {M13}; {M21}, {M22}, {M23}; {M31}, {M32}, {M33}]"; +} diff --git a/Sources/ModelingEvolution.Drawing/Polyline3.cs b/Sources/ModelingEvolution.Drawing/Polyline3.cs index 44a26e3..d3bd7b2 100644 --- a/Sources/ModelingEvolution.Drawing/Polyline3.cs +++ b/Sources/ModelingEvolution.Drawing/Polyline3.cs @@ -1,423 +1,423 @@ -using System.Numerics; -using System.Runtime.InteropServices; -using System.Text.Json.Serialization; -using ProtoBuf; - -namespace ModelingEvolution.Drawing; - -/// -/// Represents an immutable open polyline defined by a sequence of 3D points. -/// Unlike Polygon, a polyline does not close — Edges() yields n-1 segments with no wrap-around. -/// -/// The numeric type used for coordinates. -[JsonConverter(typeof(Polyline3JsonConverterFactory))] -[ProtoContract] -public readonly record struct Polyline3 : IPoolable, Lease>> - where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, - IFloatingPointIeee754, IMinMaxValue, IParsable -{ - internal readonly ReadOnlyMemory> _points; - - [ProtoMember(1)] - private Point3[] ProtoPoints - { - get - { - if (_points.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_points, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _points.ToArray(); - } - init => _points = value ?? ReadOnlyMemory>.Empty; - } - - /// - /// Gets the number of vertices in this polyline. - /// - [JsonIgnore] - public int Count => _points.Length; - - /// - /// Returns a read-only span over the polyline's vertices. - /// Hoist the result before loops — this is a method call, not a field access. - /// - public ReadOnlySpan> AsSpan() => _points.Span; - - /// - /// Gets the vertex at the specified index. - /// - public Point3 this[int index] => _points.Span[index]; - - /// - /// Gets a read-only list of all vertices. For high-performance code, prefer AsSpan() instead. - /// - public IReadOnlyList> Points - { - get - { - if (_points.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_points, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _points.ToArray(); - } - } - - /// - /// Gets the first vertex of the polyline. - /// - [JsonIgnore] - public Point3 Start => _points.Span[0]; - - /// - /// Gets the last vertex of the polyline. - /// - [JsonIgnore] - public Point3 End => _points.Span[^1]; - - #region Constructors - - /// - /// Initializes a new empty polyline. - /// - public Polyline3() - { - _points = ReadOnlyMemory>.Empty; - } - - /// - /// Initializes a new polyline wrapping a ReadOnlyMemory of points. Zero-copy. - /// - public Polyline3(ReadOnlyMemory> memory) - { - _points = memory; - } - - /// - /// Initializes a new polyline from the specified points array. - /// - public Polyline3(params Point3[] points) - { - _points = points; - } - - /// - /// Initializes a new polyline from the specified list of points (copies to array). - /// - public Polyline3(IList> points) - { - if (points is Point3[] arr) - _points = arr; - else - { - var tmp = new Point3[points.Count]; - for (int i = 0; i < points.Count; i++) - tmp[i] = points[i]; - _points = tmp; - } - } - - /// - /// Initializes a new polyline from a flat list of coordinate values (x1, y1, z1, x2, y2, z2, ...). - /// - public Polyline3(IReadOnlyList coords) - { - var tmp = new Point3[coords.Count / 3]; - for (int i = 0; i < coords.Count; i += 3) - tmp[i / 3] = new Point3(coords[i], coords[i + 1], coords[i + 2]); - _points = tmp; - } - - #endregion - - #region Geometry - - /// - /// Computes the total length of this polyline (sum of all edge lengths). - /// - public T Length() - { - var span = AsSpan(); - int n = span.Length; - if (n < 2) return T.Zero; - - T length = T.Zero; - for (int i = 0; i < n - 1; i++) - length += Point3.Distance(span[i], span[i + 1]); - return length; - } - - /// - /// Computes the centroid (center of mass) of this polyline, weighted by segment length. - /// Each segment contributes its midpoint weighted by its length. - /// - public Point3 Centroid() - { - var span = AsSpan(); - if (span.Length == 0) return Point3.Zero; - if (span.Length == 1) return span[0]; - - T totalLength = T.Zero; - T cx = T.Zero; - T cy = T.Zero; - T cz = T.Zero; - - for (int i = 0; i < span.Length - 1; i++) - { - var segLen = Point3.Distance(span[i], span[i + 1]); - var two = T.CreateTruncating(2); - cx += (span[i].X + span[i + 1].X) / two * segLen; - cy += (span[i].Y + span[i + 1].Y) / two * segLen; - cz += (span[i].Z + span[i + 1].Z) / two * segLen; - totalLength += segLen; - } - - if (totalLength == T.Zero) return span[0]; - - return new Point3(cx / totalLength, cy / totalLength, cz / totalLength); - } - - /// - /// Returns a new polyline with the vertex order reversed. - /// - public Polyline3 Reverse() - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var reversed = mem.Span; - for (int i = 0; i < span.Length; i++) - reversed[i] = span[span.Length - 1 - i]; - return new Polyline3(mem); - } - - /// - /// Returns the edges of this polyline as 3D segments. Open — yields n-1 segments with no wrap-around. - /// - public ReadOnlyMemory> Edges() - { - var span = AsSpan(); - int n = span.Length; - if (n < 2) return ReadOnlyMemory>.Empty; - var mem = Alloc.Memory>(n - 1); - var edges = mem.Span; - for (int i = 0; i < n - 1; i++) - edges[i] = new Segment3(span[i], span[i + 1]); - return mem; - } - - /// - /// Returns a new polyline scaled by the given factor around the centroid. - /// - public Polyline3 Scale(T factor) - { - var span = AsSpan(); - if (span.Length == 0) return this; - - var c = Centroid(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - { - points[i] = new Point3( - c.X + (span[i].X - c.X) * factor, - c.Y + (span[i].Y - c.Y) * factor, - c.Z + (span[i].Z - c.Z) * factor); - } - return new Polyline3(mem); - } - - /// - /// Simplifies this polyline using the Ramer-Douglas-Peucker algorithm in 3D. - /// Removes points closer than to the line between their neighbors. - /// - public Polyline3 Simplify(T epsilon) - { - var span = AsSpan(); - if (span.Length < 3) return this; - - var epsSq = epsilon * epsilon; - Span keep = span.Length <= 1024 ? stackalloc bool[span.Length] : new bool[span.Length]; - keep[0] = true; - keep[span.Length - 1] = true; - - // Iterative stack-based RDP - int stackSize = span.Length * 2; - Span stack = stackSize <= 1024 ? stackalloc int[stackSize] : new int[stackSize]; - int top = 0; - stack[top++] = 0; - stack[top++] = span.Length - 1; - - while (top >= 2) - { - int end = stack[--top]; - int start = stack[--top]; - - var maxDistSq = T.Zero; - int maxIdx = start; - - var a = span[start]; - var b = span[end]; - var ab = (Vector3)(b - a); - var abLenSq = ab.LengthSquared; - - for (int i = start + 1; i < end; i++) - { - T distSq; - if (abLenSq < T.CreateTruncating(1e-18)) - { - // Degenerate segment — distance to start point - distSq = Point3.DistanceSquared(span[i], a); - } - else - { - // Distance from point to line via cross product: |AP x AB|^2 / |AB|^2 - var ap = (Vector3)(span[i] - a); - var cross = Vector3.Cross(ap, ab); - distSq = cross.LengthSquared / abLenSq; - } - - if (distSq > maxDistSq) - { - maxDistSq = distSq; - maxIdx = i; - } - } - - if (maxDistSq > epsSq) - { - keep[maxIdx] = true; - stack[top++] = start; - stack[top++] = maxIdx; - stack[top++] = maxIdx; - stack[top++] = end; - } - } - - int count = 0; - for (int i = 0; i < span.Length; i++) - if (keep[i]) count++; - - if (count == span.Length) return this; - - var mem = Alloc.Memory>(count); - var dst = mem.Span; - int idx = 0; - for (int i = 0; i < span.Length; i++) - if (keep[i]) dst[idx++] = span[i]; - - return new Polyline3(mem); - } - - /// - /// Returns a new polyline with all points transformed by the given frame (rotation + translation). - /// - public Polyline3 Transform(in Frame3 frame) - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - points[i] = frame.TransformPoint(span[i]); - return new Polyline3(mem); - } - - #endregion - - #region Operators - - /// - /// Translates all points by adding the given vector. - /// - public static Polyline3 operator +(in Polyline3 a, in Vector3 f) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - points[i] = span[i] + f; - return new Polyline3(mem); - } - - /// - /// Translates all points by subtracting the given vector. - /// - public static Polyline3 operator -(in Polyline3 a, in Vector3 f) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - points[i] = span[i] - f; - return new Polyline3(mem); - } - - /// - /// Scales all points by a uniform scalar. - /// - public static Polyline3 operator *(in Polyline3 a, T scalar) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - points[i] = span[i] * scalar; - return new Polyline3(mem); - } - - /// - /// Divides all points by a uniform scalar. - /// - public static Polyline3 operator /(in Polyline3 a, T scalar) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var points = mem.Span; - for (int i = 0; i < span.Length; i++) - points[i] = span[i] / scalar; - return new Polyline3(mem); - } - - #endregion - - #region Equality - - /// - public bool Equals(Polyline3 other) => AsSpan().SequenceEqual(other.AsSpan()); - - /// - public override int GetHashCode() - { - var hash = new HashCode(); - var span = AsSpan(); - for (int i = 0; i < span.Length; i++) - hash.Add(span[i]); - return hash.ToHashCode(); - } - - #endregion - - /// - /// Detaches this polyline's backing memory from the given scope and returns a lease. - /// The caller becomes responsible for disposing the lease to return memory to the pool. - /// - public Lease> DetachFrom(AllocationScope scope) - { - if (!MemoryMarshal.TryGetArray(_points, out var seg)) - throw new InvalidOperationException("Cannot detach non-array-backed memory."); - var owner = scope.UntrackMemory(new Memory>(seg.Array!, seg.Offset, seg.Count)); - return new Lease> { _owner = owner }; - } - - internal Point3[] AsArray() - { - if (_points.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_points, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _points.ToArray(); - } - - /// - public override string ToString() => $"Polyline3({Count} points)"; -} +using System.Numerics; +using System.Runtime.InteropServices; +using System.Text.Json.Serialization; +using ProtoBuf; + +namespace ModelingEvolution.Drawing; + +/// +/// Represents an immutable open polyline defined by a sequence of 3D points. +/// Unlike Polygon, a polyline does not close — Edges() yields n-1 segments with no wrap-around. +/// +/// The numeric type used for coordinates. +[JsonConverter(typeof(Polyline3JsonConverterFactory))] +[ProtoContract] +public readonly record struct Polyline3 : IPoolable, Lease>> + where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, + IFloatingPointIeee754, IMinMaxValue, IParsable +{ + internal readonly ReadOnlyMemory> _points; + + [ProtoMember(1)] + private Point3[] ProtoPoints + { + get + { + if (_points.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_points, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _points.ToArray(); + } + init => _points = value ?? ReadOnlyMemory>.Empty; + } + + /// + /// Gets the number of vertices in this polyline. + /// + [JsonIgnore] + public int Count => _points.Length; + + /// + /// Returns a read-only span over the polyline's vertices. + /// Hoist the result before loops — this is a method call, not a field access. + /// + public ReadOnlySpan> AsSpan() => _points.Span; + + /// + /// Gets the vertex at the specified index. + /// + public Point3 this[int index] => _points.Span[index]; + + /// + /// Gets a read-only list of all vertices. For high-performance code, prefer AsSpan() instead. + /// + public IReadOnlyList> Points + { + get + { + if (_points.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_points, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _points.ToArray(); + } + } + + /// + /// Gets the first vertex of the polyline. + /// + [JsonIgnore] + public Point3 Start => _points.Span[0]; + + /// + /// Gets the last vertex of the polyline. + /// + [JsonIgnore] + public Point3 End => _points.Span[^1]; + + #region Constructors + + /// + /// Initializes a new empty polyline. + /// + public Polyline3() + { + _points = ReadOnlyMemory>.Empty; + } + + /// + /// Initializes a new polyline wrapping a ReadOnlyMemory of points. Zero-copy. + /// + public Polyline3(ReadOnlyMemory> memory) + { + _points = memory; + } + + /// + /// Initializes a new polyline from the specified points array. + /// + public Polyline3(params Point3[] points) + { + _points = points; + } + + /// + /// Initializes a new polyline from the specified list of points (copies to array). + /// + public Polyline3(IList> points) + { + if (points is Point3[] arr) + _points = arr; + else + { + var tmp = new Point3[points.Count]; + for (int i = 0; i < points.Count; i++) + tmp[i] = points[i]; + _points = tmp; + } + } + + /// + /// Initializes a new polyline from a flat list of coordinate values (x1, y1, z1, x2, y2, z2, ...). + /// + public Polyline3(IReadOnlyList coords) + { + var tmp = new Point3[coords.Count / 3]; + for (int i = 0; i < coords.Count; i += 3) + tmp[i / 3] = new Point3(coords[i], coords[i + 1], coords[i + 2]); + _points = tmp; + } + + #endregion + + #region Geometry + + /// + /// Computes the total length of this polyline (sum of all edge lengths). + /// + public T Length() + { + var span = AsSpan(); + int n = span.Length; + if (n < 2) return T.Zero; + + T length = T.Zero; + for (int i = 0; i < n - 1; i++) + length += Point3.Distance(span[i], span[i + 1]); + return length; + } + + /// + /// Computes the centroid (center of mass) of this polyline, weighted by segment length. + /// Each segment contributes its midpoint weighted by its length. + /// + public Point3 Centroid() + { + var span = AsSpan(); + if (span.Length == 0) return Point3.Zero; + if (span.Length == 1) return span[0]; + + T totalLength = T.Zero; + T cx = T.Zero; + T cy = T.Zero; + T cz = T.Zero; + + for (int i = 0; i < span.Length - 1; i++) + { + var segLen = Point3.Distance(span[i], span[i + 1]); + var two = T.CreateTruncating(2); + cx += (span[i].X + span[i + 1].X) / two * segLen; + cy += (span[i].Y + span[i + 1].Y) / two * segLen; + cz += (span[i].Z + span[i + 1].Z) / two * segLen; + totalLength += segLen; + } + + if (totalLength == T.Zero) return span[0]; + + return new Point3(cx / totalLength, cy / totalLength, cz / totalLength); + } + + /// + /// Returns a new polyline with the vertex order reversed. + /// + public Polyline3 Reverse() + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var reversed = mem.Span; + for (int i = 0; i < span.Length; i++) + reversed[i] = span[span.Length - 1 - i]; + return new Polyline3(mem); + } + + /// + /// Returns the edges of this polyline as 3D segments. Open — yields n-1 segments with no wrap-around. + /// + public ReadOnlyMemory> Edges() + { + var span = AsSpan(); + int n = span.Length; + if (n < 2) return ReadOnlyMemory>.Empty; + var mem = Alloc.Memory>(n - 1); + var edges = mem.Span; + for (int i = 0; i < n - 1; i++) + edges[i] = new Segment3(span[i], span[i + 1]); + return mem; + } + + /// + /// Returns a new polyline scaled by the given factor around the centroid. + /// + public Polyline3 Scale(T factor) + { + var span = AsSpan(); + if (span.Length == 0) return this; + + var c = Centroid(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + { + points[i] = new Point3( + c.X + (span[i].X - c.X) * factor, + c.Y + (span[i].Y - c.Y) * factor, + c.Z + (span[i].Z - c.Z) * factor); + } + return new Polyline3(mem); + } + + /// + /// Simplifies this polyline using the Ramer-Douglas-Peucker algorithm in 3D. + /// Removes points closer than to the line between their neighbors. + /// + public Polyline3 Simplify(T epsilon) + { + var span = AsSpan(); + if (span.Length < 3) return this; + + var epsSq = epsilon * epsilon; + Span keep = span.Length <= 1024 ? stackalloc bool[span.Length] : new bool[span.Length]; + keep[0] = true; + keep[span.Length - 1] = true; + + // Iterative stack-based RDP + int stackSize = span.Length * 2; + Span stack = stackSize <= 1024 ? stackalloc int[stackSize] : new int[stackSize]; + int top = 0; + stack[top++] = 0; + stack[top++] = span.Length - 1; + + while (top >= 2) + { + int end = stack[--top]; + int start = stack[--top]; + + var maxDistSq = T.Zero; + int maxIdx = start; + + var a = span[start]; + var b = span[end]; + var ab = (Vector3)(b - a); + var abLenSq = ab.LengthSquared; + + for (int i = start + 1; i < end; i++) + { + T distSq; + if (abLenSq < T.CreateTruncating(1e-18)) + { + // Degenerate segment — distance to start point + distSq = Point3.DistanceSquared(span[i], a); + } + else + { + // Distance from point to line via cross product: |AP x AB|^2 / |AB|^2 + var ap = (Vector3)(span[i] - a); + var cross = Vector3.Cross(ap, ab); + distSq = cross.LengthSquared / abLenSq; + } + + if (distSq > maxDistSq) + { + maxDistSq = distSq; + maxIdx = i; + } + } + + if (maxDistSq > epsSq) + { + keep[maxIdx] = true; + stack[top++] = start; + stack[top++] = maxIdx; + stack[top++] = maxIdx; + stack[top++] = end; + } + } + + int count = 0; + for (int i = 0; i < span.Length; i++) + if (keep[i]) count++; + + if (count == span.Length) return this; + + var mem = Alloc.Memory>(count); + var dst = mem.Span; + int idx = 0; + for (int i = 0; i < span.Length; i++) + if (keep[i]) dst[idx++] = span[i]; + + return new Polyline3(mem); + } + + /// + /// Returns a new polyline with all points transformed by the given frame (rotation + translation). + /// + public Polyline3 Transform(in Frame3 frame) + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + points[i] = frame.TransformPoint(span[i]); + return new Polyline3(mem); + } + + #endregion + + #region Operators + + /// + /// Translates all points by adding the given vector. + /// + public static Polyline3 operator +(in Polyline3 a, in Vector3 f) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + points[i] = span[i] + f; + return new Polyline3(mem); + } + + /// + /// Translates all points by subtracting the given vector. + /// + public static Polyline3 operator -(in Polyline3 a, in Vector3 f) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + points[i] = span[i] - f; + return new Polyline3(mem); + } + + /// + /// Scales all points by a uniform scalar. + /// + public static Polyline3 operator *(in Polyline3 a, T scalar) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + points[i] = span[i] * scalar; + return new Polyline3(mem); + } + + /// + /// Divides all points by a uniform scalar. + /// + public static Polyline3 operator /(in Polyline3 a, T scalar) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var points = mem.Span; + for (int i = 0; i < span.Length; i++) + points[i] = span[i] / scalar; + return new Polyline3(mem); + } + + #endregion + + #region Equality + + /// + public bool Equals(Polyline3 other) => AsSpan().SequenceEqual(other.AsSpan()); + + /// + public override int GetHashCode() + { + var hash = new HashCode(); + var span = AsSpan(); + for (int i = 0; i < span.Length; i++) + hash.Add(span[i]); + return hash.ToHashCode(); + } + + #endregion + + /// + /// Detaches this polyline's backing memory from the given scope and returns a lease. + /// The caller becomes responsible for disposing the lease to return memory to the pool. + /// + public Lease> DetachFrom(AllocationScope scope) + { + if (!MemoryMarshal.TryGetArray(_points, out var seg)) + throw new InvalidOperationException("Cannot detach non-array-backed memory."); + var owner = scope.UntrackMemory(new Memory>(seg.Array!, seg.Offset, seg.Count)); + return new Lease> { _owner = owner }; + } + + internal Point3[] AsArray() + { + if (_points.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_points, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _points.ToArray(); + } + + /// + public override string ToString() => $"Polyline3({Count} points)"; +} diff --git a/Sources/ModelingEvolution.Drawing/PosePath3.cs b/Sources/ModelingEvolution.Drawing/PosePath3.cs index 0cee618..c78a703 100644 --- a/Sources/ModelingEvolution.Drawing/PosePath3.cs +++ b/Sources/ModelingEvolution.Drawing/PosePath3.cs @@ -1,262 +1,262 @@ -using System.Numerics; -using System.Runtime.InteropServices; -using System.Text.Json.Serialization; -using ProtoBuf; - -namespace ModelingEvolution.Drawing; - -/// -/// Represents an immutable open path defined by a sequence of 3D poses (position + orientation). -/// This is the geometric counterpart — no time information. For time-parameterized paths, use . -/// -/// The numeric type used for coordinates. -[JsonConverter(typeof(PosePath3JsonConverterFactory))] -[ProtoContract] -public readonly record struct PosePath3 : IPoolable, Lease>> - where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, - IFloatingPointIeee754, IMinMaxValue, IParsable -{ - internal readonly ReadOnlyMemory> _poses; - - [ProtoMember(1)] - private Pose3[] ProtoPoses - { - get - { - if (_poses.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_poses, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _poses.ToArray(); - } - init => _poses = value ?? ReadOnlyMemory>.Empty; - } - - /// - /// Gets the number of poses in this path. - /// - [JsonIgnore] - public int Count => _poses.Length; - - /// - /// Returns a read-only span over the path's poses. - /// Hoist the result before loops — this is a method call, not a field access. - /// - public ReadOnlySpan> AsSpan() => _poses.Span; - - /// - /// Gets the pose at the specified index. - /// - public Pose3 this[int index] => _poses.Span[index]; - - /// - /// Gets a read-only list of all poses. For high-performance code, prefer AsSpan() instead. - /// - public IReadOnlyList> Poses - { - get - { - if (_poses.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_poses, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _poses.ToArray(); - } - } - - /// - /// Gets the first pose of the path. - /// - [JsonIgnore] - public Pose3 Start => _poses.Span[0]; - - /// - /// Gets the last pose of the path. - /// - [JsonIgnore] - public Pose3 End => _poses.Span[^1]; - - #region Constructors - - /// - /// Initializes a new empty path. - /// - public PosePath3() - { - _poses = ReadOnlyMemory>.Empty; - } - - /// - /// Initializes a new path wrapping a ReadOnlyMemory of poses. Zero-copy. - /// - public PosePath3(ReadOnlyMemory> memory) - { - _poses = memory; - } - - /// - /// Initializes a new path from the specified poses array. - /// - public PosePath3(params Pose3[] poses) - { - _poses = poses; - } - - /// - /// Initializes a new path from the specified list of poses (copies to array). - /// - public PosePath3(IList> poses) - { - if (poses is Pose3[] arr) - _poses = arr; - else - { - var tmp = new Pose3[poses.Count]; - for (int i = 0; i < poses.Count; i++) - tmp[i] = poses[i]; - _poses = tmp; - } - } - - #endregion - - #region Geometry - - /// - /// Computes the total positional length of this path (sum of distances between consecutive positions). - /// - public T Length() - { - var span = AsSpan(); - int n = span.Length; - if (n < 2) return T.Zero; - - T length = T.Zero; - for (int i = 0; i < n - 1; i++) - length += Point3.Distance(span[i].Position, span[i + 1].Position); - return length; - } - - /// - /// Returns a new path with the pose order reversed. - /// - public PosePath3 Reverse() - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var reversed = mem.Span; - for (int i = 0; i < span.Length; i++) - reversed[i] = span[span.Length - 1 - i]; - return new PosePath3(mem); - } - - /// - /// Returns a new path with every pose composed as frame · pose. - /// The elements are stored as wire ; each is converted with lengthScale 1. - /// - public PosePath3 Transform(in Frame3 frame) - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = (frame * Frame3.FromPose3(span[i], T.One)).ToPose3(T.One); - return new PosePath3(mem); - } - - /// - /// Extracts just the positions as a . - /// - public Polyline3 ToPolyline3() - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = span[i].Position; - return new Polyline3(mem); - } - - /// - /// Creates a by assigning uniform timestamps from 0 to . - /// - public Trajectory3 ToTrajectory(T totalTime) - { - var span = AsSpan(); - if (span.Length == 0) return new Trajectory3(); - if (span.Length == 1) return new Trajectory3(new Waypoint3(span[0], T.Zero)); - - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - var n = T.CreateTruncating(span.Length - 1); - for (int i = 0; i < span.Length; i++) - { - var t = totalTime * T.CreateTruncating(i) / n; - dst[i] = new Waypoint3(span[i], t); - } - return new Trajectory3(mem); - } - - #endregion - - #region Operators - - /// - /// Translates all positions by adding the given vector (rotations unchanged). - /// - public static PosePath3 operator +(in PosePath3 a, in Vector3 f) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = span[i] with { Position = span[i].Position + f }; - return new PosePath3(mem); - } - - /// - /// Translates all positions by subtracting the given vector (rotations unchanged). - /// - public static PosePath3 operator -(in PosePath3 a, in Vector3 f) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = span[i] with { Position = span[i].Position - f }; - return new PosePath3(mem); - } - - #endregion - - #region Equality - - /// - public bool Equals(PosePath3 other) => AsSpan().SequenceEqual(other.AsSpan()); - - /// - public override int GetHashCode() - { - var hash = new HashCode(); - var span = AsSpan(); - for (int i = 0; i < span.Length; i++) - hash.Add(span[i]); - return hash.ToHashCode(); - } - - #endregion - - /// - /// Detaches this path's backing memory from the given scope and returns a lease. - /// - public Lease> DetachFrom(AllocationScope scope) - { - if (!MemoryMarshal.TryGetArray(_poses, out var seg)) - throw new InvalidOperationException("Cannot detach non-array-backed memory."); - var owner = scope.UntrackMemory(new Memory>(seg.Array!, seg.Offset, seg.Count)); - return new Lease> { _owner = owner }; - } - - /// - public override string ToString() => $"PosePath3({Count} poses)"; -} +using System.Numerics; +using System.Runtime.InteropServices; +using System.Text.Json.Serialization; +using ProtoBuf; + +namespace ModelingEvolution.Drawing; + +/// +/// Represents an immutable open path defined by a sequence of 3D poses (position + orientation). +/// This is the geometric counterpart — no time information. For time-parameterized paths, use . +/// +/// The numeric type used for coordinates. +[JsonConverter(typeof(PosePath3JsonConverterFactory))] +[ProtoContract] +public readonly record struct PosePath3 : IPoolable, Lease>> + where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, + IFloatingPointIeee754, IMinMaxValue, IParsable +{ + internal readonly ReadOnlyMemory> _poses; + + [ProtoMember(1)] + private Pose3[] ProtoPoses + { + get + { + if (_poses.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_poses, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _poses.ToArray(); + } + init => _poses = value ?? ReadOnlyMemory>.Empty; + } + + /// + /// Gets the number of poses in this path. + /// + [JsonIgnore] + public int Count => _poses.Length; + + /// + /// Returns a read-only span over the path's poses. + /// Hoist the result before loops — this is a method call, not a field access. + /// + public ReadOnlySpan> AsSpan() => _poses.Span; + + /// + /// Gets the pose at the specified index. + /// + public Pose3 this[int index] => _poses.Span[index]; + + /// + /// Gets a read-only list of all poses. For high-performance code, prefer AsSpan() instead. + /// + public IReadOnlyList> Poses + { + get + { + if (_poses.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_poses, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _poses.ToArray(); + } + } + + /// + /// Gets the first pose of the path. + /// + [JsonIgnore] + public Pose3 Start => _poses.Span[0]; + + /// + /// Gets the last pose of the path. + /// + [JsonIgnore] + public Pose3 End => _poses.Span[^1]; + + #region Constructors + + /// + /// Initializes a new empty path. + /// + public PosePath3() + { + _poses = ReadOnlyMemory>.Empty; + } + + /// + /// Initializes a new path wrapping a ReadOnlyMemory of poses. Zero-copy. + /// + public PosePath3(ReadOnlyMemory> memory) + { + _poses = memory; + } + + /// + /// Initializes a new path from the specified poses array. + /// + public PosePath3(params Pose3[] poses) + { + _poses = poses; + } + + /// + /// Initializes a new path from the specified list of poses (copies to array). + /// + public PosePath3(IList> poses) + { + if (poses is Pose3[] arr) + _poses = arr; + else + { + var tmp = new Pose3[poses.Count]; + for (int i = 0; i < poses.Count; i++) + tmp[i] = poses[i]; + _poses = tmp; + } + } + + #endregion + + #region Geometry + + /// + /// Computes the total positional length of this path (sum of distances between consecutive positions). + /// + public T Length() + { + var span = AsSpan(); + int n = span.Length; + if (n < 2) return T.Zero; + + T length = T.Zero; + for (int i = 0; i < n - 1; i++) + length += Point3.Distance(span[i].Position, span[i + 1].Position); + return length; + } + + /// + /// Returns a new path with the pose order reversed. + /// + public PosePath3 Reverse() + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var reversed = mem.Span; + for (int i = 0; i < span.Length; i++) + reversed[i] = span[span.Length - 1 - i]; + return new PosePath3(mem); + } + + /// + /// Returns a new path with every pose composed as frame · pose. + /// The elements are stored as wire ; each is converted with lengthScale 1. + /// + public PosePath3 Transform(in Frame3 frame) + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = (frame * Frame3.FromPose3(span[i], T.One)).ToPose3(T.One); + return new PosePath3(mem); + } + + /// + /// Extracts just the positions as a . + /// + public Polyline3 ToPolyline3() + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = span[i].Position; + return new Polyline3(mem); + } + + /// + /// Creates a by assigning uniform timestamps from 0 to . + /// + public Trajectory3 ToTrajectory(T totalTime) + { + var span = AsSpan(); + if (span.Length == 0) return new Trajectory3(); + if (span.Length == 1) return new Trajectory3(new Waypoint3(span[0], T.Zero)); + + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + var n = T.CreateTruncating(span.Length - 1); + for (int i = 0; i < span.Length; i++) + { + var t = totalTime * T.CreateTruncating(i) / n; + dst[i] = new Waypoint3(span[i], t); + } + return new Trajectory3(mem); + } + + #endregion + + #region Operators + + /// + /// Translates all positions by adding the given vector (rotations unchanged). + /// + public static PosePath3 operator +(in PosePath3 a, in Vector3 f) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = span[i] with { Position = span[i].Position + f }; + return new PosePath3(mem); + } + + /// + /// Translates all positions by subtracting the given vector (rotations unchanged). + /// + public static PosePath3 operator -(in PosePath3 a, in Vector3 f) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = span[i] with { Position = span[i].Position - f }; + return new PosePath3(mem); + } + + #endregion + + #region Equality + + /// + public bool Equals(PosePath3 other) => AsSpan().SequenceEqual(other.AsSpan()); + + /// + public override int GetHashCode() + { + var hash = new HashCode(); + var span = AsSpan(); + for (int i = 0; i < span.Length; i++) + hash.Add(span[i]); + return hash.ToHashCode(); + } + + #endregion + + /// + /// Detaches this path's backing memory from the given scope and returns a lease. + /// + public Lease> DetachFrom(AllocationScope scope) + { + if (!MemoryMarshal.TryGetArray(_poses, out var seg)) + throw new InvalidOperationException("Cannot detach non-array-backed memory."); + var owner = scope.UntrackMemory(new Memory>(seg.Array!, seg.Offset, seg.Count)); + return new Lease> { _owner = owner }; + } + + /// + public override string ToString() => $"PosePath3({Count} poses)"; +} diff --git a/Sources/ModelingEvolution.Drawing/README.md b/Sources/ModelingEvolution.Drawing/README.md index ae354d8..e801758 100644 --- a/Sources/ModelingEvolution.Drawing/README.md +++ b/Sources/ModelingEvolution.Drawing/README.md @@ -1,820 +1,820 @@ -# ModelingEvolution.Drawing - -[![NuGet](https://img.shields.io/nuget/v/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) -[![NuGet Downloads](https://img.shields.io/nuget/dt/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) -[![CI](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml/badge.svg)](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml) -[![License](https://img.shields.io/github/license/modelingevolution/drawing.svg)](https://github.com/modelingevolution/drawing/blob/main/LICENSE) - -A high-performance .NET generic math library for 2D and 3D geometry, coordinate systems, shapes, intersections, trajectories, and drawing primitives. Built on `INumber` constraints from .NET generic math, all types work with `float`, `double`, `decimal`, or any compatible numeric type -- with zero boxing and full type safety. - -## Table of Contents - -- [Why This Library](#why-this-library) -- [Installation](#installation) -- [Quick Start](#quick-start) -- [API Reference](#api-reference) - - [2D Core Types](#2d-core-types) - - [3D Core Types](#3d-core-types) - - [Shapes (2D)](#shapes-2d) - - [Shapes (3D)](#shapes-3d) - - [Lines and Segments](#lines-and-segments) - - [Curves and Paths (2D)](#curves-and-paths-2d) - - [Polylines (2D and 3D)](#polylines-2d-and-3d) - - [Angles and Coordinate Systems](#angles-and-coordinate-systems) - - [Rotations and Orientation](#rotations-and-orientation) - - [Poses and Trajectories](#poses-and-trajectories) - - [Kinematics](#kinematics) - - [Equations](#equations) - - [Colors](#colors) - - [Data Structures and Algorithms](#data-structures-and-algorithms) - - [Interfaces](#interfaces) -- [Key Features](#key-features) - - [Generic Math](#generic-math) - - [IShape Interface Hierarchy](#ishape-interface-hierarchy) - - [Intersections](#intersections) - - [Polygon Boolean Operations](#polygon-boolean-operations) - - [Bezier Path Clipping](#bezier-path-clipping) - - [Pooled Allocation Scopes](#pooled-allocation-scopes) - - [Serialization](#serialization) - - [SVG Export](#svg-export) -- [Code Examples](#code-examples) - - [Points: Distance, Lerp, Rotate](#points-distance-lerp-rotate) - - [3D Points, Vectors, and Rotations](#3d-points-vectors-and-rotations) - - [Spherical Coordinates](#spherical-coordinates) - - [Poses and Trajectories](#poses-and-trajectories-1) - - [Shapes: Area and Perimeter](#shapes-area-and-perimeter) - - [Line-Circle Intersection](#line-circle-intersection) - - [Segment-Rectangle Clipping](#segment-rectangle-clipping) - - [Path-Rectangle Clipping](#path-rectangle-clipping) - - [Triangle Properties](#triangle-properties) - - [Polygon Operations](#polygon-operations) - - [Matrix Transformations (2D and 3D)](#matrix-transformations-2d-and-3d) -- [Requirements](#requirements) -- [License](#license) - -## Why This Library - -- **Generic math** -- write geometry code once; use it with `float`, `double`, or any `IFloatingPointIeee754` type. -- **No boxing** -- all core types are `struct` or `readonly record struct`, avoiding heap allocations. -- **Full 2D and 3D** -- from `Point` and `Vector` to `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Quaternion`, and `Trajectory3`. -- **Rich coordinate systems** -- Cartesian, polar, cylindrical, and spherical with implicit/explicit conversions between them. -- **Rich interface hierarchy** -- `IShape` unifies area, perimeter, centroid, bounding box, containment, rotation, and scaling across all shape types. -- **Comprehensive intersections** -- 21 intersection combinations (line, segment, circle, triangle, rectangle, polygon) with zero-alloc `FirstOf` variants. -- **Polygon booleans** -- union, intersection, difference, and clustering via Clipper2. -- **Exact Bezier clipping** -- `Path.Intersect(Rectangle)` computes analytically exact sub-curves, not polygon approximations. -- **Trajectory planning** -- `Trajectory3` with time-parameterized waypoints, interpolation, resampling, and a real-time `TrajectoryController`. -- **Robotics-ready** -- `Joints6` for 6-DOF joint representations, `Frame3` (position + unit quaternion) for all pose maths, `Frame3.FromSurface()` for plane-based frame construction, and `KinematicChain` for forward kinematics and geometric Jacobians of serial and tree chains. -- **Pooled allocations** -- `AllocationScope` provides arena-style pooled memory for zero-GC geometry pipelines. -- **Serialization built in** -- Protobuf (protobuf-net) attributes on all core types, plus custom JSON converters and SVG exporters. - -## Installation - -```bash -dotnet add package ModelingEvolution.Drawing -``` - -Or via the NuGet Package Manager: - -``` -Install-Package ModelingEvolution.Drawing -``` - -## Quick Start - -```csharp -using ModelingEvolution.Drawing; - -// 2D points and vectors (using double precision) -var a = new Point(1.0, 2.0); -var b = new Point(4.0, 6.0); - -double distance = a.DistanceTo(b); // 5.0 -Point mid = a.Lerp(b, 0.5); // {X=2.5, Y=4} - -// 3D points and rotations -var p = new Point3(1.0, 2.0, 3.0); -var rotation = new Rotation3(45.0, 0.0, 0.0); // 45 deg roll (wire/edit format) -var rotated = new Frame3(Point3.Zero, rotation.ToQuaternion()).TransformPoint(p); - -// Spherical coordinates -var orbit = SphericalPoint.OnEquator(5.0, Degree.Create(30)); -Point3 cartesian = orbit; // implicit conversion - -// Shapes with area and perimeter -var circle = new Circle(Point.Zero, 10.0); -double area = circle.Area(); // ~314.159 -bool inside = circle.Contains(new Point(3.0, 4.0)); // true - -// Polygon boolean operations -var poly1 = new Polygon( - new Point(0, 0), new Point(10, 0), - new Point(10, 10), new Point(0, 10)); -var poly2 = new Polygon( - new Point(5, 5), new Point(15, 5), - new Point(15, 15), new Point(5, 15)); - -Polygon merged = poly1 | poly2; // union -Polygon overlap = poly1 & poly2; // intersection - -// 3D trajectory with time-parameterized waypoints -var trajectory = new Trajectory3( - new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), - new Waypoint3(new Pose3(100, 0, 50, 0, 45, 0), 5.0)); -Pose3 atHalf = trajectory.AtTime(2.5); // interpolated pose -``` - -## API Reference - -### 2D Core Types - -| Type | Description | -|---|---| -| `Point` | 2D Cartesian point. Supports arithmetic with vectors and sizes, distance, lerp, reflect, rotate, clamp, midpoint, matrix transform, tuple conversion, and parsing. | -| `Vector` | 2D vector with magnitude, direction, dot/cross product, normalize, projection, reflect, rotate, perpendicular (CW/CCW), angle between, lerp, and matrix transform. | -| `Size` | Width/height pair with arithmetic operators, component-wise multiply/divide, and parsing. | -| `Matrix` | 3x3 affine transformation matrix. Supports translate, rotate, scale, skew, invert, append/prepend, and point/vector transforms. | -| `Matrix2x2` | 2x2 matrix for linear algebra: determinant, trace, transpose, inverse, eigendecomposition (closed-form for symmetric matrices), and point/vector transforms. | -| `Rectangle` | Axis-aligned rectangle with contains, intersect, inflate, offset, bounds, tiling, diagonal, distance-to-point, and LTRB construction. | -| `Thickness` | Four-sided thickness (top, right, bottom, left) for rectangular borders. | - -### 3D Core Types - -| Type | Description | -|---|---| -| `Point3` | 3D Cartesian point. Distance, lerp, midpoint, random generation, arithmetic with `Vector3`. Conversions to/from `System.Numerics.Vector3` and tuples. Protobuf and JSON serialization. | -| `Vector3` | 3D vector with length, normalize, dot product, cross product, angle between, projection, lerp, random unit vector. The rotation between two directions is `Quaternion.FromTo(from, to)`. Static basis vectors `EX`, `EY`, `EZ`. | -| `Matrix3x3` | 3x3 matrix for 3D transforms. Determinant, trace, transpose, inverse. Rotation factories: `RotationX`, `RotationY`, `RotationZ`, `RotationZYX`. Scale, column/row construction. Euler angle and quaternion extraction. | -| `Quaternion` | Quaternion for 3D rotations. Normalize, conjugate, inverse, from axis-angle, rotate vector, slerp, dot product. Conversions to/from `System.Numerics.Quaternion`. | -| `Segment3` | Bounded line segment in 3D. Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | - -### Shapes (2D) - -All 2D shapes implement `IShape`, providing `Area()`, `Perimeter()`, `Centroid()`, `BoundingBox()`, `Contains(Point)`, `Rotate(Degree, Point)`, and `Scale(T)`. - -| Type | Description | -|---|---| -| `Circle` | Center + radius. Intersects with lines, segments, circles, and rectangles. Tangent point detection. `PointAt(Radian)` for parameterized access. | -| `Triangle` | Three vertices. Incircle, circumcircle, orthocenter, angles, edges. Classification: `IsRight()`, `IsAcute()`, `IsObtuse()`, `IsEquilateral()`, `IsIsosceles()`, `IsScalene()`. Similarity and congruence tests. | -| `Polygon` | Immutable polygon backed by `ReadOnlyMemory>`. Shoelace area, ray-casting containment, convex hull (Graham scan), `IsConvex()`, edges, simplify, skeleton (medial axis). Boolean operations: union, intersect, subtract, cluster. Operators: `\|` (union), `&` (intersect), `-` (subtract). | -| `Rectangle` | Implements `IArea`, `IPerimeter`, `ICentroid`, `IBoundingBox`, `IScalable>`. Rotation returns `Polygon`. | - -### Shapes (3D) - -| Type | Description | -|---|---| -| `Triangle3` | Three vertices in 3D space. Area (cross product), perimeter, centroid, unit normal vector. `ToPose()` creates a coordinate frame from the triangle plane. Translate by `Vector3`. | - -### Lines and Segments - -| Type | Description | -|---|---| -| `Line` | Infinite 2D line (supports vertical lines). From two points, point + direction, or equation. Intersect with lines, segments, circles, triangles, rectangles, polygons. Distance-to-point, angle-between, parallel/perpendicular tests, project point, reflect point. | -| `Segment` | Bounded 2D line segment (start + end). Length, midpoint, direction, lerp, split, reverse. Intersect with segments, lines, circles, rectangles. Distance-to-point, project point, parallel test. Liang-Barsky rectangle clipping. | -| `Segment3` | Bounded 3D line segment (start + end). Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | - -### Curves and Paths (2D) - -| Type | Description | -|---|---| -| `BezierCurve` | Cubic Bezier curve (4 control points). `Evaluate(t)`, `Split(t)`, `SubCurve(t0, t1)` via De Casteljau. Extremum points, linear equation intersection, rectangle edge crossings. | -| `Path` | Immutable sequence of `BezierCurve` segments. Create from points (`FromPoints`) with smoothing, or from SVG path data (`Parse`). `Length()`, `PointAt(t)` with arc-length parameterization. Exact `Intersect(Rectangle)` that splits Bezier curves at crossing points. Transform, rotate, reverse, append. `Close()` and `ToPolygon()` for rasterization. | -| `ComplexCurve` | Composite curve of mixed Bezier and polyline segments packed in a contiguous byte buffer for zero-copy decoding. SVG export and JSON serialization. | -| `PolygonalCurve` | Mutable sequence of connected points that generates smooth Bezier segments via `GetSmoothSegment(i)`. | - -### Polylines (2D and 3D) - -| Type | Description | -|---|---| -| `Polyline` | Immutable open polyline in 2D (non-closing). Length, edges, bounding box, simplification. JSON, Protobuf, and SVG serialization. | -| `Polyline3` | Immutable open polyline in 3D. Length, edges, translate, transform. JSON and Protobuf serialization. | - -### Angles and Coordinate Systems - -| Type | Description | -|---|---| -| `Degree` | Angle in degrees. Implicit conversion from numeric values and from `Radian`. Arithmetic (+, -, *, /), normalize to (-180, 180], abs, comparison operators. | -| `Radian` | Angle in radians. Implicit conversion from `Degree`. Arithmetic (+, -, *, /), normalize to (-pi, pi], abs, comparison operators. Static `Sin`/`Cos` helpers. | -| `PolarPoint` | Polar coordinates (radius, angle). Implicit conversions to/from `Point`, explicit to `Vector2` and `Vector`. | -| `CylindricalPoint` | Cylindrical coordinates (radial distance, angle, height). Factory: `OnPlane(r, angle)`. Builder methods: `WithZ`, `WithRadialDistance`, `WithAngle`. `RotateAngle` for angular displacement. `+`/`-` operators with `Radian` and `Point3`. Conversions to/from `Point3`, `Vector3`, `SphericalPoint`, and `System.Numerics.Vector3`. | -| `SphericalPoint` | Spherical coordinates (radius, azimuth, inclination) with Degree-based storage (ISO convention: inclination from Z-axis). Factories: `OnEquator`, `FromElevation`, `NorthPole`, `SouthPole`. `Normalized`, `WithRadius`, `RotateAzimuth`, `RotateInclination`, `Rotate(az, inc)`. `AngularDistance` (Vincenty formula), `Slerp`, `Lerp`, Euclidean `Distance`. Radius scaling operators (`*`, `/`). `Point3` offset operators (`+`, `-`). Conversions to/from `Point3`, `Vector3`, `CylindricalPoint`, and `System.Numerics.Vector3`. | - -### Rotations and Orientation - -| Type | Description | -|---|---| -| `Rotation3` | Euler angle rotation (Roll/Pitch/Yaw in degrees, ZYX convention). Wire and edit format only (since 2.0): create from degrees, radians, quaternion (`FromQuaternion`) or `Matrix3x3` (`FromMatrix3x3`); `ToQuaternion()` for maths. No rotation algebra: compose, invert, slerp and rotate with `Quaternion` or `Frame3`. Protobuf and JSON serialization. | -| `Quaternion` | Unit quaternion for 3D rotations. From axis-angle, `FromRpy(roll, pitch, yaw)` (Rz·Ry·Rx, radians; the URDF rpy rule), `FromTo(from, to)` (shortest arc), `FromAxes(x, y, z)` (orthonormal basis), `FromMatrix3x3(m)` (Shepperd's method) and `ToMatrix3x3()`, rotate vector, slerp, dot, normalize, conjugate, inverse. Hamilton product operator (`*`). Conversions to/from `System.Numerics.Quaternion`. Protobuf serialization. | -| `Matrix3x3` | Also used as rotation representation. `RotationZYX(rx, ry, rz)` creates ZYX Euler rotation matrix. `ToEulerZYX()` extracts angles (gimbal lock: rz = 0). Both go through `Quaternion` (`FromRpy`/`ToMatrix3x3`, `FromMatrix3x3`/`Rotation3.FromQuaternion`), so there is one Euler formula each way. | - -### Poses and Trajectories - -| Type | Description | -|---|---| -| `Frame3` | Rigid frame = position (`Point3`) + unit `Quaternion`. All pose maths: `Multiply` / `*` (compose), `Inverse`, `TransformPoint`, `TransformVector`, `Interpolate` (position lerp + slerp), `Distance` (position error and angle error in radians). `FromSurface(a, b, c)` creates a frame from three coplanar points (right-hand rule); `FromSurface(a, b, c, h)` uses a hint point for Z; `FromSurface(points)` fits a plane. `FromPose3(pose, lengthScale)` / `ToPose3(lengthScale)` convert to and from the wire format (ZYX degrees). | -| `Pose3` | Wire and edit format only (since 2.0): position (`Point3`) + ZYX Euler orientation (`Rotation3`, degrees). Constructors, `Identity`, tuple conversions, deconstruction, parsing, equality, `Truncating()`. Protobuf and JSON serialization. Convert to `Frame3` for any maths. | -| `Waypoint3` | Timestamped `Pose3` -- an element of a trajectory. | -| `PosePath3` | Immutable sequence of `Pose3` (no time). Length, reverse, transform, extract `Polyline3`, convert to `Trajectory3` with uniform timing. | -| `Trajectory3` | Immutable time-parameterized path of `Waypoint3`. `Duration`, `Length`, `AtTime(t)` (binary-search interpolation with position lerp + rotation slerp). `Resample(interval)`, `Reverse`, `Transform`. Convert to `PosePath3` or `Polyline3`. Create a real-time `TrajectoryController` via `Control()`. | -| `TrajectoryController` | Real-time trajectory playback using `Stopwatch` clock. Start, stop, pause, resume, seek. Fires `PoseChanged` events with current pose, time, and progress. | -| `Corner` | Corner blending descriptor: `Sharp` (exact waypoint) or `Round(radius)` (arc blend before/after the waypoint). | - -### Kinematics - -| Type | Description | -|---|---| -| `KinematicChain` | Namespace `ModelingEvolution.Drawing.Kinematics`. A tree of `Link`s moved by revolute/prismatic `Joint`s (origin at q = 0, unit axis, limits) with named `ChainFrame`s; metres and radians. `Forward(q, links)` writes every link pose, `FramePose(links, frame)`, `Jacobian(links, frame, j)` writes the 6 × Dof geometric Jacobian (column-major, v then ω). Spans in, spans out, no allocation; the constructor validates topological order, unit axes, limits and unique names. | -| `Joints6` | 6-DOF joint angles stored as six `Degree` fields (no array allocation). Indexer `[0..5]`, arithmetic operators, `Lerp`, `MaxAbsDelta`, `IsWithin(tolerance)`, `IsBetween(min, max)`. `ToArray()`, `FromSpan()`, `FromArray()`, `CopyTo(Span)`. Protobuf and JSON serialization. | -| `Speed` | Scalar speed in units/second (unitless magnitude). Arithmetic, `TimeFor(distance)`, `DistanceIn(time)`. Parsing. | -| `Velocity3` | 3D velocity vector (direction + magnitude). Create from direction + speed. `Speed`, `Direction`, `DisplacementIn(time)`. Arithmetic operators. | -| `Speed` | **Unit-aware** speed tagged at compile time with its unit (`ISpeedUnit`). Stores the value in `TUnit`; `ToSi()`/`FromSi()`/`To()` normalise via the SI base (m/s). Full arithmetic, comparisons, ratio (`Speed/Speed → T`), `IParsable`. Mixing units (or a non-speed unit) is a compile error. Verified zero-cost vs raw `double`. | -| `ISpeedUnit` | Marker for speed units. Built-ins: `MetrePerSecond` (SI base), `MillimetrePerSecond`, `MetrePerMinute`. Implement to add your own. | -| `AngularSpeed` | **Unit-aware** angular speed tagged at compile time with its unit (`IAngularSpeedUnit`). Stores the value in `TUnit`; `ToSi()`/`FromSi()`/`To()` normalise via the SI base (rad/s). Full arithmetic, signed negation, comparisons, ratio (`AngularSpeed/AngularSpeed → T`), `IParsable`. A separate dimension from `Speed`, so mixing an angular speed with a linear one is a compile error. | -| `IAngularSpeedUnit` | Marker for angular-speed units. Built-ins: `RadianPerSecond` (SI base), `DegreePerSecond`. Implement to add your own. | - -### Equations - -| Type | Description | -|---|---| -| `LinearEquation` | `y = Ax + B`. Create from points, angle, or direction. Compute, zero point, intersect, perpendicular, translate, mirror. | -| `QuadraticEquation` | `ax^2 + bx + c = 0`. Zero points using the discriminant. | -| `CubicEquation` | `ax^3 + bx^2 + cx + d = 0`. Root finding via Newton-Raphson. | -| `CircleEquation` | `(x-cx)^2 + (y-cy)^2 = r^2`. Intersect with `LinearEquation`. | - -### Colors - -| Type | Description | -|---|---| -| `Color` | ARGB color as a 32-bit uint. Parse from hex (`#RRGGBB`, `#AARRGGBB`), `rgba(r,g,b,a)`, or HSV strings. `GetHue()`, `GetSaturation()`, `GetLightness()`, `GetBrightness()`, `MakeTransparent(float)`. Implicit from string or tuple. JSON and Protobuf serialization. | -| `HsvColor` | HSV (hue, saturation, value) color space with optional alpha. Implicit conversions to/from `Color`. Parse from `hsv()`/`hsva()` strings, JSON arrays, or hex. | - -### Data Structures and Algorithms - -| Type | Description | -|---|---| -| `KdTree` | 2D KD-tree for fast nearest-neighbor queries on `Point`. Build from span, add individual points, find nearest neighbor. | -| `Skeleton` | Medial axis approximation of a polygon (interior nodes + edges). Three algorithms: `StraightSkeleton`, `ChordalAxis`, and `Voronoi`. | -| `MovingAverage` | Component-wise moving average over a sliding window of 2D points. O(1) add and query via circular buffer + running sums. | -| `MovingMedian` | Component-wise moving median over a sliding window of 2D points. O(1) median read, O(n) add via maintained sorted arrays. | -| `AllocationScope` | Arena-style pooled allocation scope. Library allocations use `MemoryPool.Shared` when active. Dispose returns all tracked memory at once. Supports `Persist()` to detach individual results. | -| `AlignmentResult` | Result of a rigid 2D alignment (rotation + translation) between two point clouds, with rotation angle and residual error. | - -### Interfaces - -```csharp -// Composable geometry traits -public interface IArea { T Area(); } -public interface IPerimeter { T Perimeter(); } -public interface ICentroid { Point Centroid(); } -public interface IBoundingBox { Rectangle BoundingBox(); } -public interface IRotatable { TSelf Rotate(Degree angle, Point origin = default); } -public interface IScalable { TSelf Scale(T factor); } - -// Unified shape interface -- combines all of the above plus point containment -public interface IShape : IArea, IPerimeter, ICentroid, - IBoundingBox, IRotatable, IScalable -{ - bool Contains(Point point); -} -``` - -Implementing types: `Circle`, `Triangle`, `Polygon`, and `Rectangle` (partial -- no `IRotatable` since rotation produces a `Polygon`). - -## Key Features - -### Generic Math - -All geometry types are parameterized by `T` with constraints like `IFloatingPointIeee754`, `IMinMaxValue`, etc. This means the same code works with any numeric type: - -```csharp -// Single precision -var pf = new Point(1f, 2f); -var cf = new Circle(pf, 5f); - -// Double precision -var pd = new Point(1.0, 2.0); -var cd = new Circle(pd, 5.0); - -// Both compute area with their respective precision -float areaF = cf.Area(); -double areaD = cd.Area(); -``` - -### IShape Interface Hierarchy - -Write generic algorithms over any shape: - -```csharp -void PrintShapeInfo(TShape shape) - where T : INumber, ITrigonometricFunctions, IRootFunctions, - IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue - where TShape : IShape -{ - Console.WriteLine($"Area: {shape.Area()}"); - Console.WriteLine($"Perimeter: {shape.Perimeter()}"); - Console.WriteLine($"Centroid: {shape.Centroid()}"); - Console.WriteLine($"Bounds: {shape.BoundingBox()}"); -} -``` - -### Intersections - -The `Intersections` static class provides 21 combination overloads covering every pair of: `Line`, `Segment`, `Circle`, `Triangle`, `Rectangle`, and `Polygon`. Each combination also has a zero-allocation `FirstOf` variant. - -```csharp -// Line x Line -Point? p = Intersections.Of(line1, line2); - -// Line x Circle -> chord (Segment) or null -Segment? chord = Intersections.Of(line, circle); - -// Tangent detection -Point? tangent = Intersections.TangentPoint(line, circle); - -// Circle x Circle -> radical chord or tangent point -Segment? radicalChord = Intersections.Of(circle1, circle2); - -// Segment x Segment -Point? hit = Intersections.Of(seg1, seg2); - -// Zero-alloc first-hit variants -Segment? first = Intersections.FirstOf(line, polygon); -``` - -Instance methods are also available on each shape for convenience: - -```csharp -Segment? chord = circle.Intersect(line); -Point? hit = segment.Intersect(otherSegment); -Segment? clipped = line.Intersect(rectangle); -``` - -### Polygon Boolean Operations - -Powered by [Clipper2](https://github.com/AngusJohnson/Clipper2): - -```csharp -var a = new Polygon( /* ... */ ); -var b = new Polygon( /* ... */ ); - -// Operators -Polygon union = a | b; -Polygon intersect = a & b; -Polygon difference = a - b; - -// Methods returning multiple result polygons -List> unions = a.Union(b, removeHoles: true); -List> diffs = a.Subtract(b); - -// Batch operations -List> merged = Polygon.Union(polygons); - -// Clustering overlapping polygons -IEnumerable> clusters = Polygon.Cluster(polygons); -``` - -### Bezier Path Clipping - -`Path.Intersect(Rectangle)` performs analytically exact Bezier-rectangle clipping. It finds the precise parameter values where each cubic Bezier segment crosses the rectangle edges, then uses `SubCurve(t0, t1)` via De Casteljau subdivision to return exact sub-paths -- not polygon approximations. - -```csharp -var path = Path.FromPoints(points); -IEnumerable> clipped = path.Intersect(viewport); - -foreach (var subPath in clipped) -{ - // Each sub-path contains only the Bezier segments inside the rectangle, - // with curves split precisely at the rectangle boundaries. -} -``` - -### Pooled Allocation Scopes - -When performing many geometry operations that allocate intermediate results (polygon booleans, skeleton computation, curve corrections), use `AllocationScope` to avoid GC pressure: - -```csharp -using var scope = AllocationScope.Begin(); - -var polygon = somePolygon * new Size(2, 2); // pooled -var skeleton = polygon.Skeleton(); // pooled -var lease = scope.Persist(ref skeleton); // detach from scope - -// scope.Dispose() returns everything except skeleton's memory -// lease.Dispose() returns skeleton's memory when you're done -``` - -### Serialization - -**Protobuf** -- all core types carry `[ProtoContract]` / `[ProtoMember]` attributes for use with protobuf-net: - -```csharp -var point = new Point(1f, 2f); -byte[] bytes = Serializer.Serialize(point); -``` - -**JSON** -- custom `System.Text.Json` converters for `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Polygon`, `Polyline`, `Polyline3`, `PosePath3`, `Trajectory3`, `Path`, `ComplexCurve`, `BezierCurve`, `Matrix2x2`, `Matrix3x3`, `Skeleton`, `Color`, and `HsvColor`: - -```csharp -var pose = new Pose3(100, 200, 300, 45, 90, 0); -string json = JsonSerializer.Serialize(pose); // [100,200,300,45,90,0] - -var joints = new Joints6(10, 20, 30, 40, 50, 60); -string j = JsonSerializer.Serialize(joints); // [10,20,30,40,50,60] -``` - -**String parsing** -- `IParsable` implementations on `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Rectangle`, `Size`, `Path`, `ComplexCurve`, `Speed`, `Color`, and `HsvColor`: - -```csharp -var point = Point.Parse("1.5, 2.5"); -var point3 = Point3.Parse("1.0 2.0 3.0"); -var pose = Pose3.Parse("100, 200, 300, 45, 90, 0"); -var rect = Rectangle.Parse("0 0 100 200"); -var color = Color.Parse("#FF8800"); -var path = Path.Parse("M 0 0 C 1 2, 3 4, 5 6", null); -``` - -### SVG Export - -`Polygon`, `Polyline`, `ComplexCurve`, `Skeleton`, and `Path` support SVG export via the `[SvgExporter]` attribute and the `SvgExporter` infrastructure: - -```csharp -// Path.ToString() produces SVG path data -var path = Path.FromPoints(points); -string svgData = path.ToString(); // "M 0 0 C 1.5 2.3, 3.1 4.2, 5 6 ..." -``` - -## Code Examples - -### Points: Distance, Lerp, Rotate - -```csharp -var a = new Point(0, 0); -var b = new Point(3, 4); - -double dist = a.DistanceTo(b); // 5.0 -Point quarter = a.Lerp(b, 0.25); // {X=0.75, Y=1} -Point reflected = a.Reflect(b); // {X=6, Y=8} - -// Rotate 90 degrees around the origin -Degree angle = 90.0; -Point rotated = b.Rotate(angle); // {X=-4, Y=3} - -// Rotate around a custom center -var center = new Point(1, 1); -Point rotated2 = b.Rotate(angle, center); - -// Clamp to a rectangle -var bounds = new Rectangle(0, 0, 10, 10); -Point clamped = new Point(15, -3).Clamp(bounds); // {X=10, Y=0} -``` - -### 3D Points, Vectors, and Rotations - -```csharp -// 3D point operations -var p1 = new Point3(1, 2, 3); -var p2 = new Point3(4, 6, 3); -double dist3d = Point3.Distance(p1, p2); // 5.0 -Point3 mid3d = Point3.Lerp(p1, p2, 0.5); - -// 3D vector operations -var v = new Vector3(1, 0, 0); -var w = new Vector3(0, 1, 0); -var cross = Vector3.Cross(v, w); // (0, 0, 1) -double dot = Vector3.Dot(v, w); // 0.0 - -// Compute rotation between two directions -var rot = Quaternion.FromTo(v, w); -Vector3 result = rot.Rotate(v); // approximately (0, 1, 0) - -// Euler angles (ZYX convention) are a wire format: convert once, then use the quaternion -var rotation = Rotation3.FromDegrees(45, 0, 0).ToQuaternion(); -Vector3 rotated = rotation.Rotate((Vector3)p1); - -// Quaternion slerp -var r1 = Quaternion.Identity; -var r2 = Quaternion.FromAxisAngle(Vector3.EZ, Math.PI / 2); -var halfway = Quaternion.Slerp(r1, r2, 0.5); - -// Matrix3x3 rotation -var m = Matrix3x3.RotationZ(Degree.Create(45)); -var transformed = m.Transform(new Point3(1, 0, 0)); -``` - -### Spherical Coordinates - -```csharp -// Create spherical points with factory methods -var northPole = SphericalPoint.NorthPole(10.0); // +Z, radius=10 -var equator = SphericalPoint.OnEquator(5.0, Degree.Create(45)); -var elevated = SphericalPoint.FromElevation(5.0, - Degree.Create(0), Degree.Create(30)); - -// Convert to/from Cartesian -Point3 cartesian = equator; // implicit conversion -SphericalPoint spherical = cartesian; // implicit conversion - -// Orbit around a 3D center -var center = new Point3(10, 50, 0); -var points = new List>(); -for (int az = 0; az < 360; az += 30) - points.Add(center + SphericalPoint.OnEquator(5.0, Degree.Create(az))); - -// Rotation (O(1) angle operations) -var sp = SphericalPoint.OnEquator(5.0, Degree.Create(0)); -var rotated = sp.RotateAzimuth(Degree.Create(45)); // rotate around Z - -// Angular distance (Vincenty formula -- numerically stable) -var a = SphericalPoint.NorthPole(1.0); -var b = SphericalPoint.SouthPole(1.0); -Degree angDist = SphericalPoint.AngularDistance(a, b); // 180 degrees - -// Spherical linear interpolation -var mid = SphericalPoint.Slerp(a, b, 0.5); - -// Cylindrical coordinate conversions -CylindricalPoint cyl = sp; // implicit conversion -SphericalPoint back = cyl; // implicit conversion -``` - -### Poses and Trajectories - -```csharp -// Pose = position + orientation -var pose = new Pose3(100, 200, 300, 45, 0, 0); // x,y,z, rx,ry,rz (degrees) - -// Maths composes Frame3 (position + quaternion); Pose3 converts once, here mm -> m -var world = Frame3.FromPose3(new Pose3(10, 0, 0, 0, 0, 0), 0.001); -var local = Frame3.FromPose3(new Pose3(5, 0, 0, 0, 0, 90), 0.001); -var combined = world * local; -Pose3 back = combined.ToPose3(1000); // m -> mm, ZYX degrees - -// Create frame from a surface (3 points define a plane) -var a = new Point3(0, 0, 0); -var b = new Point3(1, 0, 0); -var c = new Point3(0, 1, 0); -var surface = Frame3.FromSurface(a, b, c); // Z-axis via right-hand rule - -// Trajectory with timed waypoints -var trajectory = new Trajectory3( - new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), - new Waypoint3(new Pose3(50, 0, 25, 0, 30, 0), 2.5), - new Waypoint3(new Pose3(100, 0, 0, 0, 0, 0), 5.0)); - -// Interpolate at any time (binary search + lerp/slerp) -Pose3 atT = trajectory.AtTime(1.25); - -// Resample at uniform intervals -var resampled = trajectory.Resample(0.1); // every 0.1 seconds - -// Real-time playback -var controller = trajectory.Control(); -controller.PoseChanged += (_, e) => Console.WriteLine($"t={e.Time} pose={e.Pose}"); -controller.Start(); - -// 6-DOF joint angles for robotics -var home = new Joints6(0, -90, 0, 0, 90, 0); -var target = new Joints6(45, -60, 30, 0, 90, -45); -var halfway = Joints6.Lerp(home, target, 0.5); -bool inRange = target.IsBetween(minLimits, maxLimits); -``` - -### Shapes: Area and Perimeter - -```csharp -// Circle -var circle = new Circle(new Point(5, 5), 3.0); -double cArea = circle.Area(); // ~28.274 -double cPerim = circle.Perimeter(); // ~18.850 - -// Triangle -var triangle = new Triangle( - new Point(0, 0), - new Point(4, 0), - new Point(0, 3)); -double tArea = triangle.Area(); // 6.0 -double tPerim = triangle.Perimeter(); // 12.0 - -// Polygon (shoelace formula) -var poly = new Polygon( - new Point(0, 0), new Point(4, 0), - new Point(4, 3), new Point(0, 3)); -double pArea = poly.Area(); // 12.0 - -// 3D triangle -var t3 = new Triangle3( - new Point3(0, 0, 0), - new Point3(4, 0, 0), - new Point3(0, 3, 0)); -double area3 = t3.Area; // 6.0 -Vector3 normal = t3.Normal; // (0, 0, 1) -``` - -### Line-Circle Intersection - -```csharp -var circle = new Circle(new Point(0, 0), 5.0); -var line = Line.From(new Point(-10, 0), new Point(10, 0)); - -// Secant -- returns the chord as a segment -Segment? chord = circle.Intersect(line); -// chord.Value.Start ~ {X=-5, Y=0}, chord.Value.End ~ {X=5, Y=0} - -// Tangent detection -var tangentLine = Line.Horizontal(5.0); -Point? tangent = circle.TangentPoint(tangentLine); -// tangent.Value ~ {X=0, Y=5} -``` - -### Segment-Rectangle Clipping - -```csharp -var viewport = new Rectangle(0, 0, 100, 100); -var segment = new Segment( - new Point(-20, 50), - new Point(150, 50)); - -// Liang-Barsky clipping -Segment? clipped = segment.Intersect(viewport); -// clipped.Value: Start={X=0, Y=50}, End={X=100, Y=50} -``` - -### Path-Rectangle Clipping - -```csharp -var points = new[] -{ - new Point(10, 50), - new Point(50, 10), - new Point(90, 50), - new Point(130, 90) -}; -var path = Path.FromPoints(points); -var viewport = new Rectangle(20, 20, 60, 60); // 20,20 to 80,80 - -// Exact Bezier clipping -- returns sub-paths with curves split at boundaries -foreach (var subPath in path.Intersect(viewport)) -{ - Console.WriteLine($"Sub-path with {subPath.Count} Bezier segments"); -} -``` - -### Triangle Properties - -```csharp -var t = new Triangle( - new Point(0, 0), - new Point(4, 0), - new Point(2, 3)); - -// Derived circles -Circle incircle = t.Incircle(); // largest circle inside -Circle circumcircle = t.Circumcircle(); // passes through all vertices - -// Center points -Point centroid = t.Centroid(); -Point orthocenter = t.Orthocenter; - -// Classification -bool right = t.IsRight(); -bool equilateral = t.IsEquilateral(); -bool isosceles = t.IsIsosceles(); - -// Similarity and congruence -var t2 = new Triangle( - new Point(0, 0), - new Point(8, 0), - new Point(4, 6)); -bool similar = t.IsSimilarTo(t2); // true (same shape, scaled 2x) -bool congruent = t.IsCongruentTo(t2); // false (different size) - -// Interior angles -var (atA, atB, atC) = t.Angles; -``` - -### Polygon Operations - -```csharp -// Convex hull -var cloud = new Polygon( - new Point(0, 0), new Point(1, 3), - new Point(2, 1), new Point(4, 4), - new Point(3, 0), new Point(5, 2)); -Polygon hull = cloud.ConvexHull(); - -// Point containment (ray casting) -bool inside = hull.Contains(new Point(2, 2)); - -// Boolean operations -var a = new Polygon( /* ... */ ); -var b = new Polygon( /* ... */ ); -List> union = a.Union(b, removeHoles: true); -List> diff = a.Subtract(b); -List> inter = a.Intersect(b); - -// Simplify (remove collinear/close points) -Polygon simplified = hull.Simplify(epsilon: 0.1); - -// Convexity test -bool convex = hull.IsConvex(); - -// Clustering overlapping polygons -var clusters = Polygon.Cluster(polygons); -``` - -### Matrix Transformations (2D and 3D) - -```csharp -// 2D affine transformation -var matrix = Matrix.Identity; -matrix.Translate(10.0, 20.0); -matrix.Rotate(Degree.Create(45.0)); -matrix.Scale(2.0, 2.0); - -Point transformed = matrix.Transform(new Point(1, 0)); -Vector rotatedVec = matrix.Transform(new Vector(1, 0)); - -var combined = matrix1 * matrix2; -matrix.Invert(); - -// 3D rotation matrix -var rx = Matrix3x3.RotationX(Degree.Create(45)); -var ry = Matrix3x3.RotationY(Degree.Create(30)); -var rz = Matrix3x3.RotationZ(Degree.Create(60)); - -// Compose rotations -var rotation = rz * ry * rx; -var p = new Point3(1, 0, 0); -Point3 rotated3 = rotation * p; // or rotation.Transform(p) - -// Extract Euler angles -var (eRx, eRy, eRz) = rotation.ToEulerZYX(); - -// Convert to quaternion and back -Quaternion q = Quaternion.FromMatrix3x3(rotation); -Matrix3x3 back = q.ToMatrix3x3(); -``` - -### Unit-Aware Speed - -```csharp -using ModelingEvolution.Drawing.Units; - -// The unit is part of the type — the value is stored in that unit. -Speed> feed = new(300); // 300 m/min - -// Normalise to the SI base (m/s) or convert to another speed unit. -double si = feed.ToSi(); // 5.0 (m/s) -var inMmPerS = feed.To>(); // 5000 mm/s - -// Same-unit arithmetic, scalar scaling, and a dimensionless ratio. -var faster = feed * 2.0; // 600 m/min -double ratio = faster / feed; // 2.0 - -// Cross-unit add normalises through SI; result stays in the left unit. -var total = inMmPerS.Add(feed); // (5000 + 5000) mm/s - -Console.WriteLine(feed); // "300 m/min" - -// Compile-time safety: -// feed + inMmPerS; // error: different units — use .Add(...) -// feed.To>(); // error: not an ISpeedUnit -``` - -### Unit-Aware Angular Speed - -`AngularSpeed` mirrors `Speed` for rotation, on its own dimension with the SI -base rad/s. Keeping the two dimensions apart is the point: a rotary axis and a linear axis cannot be -commanded with each other's speed. - -```csharp -using ModelingEvolution.Drawing.Units; - -// The unit is part of the type — the value is stored in that unit. -AngularSpeed> tilt = new(5); // 5 °/s - -// Normalise to the SI base (rad/s) or convert to another angular-speed unit. -double si = tilt.ToSi(); // 0.0873 (rad/s) -var inRadPerS = tilt.To>(); - -// Signed: negating reverses the sense of rotation. -var reverse = -tilt; // -5 °/s - -// Scale by a percentage of a maximum, e.g. a jog knob at 20% of the axis limit. -var jog = tilt * new Percentage(20f); // 1 °/s - -Console.WriteLine(tilt); // "5 °/s" - -// Compile-time safety — the dimensions do not mix: -// tilt + new Speed>(20); // error: different types -// tilt.To>(); // error: not an IAngularSpeedUnit -// Speed> wrong; // error: not an ISpeedUnit -``` - -## Requirements - -- **.NET 10.0** or later (uses generic math interfaces from .NET 7+) -- **Dependencies**: [Clipper2](https://www.nuget.org/packages/Clipper2) 1.4.0, [protobuf-net](https://www.nuget.org/packages/protobuf-net) 3.2.45 - -## License - -See [LICENSE](LICENSE) for details. +# ModelingEvolution.Drawing + +[![NuGet](https://img.shields.io/nuget/v/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) +[![NuGet Downloads](https://img.shields.io/nuget/dt/ModelingEvolution.Drawing.svg)](https://www.nuget.org/packages/ModelingEvolution.Drawing) +[![CI](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml/badge.svg)](https://github.com/modelingevolution/drawing/actions/workflows/ci.yml) +[![License](https://img.shields.io/github/license/modelingevolution/drawing.svg)](https://github.com/modelingevolution/drawing/blob/main/LICENSE) + +A high-performance .NET generic math library for 2D and 3D geometry, coordinate systems, shapes, intersections, trajectories, and drawing primitives. Built on `INumber` constraints from .NET generic math, all types work with `float`, `double`, `decimal`, or any compatible numeric type -- with zero boxing and full type safety. + +## Table of Contents + +- [Why This Library](#why-this-library) +- [Installation](#installation) +- [Quick Start](#quick-start) +- [API Reference](#api-reference) + - [2D Core Types](#2d-core-types) + - [3D Core Types](#3d-core-types) + - [Shapes (2D)](#shapes-2d) + - [Shapes (3D)](#shapes-3d) + - [Lines and Segments](#lines-and-segments) + - [Curves and Paths (2D)](#curves-and-paths-2d) + - [Polylines (2D and 3D)](#polylines-2d-and-3d) + - [Angles and Coordinate Systems](#angles-and-coordinate-systems) + - [Rotations and Orientation](#rotations-and-orientation) + - [Poses and Trajectories](#poses-and-trajectories) + - [Kinematics](#kinematics) + - [Equations](#equations) + - [Colors](#colors) + - [Data Structures and Algorithms](#data-structures-and-algorithms) + - [Interfaces](#interfaces) +- [Key Features](#key-features) + - [Generic Math](#generic-math) + - [IShape Interface Hierarchy](#ishape-interface-hierarchy) + - [Intersections](#intersections) + - [Polygon Boolean Operations](#polygon-boolean-operations) + - [Bezier Path Clipping](#bezier-path-clipping) + - [Pooled Allocation Scopes](#pooled-allocation-scopes) + - [Serialization](#serialization) + - [SVG Export](#svg-export) +- [Code Examples](#code-examples) + - [Points: Distance, Lerp, Rotate](#points-distance-lerp-rotate) + - [3D Points, Vectors, and Rotations](#3d-points-vectors-and-rotations) + - [Spherical Coordinates](#spherical-coordinates) + - [Poses and Trajectories](#poses-and-trajectories-1) + - [Shapes: Area and Perimeter](#shapes-area-and-perimeter) + - [Line-Circle Intersection](#line-circle-intersection) + - [Segment-Rectangle Clipping](#segment-rectangle-clipping) + - [Path-Rectangle Clipping](#path-rectangle-clipping) + - [Triangle Properties](#triangle-properties) + - [Polygon Operations](#polygon-operations) + - [Matrix Transformations (2D and 3D)](#matrix-transformations-2d-and-3d) +- [Requirements](#requirements) +- [License](#license) + +## Why This Library + +- **Generic math** -- write geometry code once; use it with `float`, `double`, or any `IFloatingPointIeee754` type. +- **No boxing** -- all core types are `struct` or `readonly record struct`, avoiding heap allocations. +- **Full 2D and 3D** -- from `Point` and `Vector` to `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Quaternion`, and `Trajectory3`. +- **Rich coordinate systems** -- Cartesian, polar, cylindrical, and spherical with implicit/explicit conversions between them. +- **Rich interface hierarchy** -- `IShape` unifies area, perimeter, centroid, bounding box, containment, rotation, and scaling across all shape types. +- **Comprehensive intersections** -- 21 intersection combinations (line, segment, circle, triangle, rectangle, polygon) with zero-alloc `FirstOf` variants. +- **Polygon booleans** -- union, intersection, difference, and clustering via Clipper2. +- **Exact Bezier clipping** -- `Path.Intersect(Rectangle)` computes analytically exact sub-curves, not polygon approximations. +- **Trajectory planning** -- `Trajectory3` with time-parameterized waypoints, interpolation, resampling, and a real-time `TrajectoryController`. +- **Robotics-ready** -- `Joints6` for 6-DOF joint representations, `Frame3` (position + unit quaternion) for all pose maths, `Frame3.FromSurface()` for plane-based frame construction, and `KinematicChain` for forward kinematics and geometric Jacobians of serial and tree chains. +- **Pooled allocations** -- `AllocationScope` provides arena-style pooled memory for zero-GC geometry pipelines. +- **Serialization built in** -- Protobuf (protobuf-net) attributes on all core types, plus custom JSON converters and SVG exporters. + +## Installation + +```bash +dotnet add package ModelingEvolution.Drawing +``` + +Or via the NuGet Package Manager: + +``` +Install-Package ModelingEvolution.Drawing +``` + +## Quick Start + +```csharp +using ModelingEvolution.Drawing; + +// 2D points and vectors (using double precision) +var a = new Point(1.0, 2.0); +var b = new Point(4.0, 6.0); + +double distance = a.DistanceTo(b); // 5.0 +Point mid = a.Lerp(b, 0.5); // {X=2.5, Y=4} + +// 3D points and rotations +var p = new Point3(1.0, 2.0, 3.0); +var rotation = new Rotation3(45.0, 0.0, 0.0); // 45 deg roll (wire/edit format) +var rotated = new Frame3(Point3.Zero, rotation.ToQuaternion()).TransformPoint(p); + +// Spherical coordinates +var orbit = SphericalPoint.OnEquator(5.0, Degree.Create(30)); +Point3 cartesian = orbit; // implicit conversion + +// Shapes with area and perimeter +var circle = new Circle(Point.Zero, 10.0); +double area = circle.Area(); // ~314.159 +bool inside = circle.Contains(new Point(3.0, 4.0)); // true + +// Polygon boolean operations +var poly1 = new Polygon( + new Point(0, 0), new Point(10, 0), + new Point(10, 10), new Point(0, 10)); +var poly2 = new Polygon( + new Point(5, 5), new Point(15, 5), + new Point(15, 15), new Point(5, 15)); + +Polygon merged = poly1 | poly2; // union +Polygon overlap = poly1 & poly2; // intersection + +// 3D trajectory with time-parameterized waypoints +var trajectory = new Trajectory3( + new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), + new Waypoint3(new Pose3(100, 0, 50, 0, 45, 0), 5.0)); +Pose3 atHalf = trajectory.AtTime(2.5); // interpolated pose +``` + +## API Reference + +### 2D Core Types + +| Type | Description | +|---|---| +| `Point` | 2D Cartesian point. Supports arithmetic with vectors and sizes, distance, lerp, reflect, rotate, clamp, midpoint, matrix transform, tuple conversion, and parsing. | +| `Vector` | 2D vector with magnitude, direction, dot/cross product, normalize, projection, reflect, rotate, perpendicular (CW/CCW), angle between, lerp, and matrix transform. | +| `Size` | Width/height pair with arithmetic operators, component-wise multiply/divide, and parsing. | +| `Matrix` | 3x3 affine transformation matrix. Supports translate, rotate, scale, skew, invert, append/prepend, and point/vector transforms. | +| `Matrix2x2` | 2x2 matrix for linear algebra: determinant, trace, transpose, inverse, eigendecomposition (closed-form for symmetric matrices), and point/vector transforms. | +| `Rectangle` | Axis-aligned rectangle with contains, intersect, inflate, offset, bounds, tiling, diagonal, distance-to-point, and LTRB construction. | +| `Thickness` | Four-sided thickness (top, right, bottom, left) for rectangular borders. | + +### 3D Core Types + +| Type | Description | +|---|---| +| `Point3` | 3D Cartesian point. Distance, lerp, midpoint, random generation, arithmetic with `Vector3`. Conversions to/from `System.Numerics.Vector3` and tuples. Protobuf and JSON serialization. | +| `Vector3` | 3D vector with length, normalize, dot product, cross product, angle between, projection, lerp, random unit vector. The rotation between two directions is `Quaternion.FromTo(from, to)`. Static basis vectors `EX`, `EY`, `EZ`. | +| `Matrix3x3` | 3x3 matrix for 3D transforms. Determinant, trace, transpose, inverse. Rotation factories: `RotationX`, `RotationY`, `RotationZ`, `RotationZYX`. Scale, column/row construction. Euler angle and quaternion extraction. | +| `Quaternion` | Quaternion for 3D rotations. Normalize, conjugate, inverse, from axis-angle, rotate vector, slerp, dot product. Conversions to/from `System.Numerics.Quaternion`. | +| `Segment3` | Bounded line segment in 3D. Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | + +### Shapes (2D) + +All 2D shapes implement `IShape`, providing `Area()`, `Perimeter()`, `Centroid()`, `BoundingBox()`, `Contains(Point)`, `Rotate(Degree, Point)`, and `Scale(T)`. + +| Type | Description | +|---|---| +| `Circle` | Center + radius. Intersects with lines, segments, circles, and rectangles. Tangent point detection. `PointAt(Radian)` for parameterized access. | +| `Triangle` | Three vertices. Incircle, circumcircle, orthocenter, angles, edges. Classification: `IsRight()`, `IsAcute()`, `IsObtuse()`, `IsEquilateral()`, `IsIsosceles()`, `IsScalene()`. Similarity and congruence tests. | +| `Polygon` | Immutable polygon backed by `ReadOnlyMemory>`. Shoelace area, ray-casting containment, convex hull (Graham scan), `IsConvex()`, edges, simplify, skeleton (medial axis). Boolean operations: union, intersect, subtract, cluster. Operators: `\|` (union), `&` (intersect), `-` (subtract). | +| `Rectangle` | Implements `IArea`, `IPerimeter`, `ICentroid`, `IBoundingBox`, `IScalable>`. Rotation returns `Polygon`. | + +### Shapes (3D) + +| Type | Description | +|---|---| +| `Triangle3` | Three vertices in 3D space. Area (cross product), perimeter, centroid, unit normal vector. `ToPose()` creates a coordinate frame from the triangle plane. Translate by `Vector3`. | + +### Lines and Segments + +| Type | Description | +|---|---| +| `Line` | Infinite 2D line (supports vertical lines). From two points, point + direction, or equation. Intersect with lines, segments, circles, triangles, rectangles, polygons. Distance-to-point, angle-between, parallel/perpendicular tests, project point, reflect point. | +| `Segment` | Bounded 2D line segment (start + end). Length, midpoint, direction, lerp, split, reverse. Intersect with segments, lines, circles, rectangles. Distance-to-point, project point, parallel test. Liang-Barsky rectangle clipping. | +| `Segment3` | Bounded 3D line segment (start + end). Length, midpoint, direction, lerp, split, reverse, distance-to-point, project point. | + +### Curves and Paths (2D) + +| Type | Description | +|---|---| +| `BezierCurve` | Cubic Bezier curve (4 control points). `Evaluate(t)`, `Split(t)`, `SubCurve(t0, t1)` via De Casteljau. Extremum points, linear equation intersection, rectangle edge crossings. | +| `Path` | Immutable sequence of `BezierCurve` segments. Create from points (`FromPoints`) with smoothing, or from SVG path data (`Parse`). `Length()`, `PointAt(t)` with arc-length parameterization. Exact `Intersect(Rectangle)` that splits Bezier curves at crossing points. Transform, rotate, reverse, append. `Close()` and `ToPolygon()` for rasterization. | +| `ComplexCurve` | Composite curve of mixed Bezier and polyline segments packed in a contiguous byte buffer for zero-copy decoding. SVG export and JSON serialization. | +| `PolygonalCurve` | Mutable sequence of connected points that generates smooth Bezier segments via `GetSmoothSegment(i)`. | + +### Polylines (2D and 3D) + +| Type | Description | +|---|---| +| `Polyline` | Immutable open polyline in 2D (non-closing). Length, edges, bounding box, simplification. JSON, Protobuf, and SVG serialization. | +| `Polyline3` | Immutable open polyline in 3D. Length, edges, translate, transform. JSON and Protobuf serialization. | + +### Angles and Coordinate Systems + +| Type | Description | +|---|---| +| `Degree` | Angle in degrees. Implicit conversion from numeric values and from `Radian`. Arithmetic (+, -, *, /), normalize to (-180, 180], abs, comparison operators. | +| `Radian` | Angle in radians. Implicit conversion from `Degree`. Arithmetic (+, -, *, /), normalize to (-pi, pi], abs, comparison operators. Static `Sin`/`Cos` helpers. | +| `PolarPoint` | Polar coordinates (radius, angle). Implicit conversions to/from `Point`, explicit to `Vector2` and `Vector`. | +| `CylindricalPoint` | Cylindrical coordinates (radial distance, angle, height). Factory: `OnPlane(r, angle)`. Builder methods: `WithZ`, `WithRadialDistance`, `WithAngle`. `RotateAngle` for angular displacement. `+`/`-` operators with `Radian` and `Point3`. Conversions to/from `Point3`, `Vector3`, `SphericalPoint`, and `System.Numerics.Vector3`. | +| `SphericalPoint` | Spherical coordinates (radius, azimuth, inclination) with Degree-based storage (ISO convention: inclination from Z-axis). Factories: `OnEquator`, `FromElevation`, `NorthPole`, `SouthPole`. `Normalized`, `WithRadius`, `RotateAzimuth`, `RotateInclination`, `Rotate(az, inc)`. `AngularDistance` (Vincenty formula), `Slerp`, `Lerp`, Euclidean `Distance`. Radius scaling operators (`*`, `/`). `Point3` offset operators (`+`, `-`). Conversions to/from `Point3`, `Vector3`, `CylindricalPoint`, and `System.Numerics.Vector3`. | + +### Rotations and Orientation + +| Type | Description | +|---|---| +| `Rotation3` | Euler angle rotation (Roll/Pitch/Yaw in degrees, ZYX convention). Wire and edit format only (since 2.0): create from degrees, radians, quaternion (`FromQuaternion`) or `Matrix3x3` (`FromMatrix3x3`); `ToQuaternion()` for maths. No rotation algebra: compose, invert, slerp and rotate with `Quaternion` or `Frame3`. Protobuf and JSON serialization. | +| `Quaternion` | Unit quaternion for 3D rotations. From axis-angle, `FromRpy(roll, pitch, yaw)` (Rz·Ry·Rx, radians; the URDF rpy rule), `FromTo(from, to)` (shortest arc), `FromAxes(x, y, z)` (orthonormal basis), `FromMatrix3x3(m)` (Shepperd's method) and `ToMatrix3x3()`, rotate vector, slerp, dot, normalize, conjugate, inverse. Hamilton product operator (`*`). Conversions to/from `System.Numerics.Quaternion`. Protobuf serialization. | +| `Matrix3x3` | Also used as rotation representation. `RotationZYX(rx, ry, rz)` creates ZYX Euler rotation matrix. `ToEulerZYX()` extracts angles (gimbal lock: rz = 0). Both go through `Quaternion` (`FromRpy`/`ToMatrix3x3`, `FromMatrix3x3`/`Rotation3.FromQuaternion`), so there is one Euler formula each way. | + +### Poses and Trajectories + +| Type | Description | +|---|---| +| `Frame3` | Rigid frame = position (`Point3`) + unit `Quaternion`. All pose maths: `Multiply` / `*` (compose), `Inverse`, `TransformPoint`, `TransformVector`, `Interpolate` (position lerp + slerp), `Distance` (position error and angle error in radians). `FromSurface(a, b, c)` creates a frame from three coplanar points (right-hand rule); `FromSurface(a, b, c, h)` uses a hint point for Z; `FromSurface(points)` fits a plane. `FromPose3(pose, lengthScale)` / `ToPose3(lengthScale)` convert to and from the wire format (ZYX degrees). | +| `Pose3` | Wire and edit format only (since 2.0): position (`Point3`) + ZYX Euler orientation (`Rotation3`, degrees). Constructors, `Identity`, tuple conversions, deconstruction, parsing, equality, `Truncating()`. Protobuf and JSON serialization. Convert to `Frame3` for any maths. | +| `Waypoint3` | Timestamped `Pose3` -- an element of a trajectory. | +| `PosePath3` | Immutable sequence of `Pose3` (no time). Length, reverse, transform, extract `Polyline3`, convert to `Trajectory3` with uniform timing. | +| `Trajectory3` | Immutable time-parameterized path of `Waypoint3`. `Duration`, `Length`, `AtTime(t)` (binary-search interpolation with position lerp + rotation slerp). `Resample(interval)`, `Reverse`, `Transform`. Convert to `PosePath3` or `Polyline3`. Create a real-time `TrajectoryController` via `Control()`. | +| `TrajectoryController` | Real-time trajectory playback using `Stopwatch` clock. Start, stop, pause, resume, seek. Fires `PoseChanged` events with current pose, time, and progress. | +| `Corner` | Corner blending descriptor: `Sharp` (exact waypoint) or `Round(radius)` (arc blend before/after the waypoint). | + +### Kinematics + +| Type | Description | +|---|---| +| `KinematicChain` | Namespace `ModelingEvolution.Drawing.Kinematics`. A tree of `Link`s moved by revolute/prismatic `Joint`s (origin at q = 0, unit axis, limits) with named `ChainFrame`s; metres and radians. `Forward(q, links)` writes every link pose, `FramePose(links, frame)`, `Jacobian(links, frame, j)` writes the 6 × Dof geometric Jacobian (column-major, v then ω). Spans in, spans out, no allocation; the constructor validates topological order, unit axes, limits and unique names. | +| `Joints6` | 6-DOF joint angles stored as six `Degree` fields (no array allocation). Indexer `[0..5]`, arithmetic operators, `Lerp`, `MaxAbsDelta`, `IsWithin(tolerance)`, `IsBetween(min, max)`. `ToArray()`, `FromSpan()`, `FromArray()`, `CopyTo(Span)`. Protobuf and JSON serialization. | +| `Speed` | Scalar speed in units/second (unitless magnitude). Arithmetic, `TimeFor(distance)`, `DistanceIn(time)`. Parsing. | +| `Velocity3` | 3D velocity vector (direction + magnitude). Create from direction + speed. `Speed`, `Direction`, `DisplacementIn(time)`. Arithmetic operators. | +| `Speed` | **Unit-aware** speed tagged at compile time with its unit (`ISpeedUnit`). Stores the value in `TUnit`; `ToSi()`/`FromSi()`/`To()` normalise via the SI base (m/s). Full arithmetic, comparisons, ratio (`Speed/Speed → T`), `IParsable`. Mixing units (or a non-speed unit) is a compile error. Verified zero-cost vs raw `double`. | +| `ISpeedUnit` | Marker for speed units. Built-ins: `MetrePerSecond` (SI base), `MillimetrePerSecond`, `MetrePerMinute`. Implement to add your own. | +| `AngularSpeed` | **Unit-aware** angular speed tagged at compile time with its unit (`IAngularSpeedUnit`). Stores the value in `TUnit`; `ToSi()`/`FromSi()`/`To()` normalise via the SI base (rad/s). Full arithmetic, signed negation, comparisons, ratio (`AngularSpeed/AngularSpeed → T`), `IParsable`. A separate dimension from `Speed`, so mixing an angular speed with a linear one is a compile error. | +| `IAngularSpeedUnit` | Marker for angular-speed units. Built-ins: `RadianPerSecond` (SI base), `DegreePerSecond`. Implement to add your own. | + +### Equations + +| Type | Description | +|---|---| +| `LinearEquation` | `y = Ax + B`. Create from points, angle, or direction. Compute, zero point, intersect, perpendicular, translate, mirror. | +| `QuadraticEquation` | `ax^2 + bx + c = 0`. Zero points using the discriminant. | +| `CubicEquation` | `ax^3 + bx^2 + cx + d = 0`. Root finding via Newton-Raphson. | +| `CircleEquation` | `(x-cx)^2 + (y-cy)^2 = r^2`. Intersect with `LinearEquation`. | + +### Colors + +| Type | Description | +|---|---| +| `Color` | ARGB color as a 32-bit uint. Parse from hex (`#RRGGBB`, `#AARRGGBB`), `rgba(r,g,b,a)`, or HSV strings. `GetHue()`, `GetSaturation()`, `GetLightness()`, `GetBrightness()`, `MakeTransparent(float)`. Implicit from string or tuple. JSON and Protobuf serialization. | +| `HsvColor` | HSV (hue, saturation, value) color space with optional alpha. Implicit conversions to/from `Color`. Parse from `hsv()`/`hsva()` strings, JSON arrays, or hex. | + +### Data Structures and Algorithms + +| Type | Description | +|---|---| +| `KdTree` | 2D KD-tree for fast nearest-neighbor queries on `Point`. Build from span, add individual points, find nearest neighbor. | +| `Skeleton` | Medial axis approximation of a polygon (interior nodes + edges). Three algorithms: `StraightSkeleton`, `ChordalAxis`, and `Voronoi`. | +| `MovingAverage` | Component-wise moving average over a sliding window of 2D points. O(1) add and query via circular buffer + running sums. | +| `MovingMedian` | Component-wise moving median over a sliding window of 2D points. O(1) median read, O(n) add via maintained sorted arrays. | +| `AllocationScope` | Arena-style pooled allocation scope. Library allocations use `MemoryPool.Shared` when active. Dispose returns all tracked memory at once. Supports `Persist()` to detach individual results. | +| `AlignmentResult` | Result of a rigid 2D alignment (rotation + translation) between two point clouds, with rotation angle and residual error. | + +### Interfaces + +```csharp +// Composable geometry traits +public interface IArea { T Area(); } +public interface IPerimeter { T Perimeter(); } +public interface ICentroid { Point Centroid(); } +public interface IBoundingBox { Rectangle BoundingBox(); } +public interface IRotatable { TSelf Rotate(Degree angle, Point origin = default); } +public interface IScalable { TSelf Scale(T factor); } + +// Unified shape interface -- combines all of the above plus point containment +public interface IShape : IArea, IPerimeter, ICentroid, + IBoundingBox, IRotatable, IScalable +{ + bool Contains(Point point); +} +``` + +Implementing types: `Circle`, `Triangle`, `Polygon`, and `Rectangle` (partial -- no `IRotatable` since rotation produces a `Polygon`). + +## Key Features + +### Generic Math + +All geometry types are parameterized by `T` with constraints like `IFloatingPointIeee754`, `IMinMaxValue`, etc. This means the same code works with any numeric type: + +```csharp +// Single precision +var pf = new Point(1f, 2f); +var cf = new Circle(pf, 5f); + +// Double precision +var pd = new Point(1.0, 2.0); +var cd = new Circle(pd, 5.0); + +// Both compute area with their respective precision +float areaF = cf.Area(); +double areaD = cd.Area(); +``` + +### IShape Interface Hierarchy + +Write generic algorithms over any shape: + +```csharp +void PrintShapeInfo(TShape shape) + where T : INumber, ITrigonometricFunctions, IRootFunctions, + IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue + where TShape : IShape +{ + Console.WriteLine($"Area: {shape.Area()}"); + Console.WriteLine($"Perimeter: {shape.Perimeter()}"); + Console.WriteLine($"Centroid: {shape.Centroid()}"); + Console.WriteLine($"Bounds: {shape.BoundingBox()}"); +} +``` + +### Intersections + +The `Intersections` static class provides 21 combination overloads covering every pair of: `Line`, `Segment`, `Circle`, `Triangle`, `Rectangle`, and `Polygon`. Each combination also has a zero-allocation `FirstOf` variant. + +```csharp +// Line x Line +Point? p = Intersections.Of(line1, line2); + +// Line x Circle -> chord (Segment) or null +Segment? chord = Intersections.Of(line, circle); + +// Tangent detection +Point? tangent = Intersections.TangentPoint(line, circle); + +// Circle x Circle -> radical chord or tangent point +Segment? radicalChord = Intersections.Of(circle1, circle2); + +// Segment x Segment +Point? hit = Intersections.Of(seg1, seg2); + +// Zero-alloc first-hit variants +Segment? first = Intersections.FirstOf(line, polygon); +``` + +Instance methods are also available on each shape for convenience: + +```csharp +Segment? chord = circle.Intersect(line); +Point? hit = segment.Intersect(otherSegment); +Segment? clipped = line.Intersect(rectangle); +``` + +### Polygon Boolean Operations + +Powered by [Clipper2](https://github.com/AngusJohnson/Clipper2): + +```csharp +var a = new Polygon( /* ... */ ); +var b = new Polygon( /* ... */ ); + +// Operators +Polygon union = a | b; +Polygon intersect = a & b; +Polygon difference = a - b; + +// Methods returning multiple result polygons +List> unions = a.Union(b, removeHoles: true); +List> diffs = a.Subtract(b); + +// Batch operations +List> merged = Polygon.Union(polygons); + +// Clustering overlapping polygons +IEnumerable> clusters = Polygon.Cluster(polygons); +``` + +### Bezier Path Clipping + +`Path.Intersect(Rectangle)` performs analytically exact Bezier-rectangle clipping. It finds the precise parameter values where each cubic Bezier segment crosses the rectangle edges, then uses `SubCurve(t0, t1)` via De Casteljau subdivision to return exact sub-paths -- not polygon approximations. + +```csharp +var path = Path.FromPoints(points); +IEnumerable> clipped = path.Intersect(viewport); + +foreach (var subPath in clipped) +{ + // Each sub-path contains only the Bezier segments inside the rectangle, + // with curves split precisely at the rectangle boundaries. +} +``` + +### Pooled Allocation Scopes + +When performing many geometry operations that allocate intermediate results (polygon booleans, skeleton computation, curve corrections), use `AllocationScope` to avoid GC pressure: + +```csharp +using var scope = AllocationScope.Begin(); + +var polygon = somePolygon * new Size(2, 2); // pooled +var skeleton = polygon.Skeleton(); // pooled +var lease = scope.Persist(ref skeleton); // detach from scope + +// scope.Dispose() returns everything except skeleton's memory +// lease.Dispose() returns skeleton's memory when you're done +``` + +### Serialization + +**Protobuf** -- all core types carry `[ProtoContract]` / `[ProtoMember]` attributes for use with protobuf-net: + +```csharp +var point = new Point(1f, 2f); +byte[] bytes = Serializer.Serialize(point); +``` + +**JSON** -- custom `System.Text.Json` converters for `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Polygon`, `Polyline`, `Polyline3`, `PosePath3`, `Trajectory3`, `Path`, `ComplexCurve`, `BezierCurve`, `Matrix2x2`, `Matrix3x3`, `Skeleton`, `Color`, and `HsvColor`: + +```csharp +var pose = new Pose3(100, 200, 300, 45, 90, 0); +string json = JsonSerializer.Serialize(pose); // [100,200,300,45,90,0] + +var joints = new Joints6(10, 20, 30, 40, 50, 60); +string j = JsonSerializer.Serialize(joints); // [10,20,30,40,50,60] +``` + +**String parsing** -- `IParsable` implementations on `Point`, `Point3`, `Vector3`, `Rotation3`, `Pose3`, `Joints6`, `Rectangle`, `Size`, `Path`, `ComplexCurve`, `Speed`, `Color`, and `HsvColor`: + +```csharp +var point = Point.Parse("1.5, 2.5"); +var point3 = Point3.Parse("1.0 2.0 3.0"); +var pose = Pose3.Parse("100, 200, 300, 45, 90, 0"); +var rect = Rectangle.Parse("0 0 100 200"); +var color = Color.Parse("#FF8800"); +var path = Path.Parse("M 0 0 C 1 2, 3 4, 5 6", null); +``` + +### SVG Export + +`Polygon`, `Polyline`, `ComplexCurve`, `Skeleton`, and `Path` support SVG export via the `[SvgExporter]` attribute and the `SvgExporter` infrastructure: + +```csharp +// Path.ToString() produces SVG path data +var path = Path.FromPoints(points); +string svgData = path.ToString(); // "M 0 0 C 1.5 2.3, 3.1 4.2, 5 6 ..." +``` + +## Code Examples + +### Points: Distance, Lerp, Rotate + +```csharp +var a = new Point(0, 0); +var b = new Point(3, 4); + +double dist = a.DistanceTo(b); // 5.0 +Point quarter = a.Lerp(b, 0.25); // {X=0.75, Y=1} +Point reflected = a.Reflect(b); // {X=6, Y=8} + +// Rotate 90 degrees around the origin +Degree angle = 90.0; +Point rotated = b.Rotate(angle); // {X=-4, Y=3} + +// Rotate around a custom center +var center = new Point(1, 1); +Point rotated2 = b.Rotate(angle, center); + +// Clamp to a rectangle +var bounds = new Rectangle(0, 0, 10, 10); +Point clamped = new Point(15, -3).Clamp(bounds); // {X=10, Y=0} +``` + +### 3D Points, Vectors, and Rotations + +```csharp +// 3D point operations +var p1 = new Point3(1, 2, 3); +var p2 = new Point3(4, 6, 3); +double dist3d = Point3.Distance(p1, p2); // 5.0 +Point3 mid3d = Point3.Lerp(p1, p2, 0.5); + +// 3D vector operations +var v = new Vector3(1, 0, 0); +var w = new Vector3(0, 1, 0); +var cross = Vector3.Cross(v, w); // (0, 0, 1) +double dot = Vector3.Dot(v, w); // 0.0 + +// Compute rotation between two directions +var rot = Quaternion.FromTo(v, w); +Vector3 result = rot.Rotate(v); // approximately (0, 1, 0) + +// Euler angles (ZYX convention) are a wire format: convert once, then use the quaternion +var rotation = Rotation3.FromDegrees(45, 0, 0).ToQuaternion(); +Vector3 rotated = rotation.Rotate((Vector3)p1); + +// Quaternion slerp +var r1 = Quaternion.Identity; +var r2 = Quaternion.FromAxisAngle(Vector3.EZ, Math.PI / 2); +var halfway = Quaternion.Slerp(r1, r2, 0.5); + +// Matrix3x3 rotation +var m = Matrix3x3.RotationZ(Degree.Create(45)); +var transformed = m.Transform(new Point3(1, 0, 0)); +``` + +### Spherical Coordinates + +```csharp +// Create spherical points with factory methods +var northPole = SphericalPoint.NorthPole(10.0); // +Z, radius=10 +var equator = SphericalPoint.OnEquator(5.0, Degree.Create(45)); +var elevated = SphericalPoint.FromElevation(5.0, + Degree.Create(0), Degree.Create(30)); + +// Convert to/from Cartesian +Point3 cartesian = equator; // implicit conversion +SphericalPoint spherical = cartesian; // implicit conversion + +// Orbit around a 3D center +var center = new Point3(10, 50, 0); +var points = new List>(); +for (int az = 0; az < 360; az += 30) + points.Add(center + SphericalPoint.OnEquator(5.0, Degree.Create(az))); + +// Rotation (O(1) angle operations) +var sp = SphericalPoint.OnEquator(5.0, Degree.Create(0)); +var rotated = sp.RotateAzimuth(Degree.Create(45)); // rotate around Z + +// Angular distance (Vincenty formula -- numerically stable) +var a = SphericalPoint.NorthPole(1.0); +var b = SphericalPoint.SouthPole(1.0); +Degree angDist = SphericalPoint.AngularDistance(a, b); // 180 degrees + +// Spherical linear interpolation +var mid = SphericalPoint.Slerp(a, b, 0.5); + +// Cylindrical coordinate conversions +CylindricalPoint cyl = sp; // implicit conversion +SphericalPoint back = cyl; // implicit conversion +``` + +### Poses and Trajectories + +```csharp +// Pose = position + orientation +var pose = new Pose3(100, 200, 300, 45, 0, 0); // x,y,z, rx,ry,rz (degrees) + +// Maths composes Frame3 (position + quaternion); Pose3 converts once, here mm -> m +var world = Frame3.FromPose3(new Pose3(10, 0, 0, 0, 0, 0), 0.001); +var local = Frame3.FromPose3(new Pose3(5, 0, 0, 0, 0, 90), 0.001); +var combined = world * local; +Pose3 back = combined.ToPose3(1000); // m -> mm, ZYX degrees + +// Create frame from a surface (3 points define a plane) +var a = new Point3(0, 0, 0); +var b = new Point3(1, 0, 0); +var c = new Point3(0, 1, 0); +var surface = Frame3.FromSurface(a, b, c); // Z-axis via right-hand rule + +// Trajectory with timed waypoints +var trajectory = new Trajectory3( + new Waypoint3(new Pose3(0, 0, 0, 0, 0, 0), 0.0), + new Waypoint3(new Pose3(50, 0, 25, 0, 30, 0), 2.5), + new Waypoint3(new Pose3(100, 0, 0, 0, 0, 0), 5.0)); + +// Interpolate at any time (binary search + lerp/slerp) +Pose3 atT = trajectory.AtTime(1.25); + +// Resample at uniform intervals +var resampled = trajectory.Resample(0.1); // every 0.1 seconds + +// Real-time playback +var controller = trajectory.Control(); +controller.PoseChanged += (_, e) => Console.WriteLine($"t={e.Time} pose={e.Pose}"); +controller.Start(); + +// 6-DOF joint angles for robotics +var home = new Joints6(0, -90, 0, 0, 90, 0); +var target = new Joints6(45, -60, 30, 0, 90, -45); +var halfway = Joints6.Lerp(home, target, 0.5); +bool inRange = target.IsBetween(minLimits, maxLimits); +``` + +### Shapes: Area and Perimeter + +```csharp +// Circle +var circle = new Circle(new Point(5, 5), 3.0); +double cArea = circle.Area(); // ~28.274 +double cPerim = circle.Perimeter(); // ~18.850 + +// Triangle +var triangle = new Triangle( + new Point(0, 0), + new Point(4, 0), + new Point(0, 3)); +double tArea = triangle.Area(); // 6.0 +double tPerim = triangle.Perimeter(); // 12.0 + +// Polygon (shoelace formula) +var poly = new Polygon( + new Point(0, 0), new Point(4, 0), + new Point(4, 3), new Point(0, 3)); +double pArea = poly.Area(); // 12.0 + +// 3D triangle +var t3 = new Triangle3( + new Point3(0, 0, 0), + new Point3(4, 0, 0), + new Point3(0, 3, 0)); +double area3 = t3.Area; // 6.0 +Vector3 normal = t3.Normal; // (0, 0, 1) +``` + +### Line-Circle Intersection + +```csharp +var circle = new Circle(new Point(0, 0), 5.0); +var line = Line.From(new Point(-10, 0), new Point(10, 0)); + +// Secant -- returns the chord as a segment +Segment? chord = circle.Intersect(line); +// chord.Value.Start ~ {X=-5, Y=0}, chord.Value.End ~ {X=5, Y=0} + +// Tangent detection +var tangentLine = Line.Horizontal(5.0); +Point? tangent = circle.TangentPoint(tangentLine); +// tangent.Value ~ {X=0, Y=5} +``` + +### Segment-Rectangle Clipping + +```csharp +var viewport = new Rectangle(0, 0, 100, 100); +var segment = new Segment( + new Point(-20, 50), + new Point(150, 50)); + +// Liang-Barsky clipping +Segment? clipped = segment.Intersect(viewport); +// clipped.Value: Start={X=0, Y=50}, End={X=100, Y=50} +``` + +### Path-Rectangle Clipping + +```csharp +var points = new[] +{ + new Point(10, 50), + new Point(50, 10), + new Point(90, 50), + new Point(130, 90) +}; +var path = Path.FromPoints(points); +var viewport = new Rectangle(20, 20, 60, 60); // 20,20 to 80,80 + +// Exact Bezier clipping -- returns sub-paths with curves split at boundaries +foreach (var subPath in path.Intersect(viewport)) +{ + Console.WriteLine($"Sub-path with {subPath.Count} Bezier segments"); +} +``` + +### Triangle Properties + +```csharp +var t = new Triangle( + new Point(0, 0), + new Point(4, 0), + new Point(2, 3)); + +// Derived circles +Circle incircle = t.Incircle(); // largest circle inside +Circle circumcircle = t.Circumcircle(); // passes through all vertices + +// Center points +Point centroid = t.Centroid(); +Point orthocenter = t.Orthocenter; + +// Classification +bool right = t.IsRight(); +bool equilateral = t.IsEquilateral(); +bool isosceles = t.IsIsosceles(); + +// Similarity and congruence +var t2 = new Triangle( + new Point(0, 0), + new Point(8, 0), + new Point(4, 6)); +bool similar = t.IsSimilarTo(t2); // true (same shape, scaled 2x) +bool congruent = t.IsCongruentTo(t2); // false (different size) + +// Interior angles +var (atA, atB, atC) = t.Angles; +``` + +### Polygon Operations + +```csharp +// Convex hull +var cloud = new Polygon( + new Point(0, 0), new Point(1, 3), + new Point(2, 1), new Point(4, 4), + new Point(3, 0), new Point(5, 2)); +Polygon hull = cloud.ConvexHull(); + +// Point containment (ray casting) +bool inside = hull.Contains(new Point(2, 2)); + +// Boolean operations +var a = new Polygon( /* ... */ ); +var b = new Polygon( /* ... */ ); +List> union = a.Union(b, removeHoles: true); +List> diff = a.Subtract(b); +List> inter = a.Intersect(b); + +// Simplify (remove collinear/close points) +Polygon simplified = hull.Simplify(epsilon: 0.1); + +// Convexity test +bool convex = hull.IsConvex(); + +// Clustering overlapping polygons +var clusters = Polygon.Cluster(polygons); +``` + +### Matrix Transformations (2D and 3D) + +```csharp +// 2D affine transformation +var matrix = Matrix.Identity; +matrix.Translate(10.0, 20.0); +matrix.Rotate(Degree.Create(45.0)); +matrix.Scale(2.0, 2.0); + +Point transformed = matrix.Transform(new Point(1, 0)); +Vector rotatedVec = matrix.Transform(new Vector(1, 0)); + +var combined = matrix1 * matrix2; +matrix.Invert(); + +// 3D rotation matrix +var rx = Matrix3x3.RotationX(Degree.Create(45)); +var ry = Matrix3x3.RotationY(Degree.Create(30)); +var rz = Matrix3x3.RotationZ(Degree.Create(60)); + +// Compose rotations +var rotation = rz * ry * rx; +var p = new Point3(1, 0, 0); +Point3 rotated3 = rotation * p; // or rotation.Transform(p) + +// Extract Euler angles +var (eRx, eRy, eRz) = rotation.ToEulerZYX(); + +// Convert to quaternion and back +Quaternion q = Quaternion.FromMatrix3x3(rotation); +Matrix3x3 back = q.ToMatrix3x3(); +``` + +### Unit-Aware Speed + +```csharp +using ModelingEvolution.Drawing.Units; + +// The unit is part of the type — the value is stored in that unit. +Speed> feed = new(300); // 300 m/min + +// Normalise to the SI base (m/s) or convert to another speed unit. +double si = feed.ToSi(); // 5.0 (m/s) +var inMmPerS = feed.To>(); // 5000 mm/s + +// Same-unit arithmetic, scalar scaling, and a dimensionless ratio. +var faster = feed * 2.0; // 600 m/min +double ratio = faster / feed; // 2.0 + +// Cross-unit add normalises through SI; result stays in the left unit. +var total = inMmPerS.Add(feed); // (5000 + 5000) mm/s + +Console.WriteLine(feed); // "300 m/min" + +// Compile-time safety: +// feed + inMmPerS; // error: different units — use .Add(...) +// feed.To>(); // error: not an ISpeedUnit +``` + +### Unit-Aware Angular Speed + +`AngularSpeed` mirrors `Speed` for rotation, on its own dimension with the SI +base rad/s. Keeping the two dimensions apart is the point: a rotary axis and a linear axis cannot be +commanded with each other's speed. + +```csharp +using ModelingEvolution.Drawing.Units; + +// The unit is part of the type — the value is stored in that unit. +AngularSpeed> tilt = new(5); // 5 °/s + +// Normalise to the SI base (rad/s) or convert to another angular-speed unit. +double si = tilt.ToSi(); // 0.0873 (rad/s) +var inRadPerS = tilt.To>(); + +// Signed: negating reverses the sense of rotation. +var reverse = -tilt; // -5 °/s + +// Scale by a percentage of a maximum, e.g. a jog knob at 20% of the axis limit. +var jog = tilt * new Percentage(20f); // 1 °/s + +Console.WriteLine(tilt); // "5 °/s" + +// Compile-time safety — the dimensions do not mix: +// tilt + new Speed>(20); // error: different types +// tilt.To>(); // error: not an IAngularSpeedUnit +// Speed> wrong; // error: not an ISpeedUnit +``` + +## Requirements + +- **.NET 10.0** or later (uses generic math interfaces from .NET 7+) +- **Dependencies**: [Clipper2](https://www.nuget.org/packages/Clipper2) 1.4.0, [protobuf-net](https://www.nuget.org/packages/protobuf-net) 3.2.45 + +## License + +See [LICENSE](LICENSE) for details. diff --git a/Sources/ModelingEvolution.Drawing/Trajectory3.cs b/Sources/ModelingEvolution.Drawing/Trajectory3.cs index 6f7e34f..147db50 100644 --- a/Sources/ModelingEvolution.Drawing/Trajectory3.cs +++ b/Sources/ModelingEvolution.Drawing/Trajectory3.cs @@ -1,340 +1,340 @@ -using System.Numerics; -using System.Runtime.InteropServices; -using System.Text.Json.Serialization; -using ProtoBuf; - -namespace ModelingEvolution.Drawing; - -/// -/// Represents an immutable time-parameterized path through 3D poses. -/// Each element is a containing a and a timestamp. -/// For untimed paths, use . -/// -/// The numeric type used for coordinates and time. -[JsonConverter(typeof(Trajectory3JsonConverterFactory))] -[ProtoContract] -public readonly record struct Trajectory3 : IPoolable, Lease>> - where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, - IFloatingPointIeee754, IMinMaxValue, IParsable -{ - internal readonly ReadOnlyMemory> _waypoints; - - [ProtoMember(1)] - private Waypoint3[] ProtoWaypoints - { - get - { - if (_waypoints.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_waypoints, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _waypoints.ToArray(); - } - init => _waypoints = value ?? ReadOnlyMemory>.Empty; - } - - /// - /// Gets the number of waypoints in this trajectory. - /// - [JsonIgnore] - public int Count => _waypoints.Length; - - /// - /// Returns a read-only span over the trajectory's waypoints. - /// Hoist the result before loops — this is a method call, not a field access. - /// - public ReadOnlySpan> AsSpan() => _waypoints.Span; - - /// - /// Gets the waypoint at the specified index. - /// - public Waypoint3 this[int index] => _waypoints.Span[index]; - - /// - /// Gets a read-only list of all waypoints. For high-performance code, prefer AsSpan() instead. - /// - public IReadOnlyList> Waypoints - { - get - { - if (_waypoints.Length == 0) return Array.Empty>(); - if (MemoryMarshal.TryGetArray(_waypoints, out var seg) - && seg.Offset == 0 && seg.Count == seg.Array!.Length) - return seg.Array; - return _waypoints.ToArray(); - } - } - - /// - /// Gets the first waypoint. - /// - [JsonIgnore] - public Waypoint3 Start => _waypoints.Span[0]; - - /// - /// Gets the last waypoint. - /// - [JsonIgnore] - public Waypoint3 End => _waypoints.Span[^1]; - - #region Constructors - - /// - /// Initializes a new empty trajectory. - /// - public Trajectory3() - { - _waypoints = ReadOnlyMemory>.Empty; - } - - /// - /// Initializes a new trajectory wrapping a ReadOnlyMemory of waypoints. Zero-copy. - /// - public Trajectory3(ReadOnlyMemory> memory) - { - _waypoints = memory; - } - - /// - /// Initializes a new trajectory from the specified waypoints array. - /// - public Trajectory3(params Waypoint3[] waypoints) - { - _waypoints = waypoints; - } - - /// - /// Initializes a new trajectory from the specified list of waypoints (copies to array). - /// - public Trajectory3(IList> waypoints) - { - if (waypoints is Waypoint3[] arr) - _waypoints = arr; - else - { - var tmp = new Waypoint3[waypoints.Count]; - for (int i = 0; i < waypoints.Count; i++) - tmp[i] = waypoints[i]; - _waypoints = tmp; - } - } - - #endregion - - #region Geometry & Kinematics - - /// - /// Gets the total duration of this trajectory (last time minus first time). - /// - public T Duration() - { - var span = AsSpan(); - if (span.Length < 2) return T.Zero; - return span[^1].Time - span[0].Time; - } - - /// - /// Computes the total positional length of this trajectory (sum of distances between consecutive positions). - /// - public T Length() - { - var span = AsSpan(); - int n = span.Length; - if (n < 2) return T.Zero; - - T length = T.Zero; - for (int i = 0; i < n - 1; i++) - length += Point3.Distance(span[i].Position, span[i + 1].Position); - return length; - } - - /// - /// Interpolates the pose at the given time using linear position interpolation and Slerp for rotation. - /// Clamps to the first/last waypoint if is outside the trajectory range. - /// - public Pose3 AtTime(T time) - { - var span = AsSpan(); - if (span.Length == 0) return Pose3.Identity; - if (span.Length == 1) return span[0].Pose; - - // Clamp to range - if (time <= span[0].Time) return span[0].Pose; - if (time >= span[^1].Time) return span[^1].Pose; - - // Binary search for the interval - int lo = 0, hi = span.Length - 2; - while (lo < hi) - { - int mid = (lo + hi) / 2; - if (span[mid + 1].Time < time) - lo = mid + 1; - else - hi = mid; - } - - var a = span[lo]; - var b = span[lo + 1]; - var dt = b.Time - a.Time; - if (dt == T.Zero) return a.Pose; - - var t = (time - a.Time) / dt; - return Frame3.Interpolate(Frame3.FromPose3(a.Pose, T.One), Frame3.FromPose3(b.Pose, T.One), t).ToPose3(T.One); - } - - /// - /// Returns a new trajectory with the waypoint order and timestamps reversed. - /// The new first waypoint has time 0, preserving the original time intervals. - /// - public Trajectory3 Reverse() - { - var span = AsSpan(); - if (span.Length == 0) return this; - - var totalTime = Duration(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - var t0 = span[0].Time; - for (int i = 0; i < span.Length; i++) - { - var original = span[span.Length - 1 - i]; - var newTime = totalTime - (original.Time - t0); - dst[i] = new Waypoint3(original.Pose, t0 + newTime); - } - return new Trajectory3(mem); - } - - /// - /// Returns a new trajectory with every waypoint pose composed as frame · pose. - /// The waypoint poses are stored as wire ; each is converted with lengthScale 1. - /// Timestamps are preserved. - /// - public Trajectory3 Transform(in Frame3 frame) - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = new Waypoint3((frame * Frame3.FromPose3(span[i].Pose, T.One)).ToPose3(T.One), span[i].Time); - return new Trajectory3(mem); - } - - /// - /// Strips timestamps and returns a . - /// - public PosePath3 ToPosePath() - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = span[i].Pose; - return new PosePath3(mem); - } - - /// - /// Extracts just the positions as a . - /// - public Polyline3 ToPolyline3() - { - var span = AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = span[i].Position; - return new Polyline3(mem); - } - - /// - /// Resamples the trajectory at uniform time intervals. - /// - /// Time between consecutive samples. - public Trajectory3 Resample(T interval) - { - var span = AsSpan(); - if (span.Length < 2) return this; - - var duration = Duration(); - var t0 = span[0].Time; - int sampleCount = int.CreateChecked(T.Floor(duration / interval)) + 1; - var mem = Alloc.Memory>(sampleCount); - var dst = mem.Span; - - for (int i = 0; i < sampleCount; i++) - { - var time = t0 + interval * T.CreateTruncating(i); - if (time > t0 + duration) time = t0 + duration; - dst[i] = new Waypoint3(AtTime(time), time); - } - - return new Trajectory3(mem); - } - - /// - /// Creates a for real-time playback of this trajectory. - /// - public TrajectoryController Control() => new(this); - - #endregion - - #region Operators - - /// - /// Translates all positions by adding the given vector (rotations and timestamps unchanged). - /// - public static Trajectory3 operator +(in Trajectory3 a, in Vector3 f) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = new Waypoint3(span[i].Pose with { Position = span[i].Pose.Position + f }, span[i].Time); - return new Trajectory3(mem); - } - - /// - /// Translates all positions by subtracting the given vector (rotations and timestamps unchanged). - /// - public static Trajectory3 operator -(in Trajectory3 a, in Vector3 f) - { - var span = a.AsSpan(); - var mem = Alloc.Memory>(span.Length); - var dst = mem.Span; - for (int i = 0; i < span.Length; i++) - dst[i] = new Waypoint3(span[i].Pose with { Position = span[i].Pose.Position - f }, span[i].Time); - return new Trajectory3(mem); - } - - #endregion - - #region Equality - - /// - public bool Equals(Trajectory3 other) => AsSpan().SequenceEqual(other.AsSpan()); - - /// - public override int GetHashCode() - { - var hash = new HashCode(); - var span = AsSpan(); - for (int i = 0; i < span.Length; i++) - hash.Add(span[i]); - return hash.ToHashCode(); - } - - #endregion - - /// - /// Detaches this trajectory's backing memory from the given scope and returns a lease. - /// - public Lease> DetachFrom(AllocationScope scope) - { - if (!MemoryMarshal.TryGetArray(_waypoints, out var seg)) - throw new InvalidOperationException("Cannot detach non-array-backed memory."); - var owner = scope.UntrackMemory(new Memory>(seg.Array!, seg.Offset, seg.Count)); - return new Lease> { _owner = owner }; - } - - /// - public override string ToString() => $"Trajectory3({Count} waypoints)"; -} +using System.Numerics; +using System.Runtime.InteropServices; +using System.Text.Json.Serialization; +using ProtoBuf; + +namespace ModelingEvolution.Drawing; + +/// +/// Represents an immutable time-parameterized path through 3D poses. +/// Each element is a containing a and a timestamp. +/// For untimed paths, use . +/// +/// The numeric type used for coordinates and time. +[JsonConverter(typeof(Trajectory3JsonConverterFactory))] +[ProtoContract] +public readonly record struct Trajectory3 : IPoolable, Lease>> + where T : INumber, ITrigonometricFunctions, IRootFunctions, IFloatingPoint, ISignedNumber, + IFloatingPointIeee754, IMinMaxValue, IParsable +{ + internal readonly ReadOnlyMemory> _waypoints; + + [ProtoMember(1)] + private Waypoint3[] ProtoWaypoints + { + get + { + if (_waypoints.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_waypoints, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _waypoints.ToArray(); + } + init => _waypoints = value ?? ReadOnlyMemory>.Empty; + } + + /// + /// Gets the number of waypoints in this trajectory. + /// + [JsonIgnore] + public int Count => _waypoints.Length; + + /// + /// Returns a read-only span over the trajectory's waypoints. + /// Hoist the result before loops — this is a method call, not a field access. + /// + public ReadOnlySpan> AsSpan() => _waypoints.Span; + + /// + /// Gets the waypoint at the specified index. + /// + public Waypoint3 this[int index] => _waypoints.Span[index]; + + /// + /// Gets a read-only list of all waypoints. For high-performance code, prefer AsSpan() instead. + /// + public IReadOnlyList> Waypoints + { + get + { + if (_waypoints.Length == 0) return Array.Empty>(); + if (MemoryMarshal.TryGetArray(_waypoints, out var seg) + && seg.Offset == 0 && seg.Count == seg.Array!.Length) + return seg.Array; + return _waypoints.ToArray(); + } + } + + /// + /// Gets the first waypoint. + /// + [JsonIgnore] + public Waypoint3 Start => _waypoints.Span[0]; + + /// + /// Gets the last waypoint. + /// + [JsonIgnore] + public Waypoint3 End => _waypoints.Span[^1]; + + #region Constructors + + /// + /// Initializes a new empty trajectory. + /// + public Trajectory3() + { + _waypoints = ReadOnlyMemory>.Empty; + } + + /// + /// Initializes a new trajectory wrapping a ReadOnlyMemory of waypoints. Zero-copy. + /// + public Trajectory3(ReadOnlyMemory> memory) + { + _waypoints = memory; + } + + /// + /// Initializes a new trajectory from the specified waypoints array. + /// + public Trajectory3(params Waypoint3[] waypoints) + { + _waypoints = waypoints; + } + + /// + /// Initializes a new trajectory from the specified list of waypoints (copies to array). + /// + public Trajectory3(IList> waypoints) + { + if (waypoints is Waypoint3[] arr) + _waypoints = arr; + else + { + var tmp = new Waypoint3[waypoints.Count]; + for (int i = 0; i < waypoints.Count; i++) + tmp[i] = waypoints[i]; + _waypoints = tmp; + } + } + + #endregion + + #region Geometry & Kinematics + + /// + /// Gets the total duration of this trajectory (last time minus first time). + /// + public T Duration() + { + var span = AsSpan(); + if (span.Length < 2) return T.Zero; + return span[^1].Time - span[0].Time; + } + + /// + /// Computes the total positional length of this trajectory (sum of distances between consecutive positions). + /// + public T Length() + { + var span = AsSpan(); + int n = span.Length; + if (n < 2) return T.Zero; + + T length = T.Zero; + for (int i = 0; i < n - 1; i++) + length += Point3.Distance(span[i].Position, span[i + 1].Position); + return length; + } + + /// + /// Interpolates the pose at the given time using linear position interpolation and Slerp for rotation. + /// Clamps to the first/last waypoint if is outside the trajectory range. + /// + public Pose3 AtTime(T time) + { + var span = AsSpan(); + if (span.Length == 0) return Pose3.Identity; + if (span.Length == 1) return span[0].Pose; + + // Clamp to range + if (time <= span[0].Time) return span[0].Pose; + if (time >= span[^1].Time) return span[^1].Pose; + + // Binary search for the interval + int lo = 0, hi = span.Length - 2; + while (lo < hi) + { + int mid = (lo + hi) / 2; + if (span[mid + 1].Time < time) + lo = mid + 1; + else + hi = mid; + } + + var a = span[lo]; + var b = span[lo + 1]; + var dt = b.Time - a.Time; + if (dt == T.Zero) return a.Pose; + + var t = (time - a.Time) / dt; + return Frame3.Interpolate(Frame3.FromPose3(a.Pose, T.One), Frame3.FromPose3(b.Pose, T.One), t).ToPose3(T.One); + } + + /// + /// Returns a new trajectory with the waypoint order and timestamps reversed. + /// The new first waypoint has time 0, preserving the original time intervals. + /// + public Trajectory3 Reverse() + { + var span = AsSpan(); + if (span.Length == 0) return this; + + var totalTime = Duration(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + var t0 = span[0].Time; + for (int i = 0; i < span.Length; i++) + { + var original = span[span.Length - 1 - i]; + var newTime = totalTime - (original.Time - t0); + dst[i] = new Waypoint3(original.Pose, t0 + newTime); + } + return new Trajectory3(mem); + } + + /// + /// Returns a new trajectory with every waypoint pose composed as frame · pose. + /// The waypoint poses are stored as wire ; each is converted with lengthScale 1. + /// Timestamps are preserved. + /// + public Trajectory3 Transform(in Frame3 frame) + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = new Waypoint3((frame * Frame3.FromPose3(span[i].Pose, T.One)).ToPose3(T.One), span[i].Time); + return new Trajectory3(mem); + } + + /// + /// Strips timestamps and returns a . + /// + public PosePath3 ToPosePath() + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = span[i].Pose; + return new PosePath3(mem); + } + + /// + /// Extracts just the positions as a . + /// + public Polyline3 ToPolyline3() + { + var span = AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = span[i].Position; + return new Polyline3(mem); + } + + /// + /// Resamples the trajectory at uniform time intervals. + /// + /// Time between consecutive samples. + public Trajectory3 Resample(T interval) + { + var span = AsSpan(); + if (span.Length < 2) return this; + + var duration = Duration(); + var t0 = span[0].Time; + int sampleCount = int.CreateChecked(T.Floor(duration / interval)) + 1; + var mem = Alloc.Memory>(sampleCount); + var dst = mem.Span; + + for (int i = 0; i < sampleCount; i++) + { + var time = t0 + interval * T.CreateTruncating(i); + if (time > t0 + duration) time = t0 + duration; + dst[i] = new Waypoint3(AtTime(time), time); + } + + return new Trajectory3(mem); + } + + /// + /// Creates a for real-time playback of this trajectory. + /// + public TrajectoryController Control() => new(this); + + #endregion + + #region Operators + + /// + /// Translates all positions by adding the given vector (rotations and timestamps unchanged). + /// + public static Trajectory3 operator +(in Trajectory3 a, in Vector3 f) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = new Waypoint3(span[i].Pose with { Position = span[i].Pose.Position + f }, span[i].Time); + return new Trajectory3(mem); + } + + /// + /// Translates all positions by subtracting the given vector (rotations and timestamps unchanged). + /// + public static Trajectory3 operator -(in Trajectory3 a, in Vector3 f) + { + var span = a.AsSpan(); + var mem = Alloc.Memory>(span.Length); + var dst = mem.Span; + for (int i = 0; i < span.Length; i++) + dst[i] = new Waypoint3(span[i].Pose with { Position = span[i].Pose.Position - f }, span[i].Time); + return new Trajectory3(mem); + } + + #endregion + + #region Equality + + /// + public bool Equals(Trajectory3 other) => AsSpan().SequenceEqual(other.AsSpan()); + + /// + public override int GetHashCode() + { + var hash = new HashCode(); + var span = AsSpan(); + for (int i = 0; i < span.Length; i++) + hash.Add(span[i]); + return hash.ToHashCode(); + } + + #endregion + + /// + /// Detaches this trajectory's backing memory from the given scope and returns a lease. + /// + public Lease> DetachFrom(AllocationScope scope) + { + if (!MemoryMarshal.TryGetArray(_waypoints, out var seg)) + throw new InvalidOperationException("Cannot detach non-array-backed memory."); + var owner = scope.UntrackMemory(new Memory>(seg.Array!, seg.Offset, seg.Count)); + return new Lease> { _owner = owner }; + } + + /// + public override string ToString() => $"Trajectory3({Count} waypoints)"; +} diff --git a/Sources/ModelingEvolution.Drawing/Trajectory3Builder.cs b/Sources/ModelingEvolution.Drawing/Trajectory3Builder.cs index 9979f79..a8e2261 100644 --- a/Sources/ModelingEvolution.Drawing/Trajectory3Builder.cs +++ b/Sources/ModelingEvolution.Drawing/Trajectory3Builder.cs @@ -1,433 +1,433 @@ -using System.Numerics; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; - -namespace ModelingEvolution.Drawing; - -/// -/// Fluent builder for constructing by appending waypoints. -/// Tracks current time and last pose for speed-based operations. -/// Supports corner blending via to create smooth arcs at waypoints. -/// -public sealed class Trajectory3Builder - where T : INumber, ITrigonometricFunctions, IRootFunctions, - IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue, IParsable -{ - private Waypoint3[] _buffer; - private Corner[] _corners; - private Speed[] _cornerSpeeds; - private int _count; - private T _currentTime; - private bool _hasCorners; - - private static readonly T Two = T.CreateTruncating(2); - private static readonly T Epsilon = T.CreateTruncating(1e-9); - - public Trajectory3Builder(int initialCapacity = 64) - { - _buffer = new Waypoint3[initialCapacity]; - _corners = new Corner[initialCapacity]; - _cornerSpeeds = new Speed[initialCapacity]; - _count = 0; - _currentTime = T.Zero; - } - - /// Gets the number of waypoints added so far. - public int Count => _count; - - /// Gets the current accumulated time. - public T CurrentTime => _currentTime; - - /// - /// Number of arc subdivision points per 90-degree turn. Default is 8. - /// Higher values produce smoother arcs. - /// - public int ArcResolution { get; set; } = 8; - - #region Add methods - - /// - /// Adds a waypoint with the given pose at the current accumulated time. - /// If this is the first waypoint, time starts at zero. - /// - public Trajectory3Builder Add(Pose3 pose) - { - EnsureCapacity(1); - _buffer[_count++] = new Waypoint3(pose, _currentTime); - return this; - } - - /// - /// Adds a waypoint with the given pose at an explicit time. - /// Updates the current accumulated time. - /// - public Trajectory3Builder Add(Pose3 pose, T time) - { - EnsureCapacity(1); - _buffer[_count++] = new Waypoint3(pose, time); - _currentTime = time; - return this; - } - - /// - /// Adds a waypoint directly. Updates the current accumulated time. - /// - public Trajectory3Builder Add(Waypoint3 waypoint) - { - EnsureCapacity(1); - _buffer[_count++] = waypoint; - _currentTime = waypoint.Time; - return this; - } - - /// - /// Adds a waypoint from raw position, rotation, and time values. - /// - public Trajectory3Builder Add(T x, T y, T z, T rx, T ry, T rz, T time) - { - return Add(new Pose3(x, y, z, rx, ry, rz), time); - } - - #endregion - - #region MoveTo methods - - /// - /// Moves to the given pose at the specified speed. - /// The time delta is computed as distance / speed from the last waypoint's position. - /// - public Trajectory3Builder MoveTo(Pose3 pose, Speed speed) - { - if (_count == 0) - return Add(pose); - - var lastPose = _buffer[_count - 1].Pose; - var distance = Point3.Distance(lastPose.Position, pose.Position); - var dt = distance / (T)speed; - _currentTime += dt; - - EnsureCapacity(1); - _buffer[_count++] = new Waypoint3(pose, _currentTime); - return this; - } - - /// - /// Moves to the given pose at the specified speed with a corner blend. - /// - public Trajectory3Builder MoveTo(Pose3 pose, Speed speed, Corner corner) - { - if (_count == 0) - return Add(pose); - - var lastPose = _buffer[_count - 1].Pose; - var distance = Point3.Distance(lastPose.Position, pose.Position); - var dt = distance / (T)speed; - _currentTime += dt; - - EnsureCapacity(1); - _corners[_count] = corner; - _hasCorners = _hasCorners || !corner.IsSharp; - _buffer[_count++] = new Waypoint3(pose, _currentTime); - return this; - } - - /// - /// Moves to the given pose at the specified speed with a corner blend and explicit corner speed. - /// - public Trajectory3Builder MoveTo(Pose3 pose, Speed speed, Corner corner, Speed cornerSpeed) - { - if (_count == 0) - return Add(pose); - - var lastPose = _buffer[_count - 1].Pose; - var distance = Point3.Distance(lastPose.Position, pose.Position); - var dt = distance / (T)speed; - _currentTime += dt; - - EnsureCapacity(1); - _corners[_count] = corner; - _cornerSpeeds[_count] = cornerSpeed; - _hasCorners = _hasCorners || !corner.IsSharp; - _buffer[_count++] = new Waypoint3(pose, _currentTime); - return this; - } - - /// - /// Moves to the given position and rotation at the specified speed. - /// - public Trajectory3Builder MoveTo(Point3 position, Rotation3 rotation, Speed speed) - => MoveTo(new Pose3(position, rotation), speed); - - /// - /// Moves to the given position and rotation at the specified speed with a corner blend. - /// - public Trajectory3Builder MoveTo(Point3 position, Rotation3 rotation, Speed speed, Corner corner) - => MoveTo(new Pose3(position, rotation), speed, corner); - - /// - /// Moves to the given position and rotation at the specified speed with a corner blend and explicit corner speed. - /// - public Trajectory3Builder MoveTo(Point3 position, Rotation3 rotation, Speed speed, Corner corner, Speed cornerSpeed) - => MoveTo(new Pose3(position, rotation), speed, corner, cornerSpeed); - - #endregion - - #region Append methods - - /// - /// Appends a span of waypoints. Updates the current accumulated time to the last waypoint's time. - /// - public Trajectory3Builder AddRange(ReadOnlySpan> waypoints) - { - EnsureCapacity(waypoints.Length); - waypoints.CopyTo(_buffer.AsSpan(_count)); - _count += waypoints.Length; - if (waypoints.Length > 0) - _currentTime = waypoints[^1].Time; - return this; - } - - /// - /// Appends all waypoints from an existing trajectory, shifting timestamps so the first - /// appended waypoint starts at the current accumulated time. - /// - public Trajectory3Builder AddTrajectory(in Trajectory3 trajectory) - { - var span = trajectory.AsSpan(); - if (span.Length == 0) return this; - - var offset = _currentTime - span[0].Time; - EnsureCapacity(span.Length); - for (int i = 0; i < span.Length; i++) - _buffer[_count++] = new Waypoint3(span[i].Pose, span[i].Time + offset); - _currentTime = span[^1].Time + offset; - return this; - } - - /// - /// Appends a pose path with uniformly distributed timestamps between startTime and endTime. - /// - public Trajectory3Builder AddPath(in PosePath3 path, T startTime, T endTime) - { - var span = path.AsSpan(); - if (span.Length == 0) return this; - if (span.Length == 1) - return Add(span[0], startTime); - - var n = T.CreateTruncating(span.Length - 1); - EnsureCapacity(span.Length); - for (int i = 0; i < span.Length; i++) - { - var t = startTime + (endTime - startTime) * T.CreateTruncating(i) / n; - _buffer[_count++] = new Waypoint3(span[i], t); - } - _currentTime = endTime; - return this; - } - - #endregion - - #region Build - - /// - /// Builds an immutable from the accumulated waypoints. - /// Corners with are expanded into smooth arcs. - /// Uses to respect active . - /// - public Trajectory3 Build() - { - if (_count == 0) return new Trajectory3(); - - // Fast path: no corners to expand - if (!_hasCorners) - { - var mem = Alloc.Memory>(_count); - _buffer.AsSpan(0, _count).CopyTo(mem.Span); - return new Trajectory3(mem); - } - - // Slow path: expand corners into arc waypoints - var output = new List>(_count * 2); - var timeShift = T.Zero; - - for (int i = 0; i < _count; i++) - { - // First and last waypoints cannot have corners - if (i == 0 || i == _count - 1 || _corners[i].IsSharp) - { - output.Add(new Waypoint3(_buffer[i].Pose, _buffer[i].Time + timeShift)); - continue; - } - - ExpandCorner(output, i, ref timeShift); - } - - var result = Alloc.Memory>(output.Count); - CollectionsMarshal.AsSpan(output).CopyTo(result.Span); - return new Trajectory3(result); - } - - private void ExpandCorner(List> output, int index, ref T timeShift) - { - var prev = _buffer[index - 1]; - var curr = _buffer[index]; - var next = _buffer[index + 1]; - var corner = _corners[index]; - var cornerSpeed = _cornerSpeeds[index]; - - var prevPos = prev.Position; - var currPos = curr.Position; - var nextPos = next.Position; - - // Directions from corner toward adjacent waypoints - var dIn = prevPos - currPos; // Vector3: from curr toward prev - var dOut = nextPos - currPos; // Vector3: from curr toward next - var distIn = dIn.Length; - var distOut = dOut.Length; - - // Clamp radius to half the shorter adjacent segment (prevents overlap) - var maxR = T.Min(distIn, distOut) / Two; - var r = T.Min(corner.Radius, maxR); - - if (distIn < Epsilon || distOut < Epsilon || r < Epsilon) - { - output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); - return; - } - - var dInNorm = dIn / distIn; - var dOutNorm = dOut / distOut; - - // Angle between the two directions from the corner vertex (interior angle) - var cosTheta = T.Clamp(Vector3.Dot(dInNorm, dOutNorm), -T.One, T.One); - var theta = T.Acos(cosTheta); - - // Deflection angle (how much the path actually turns) - var alpha = T.Pi - theta; - - // If nearly straight, skip blending - if (alpha < T.CreateTruncating(0.01)) - { - output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); - return; - } - - // Blend start/end points - var p1 = currPos + dInNorm * r; // on incoming segment, r from corner - var p2 = currPos + dOutNorm * r; // on outgoing segment, r from corner - - // Arc geometry: inscribed circle tangent to both segments at P1 and P2 - var halfTheta = theta / Two; - var sinHalf = T.Sin(halfTheta); - var cosHalf = T.Cos(halfTheta); - - if (T.Abs(cosHalf) < Epsilon) - { - // Degenerate case (U-turn), fall back to sharp corner - output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); - return; - } - - var arcRadius = r * sinHalf / cosHalf; // R = r * tan(theta/2) - var bisector = dInNorm + dOutNorm; - var bisectorLen = bisector.Length; - - if (bisectorLen < Epsilon) - { - output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); - return; - } - - bisector = bisector / bisectorLen; - var centerDist = r / cosHalf; - var arcCenter = currPos + bisector * centerDist; - - // Radius vectors from arc center to blend points - var v1 = p1 - arcCenter; - var v2 = p2 - arcCenter; - - // Rotation axis: cross(v1, v2) gives correct direction for P1→P2 arc - var arcNormal = Vector3.Cross(v1, v2); - var arcNormalLen = arcNormal.Length; - - if (arcNormalLen < Epsilon) - { - output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); - return; - } - - arcNormal = arcNormal / arcNormalLen; - - // Timestamps (with accumulated shift from previous corners) - var prevTime = prev.Time + timeShift; - var currTime = curr.Time + timeShift; - var nextTime = next.Time + timeShift; - - // P1 time: proportional position on incoming segment - var t_P1 = prevTime + (currTime - prevTime) * (distIn - r) / distIn; - // P2 natural time: proportional position on outgoing segment - var t_P2_natural = currTime + (nextTime - currTime) * r / distOut; - - // Arc length and duration - var arcLen = arcRadius * alpha; - var naturalDuration = t_P2_natural - t_P1; - T arcDuration; - - if (cornerSpeed._val > T.Zero) - arcDuration = arcLen / cornerSpeed._val; - else - arcDuration = naturalDuration; - - var t_P2 = t_P1 + arcDuration; - - // Update cumulative time shift - timeShift += arcDuration - naturalDuration; - - // Rotation interpolation: compute rotations at blend start/end - var rotFracIn = (distIn - r) / distIn; // how far P1 is along prev→curr (0..1) - var rotFracOut = r / distOut; // how far P2 is along curr→next (0..1) - // Quaternion slerp on the stored wire poses (lengthScale 1: only the rotations are used) - var qPrev = Frame3.FromPose3(prev.Pose, T.One).Rotation; - var qCurr = Frame3.FromPose3(curr.Pose, T.One).Rotation; - var qNext = Frame3.FromPose3(next.Pose, T.One).Rotation; - var rotP1 = Quaternion.Slerp(qPrev, qCurr, rotFracIn); - var rotP2 = Quaternion.Slerp(qCurr, qNext, rotFracOut); - - // Number of arc points (proportional to turn angle) - int arcPointCount = Math.Max(3, - (int)Math.Ceiling(double.CreateTruncating(alpha) / (Math.PI / 2.0) * ArcResolution)); - - // Generate arc points via Rodrigues rotation - for (int j = 0; j < arcPointCount; j++) - { - var t = T.CreateTruncating(j) / T.CreateTruncating(arcPointCount - 1); - - // Rotate v1 around arcNormal by angle (alpha * t) - var angle = alpha * t; - var cosA = T.Cos(angle); - var sinA = T.Sin(angle); - var crossNV1 = Vector3.Cross(arcNormal, v1); - var vRot = v1 * cosA + crossNV1 * sinA; - - var arcPos = arcCenter + vRot; - var arcTime = t_P1 + arcDuration * t; - var arcRot = Quaternion.Slerp(rotP1, rotP2, t); - - output.Add(new Waypoint3(new Frame3(arcPos, arcRot).ToPose3(T.One), arcTime)); - } - } - - #endregion - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private void EnsureCapacity(int additional) - { - var required = _count + additional; - if (required <= _buffer.Length) return; - var newSize = _buffer.Length; - while (newSize < required) newSize *= 2; - Array.Resize(ref _buffer, newSize); - Array.Resize(ref _corners, newSize); - Array.Resize(ref _cornerSpeeds, newSize); - } -} +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +namespace ModelingEvolution.Drawing; + +/// +/// Fluent builder for constructing by appending waypoints. +/// Tracks current time and last pose for speed-based operations. +/// Supports corner blending via to create smooth arcs at waypoints. +/// +public sealed class Trajectory3Builder + where T : INumber, ITrigonometricFunctions, IRootFunctions, + IFloatingPoint, ISignedNumber, IFloatingPointIeee754, IMinMaxValue, IParsable +{ + private Waypoint3[] _buffer; + private Corner[] _corners; + private Speed[] _cornerSpeeds; + private int _count; + private T _currentTime; + private bool _hasCorners; + + private static readonly T Two = T.CreateTruncating(2); + private static readonly T Epsilon = T.CreateTruncating(1e-9); + + public Trajectory3Builder(int initialCapacity = 64) + { + _buffer = new Waypoint3[initialCapacity]; + _corners = new Corner[initialCapacity]; + _cornerSpeeds = new Speed[initialCapacity]; + _count = 0; + _currentTime = T.Zero; + } + + /// Gets the number of waypoints added so far. + public int Count => _count; + + /// Gets the current accumulated time. + public T CurrentTime => _currentTime; + + /// + /// Number of arc subdivision points per 90-degree turn. Default is 8. + /// Higher values produce smoother arcs. + /// + public int ArcResolution { get; set; } = 8; + + #region Add methods + + /// + /// Adds a waypoint with the given pose at the current accumulated time. + /// If this is the first waypoint, time starts at zero. + /// + public Trajectory3Builder Add(Pose3 pose) + { + EnsureCapacity(1); + _buffer[_count++] = new Waypoint3(pose, _currentTime); + return this; + } + + /// + /// Adds a waypoint with the given pose at an explicit time. + /// Updates the current accumulated time. + /// + public Trajectory3Builder Add(Pose3 pose, T time) + { + EnsureCapacity(1); + _buffer[_count++] = new Waypoint3(pose, time); + _currentTime = time; + return this; + } + + /// + /// Adds a waypoint directly. Updates the current accumulated time. + /// + public Trajectory3Builder Add(Waypoint3 waypoint) + { + EnsureCapacity(1); + _buffer[_count++] = waypoint; + _currentTime = waypoint.Time; + return this; + } + + /// + /// Adds a waypoint from raw position, rotation, and time values. + /// + public Trajectory3Builder Add(T x, T y, T z, T rx, T ry, T rz, T time) + { + return Add(new Pose3(x, y, z, rx, ry, rz), time); + } + + #endregion + + #region MoveTo methods + + /// + /// Moves to the given pose at the specified speed. + /// The time delta is computed as distance / speed from the last waypoint's position. + /// + public Trajectory3Builder MoveTo(Pose3 pose, Speed speed) + { + if (_count == 0) + return Add(pose); + + var lastPose = _buffer[_count - 1].Pose; + var distance = Point3.Distance(lastPose.Position, pose.Position); + var dt = distance / (T)speed; + _currentTime += dt; + + EnsureCapacity(1); + _buffer[_count++] = new Waypoint3(pose, _currentTime); + return this; + } + + /// + /// Moves to the given pose at the specified speed with a corner blend. + /// + public Trajectory3Builder MoveTo(Pose3 pose, Speed speed, Corner corner) + { + if (_count == 0) + return Add(pose); + + var lastPose = _buffer[_count - 1].Pose; + var distance = Point3.Distance(lastPose.Position, pose.Position); + var dt = distance / (T)speed; + _currentTime += dt; + + EnsureCapacity(1); + _corners[_count] = corner; + _hasCorners = _hasCorners || !corner.IsSharp; + _buffer[_count++] = new Waypoint3(pose, _currentTime); + return this; + } + + /// + /// Moves to the given pose at the specified speed with a corner blend and explicit corner speed. + /// + public Trajectory3Builder MoveTo(Pose3 pose, Speed speed, Corner corner, Speed cornerSpeed) + { + if (_count == 0) + return Add(pose); + + var lastPose = _buffer[_count - 1].Pose; + var distance = Point3.Distance(lastPose.Position, pose.Position); + var dt = distance / (T)speed; + _currentTime += dt; + + EnsureCapacity(1); + _corners[_count] = corner; + _cornerSpeeds[_count] = cornerSpeed; + _hasCorners = _hasCorners || !corner.IsSharp; + _buffer[_count++] = new Waypoint3(pose, _currentTime); + return this; + } + + /// + /// Moves to the given position and rotation at the specified speed. + /// + public Trajectory3Builder MoveTo(Point3 position, Rotation3 rotation, Speed speed) + => MoveTo(new Pose3(position, rotation), speed); + + /// + /// Moves to the given position and rotation at the specified speed with a corner blend. + /// + public Trajectory3Builder MoveTo(Point3 position, Rotation3 rotation, Speed speed, Corner corner) + => MoveTo(new Pose3(position, rotation), speed, corner); + + /// + /// Moves to the given position and rotation at the specified speed with a corner blend and explicit corner speed. + /// + public Trajectory3Builder MoveTo(Point3 position, Rotation3 rotation, Speed speed, Corner corner, Speed cornerSpeed) + => MoveTo(new Pose3(position, rotation), speed, corner, cornerSpeed); + + #endregion + + #region Append methods + + /// + /// Appends a span of waypoints. Updates the current accumulated time to the last waypoint's time. + /// + public Trajectory3Builder AddRange(ReadOnlySpan> waypoints) + { + EnsureCapacity(waypoints.Length); + waypoints.CopyTo(_buffer.AsSpan(_count)); + _count += waypoints.Length; + if (waypoints.Length > 0) + _currentTime = waypoints[^1].Time; + return this; + } + + /// + /// Appends all waypoints from an existing trajectory, shifting timestamps so the first + /// appended waypoint starts at the current accumulated time. + /// + public Trajectory3Builder AddTrajectory(in Trajectory3 trajectory) + { + var span = trajectory.AsSpan(); + if (span.Length == 0) return this; + + var offset = _currentTime - span[0].Time; + EnsureCapacity(span.Length); + for (int i = 0; i < span.Length; i++) + _buffer[_count++] = new Waypoint3(span[i].Pose, span[i].Time + offset); + _currentTime = span[^1].Time + offset; + return this; + } + + /// + /// Appends a pose path with uniformly distributed timestamps between startTime and endTime. + /// + public Trajectory3Builder AddPath(in PosePath3 path, T startTime, T endTime) + { + var span = path.AsSpan(); + if (span.Length == 0) return this; + if (span.Length == 1) + return Add(span[0], startTime); + + var n = T.CreateTruncating(span.Length - 1); + EnsureCapacity(span.Length); + for (int i = 0; i < span.Length; i++) + { + var t = startTime + (endTime - startTime) * T.CreateTruncating(i) / n; + _buffer[_count++] = new Waypoint3(span[i], t); + } + _currentTime = endTime; + return this; + } + + #endregion + + #region Build + + /// + /// Builds an immutable from the accumulated waypoints. + /// Corners with are expanded into smooth arcs. + /// Uses to respect active . + /// + public Trajectory3 Build() + { + if (_count == 0) return new Trajectory3(); + + // Fast path: no corners to expand + if (!_hasCorners) + { + var mem = Alloc.Memory>(_count); + _buffer.AsSpan(0, _count).CopyTo(mem.Span); + return new Trajectory3(mem); + } + + // Slow path: expand corners into arc waypoints + var output = new List>(_count * 2); + var timeShift = T.Zero; + + for (int i = 0; i < _count; i++) + { + // First and last waypoints cannot have corners + if (i == 0 || i == _count - 1 || _corners[i].IsSharp) + { + output.Add(new Waypoint3(_buffer[i].Pose, _buffer[i].Time + timeShift)); + continue; + } + + ExpandCorner(output, i, ref timeShift); + } + + var result = Alloc.Memory>(output.Count); + CollectionsMarshal.AsSpan(output).CopyTo(result.Span); + return new Trajectory3(result); + } + + private void ExpandCorner(List> output, int index, ref T timeShift) + { + var prev = _buffer[index - 1]; + var curr = _buffer[index]; + var next = _buffer[index + 1]; + var corner = _corners[index]; + var cornerSpeed = _cornerSpeeds[index]; + + var prevPos = prev.Position; + var currPos = curr.Position; + var nextPos = next.Position; + + // Directions from corner toward adjacent waypoints + var dIn = prevPos - currPos; // Vector3: from curr toward prev + var dOut = nextPos - currPos; // Vector3: from curr toward next + var distIn = dIn.Length; + var distOut = dOut.Length; + + // Clamp radius to half the shorter adjacent segment (prevents overlap) + var maxR = T.Min(distIn, distOut) / Two; + var r = T.Min(corner.Radius, maxR); + + if (distIn < Epsilon || distOut < Epsilon || r < Epsilon) + { + output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); + return; + } + + var dInNorm = dIn / distIn; + var dOutNorm = dOut / distOut; + + // Angle between the two directions from the corner vertex (interior angle) + var cosTheta = T.Clamp(Vector3.Dot(dInNorm, dOutNorm), -T.One, T.One); + var theta = T.Acos(cosTheta); + + // Deflection angle (how much the path actually turns) + var alpha = T.Pi - theta; + + // If nearly straight, skip blending + if (alpha < T.CreateTruncating(0.01)) + { + output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); + return; + } + + // Blend start/end points + var p1 = currPos + dInNorm * r; // on incoming segment, r from corner + var p2 = currPos + dOutNorm * r; // on outgoing segment, r from corner + + // Arc geometry: inscribed circle tangent to both segments at P1 and P2 + var halfTheta = theta / Two; + var sinHalf = T.Sin(halfTheta); + var cosHalf = T.Cos(halfTheta); + + if (T.Abs(cosHalf) < Epsilon) + { + // Degenerate case (U-turn), fall back to sharp corner + output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); + return; + } + + var arcRadius = r * sinHalf / cosHalf; // R = r * tan(theta/2) + var bisector = dInNorm + dOutNorm; + var bisectorLen = bisector.Length; + + if (bisectorLen < Epsilon) + { + output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); + return; + } + + bisector = bisector / bisectorLen; + var centerDist = r / cosHalf; + var arcCenter = currPos + bisector * centerDist; + + // Radius vectors from arc center to blend points + var v1 = p1 - arcCenter; + var v2 = p2 - arcCenter; + + // Rotation axis: cross(v1, v2) gives correct direction for P1→P2 arc + var arcNormal = Vector3.Cross(v1, v2); + var arcNormalLen = arcNormal.Length; + + if (arcNormalLen < Epsilon) + { + output.Add(new Waypoint3(curr.Pose, curr.Time + timeShift)); + return; + } + + arcNormal = arcNormal / arcNormalLen; + + // Timestamps (with accumulated shift from previous corners) + var prevTime = prev.Time + timeShift; + var currTime = curr.Time + timeShift; + var nextTime = next.Time + timeShift; + + // P1 time: proportional position on incoming segment + var t_P1 = prevTime + (currTime - prevTime) * (distIn - r) / distIn; + // P2 natural time: proportional position on outgoing segment + var t_P2_natural = currTime + (nextTime - currTime) * r / distOut; + + // Arc length and duration + var arcLen = arcRadius * alpha; + var naturalDuration = t_P2_natural - t_P1; + T arcDuration; + + if (cornerSpeed._val > T.Zero) + arcDuration = arcLen / cornerSpeed._val; + else + arcDuration = naturalDuration; + + var t_P2 = t_P1 + arcDuration; + + // Update cumulative time shift + timeShift += arcDuration - naturalDuration; + + // Rotation interpolation: compute rotations at blend start/end + var rotFracIn = (distIn - r) / distIn; // how far P1 is along prev→curr (0..1) + var rotFracOut = r / distOut; // how far P2 is along curr→next (0..1) + // Quaternion slerp on the stored wire poses (lengthScale 1: only the rotations are used) + var qPrev = Frame3.FromPose3(prev.Pose, T.One).Rotation; + var qCurr = Frame3.FromPose3(curr.Pose, T.One).Rotation; + var qNext = Frame3.FromPose3(next.Pose, T.One).Rotation; + var rotP1 = Quaternion.Slerp(qPrev, qCurr, rotFracIn); + var rotP2 = Quaternion.Slerp(qCurr, qNext, rotFracOut); + + // Number of arc points (proportional to turn angle) + int arcPointCount = Math.Max(3, + (int)Math.Ceiling(double.CreateTruncating(alpha) / (Math.PI / 2.0) * ArcResolution)); + + // Generate arc points via Rodrigues rotation + for (int j = 0; j < arcPointCount; j++) + { + var t = T.CreateTruncating(j) / T.CreateTruncating(arcPointCount - 1); + + // Rotate v1 around arcNormal by angle (alpha * t) + var angle = alpha * t; + var cosA = T.Cos(angle); + var sinA = T.Sin(angle); + var crossNV1 = Vector3.Cross(arcNormal, v1); + var vRot = v1 * cosA + crossNV1 * sinA; + + var arcPos = arcCenter + vRot; + var arcTime = t_P1 + arcDuration * t; + var arcRot = Quaternion.Slerp(rotP1, rotP2, t); + + output.Add(new Waypoint3(new Frame3(arcPos, arcRot).ToPose3(T.One), arcTime)); + } + } + + #endregion + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private void EnsureCapacity(int additional) + { + var required = _count + additional; + if (required <= _buffer.Length) return; + var newSize = _buffer.Length; + while (newSize < required) newSize *= 2; + Array.Resize(ref _buffer, newSize); + Array.Resize(ref _corners, newSize); + Array.Resize(ref _cornerSpeeds, newSize); + } +} From bd027b820d318d6e658651e5aaeeea179b382aac Mon Sep 17 00:00:00 2001 From: Rafal Maciag Date: Sat, 24 Oct 2026 19:02:29 +0000 Subject: [PATCH 6/6] =?UTF-8?q?S3:=20exact=20Euler=20extraction=20near=20r?= =?UTF-8?q?y=20=3D=20=C2=B190=C2=B0;=20drop=20Point3=20+=20Rotation3=20(ep?= =?UTF-8?q?ic-129)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Rotation3.FromQuaternion: ry = atan2(sin, cos) with cos = sqrt(r00² + r10²); gimbal branch only when cos ry < 1e-9 (rz = 0, rx = atan2(-r12, r11), exact for any ry). The old branch (|sin ry| >= 1 - 1e-6) forced ry = ±90° and changed the rotation by up to 0.08°. Reaches ToEulerZYX, FromMatrix3x3, Frame3.ToPose3 and PosePath3Builder.Step. - Tests over |ry| in [89.9°, 90°] (10 000 seeded) and at exactly ±90°: matrices within 1e-8 (1e-12 at ±90°); ToEulerZYX compared with master's angles over the full ry range where master was exact. - Point3 operator +(Point3, Rotation3) → Pose3 deleted (use Pose3.From); EulerSurfaceTests also checks Point3. - Quaternion.FromTo doc states the half-turn axis for opposite directions. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_014wH5aL3TZhJNvLqa8YTku8 --- .../EulerSurfaceTests.cs | 10 ++- .../Frame3EulerTests.cs | 47 ++++++++++++ .../Matrix3x3Tests.cs | 71 +++++++++++++++++-- .../Point3Tests.cs | 18 ----- Sources/ModelingEvolution.Drawing/Point3.cs | 2 - .../ModelingEvolution.Drawing/Quaternion.cs | 4 +- .../ModelingEvolution.Drawing/Rotation3.cs | 50 ++++++------- 7 files changed, 143 insertions(+), 59 deletions(-) diff --git a/Sources/ModelingEvolution.Drawing.Tests/EulerSurfaceTests.cs b/Sources/ModelingEvolution.Drawing.Tests/EulerSurfaceTests.cs index d7425d2..f11d784 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/EulerSurfaceTests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/EulerSurfaceTests.cs @@ -4,8 +4,8 @@ namespace ModelingEvolution.Drawing.Tests; /// -/// Epic-129 Q7: and are wire and edit formats. The vector and matrix -/// maths types neither take nor return them; direction → rotation is . +/// Epic-129 Q7: and are wire and edit formats. The vector, point and +/// matrix maths types neither take nor return them (Pose3.From(p, r) builds a pose); direction → rotation is . /// public class EulerSurfaceTests { @@ -39,6 +39,12 @@ public void Vector3_HasNoMemberTakingOrReturningRotation3OrPose3() MembersTouchingWireTypes(typeof(Vector3)).Should().BeEmpty(); } + [Fact] + public void Point3_HasNoMemberTakingOrReturningRotation3OrPose3() + { + MembersTouchingWireTypes(typeof(Point3)).Should().BeEmpty(); + } + [Fact] public void Matrix3x3_HasNoMemberTakingOrReturningRotation3OrPose3() { diff --git a/Sources/ModelingEvolution.Drawing.Tests/Frame3EulerTests.cs b/Sources/ModelingEvolution.Drawing.Tests/Frame3EulerTests.cs index 6b86232..0453efa 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Frame3EulerTests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Frame3EulerTests.cs @@ -34,6 +34,53 @@ public void FromPose3_ToPose3_RoundTripsOver10000RandomPoses() } } + private static double NearGimbalRy(Random rng) => (rng.Next(2) == 0 ? 1 : -1) * (89.9 + rng.NextDouble() * 0.1); + + private static double[,] RotationOf(Pose3 p) => + Mul(Mul(Rz((double)p.Rz * Math.PI / 180), Ry((double)p.Ry * Math.PI / 180)), Rx((double)p.Rx * Math.PI / 180)); + + private static void ShouldBeSameRotation(double[,] actual, double[,] expected, double tol, string because) + { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + Math.Abs(actual[i, j] - expected[i, j]).Should().BeLessThan(tol, because); + } + + [Fact] + public void FromPose3_ToPose3_KeepsTheRotationNearGimbal_10000Random() + { + // |ry| in [89.9°, 90°]: the rx/rz split may change, the rotation may not (every element within 1e-8). + var rng = new Random(129); + for (int i = 0; i < 10_000; i++) + { + var p = new Pose3(Uniform(rng, 2000), Uniform(rng, 2000), Uniform(rng, 2000), + Uniform(rng, 180), NearGimbalRy(rng), Uniform(rng, 180)); + + var back = Frame3.FromPose3(p, 0.001).ToPose3(1000); + + Math.Abs(back.X - p.X).Should().BeLessThan(1e-9); + Math.Abs(back.Y - p.Y).Should().BeLessThan(1e-9); + Math.Abs(back.Z - p.Z).Should().BeLessThan(1e-9); + ShouldBeSameRotation(RotationOf(back), RotationOf(p), 1e-8, $"rotation of {p}"); + } + } + + [Theory] + [InlineData(30, 90, 0)] + [InlineData(30, 90, 45)] + [InlineData(-120, -90, 0)] + [InlineData(-120, -90, 70)] + public void FromPose3_ToPose3_AtExactly90_KeepsTheRotationWithRzZero(double rx, double ry, double rz) + { + var p = new Pose3(0, 0, 0, rx, ry, rz); + + var back = Frame3.FromPose3(p, 1).ToPose3(1); + + ((double)back.Ry).Should().BeApproximately(ry, 1e-6); + ((double)back.Rz).Should().Be(0); + ShouldBeSameRotation(RotationOf(back), RotationOf(p), 1e-12, $"rotation of {p}"); + } + [Fact] public void FromPose3_ScalesPositionAndKeepsRotation() { diff --git a/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs index e3aaa34..dbedfa2 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs @@ -564,22 +564,79 @@ public void RotationZYX_EqualsMasterClosedForm_10000Random() } } + // Master extracted rx = atan2(M32, M33), ry = asin(-M31), rz = atan2(M21, M11) when |cos ry| > 1e-6 (exact there), + // and forced rz = 0 inside that band. + private static void ShouldMatchMaster(Matrix3x3 m, double angleTol) + { + var (erx, ery, erz) = m.ToEulerZYX(); + var cosRy = Math.Sqrt(m.M11 * m.M11 + m.M21 * m.M21); + if (cosRy > 1e-6) + { + ((double)erx).Should().BeApproximately(Math.Atan2(m.M32, m.M33) * 180 / Math.PI, angleTol); + ((double)ery).Should().BeApproximately(Math.Asin(-m.M31) * 180 / Math.PI, angleTol); + ((double)erz).Should().BeApproximately(Math.Atan2(m.M21, m.M11) * 180 / Math.PI, angleTol); + } + else + { + AssertMatricesEqual(MasterRotationZYX((double)erx, (double)ery, (double)erz), m, 1e-8); + } + } + + [Fact] + public void ToEulerZYX_EqualsMasterAngles_10000Random() + { + var rng = new Random(129); + for (int i = 0; i < 10_000; i++) + { + double rx = rng.NextDouble() * 358 - 179, ry = rng.NextDouble() * 180 - 90, rz = rng.NextDouble() * 358 - 179; + ShouldMatchMaster(MasterRotationZYX(rx, ry, rz), 1e-9); + } + } + + [Fact] + public void ToEulerZYX_EqualsMasterNearGimbal_10000Random() + { + // |ry| in [89.9°, 90°]: master's angles where master was exact (atan2 of elements of size cos ry, so the + // tolerance is wider), the same matrix inside master's band. + var rng = new Random(129); + for (int i = 0; i < 10_000; i++) + { + double rx = rng.NextDouble() * 358 - 179, rz = rng.NextDouble() * 358 - 179; + double ry = (rng.Next(2) == 0 ? 1 : -1) * (89.9 + rng.NextDouble() * 0.1); + ShouldMatchMaster(MasterRotationZYX(rx, ry, rz), 1e-6); + } + } + [Fact] - public void ToEulerZYX_EqualsMasterAngles_OutsideGimbalBand_10000Random() + public void RotationZYX_ToEulerZYX_RoundTripsNearGimbal_10000Random() { - // Master extracted rx = atan2(M32, M33), ry = asin(-M31), rz = atan2(M21, M11) when |cos ry| > 1e-6. var rng = new Random(129); for (int i = 0; i < 10_000; i++) { - double rx = rng.NextDouble() * 358 - 179, ry = rng.NextDouble() * 178 - 89, rz = rng.NextDouble() * 358 - 179; - var m = MasterRotationZYX(rx, ry, rz); + double rx = rng.NextDouble() * 358 - 179, rz = rng.NextDouble() * 358 - 179; + double ry = (rng.Next(2) == 0 ? 1 : -1) * (89.9 + rng.NextDouble() * 0.1); + var m = Matrix3x3.RotationZYX(rx, ry, rz); + var (erx, ery, erz) = m.ToEulerZYX(); - ((double)erx).Should().BeApproximately(Math.Atan2(m.M32, m.M33) * 180 / Math.PI, 1e-9); - ((double)ery).Should().BeApproximately(Math.Asin(-m.M31) * 180 / Math.PI, 1e-9); - ((double)erz).Should().BeApproximately(Math.Atan2(m.M21, m.M11) * 180 / Math.PI, 1e-9); + + AssertMatricesEqual(Matrix3x3.RotationZYX(erx, ery, erz), m, 1e-8); } } + [Theory] + [InlineData(30, 90, 45)] + [InlineData(-120, -90, 70)] + public void RotationZYX_ToEulerZYX_AtExactly90_KeepsTheMatrixWithRzZero(double rx, double ry, double rz) + { + var m = Matrix3x3.RotationZYX(rx, ry, rz); + + var (erx, ery, erz) = m.ToEulerZYX(); + + ((double)erz).Should().Be(0); + ((double)ery).Should().BeApproximately(ry, 1e-6); + AssertMatricesEqual(Matrix3x3.RotationZYX(erx, ery, erz), m, 1e-12); + } + #endregion private static void AssertIsIdentity(Matrix3x3 m, double tol = Tol) diff --git a/Sources/ModelingEvolution.Drawing.Tests/Point3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Point3Tests.cs index 06c8be5..a159fbe 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Point3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Point3Tests.cs @@ -109,24 +109,6 @@ public void Point3_Equality_Works() a.Equals(b).Should().BeTrue(); } - [Fact] - public void Point3_AddRotation_ReturnsPose3() - { - var point = new Point3(10, 20, 30); - var rotation = new Rotation3(45, 90, -30); - - Pose3 pose = point + rotation; - - pose.Position.Should().Be(point); - pose.Rotation.Should().Be(rotation); - pose.X.Should().Be(10); - pose.Y.Should().Be(20); - pose.Z.Should().Be(30); - pose.Rx.Should().Be(45); - pose.Ry.Should().Be(90); - pose.Rz.Should().Be(-30); - } - #endregion #region Methods diff --git a/Sources/ModelingEvolution.Drawing/Point3.cs b/Sources/ModelingEvolution.Drawing/Point3.cs index 27b9d7e..f6151eb 100644 --- a/Sources/ModelingEvolution.Drawing/Point3.cs +++ b/Sources/ModelingEvolution.Drawing/Point3.cs @@ -202,8 +202,6 @@ public Point3 Truncating() [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Point3 operator /(Point3 pt, T scalar) => new(pt._x / scalar, pt._y / scalar, pt._z / scalar); [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Pose3 operator +(Point3 pt, Rotation3 r) => new(pt, r); - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool operator ==(Point3 a, Point3 b) => a._x == b._x && a._y == b._y && a._z == b._z; [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool operator !=(Point3 a, Point3 b) => !(a == b); diff --git a/Sources/ModelingEvolution.Drawing/Quaternion.cs b/Sources/ModelingEvolution.Drawing/Quaternion.cs index 35f5a19..a212e28 100644 --- a/Sources/ModelingEvolution.Drawing/Quaternion.cs +++ b/Sources/ModelingEvolution.Drawing/Quaternion.cs @@ -159,7 +159,9 @@ public static Quaternion FromRpy(T roll, T pitch, T yaw) /// /// Creates the shortest-arc rotation that turns the direction of into the direction of - /// . For opposite directions it turns by π about an axis perpendicular to . + /// . For opposite directions (1 + from̂·tô < 1e-12) it is the half-turn about + /// normalize(EX × from), or about normalize(EY × from) when is along X + /// (|EX × from̂|² < 1e-12); so +X → −X turns about −Z and +Z → −Z about −Y. /// /// Thrown when either vector is zero. public static Quaternion FromTo(Vector3 from, Vector3 to) diff --git a/Sources/ModelingEvolution.Drawing/Rotation3.cs b/Sources/ModelingEvolution.Drawing/Rotation3.cs index 78015e9..3c3f822 100644 --- a/Sources/ModelingEvolution.Drawing/Rotation3.cs +++ b/Sources/ModelingEvolution.Drawing/Rotation3.cs @@ -24,6 +24,7 @@ namespace ModelingEvolution.Drawing; private readonly Degree _rz; // Yaw - rotation around Z axis private static readonly T Two = T.CreateTruncating(2); + private static readonly T GimbalCos = T.CreateTruncating(1e-9); /// /// Represents zero rotation (identity). @@ -144,43 +145,34 @@ public Quaternion ToQuaternion() } /// - /// Creates a rotation from a quaternion. + /// Creates a rotation from a quaternion: the one rotation → Euler (ZYX, R = Rz·Ry·Rx) formula of the library. + /// The result reproduces the rotation for every ry, ±90° included; when cos ry < 1e-9 (gimbal lock) rz = 0. /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Rotation3 FromQuaternion(Quaternion q) { - // Extract Euler angles from quaternion (ZYX order) - var sinp = Two * (q.W * q.Y - q.Z * q.X); - T rx, ry, rz; - - var gimbalThreshold = T.One - T.CreateTruncating(1e-6); - - if (T.Abs(sinp) >= gimbalThreshold) + // Extract ZYX Euler angles (R = Rz·Ry·Rx) from the rotation matrix elements of q. + // ry = atan2(sin, cos) keeps full precision near ±90°; the gimbal branch is taken only when cos ry is + // below 1e-9, where rx and rz share one degree of freedom: rz = 0 and rx from R = Ry·Rx, exact for any ry. + var sinp = Two * (q.W * q.Y - q.Z * q.X); // -r20 + var r00 = T.One - Two * (q.Y * q.Y + q.Z * q.Z); + var r10 = Two * (q.W * q.Z + q.X * q.Y); + var cosp = T.Sqrt(r00 * r00 + r10 * r10); + var ry = T.Atan2(sinp, cosp); + T rx, rz; + + if (cosp < GimbalCos) { - // Gimbal lock: Ry ~ +/-90 deg. Rx and Rz share one degree of freedom. - // Convention: set Rz = 0, recover Rx from rotation matrix R[0][1], R[0][2]. - ry = T.CopySign(T.Pi / Two, sinp); rz = T.Zero; - - var r01 = Two * (q.X * q.Y - q.W * q.Z); - var r02 = Two * (q.X * q.Z + q.W * q.Y); - - rx = sinp > T.Zero - ? T.Atan2(r01, r02) - : T.Atan2(-r01, -r02); + var r12 = Two * (q.Y * q.Z - q.W * q.X); + var r11 = T.One - Two * (q.X * q.X + q.Z * q.Z); + rx = T.Atan2(-r12, r11); } else { - // Normal case - var sinrCosp = Two * (q.W * q.X + q.Y * q.Z); - var cosrCosp = T.One - Two * (q.X * q.X + q.Y * q.Y); - rx = T.Atan2(sinrCosp, cosrCosp); - - ry = T.Asin(sinp); - - var sinyCosp = Two * (q.W * q.Z + q.X * q.Y); - var cosyCosp = T.One - Two * (q.Y * q.Y + q.Z * q.Z); - rz = T.Atan2(sinyCosp, cosyCosp); + var r21 = Two * (q.W * q.X + q.Y * q.Z); + var r22 = T.One - Two * (q.X * q.X + q.Y * q.Y); + rx = T.Atan2(r21, r22); + rz = T.Atan2(r10, r00); } // rx, ry, rz are in radians — wrap in Radian then implicit-convert to Degree