diff --git a/.github/scripts/grep-gate.sh b/.github/scripts/grep-gate.sh new file mode 100755 index 0000000..1ae46b0 --- /dev/null +++ b/.github/scripts/grep-gate.sh @@ -0,0 +1,28 @@ +#!/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/' + # 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 +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..60ea306 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. @@ -93,8 +93,8 @@ 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); +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)); @@ -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. | @@ -202,15 +202,16 @@ 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. | -| `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. | +| `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 | |---|---| -| `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. | @@ -479,17 +481,17 @@ 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); +var rot = Quaternion.FromTo(v, 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); +// 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 = Rotation3.FromDegrees(0, 0, 0); -var r2 = Rotation3.FromDegrees(0, 0, 90); -var halfway = Rotation3.Slerp(r1, r2, 0.5); +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)); @@ -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( @@ -744,8 +747,9 @@ Point3 rotated3 = rotation * p; // or rotation.Transform(p) // Extract Euler angles var (eRx, eRy, eRz) = rotation.ToEulerZYX(); -// Convert to quaternion -Quaternion q = rotation.ToQuaternion(); +// Convert to quaternion and back +Quaternion q = Quaternion.FromMatrix3x3(rotation); +Matrix3x3 back = q.ToMatrix3x3(); ``` ### Unit-Aware Speed diff --git a/Sources/ModelingEvolution.Drawing.Tests/EulerSurfaceTests.cs b/Sources/ModelingEvolution.Drawing.Tests/EulerSurfaceTests.cs new file mode 100644 index 0000000..f11d784 --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/EulerSurfaceTests.cs @@ -0,0 +1,53 @@ +using System.Reflection; +using FluentAssertions; + +namespace ModelingEvolution.Drawing.Tests; + +/// +/// 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 +{ + 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 Point3_HasNoMemberTakingOrReturningRotation3OrPose3() + { + MembersTouchingWireTypes(typeof(Point3)).Should().BeEmpty(); + } + + [Fact] + public void Matrix3x3_HasNoMemberTakingOrReturningRotation3OrPose3() + { + MembersTouchingWireTypes(typeof(Matrix3x3)).Should().BeEmpty(); + } +} diff --git a/Sources/ModelingEvolution.Drawing.Tests/Frame3EulerTests.cs b/Sources/ModelingEvolution.Drawing.Tests/Frame3EulerTests.cs new file mode 100644 index 0000000..0453efa --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Frame3EulerTests.cs @@ -0,0 +1,140 @@ +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}"); + } + } + + 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() + { + 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..fecc8db --- /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.TransformVector(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.TransformVector(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.TransformVector(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.TransformVector(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.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); + // 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.TransformVector(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.TransformVector(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.TransformVector(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.TransformVector(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.TransformVector(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.TransformVector(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.TransformVector(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.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.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); + } + + [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..5be2f6a --- /dev/null +++ b/Sources/ModelingEvolution.Drawing.Tests/Frame3Tests.cs @@ -0,0 +1,198 @@ +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 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); + } + } + + [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/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/Matrix3x3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs index 1e9bd43..dbedfa2 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Matrix3x3Tests.cs @@ -327,17 +327,17 @@ public void ToEulerZYX_GimbalLock_AtPositive90() 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(); + var q = Quaternion.FromMatrix3x3(m); // Convert quaternion back to rotation, then to matrix var rot = Rotation3.FromQuaternion(q); - var m2 = rot.ToMatrix3x3(); + var m2 = rot.ToQuaternion().ToMatrix3x3(); AssertMatricesEqual(m, m2, 1e-9); } [Fact] public void ToQuaternion_Identity_IsUnitQuaternion() { - var q = Matrix3x3.Identity.ToQuaternion(); + var q = Quaternion.FromMatrix3x3(Matrix3x3.Identity); q.W.Should().BeApproximately(1, TolLoose); q.X.Should().BeApproximately(0, TolLoose); q.Y.Should().BeApproximately(0, TolLoose); @@ -352,10 +352,10 @@ public void ToQuaternion_Identity_IsUnitQuaternion() [InlineData(10, 20, 30)] [InlineData(-15, 45, -60)] [InlineData(0, 0, 0)] - public void Rotation3_ToMatrix3x3_RoundTrip(double rxd, double ryd, double rzd) + public void Rotation3_QuaternionMatrix_RoundTrip(double rxd, double ryd, double rzd) { var rot = new Rotation3(rxd, ryd, rzd); - var m = rot.ToMatrix3x3(); + var m = rot.ToQuaternion().ToMatrix3x3(); var rot2 = Rotation3.FromMatrix3x3(m); ((double)rot2.Rx).Should().BeApproximately(rxd, TolLoose); ((double)rot2.Ry).Should().BeApproximately(ryd, TolLoose); @@ -363,13 +363,13 @@ public void Rotation3_ToMatrix3x3_RoundTrip(double rxd, double ryd, double rzd) } [Fact] - public void Rotation3_Rotate_MatchesMatrix_Transform() + public void Rotation3_QuaternionRotate_MatchesMatrix_Transform() { var rot = new Rotation3(30, 45, 60); - var m = rot.ToMatrix3x3(); + var m = Matrix3x3.RotationZYX(rot.Rx, rot.Ry, rot.Rz); var v = new Vector3(1, 2, 3); - var rotated = rot.Rotate(v); + var rotated = rot.ToQuaternion().Rotate(v); var transformed = m.Transform(v); rotated.X.Should().BeApproximately(transformed.X, TolLoose); @@ -486,6 +486,159 @@ public void ToString_Format() #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); + } + } + + // 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 RotationZYX_ToEulerZYX_RoundTripsNearGimbal_10000Random() + { + 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); + var m = Matrix3x3.RotationZYX(rx, ry, rz); + + var (erx, ery, erz) = m.ToEulerZYX(); + + 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) { m.M11.Should().BeApproximately(1, 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.Tests/Polyline3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Polyline3Tests.cs index 2483fa2..5367a66 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Polyline3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Polyline3Tests.cs @@ -458,4 +458,17 @@ public void Json_Empty() 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/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/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/PosePath3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/PosePath3Tests.cs index 034671c..c66b581 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/PosePath3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/PosePath3Tests.cs @@ -149,4 +149,19 @@ 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/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.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..0866aa6 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,19 +351,19 @@ 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); } [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 backToDir = (Vector3)rotation; + var rotation = Rotation3.FromQuaternion(Quaternion.FromTo(Vector3.EZ, originalDir)); + 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/Trajectory3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Trajectory3Tests.cs index 9bece25..d22056e 100644 --- a/Sources/ModelingEvolution.Drawing.Tests/Trajectory3Tests.cs +++ b/Sources/ModelingEvolution.Drawing.Tests/Trajectory3Tests.cs @@ -221,4 +221,20 @@ 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.Tests/Vector3Tests.cs b/Sources/ModelingEvolution.Drawing.Tests/Vector3Tests.cs index 57018a1..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.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.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.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.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.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 = (Vector3)rotation; - 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 = (Vector3)rotation; - 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 = (Vector3)rotation; - 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 = (Vector3)rotation; - - // 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 = (Vector3)rotation; - - // 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/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..a8e9418 --- /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].TransformVector(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/Matrix3x3.cs b/Sources/ModelingEvolution.Drawing/Matrix3x3.cs index 89275ed..5606798 100644 --- a/Sources/ModelingEvolution.Drawing/Matrix3x3.cs +++ b/Sources/ModelingEvolution.Drawing/Matrix3x3.cs @@ -5,7 +5,9 @@ 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. +/// operations and 3D rotation matrix construction. Rotation ⇄ matrix goes through +/// (, ); the Euler edge is +/// / . /// [Matrix3x3JsonConverter] public readonly record struct Matrix3x3( @@ -15,8 +17,6 @@ public readonly record struct Matrix3x3( 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, @@ -182,21 +182,12 @@ public static Matrix3x3 RotationZ(Degree angle) } /// - /// Creates a ZYX Euler rotation matrix: R = Rz(rz) * Ry(ry) * Rx(rx). - /// This matches the convention used by . + /// 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). /// - [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); - } + public static Matrix3x3 RotationZYX(Degree rx, Degree ry, Degree rz) => + Quaternion.FromRpy((T)(Radian)rx, (T)(Radian)ry, (T)(Radian)rz).ToMatrix3x3(); #endregion @@ -215,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) @@ -230,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. @@ -251,86 +241,17 @@ public static Matrix3x3 SimilarityFromVectors(Vector3 source, Vector3 t #endregion - #region Euler & Quaternion Conversions + #region Euler Conversion /// - /// Extracts ZYX Euler angles (in degrees) from this rotation matrix. - /// Assumes R = Rz(rz) * Ry(ry) * Rx(rx). Handles gimbal lock at ry = ±90°. + /// 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 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); + var r = Rotation3.FromQuaternion(Quaternion.FromMatrix3x3(this)); + return (r.Rx, r.Ry, r.Rz); } #endregion 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/Polyline3.cs b/Sources/ModelingEvolution.Drawing/Polyline3.cs index 2588782..d3bd7b2 100644 --- a/Sources/ModelingEvolution.Drawing/Polyline3.cs +++ b/Sources/ModelingEvolution.Drawing/Polyline3.cs @@ -310,28 +310,15 @@ public Polyline3 Simplify(T epsilon) } /// - /// Returns a new polyline with all points rotated by the given rotation. + /// Returns a new polyline with all points transformed by the given frame (rotation + translation). /// - public Polyline3 Transform(Rotation3 rotation) + 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] = 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 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..c78a703 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; } @@ -151,15 +151,16 @@ public PosePath3 Reverse() } /// - /// Returns a new path with all poses transformed by the given pose (rotation + translation). + /// 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(Pose3 pose) + 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] = 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/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 4394743..a212e28 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,114 @@ 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 (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) + { + 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) => + 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) + { + var quarter = T.One / (Two * Two); + 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, (m.M32 - m.M23) / s, (m.M13 - m.M31) / s, (m.M21 - m.M12) / s); + } + else if (m.M11 > m.M22 && m.M11 > m.M33) + { + 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 (m.M22 > m.M33) + { + 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 + 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 1162b46..e801758 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. @@ -93,8 +93,8 @@ 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); +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)); @@ -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. | @@ -202,15 +202,16 @@ 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. | -| `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. | +| `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 | |---|---| -| `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. | @@ -481,17 +483,17 @@ 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); +var rot = Quaternion.FromTo(v, 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); +// 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 = Rotation3.FromDegrees(0, 0, 0); -var r2 = Rotation3.FromDegrees(0, 0, 90); -var halfway = Rotation3.Slerp(r1, r2, 0.5); +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)); @@ -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( @@ -746,8 +749,9 @@ Point3 rotated3 = rotation * p; // or rotation.Transform(p) // Extract Euler angles var (eRx, eRy, eRz) = rotation.ToEulerZYX(); -// Convert to quaternion -Quaternion q = rotation.ToQuaternion(); +// Convert to quaternion and back +Quaternion q = Quaternion.FromMatrix3x3(rotation); +Matrix3x3 back = q.ToMatrix3x3(); ``` ### Unit-Aware Speed diff --git a/Sources/ModelingEvolution.Drawing/Rotation3.cs b/Sources/ModelingEvolution.Drawing/Rotation3.cs index 907421f..3c3f822 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] @@ -22,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). @@ -138,63 +141,38 @@ 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); } /// - /// 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 @@ -204,64 +182,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. /// @@ -274,10 +194,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)] @@ -286,20 +202,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 @@ -308,13 +215,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/Trajectory3.cs b/Sources/ModelingEvolution.Drawing/Trajectory3.cs index 00cd1b7..147db50 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); } /// @@ -205,16 +205,17 @@ public Trajectory3 Reverse() } /// - /// Returns a new trajectory with all poses transformed by the given pose (rotation + translation). + /// 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(Pose3 pose) + 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(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..a8e2261 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; @@ -386,8 +386,12 @@ private void ExpandCorner(List> output, int index, ref T timeShift) // 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); + // 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, @@ -407,9 +411,9 @@ private void ExpandCorner(List> output, int index, ref T timeShift) var arcPos = arcCenter + vRot; var arcTime = t_P1 + arcDuration * t; - var arcRot = Rotation3.Slerp(rotP1, rotP2, t); + var arcRot = Quaternion.Slerp(rotP1, rotP2, t); - output.Add(new Waypoint3(new Pose3(arcPos, arcRot), arcTime)); + output.Add(new Waypoint3(new Frame3(arcPos, arcRot).ToPose3(T.One), arcTime)); } } diff --git a/Sources/ModelingEvolution.Drawing/Triangle3.cs b/Sources/ModelingEvolution.Drawing/Triangle3.cs index e53efad..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() - { - // Get rotation from surface, then create pose at centroid - var surfacePose = Pose3.FromSurface(_a, _b, _c); - return new Pose3(Centroid, surfacePose.Rotation); - } + /// A frame representing the triangle's plane orientation at its center. + public Frame3 ToFrame() => new(Centroid, Frame3.FromSurface(_a, _b, _c).Rotation); #region Operators diff --git a/Sources/ModelingEvolution.Drawing/Vector3.cs b/Sources/ModelingEvolution.Drawing/Vector3.cs index 5ce7397..c5afcd8 100644 --- a/Sources/ModelingEvolution.Drawing/Vector3.cs +++ b/Sources/ModelingEvolution.Drawing/Vector3.cs @@ -190,84 +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.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. - /// - 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)); - } - /// /// Calculates the projection of this vector onto another vector. /// @@ -332,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)] @@ -376,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