diff --git a/Bearded.Utilities.Testing/Geometry/Bivector2Assertions.cs b/Bearded.Utilities.Testing/Geometry/Bivector2Assertions.cs index 945f030a..6494510d 100644 --- a/Bearded.Utilities.Testing/Geometry/Bivector2Assertions.cs +++ b/Bearded.Utilities.Testing/Geometry/Bivector2Assertions.cs @@ -27,7 +27,7 @@ public AndConstraint NotBe(Bivector2 other, string because public AndConstraint BeApproximately( Bivector2 other, float precision, string because = "", params object[] becauseArgs) { - subject.Magnitude.Should().BeApproximately(other.Magnitude, precision, because, becauseArgs); + subject.Xy.Should().BeApproximately(other.Xy, precision, because, becauseArgs); return new AndConstraint(this); } @@ -35,7 +35,7 @@ public AndConstraint BeApproximately( public AndConstraint NotBeApproximately( Bivector2 other, float precision, string because = "", params object[] becauseArgs) { - subject.Magnitude.Should().NotBeApproximately(other.Magnitude, precision, because, becauseArgs); + subject.Xy.Should().NotBeApproximately(other.Xy, precision, because, becauseArgs); return new AndConstraint(this); } } diff --git a/Bearded.Utilities.Tests/Assertions/Vector3Assertions.cs b/Bearded.Utilities.Tests/Assertions/Vector3Assertions.cs new file mode 100644 index 00000000..3a3b8974 --- /dev/null +++ b/Bearded.Utilities.Tests/Assertions/Vector3Assertions.cs @@ -0,0 +1,39 @@ +using System; +using FluentAssertions; +using FluentAssertions.Execution; +using OpenTK.Mathematics; + +namespace Bearded.Utilities.Tests.Assertions +{ + sealed class Vector3Assertions + { + private readonly Vector3 subject; + + public Vector3Assertions(Vector3 subject) + { + this.subject = subject; + } + + [CustomAssertion] + public AndConstraint BeApproximately( + Vector3 expectedValue, + float precision, + string because = "", + params object[] becauseArgs) + { + var xDifference = Math.Abs(subject.X - expectedValue.X); + var yDifference = Math.Abs(subject.Y - expectedValue.Y); + var zDifference = Math.Abs(subject.Z - expectedValue.Z); + + Execute.Assertion + .BecauseOf(because, becauseArgs) + .ForCondition(xDifference <= precision && yDifference <= precision && zDifference <= precision) + .FailWith( + "Expected {context:value} to be approximately {1} +/- {2}{reason}, " + + "but {0}'s coordinates differed by {3} (x), {4} (y), and {5} (z).", + subject, expectedValue, precision, xDifference, yDifference, zDifference); + + return new AndConstraint(this); + } + } +} diff --git a/Bearded.Utilities.Tests/Assertions/Vector3Extensions.cs b/Bearded.Utilities.Tests/Assertions/Vector3Extensions.cs new file mode 100644 index 00000000..eeb275cd --- /dev/null +++ b/Bearded.Utilities.Tests/Assertions/Vector3Extensions.cs @@ -0,0 +1,9 @@ +using OpenTK.Mathematics; + +namespace Bearded.Utilities.Tests.Assertions +{ + static class Vector3Extensions + { + public static Vector3Assertions Should(this Vector3 subject) => new Vector3Assertions(subject); + } +} diff --git a/Bearded.Utilities.Tests/Geometry/Bivector2Tests.cs b/Bearded.Utilities.Tests/Geometry/Bivector2Tests.cs index e55fdd8a..927a19a9 100644 --- a/Bearded.Utilities.Tests/Geometry/Bivector2Tests.cs +++ b/Bearded.Utilities.Tests/Geometry/Bivector2Tests.cs @@ -12,7 +12,7 @@ namespace Bearded.Utilities.Tests.Geometry; public sealed class Bivector2Tests { - private const float epsilon = 1E-6f; + private const float epsilon = 1E-3f; public Bivector2Tests() { @@ -63,8 +63,52 @@ public void WedgeIsAntiSymmetric(Vector2 left, Vector2 right) wedge1.Should().Be(-wedge2); } + [Fact] + public void UnitBivectorHasMagnitudeOne() + { + Bivector2.Unit.Magnitude.Should().BeApproximately(1, epsilon); + Bivector2.Unit.MagnitudeSquared.Should().BeApproximately(1, epsilon); + } + + [Fact] + public void ZeroBivectorHasMagnitudeZero() + { + Bivector2.Zero.Magnitude.Should().BeApproximately(0, epsilon); + Bivector2.Zero.MagnitudeSquared.Should().BeApproximately(0, epsilon); + } + [Property] - public void AddingBivectorsAddsMagnitudes(float f1, float f2) + public void NormalizedNonZeroBivectorHasMagnitudeOne(float xy) + { + var bivector = new Bivector2(xy); + if (bivector == Bivector2.Zero) return; + + var normalized = bivector.Normalized(); + + normalized.Magnitude.Should().BeApproximately(1, epsilon); + normalized.MagnitudeSquared.Should().BeApproximately(1, epsilon); + } + + [Fact] + public void NormalizedZeroBivectorIsZeroBivector() + { + var bivector = Bivector2.Zero; + + bivector.Normalized().Should().Be(bivector); + } + + [Property] + public void NormalizedBivectorRetainsSign(float xy) + { + var bivector = new Bivector2(xy); + + var normalized = bivector.Normalized(); + + MathF.Sign(bivector.Xy).Should().Be(MathF.Sign(normalized.Xy)); + } + + [Property] + public void AddingBivectorsAddsComponents(float f1, float f2) { var bivector1 = new Bivector2(f1); var bivector2 = new Bivector2(f2); @@ -74,7 +118,7 @@ public void AddingBivectorsAddsMagnitudes(float f1, float f2) } [Property] - public void SubtractingBivectorsSubtractsMagnitudes(float f1, float f2) + public void SubtractingBivectorsSubtractsComponents(float f1, float f2) { var bivector1 = new Bivector2(f1); var bivector2 = new Bivector2(f2); @@ -84,7 +128,26 @@ public void SubtractingBivectorsSubtractsMagnitudes(float f1, float f2) } [Property] - public void BivectorsWithSameMagnitudeAreEqual(float magnitude) + public void ScalingBivectorScalesItsComponents(float xy, float scalar) + { + var bivector = new Bivector2(xy); + var scaled = scalar * bivector; + + scaled.Xy.Should().BeApproximately(xy * scalar, epsilon); + } + + [Property] + public void DividingBivectorByScalarDividesItsComponents(float xy, float divider) + { + if (divider == 0) return; + var bivector = new Bivector2(xy); + var scaled = bivector / divider; + + scaled.Xy.Should().BeApproximately(xy / divider, epsilon); + } + + [Property] + public void BivectorsWithSameComponentsAreEqual(float magnitude) { var bivector1 = new Bivector2(magnitude); var bivector2 = new Bivector2(magnitude); @@ -96,7 +159,7 @@ public void BivectorsWithSameMagnitudeAreEqual(float magnitude) } [Property] - public void BivectorsWithDifferentMagnitudeAreNotEqual(float f1, float f2) + public void BivectorsWithDifferentComponentsAreNotEqual(float f1, float f2) { // ReSharper disable once CompareOfFloatsByEqualityOperator if (f1 == f2) f2++; diff --git a/Bearded.Utilities.Tests/Geometry/Bivector3Tests.cs b/Bearded.Utilities.Tests/Geometry/Bivector3Tests.cs index c942aa78..30d2d841 100644 --- a/Bearded.Utilities.Tests/Geometry/Bivector3Tests.cs +++ b/Bearded.Utilities.Tests/Geometry/Bivector3Tests.cs @@ -1,3 +1,4 @@ +using System.Collections.Generic; using Bearded.Utilities.Geometry; using Bearded.Utilities.Testing.Geometry; using Bearded.Utilities.Tests.Generators; @@ -11,7 +12,7 @@ namespace Bearded.Utilities.Tests.Geometry; public sealed class Bivector3Tests { - private const float epsilon = 1E-6f; + private const float epsilon = 1E-3f; public Bivector3Tests() { @@ -62,6 +63,77 @@ public void WedgeIsAntiSymmetric(Vector3 left, Vector3 right) wedge1.Should().Be(-wedge2); } + [Property] + public void FromAxisCreatesBivectorWithSameMagnitudeAsVector(Vector3 axis) + { + var bivector = Bivector3.FromAxis(axis); + + bivector.Magnitude.Should().BeApproximately(axis.Length, epsilon); + } + + [Property] + public void FromAxisRetainsSign(Vector3 axis) + { + var bivector = Bivector3.FromAxis(axis); + var reverseBivector = Bivector3.FromAxis(-axis); + + bivector.Should().BeApproximately(-reverseBivector, epsilon); + } + + [Theory] + [MemberData(nameof(UnitVectorsWithOrthogonalPlanes))] + public void FromAxisReturnsOrthogonalPlaneToUnitVectors(Vector3 axis, Bivector3 expectedBivector) + { + var bivector = Bivector3.FromAxis(axis); + + bivector.Should().BeApproximately(expectedBivector, epsilon); + } + + public static IEnumerable UnitVectorsWithOrthogonalPlanes() + { + yield return new object[] { Vector3.UnitX, Bivector3.UnitYz }; + yield return new object[] { Vector3.UnitY, -Bivector3.UnitXz }; + yield return new object[] { Vector3.UnitZ, Bivector3.UnitXy }; + } + + [Fact] + public void UnitBivectorsHaveMagnitudeOne() + { + Bivector3.UnitXy.Magnitude.Should().BeApproximately(1, epsilon); + Bivector3.UnitXy.MagnitudeSquared.Should().BeApproximately(1, epsilon); + Bivector3.UnitYz.Magnitude.Should().BeApproximately(1, epsilon); + Bivector3.UnitYz.MagnitudeSquared.Should().BeApproximately(1, epsilon); + Bivector3.UnitXz.Magnitude.Should().BeApproximately(1, epsilon); + Bivector3.UnitXz.MagnitudeSquared.Should().BeApproximately(1, epsilon); + } + + [Fact] + public void ZeroBivectorHasMagnitudeZero() + { + Bivector3.Zero.Magnitude.Should().BeApproximately(0, epsilon); + Bivector3.Zero.MagnitudeSquared.Should().BeApproximately(0, epsilon); + } + + [Property] + public void NormalizedNonZeroBivectorHasMagnitudeOne(float xy, float yz, float xz) + { + var bivector = new Bivector3(xy, yz, xz); + if (bivector == Bivector3.Zero) return; + + var normalized = bivector.Normalized(); + + normalized.Magnitude.Should().BeApproximately(1, epsilon); + normalized.MagnitudeSquared.Should().BeApproximately(1, epsilon); + } + + [Fact] + public void NormalizedZeroBivectorIsZeroBivector() + { + var bivector = Bivector3.Zero; + + bivector.Normalized().Should().Be(bivector); + } + [Property] public void AddingBivectorsAddsComponents(float xy1, float xy2, float yz1, float yz2, float xz1, float xz2) { @@ -83,6 +155,29 @@ public void SubtractingBivectorsSubtractsComponents( sum.Should().BeApproximately(new Bivector3(xy1 - xy2, yz1 - yz2, xz1 - xz2), epsilon); } + [Property] + public void ScalingBivectorScalesItsComponents(float xy, float yz, float xz, float scalar) + { + var bivector = new Bivector3(xy, yz, xz); + var scaled = scalar * bivector; + + scaled.Xy.Should().BeApproximately(xy * scalar, epsilon); + scaled.Yz.Should().BeApproximately(yz * scalar, epsilon); + scaled.Xz.Should().BeApproximately(xz * scalar, epsilon); + } + + [Property] + public void DividingBivectorByScalarDividesItsComponents(float xy, float yz, float xz, float divider) + { + if (divider == 0) return; + var bivector = new Bivector3(xy, yz, xz); + var scaled = bivector / divider; + + scaled.Xy.Should().BeApproximately(xy / divider, epsilon); + scaled.Yz.Should().BeApproximately(yz / divider, epsilon); + scaled.Xz.Should().BeApproximately(xz / divider, epsilon); + } + [Property] public void BivectorsWithSameComponentsAreEqual(float xy, float yz, float xz) { diff --git a/Bearded.Utilities.Tests/Geometry/CircularArc2Tests.cs b/Bearded.Utilities.Tests/Geometry/CircularArc2Tests.cs index deeb89d7..9fb5eba9 100644 --- a/Bearded.Utilities.Tests/Geometry/CircularArc2Tests.cs +++ b/Bearded.Utilities.Tests/Geometry/CircularArc2Tests.cs @@ -11,7 +11,7 @@ namespace Bearded.Utilities.Tests.Geometry; public sealed class CircularArc2Tests { - private const float epsilon = 0.001f; + private const float epsilon = 1E-3f; public CircularArc2Tests() { diff --git a/Bearded.Utilities.Tests/Geometry/Rotor2Tests.cs b/Bearded.Utilities.Tests/Geometry/Rotor2Tests.cs new file mode 100644 index 00000000..2c553d4a --- /dev/null +++ b/Bearded.Utilities.Tests/Geometry/Rotor2Tests.cs @@ -0,0 +1,222 @@ +using System; +using Bearded.Utilities.Geometry; +using Bearded.Utilities.Tests.Assertions; +using Bearded.Utilities.Tests.Generators; +using FluentAssertions; +using FsCheck; +using FsCheck.Xunit; +using OpenTK.Mathematics; +using Xunit; + +namespace Bearded.Utilities.Tests.Geometry +{ + public sealed class Rotor2Tests + { + private const float epsilon = 1E-3f; + + public Rotor2Tests() + { + Arb.Register(); + Arb.Register(); + } + + [Fact] + public void IdentityRotorHasMagnitudeOne() + { + Rotor2.Identity.Magnitude.Should().BeApproximately(1, epsilon); + Rotor2.Identity.MagnitudeSquared.Should().BeApproximately(1, epsilon); + } + + [Property] + public void IdentityRotorDoesNotRotateVector(Vector2 vector) + { + AssertionExtensions.Should(Rotor2.Identity.Rotate(vector)).Be(vector); + } + + [Property] + public void NormalizedRotorHasMagnitudeOne(float scalar, Vector2 l, Vector2 r) + { + var rotor = new Rotor2(scalar, Bivector2.Wedge(l, r)); + if (rotor == new Rotor2()) return; + var normalizedRotor = rotor.Normalized(); + + normalizedRotor.Magnitude.Should().BeApproximately(1, epsilon); + normalizedRotor.MagnitudeSquared.Should().BeApproximately(1, epsilon); + } + + [Fact] + public void NormalizedZeroRotorIsZeroRotor() + { + var rotor = new Rotor2(); + + rotor.Normalized().Should().Be(rotor); + } + + [Property] + public void NormalizedRotorDoesNotChangeVectorLengthOnRotation(float scalar, Vector2 l, Vector2 r, Vector2 v) + { + var rotor = new Rotor2(scalar, Bivector2.Wedge(l, r)); + // Zero rotors are special. + if (rotor == new Rotor2()) return; + var normalizedRotor = rotor.Normalized(); + + var rotatedV = normalizedRotor.Rotate(v); + rotatedV.Length.Should().BeApproximately(v.Length, epsilon); + } + + [Property] + public void ReversedKeepsMagnitudeInvariant(float scalar, Vector2 l, Vector2 r) + { + var rotor = new Rotor2(scalar, Bivector2.Wedge(l, r)); + + var reversedRotor = rotor.Reversed(); + + reversedRotor.Magnitude.Should().BeApproximately(rotor.Magnitude, epsilon); + } + + [Property] + public void ReversedRotorRotatesInTheOppositeDirection(Vector2 from, Vector2 to) + { + if (from == Vector2.Zero) from = Vector2.UnitX; + if (to == Vector2.Zero) to = Vector2.UnitY; + if (areCollinear(from, to)) return; + + var rotor = Rotor2.Between(from.Normalized(), to.Normalized()); + var reversedRotor = rotor.Reversed(); + + var rotated = reversedRotor.Rotate(to); + + rotated.Normalized().Should().BeApproximately(from.Normalized(), epsilon); + } + + [Property] + public void RotorBetweenVectorsHasMagnitudeOne(Vector2 from, Vector2 to) + { + if (from == Vector2.Zero) from = Vector2.UnitX; + if (to == Vector2.Zero) to = Vector2.UnitY; + + var rotor = Rotor2.Between(from, to); + + rotor.Magnitude.Should().BeApproximately(1, epsilon); + rotor.MagnitudeSquared.Should().BeApproximately(1, epsilon); + } + + [Property] + public void RotorBetweenSameVectorIsIdentity(Vector2 fromTo) + { + if (fromTo == Vector2.Zero) fromTo = Vector2.One; + + var rotor = Rotor2.Between(fromTo, fromTo); + + rotor.Should().Be(Rotor2.Identity); + } + + [Property] + public void RotorBetweenVectorsShouldRotateFromToDirectionOfTo(Vector2 from, Vector2 to) + { + if (from == Vector2.Zero) from = Vector2.UnitX; + if (to == Vector2.Zero) to = Vector2.UnitY; + if (areCollinear(to, from)) return; + + var rotor = Rotor2.Between(from.Normalized(), to.Normalized()); + + var rotated = rotor.Rotate(from); + + rotated.Normalized().Should().BeApproximately(to.Normalized(), epsilon); + } + + [Property] + public void RotorFromPlaneAngleRotatesByAngle(Vector2 l, Vector2 r, float rads, Vector2 toRotate) + { + if (areCollinear(l, r) || toRotate == Vector2.Zero) return; + var plane = Bivector2.Wedge(l, r); + var angle = Angle.FromRadians(rads); + + var rotor = Rotor2.FromPlaneAngle(plane, angle); + var rotated = rotor.Rotate(toRotate); + + Angle.Between(toRotate, rotated).MagnitudeInRadians.Should().BeApproximately(MathF.Abs(rads), epsilon); + } + + [Property] + public void RotorFromPlaneAngleAppliesPlaneOrientation(Vector2 l, Vector2 r, float rads, Vector2 toRotate) + { + if (areCollinear(l, r) || toRotate == Vector2.Zero) return; + var plane = Bivector2.Wedge(l, r); + var reversePlane = -plane; + var angle = Angle.FromRadians(rads); + + var rotatedInPlane = Rotor2.FromPlaneAngle(plane, angle).Rotate(toRotate); + var rotatedInReversePlane = Rotor2.FromPlaneAngle(reversePlane, angle).Rotate(toRotate); + + Angle.Between(toRotate, rotatedInPlane).Radians.Should() + .BeApproximately(-Angle.Between(toRotate, rotatedInReversePlane).Radians, epsilon); + } + + [Property] + public void RotorFromAngleRotatesByAngle(Vector2 l, Vector2 r, float rads, Vector2 toRotate) + { + if (areCollinear(l, r) || toRotate == Vector2.Zero) return; + var plane = Bivector2.Wedge(l, r); + var angle = Angle.FromRadians(rads); + + var rotor = Rotor2.FromAngle(angle); + var rotated = rotor.Rotate(toRotate); + + Angle.Between(rotated, toRotate).Radians.Should().BeApproximately(rads, epsilon); + } + + [Property] + public void RotorsWithSameComponentsAreEqual(float scalar, Vector2 l, Vector2 r) + { + var bivector = Bivector2.Wedge(l, r); + var rotor1 = new Rotor2(scalar, bivector); + var rotor2 = new Rotor2(scalar, bivector); + + rotor1.Equals(rotor2).Should().BeTrue(); + (rotor1 == rotor2).Should().BeTrue(); + (rotor1 != rotor2).Should().BeFalse(); + rotor1.GetHashCode().Should().Be(rotor2.GetHashCode()); + } + + [Property] + public void RotorsWithDifferentScalarComponentAreNotEqual(float scalar1, float scalar2, Vector2 l, Vector2 r) + { + // ReSharper disable once CompareOfFloatsByEqualityOperator + if (scalar1 == scalar2) scalar2++; + + var bivector = Bivector2.Wedge(l, r); + var rotor1 = new Rotor2(scalar1, bivector); + var rotor2 = new Rotor2(scalar2, bivector); + + rotor1.Equals(rotor2).Should().BeFalse(); + (rotor1 == rotor2).Should().BeFalse(); + (rotor1 != rotor2).Should().BeTrue(); + } + + [Property] + public void RotorsWithDifferentBivectorComponentAreNotEqual( + float scalar, Vector2 l1, Vector2 r1, Vector2 l2, Vector2 r2) + { + var bivector1 = Bivector2.Wedge(l1, r1); + var bivector2 = Bivector2.Wedge(l2, r2); + if (bivector1 == bivector2) + bivector2 = Bivector2.Wedge(l2 + Vector2.UnitX, r2 + Vector2.UnitY); + + var rotor1 = new Rotor2(scalar, bivector1); + var rotor2 = new Rotor2(scalar, bivector2); + + rotor1.Equals(rotor2).Should().BeFalse(); + (rotor1 == rotor2).Should().BeFalse(); + (rotor1 != rotor2).Should().BeTrue(); + } + + private static bool areCollinear(Vector2 v1, Vector2 v2) + { + if (v1.Y == 0 && v2.Y == 0) return true; + if (v1 == Vector2.Zero || v2 == Vector2.Zero) return true; + if (v1.Y == 0 || v2.Y == 0) return false; + return Math.Abs(v1.X / v1.Y - v2.X / v2.Y) < epsilon; + } + } +} diff --git a/Bearded.Utilities.Tests/Geometry/Rotor3Tests.cs b/Bearded.Utilities.Tests/Geometry/Rotor3Tests.cs new file mode 100644 index 00000000..c6a2019a --- /dev/null +++ b/Bearded.Utilities.Tests/Geometry/Rotor3Tests.cs @@ -0,0 +1,238 @@ +using System; +using Bearded.Utilities.Geometry; +using Bearded.Utilities.Tests.Assertions; +using Bearded.Utilities.Tests.Generators; +using FluentAssertions; +using FsCheck; +using FsCheck.Xunit; +using OpenTK.Mathematics; +using Xunit; + +namespace Bearded.Utilities.Tests.Geometry +{ + public sealed class Rotor3Tests + { + private const float epsilon = 1E-3f; + + public Rotor3Tests() + { + Arb.Register(); + Arb.Register(); + } + + [Fact] + public void IdentityRotorHasMagnitudeOne() + { + Rotor3.Identity.Magnitude.Should().BeApproximately(1, epsilon); + Rotor3.Identity.MagnitudeSquared.Should().BeApproximately(1, epsilon); + } + + [Property] + public void IdentityRotorDoesNotRotateVector(Vector3 vector) + { + AssertionExtensions.Should(Rotor3.Identity.Rotate(vector)).Be(vector); + } + + [Property] + public void NormalizedRotorHasMagnitudeOne(float scalar, Vector3 l, Vector3 r) + { + var rotor = new Rotor3(scalar, Bivector3.Wedge(l, r)); + if (rotor == new Rotor3()) return; + var normalizedRotor = rotor.Normalized(); + + normalizedRotor.Magnitude.Should().BeApproximately(1, epsilon); + normalizedRotor.MagnitudeSquared.Should().BeApproximately(1, epsilon); + } + + [Fact] + public void NormalizedZeroRotorIsZeroRotor() + { + var rotor = new Rotor3(); + + rotor.Normalized().Should().Be(rotor); + } + + [Property] + public void NormalizedRotorDoesNotChangeVectorLengthOnRotation(float scalar, Vector3 l, Vector3 r, Vector3 v) + { + var rotor = new Rotor3(scalar, Bivector3.Wedge(l, r)); + // Zero rotors are special. + if (rotor == new Rotor3()) return; + var normalizedRotor = rotor.Normalized(); + + var rotatedV = normalizedRotor.Rotate(v); + rotatedV.Length.Should().BeApproximately(v.Length, epsilon); + } + + [Property] + public void ReversedKeepsMagnitudeInvariant(float scalar, Vector3 l, Vector3 r) + { + var rotor = new Rotor3(scalar, Bivector3.Wedge(l, r)); + + var reversedRotor = rotor.Reversed(); + + reversedRotor.Magnitude.Should().BeApproximately(rotor.Magnitude, epsilon); + } + + [Property] + public void ReversedRotorRotatesInTheOppositeDirection(Vector3 from, Vector3 to) + { + if (from == Vector3.Zero) from = Vector3.UnitX; + if (to == Vector3.Zero) to = Vector3.UnitY; + if (areCollinear(to, from)) return; + + var rotor = Rotor3.Between(from.Normalized(), to.Normalized()); + var reversedRotor = rotor.Reversed(); + + var rotated = reversedRotor.Rotate(to); + + rotated.Normalized().Should().BeApproximately(from.Normalized(), epsilon); + } + + [Property] + public void RotorBetweenVectorsHasMagnitudeOne(Vector3 from, Vector3 to) + { + if (from == Vector3.Zero) from = Vector3.UnitX; + if (to == Vector3.Zero) to = Vector3.UnitY; + + var rotor = Rotor3.Between(from, to); + + rotor.Magnitude.Should().BeApproximately(1, epsilon); + rotor.MagnitudeSquared.Should().BeApproximately(1, epsilon); + } + + [Property] + public void RotorBetweenSameVectorIsIdentity(Vector3 fromTo) + { + if (fromTo == Vector3.Zero) fromTo = Vector3.One; + + var rotor = Rotor3.Between(fromTo, fromTo); + + rotor.Should().Be(Rotor3.Identity); + } + + [Property] + public void RotorBetweenVectorsShouldRotateFromToDirectionOfTo(Vector3 from, Vector3 to) + { + if (from == Vector3.Zero) from = Vector3.UnitX; + if (to == Vector3.Zero) to = Vector3.UnitY; + if (areCollinear(from, to)) return; + + var rotor = Rotor3.Between(from.Normalized(), to.Normalized()); + + var rotated = rotor.Rotate(from); + + rotated.Normalized().Should().BeApproximately(to.Normalized(), epsilon); + } + + [Property] + public void RotorFromPlaneAngleKeepsAngleWithPlaneInvariant(Vector3 l, Vector3 r, float rads, Vector3 toRotate) + { + if (areCollinear(l, r)) return; + var plane = Bivector3.Wedge(l, r); + var angle = Angle.FromRadians(rads); + + var rotor = Rotor3.FromPlaneAngle(plane, angle); + var rotated = rotor.Rotate(toRotate); + + // Calculating the angle with the perpendicular axis is easier than calculating the angle with the plane + // itself, but the invariant still holds. + var axis = new Vector3(plane.Yz, -plane.Xz, plane.Xy); + var toRotateDotAxis = Vector3.Dot(toRotate, axis); + var rotatedDotAxis = Vector3.Dot(rotated, axis); + + rotatedDotAxis.Should().BeApproximately(toRotateDotAxis, epsilon); + } + + [Property] + public void RotorFromPlaneAngleRotatesByAngle(Vector3 l, Vector3 r, float rads, Vector3 toRotate) + { + if (areCollinear(l, r) || toRotate == Vector3.Zero) return; + var plane = Bivector3.Wedge(l, r); + var angle = Angle.FromRadians(rads); + + var rotor = Rotor3.FromPlaneAngle(plane, angle); + var rotated = rotor.Rotate(toRotate); + + Vector3.Dot(toRotate.Normalized(), rotated.Normalized()).Should().BeApproximately(MathF.Cos(rads), epsilon); + } + + [Property] + public void RotorFromAxisAngleKeepsAngleWithAxisInvariant(Vector3 axis, float rads, Vector3 toRotate) + { + if (axis == Vector3.Zero) axis = Vector3.UnitX; + if (toRotate == Vector3.Zero) toRotate = Vector3.UnitY; + var angle = Angle.FromRadians(rads); + + var rotor = Rotor3.FromAxisAngle(axis, angle); + var rotated = rotor.Rotate(toRotate); + + var toRotateDotAxis = Vector3.Dot(toRotate, axis); + var rotatedDotAxis = Vector3.Dot(rotated, axis); + + rotatedDotAxis.Should().BeApproximately(toRotateDotAxis, epsilon); + } + + [Property] + public void RotorFromAxisAngleRotatesByAngle(Vector3 axis, float rads, Vector3 toRotate) + { + if (axis == Vector3.Zero) axis = Vector3.UnitX; + if (toRotate == Vector3.Zero) toRotate = Vector3.UnitY; + var angle = Angle.FromRadians(rads); + + var rotor = Rotor3.FromAxisAngle(axis, angle); + var rotated = rotor.Rotate(toRotate); + + Vector3.Dot(toRotate.Normalized(), rotated.Normalized()).Should().BeApproximately(MathF.Cos(rads), epsilon); + } + + [Property] + public void RotorsWithSameComponentsAreEqual(float scalar, Vector3 l, Vector3 r) + { + var bivector = Bivector3.Wedge(l, r); + var rotor1 = new Rotor3(scalar, bivector); + var rotor2 = new Rotor3(scalar, bivector); + + rotor1.Equals(rotor2).Should().BeTrue(); + (rotor1 == rotor2).Should().BeTrue(); + (rotor1 != rotor2).Should().BeFalse(); + rotor1.GetHashCode().Should().Be(rotor2.GetHashCode()); + } + + [Property] + public void RotorsWithDifferentScalarComponentAreNotEqual(float scalar1, float scalar2, Vector3 l, Vector3 r) + { + // ReSharper disable once CompareOfFloatsByEqualityOperator + if (scalar1 == scalar2) scalar2++; + + var bivector = Bivector3.Wedge(l, r); + var rotor1 = new Rotor3(scalar1, bivector); + var rotor2 = new Rotor3(scalar2, bivector); + + rotor1.Equals(rotor2).Should().BeFalse(); + (rotor1 == rotor2).Should().BeFalse(); + (rotor1 != rotor2).Should().BeTrue(); + } + + [Property] + public void RotorsWithDifferentBivectorComponentAreNotEqual(float scalar, Vector3 l1, Vector3 r1, Vector3 l2, Vector3 r2) + { + var bivector1 = Bivector3.Wedge(l1, r1); + var bivector2 = Bivector3.Wedge(l2, r2); + if (bivector1 == bivector2) + bivector2 = Bivector3.Wedge(l2 + Vector3.UnitX, r2 + Vector3.UnitY); + + var rotor1 = new Rotor3(scalar, bivector1); + var rotor2 = new Rotor3(scalar, bivector2); + + rotor1.Equals(rotor2).Should().BeFalse(); + (rotor1 == rotor2).Should().BeFalse(); + (rotor1 != rotor2).Should().BeTrue(); + } + + private static bool areCollinear(Vector3 v1, Vector3 v2) + { + return Vector3.Cross(v1, v2).LengthSquared < epsilon; + } + } +} diff --git a/Bearded.Utilities/Geometry/Bivector2.cs b/Bearded.Utilities/Geometry/Bivector2.cs index 1af83e52..a03dc348 100644 --- a/Bearded.Utilities/Geometry/Bivector2.cs +++ b/Bearded.Utilities/Geometry/Bivector2.cs @@ -8,7 +8,11 @@ namespace Bearded.Utilities.Geometry; /// public readonly struct Bivector2 : IEquatable { - public float Magnitude { get; } + public float Xy { get; } + + public float Magnitude => MathF.Abs(Xy); + + public float MagnitudeSquared => Xy.Squared(); public static readonly Bivector2 Zero = new Bivector2(0); @@ -17,24 +21,32 @@ namespace Bearded.Utilities.Geometry; public static Bivector2 Wedge(Vector2 left, Vector2 right) => new Bivector2(left.X * right.Y - left.Y * right.X); - public Bivector2(float magnitude) + public Bivector2(float xy) { - Magnitude = magnitude; + Xy = xy; } + public Bivector2 Normalized() => new Bivector2(MathF.Sign(Xy)); + public static Bivector2 operator +(Bivector2 left, Bivector2 right) => - new Bivector2(left.Magnitude + right.Magnitude); + new Bivector2(left.Xy + right.Xy); public static Bivector2 operator -(Bivector2 left, Bivector2 right) => - new Bivector2(left.Magnitude - right.Magnitude); + new Bivector2(left.Xy - right.Xy); + + public static Bivector2 operator -(Bivector2 bivector) => new Bivector2(-bivector.Xy); + + public static Bivector2 operator *(float scalar, Bivector2 bivector) => new Bivector2(scalar * bivector.Xy); + + public static Bivector2 operator *(Bivector2 bivector, float scalar) => scalar * bivector; - public static Bivector2 operator -(Bivector2 bivector) => new Bivector2(-bivector.Magnitude); + public static Bivector2 operator /(Bivector2 bivector, float divider) => 1 / divider * bivector; - public bool Equals(Bivector2 other) => Magnitude.Equals(other.Magnitude); + public bool Equals(Bivector2 other) => Xy.Equals(other.Xy); public override bool Equals(object? obj) => obj is Bivector2 other && Equals(other); - public override int GetHashCode() => Magnitude.GetHashCode(); + public override int GetHashCode() => Xy.GetHashCode(); public static bool operator ==(Bivector2 left, Bivector2 right) => left.Equals(right); diff --git a/Bearded.Utilities/Geometry/Bivector3.cs b/Bearded.Utilities/Geometry/Bivector3.cs index 3d337525..eb7b84f2 100644 --- a/Bearded.Utilities/Geometry/Bivector3.cs +++ b/Bearded.Utilities/Geometry/Bivector3.cs @@ -12,6 +12,10 @@ namespace Bearded.Utilities.Geometry; public float Yz { get; } public float Xz { get; } + public float Magnitude => MagnitudeSquared.Sqrted(); + + public float MagnitudeSquared => Xy.Squared() + Yz.Squared() + Xz.Squared(); + public static readonly Bivector3 Zero = new Bivector3(0, 0, 0); public static readonly Bivector3 UnitXy = new Bivector3(1, 0, 0); @@ -26,6 +30,15 @@ public static Bivector3 Wedge(Vector3 left, Vector3 right) => left.Y * right.Z - right.Y * left.Z, left.X * right.Z - right.X * left.Z); + /// + /// Returns a counter-clockwise bivector representing the plane orthogonal from the provided axis, with + /// magnitude equal to the length of the axis. + /// + public static Bivector3 FromAxis(Vector3 axis) + { + return new Bivector3(axis.Z, axis.X, -axis.Y); + } + public Bivector3(float xy, float yz, float xz) { Xy = xy; @@ -33,6 +46,8 @@ public Bivector3(float xy, float yz, float xz) Xz = xz; } + public Bivector3 Normalized() => MagnitudeSquared == 0 ? this : this / Magnitude; + public static Bivector3 operator +(Bivector3 left, Bivector3 right) => new Bivector3(left.Xy + right.Xy, left.Yz + right.Yz, left.Xz + right.Xz); @@ -42,6 +57,13 @@ public Bivector3(float xy, float yz, float xz) public static Bivector3 operator -(Bivector3 bivector) => new Bivector3(-bivector.Xy, -bivector.Yz, -bivector.Xz); + public static Bivector3 operator *(float scalar, Bivector3 bivector) => + new Bivector3(scalar * bivector.Xy, scalar * bivector.Yz, scalar * bivector.Xz); + + public static Bivector3 operator *(Bivector3 bivector, float scalar) => scalar * bivector; + + public static Bivector3 operator /(Bivector3 bivector, float divider) => 1 / divider * bivector; + public bool Equals(Bivector3 other) => Xy.Equals(other.Xy) && Yz.Equals(other.Yz) && Xz.Equals(other.Xz); public override bool Equals(object? obj) => obj is Bivector3 other && Equals(other); diff --git a/Bearded.Utilities/Geometry/Rotor2.cs b/Bearded.Utilities/Geometry/Rotor2.cs new file mode 100644 index 00000000..5652363b --- /dev/null +++ b/Bearded.Utilities/Geometry/Rotor2.cs @@ -0,0 +1,119 @@ +using System; +using OpenTK.Mathematics; + +namespace Bearded.Utilities.Geometry +{ + /// + /// A representation of a rotation in two-dimensional space. + /// Rotors are the analog to complex numbers in geometric algebra. The underlying maths is isomorphic to the maths + /// used in complex numbers, but the underlying geometric concepts are more geometrically intuitive. + /// Normalized rotors represent a rotation only. Non-normalized rotors may also scale the vector. + /// + public readonly struct Rotor2 : IEquatable + { + /// + /// The scalar (0-dimensional) component of this rotor. + /// + public float Scalar { get; } + + /// + /// The bivector (2-dimensional) component of this rotor. + /// + public Bivector2 Bivector { get; } + + /// + /// The xy component of the bivector component of this rotor. + /// + public float Xy => Bivector.Xy; + + public float Magnitude => MagnitudeSquared.Sqrted(); + + public float MagnitudeSquared => Scalar.Squared() + Bivector.MagnitudeSquared; + + /// + /// The rotor that acts as an identity transformation on all vectors. + /// + public static Rotor2 Identity { get; } = new Rotor2(1, Bivector2.Zero); + + /// + /// Creates a rotor that rotates the from vector to the to vector, assuming these vectors are normalized. + /// The rotor between two opposite vectors is undefined, as there are multiple possible rotations. + /// + public static Rotor2 Between(Vector2 from, Vector2 to) + { + return new Rotor2(1 + Vector2.Dot(from, to), Bivector2.Wedge(to, from)).Normalized(); + } + + /// + /// Creates a rotor that rotates the from direction to the to direction. + /// The rotor between two opposite directions is undefined, as there are multiple possible rotations. + /// + public static Rotor2 Between(Direction2 from, Direction2 to) => Between(from.Vector, to.Vector); + + /// + /// Creates a rotor that rotates in the (orientation-aware) plane defined by the bivector by the specified + /// angle. + /// + public static Rotor2 FromPlaneAngle(Bivector2 plane, Angle angle) + { + var halfAngle = 0.5f * angle; + return new Rotor2(MathF.Cos(halfAngle.Radians), -MathF.Sin(halfAngle.Radians) * plane.Normalized()); + } + + /// + /// Creates a rotor that rotates by the specified angle. + /// This is possible in 2D, because there is only one axis to rotate around. + /// + public static Rotor2 FromAngle(Angle angle) => FromPlaneAngle(Bivector2.Unit, angle); + + public Rotor2(float scalar, Bivector2 bivector) + { + Scalar = scalar; + Bivector = bivector; + } + + /// + /// Returns the rotor with the same relative components, but magnitude one. + /// + public Rotor2 Normalized() + { + if (MagnitudeSquared == 0) return this; + var l = Magnitude; + return new Rotor2(Scalar / l, Bivector / l); + } + + /// + /// Rotates a vector according to the rotation defined by this rotor. + /// May not induce a proper rotation if the rotor isn't normalized. + /// + public Vector2 Rotate(Vector2 from) + { + var (xx, yy) = (Scalar, Scalar); + + // intermediate = geometricProduct(rotor, from) + var x = +from.X * xx + from.Y * Xy; + var y = -from.X * Xy + from.Y * yy; + + // result = geometricProduct(intermediate, rotor*) + return new Vector2( + +x * xx + y * Xy, + -x * Xy + y * yy + ); + } + + /// + /// Returns the rotor that does the reverse rotation. + /// + public Rotor2 Reversed() => new Rotor2(Scalar, -Bivector); + + public bool Equals(Rotor2 other) => Scalar.Equals(other.Scalar) && Bivector.Equals(other.Bivector); + + public override bool Equals(object? obj) => obj is Rotor2 other && Equals(other); + + public override int GetHashCode() => HashCode.Combine(Scalar, Bivector); + + public static bool operator ==(Rotor2 left, Rotor2 right) => left.Equals(right); + + public static bool operator !=(Rotor2 left, Rotor2 right) => !left.Equals(right); + } +} diff --git a/Bearded.Utilities/Geometry/Rotor3.cs b/Bearded.Utilities/Geometry/Rotor3.cs new file mode 100644 index 00000000..822b4614 --- /dev/null +++ b/Bearded.Utilities/Geometry/Rotor3.cs @@ -0,0 +1,141 @@ +using System; +using OpenTK.Mathematics; + +namespace Bearded.Utilities.Geometry +{ + /// + /// A representation of a rotation in three-dimensional space. + /// Rotors are the analog to quaternions in geometric algebra. The underlying maths is isomorphic to the maths used + /// in quaternions, but the underlying geometric concepts are more geometrically intuitive. + /// Normalized rotors represent a rotation only. Non-normalized rotors may also scale the vector. + /// + public readonly struct Rotor3 : IEquatable + { + /// + /// The scalar (0-dimensional) component of this rotor. + /// + public float Scalar { get; } + + /// + /// The bivector (2-dimensional) component of this rotor. + /// + public Bivector3 Bivector { get; } + + /// + /// The xy component of the bivector component of this rotor. + /// + public float Xy => Bivector.Xy; + + /// + /// The yz component of the bivector component of this rotor. + /// + public float Yz => Bivector.Yz; + + /// + /// The xz component of the bivector component of this rotor. + /// + public float Xz => Bivector.Xz; + + public float Magnitude => MagnitudeSquared.Sqrted(); + + public float MagnitudeSquared => Scalar.Squared() + Bivector.MagnitudeSquared; + + /// + /// The rotor that acts as an identity transformation on all vectors. + /// + public static Rotor3 Identity { get; } = new Rotor3(1, Bivector3.Zero); + + /// + /// Creates a rotor that rotates the from vector to the to vector, assuming these vectors are normalized. + /// The rotor between two opposite vectors is undefined, as there are infinitely many possible rotations. + /// + public static Rotor3 Between(Vector3 from, Vector3 to) + { + return new Rotor3(1 + Vector3.Dot(from, to), Bivector3.Wedge(to, from)).Normalized(); + } + + /// + /// Creates a rotor that rotates in the (orientation-aware) plane defined by the bivector by the specified + /// angle. + /// + public static Rotor3 FromPlaneAngle(Bivector3 plane, Angle angle) + { + var halfAngle = 0.5f * angle; + return new Rotor3(MathF.Cos(halfAngle.Radians), -MathF.Sin(halfAngle.Radians) * plane.Normalized()); + } + + /// + /// Creates a rotor that rotates in the xy plane (i.e. around the z axis). + /// + public static Rotor3 FromXyAngle(Angle angle) => FromPlaneAngle(Bivector3.UnitXy, angle); + + /// + /// Creates a rotor that rotates in the xy plane (i.e. around the x axis). + /// + public static Rotor3 FromYzAngle(Angle angle) => FromPlaneAngle(Bivector3.UnitYz, angle); + + /// + /// Creates a rotor that rotates in the xz plane (i.e. around the y axis). + /// + public static Rotor3 FromXzAngle(Angle angle) => FromPlaneAngle(Bivector3.UnitXz, angle); + + /// + /// Creates a rotor that rotates around the axis by the specified angle. + /// + public static Rotor3 FromAxisAngle(Vector3 axis, Angle angle) => + FromPlaneAngle(Bivector3.FromAxis(axis), angle); + + public Rotor3(float scalar, Bivector3 bivector) + { + Scalar = scalar; + Bivector = bivector; + } + + /// + /// Returns the rotor with the same relative components, but magnitude one. + /// + public Rotor3 Normalized() + { + if (MagnitudeSquared == 0) return this; + var l = Magnitude; + return new Rotor3(Scalar / l, Bivector / l); + } + + /// + /// Returns the rotor that does the reverse rotation. + /// + public Rotor3 Reversed() => new Rotor3(Scalar, -Bivector); + + /// + /// Rotates a vector according to the rotation defined by this rotor. + /// May not induce a proper rotation if the rotor isn't normalized. + /// + public Vector3 Rotate(Vector3 from) + { + var (xx, yy, zz) = (Scalar, Scalar, Scalar); + + // intermediate = geometricProduct(rotor, from) + var x = +from.X * xx + from.Y * Xy + from.Z * Xz; + var y = -from.X * Xy + from.Y * yy + from.Z * Yz; + var z = -from.X * Xz - from.Y * Yz + from.Z * zz; + var xyz = from.X * Yz - from.Y * Xz + from.Z * Xy; + + // result = geometricProduct(intermediate, rotor*) + return new Vector3( + +x * xx + y * Xy + z * Xz + xyz * Yz, + -x * Xy + y * yy + z * Yz - xyz * Xz, + -x * Xz - y * Yz + z * zz + xyz * Xy + ); + } + + public bool Equals(Rotor3 other) => Scalar.Equals(other.Scalar) && Bivector.Equals(other.Bivector); + + public override bool Equals(object? obj) => obj is Rotor3 other && Equals(other); + + public override int GetHashCode() => HashCode.Combine(Scalar, Bivector); + + public static bool operator ==(Rotor3 left, Rotor3 right) => left.Equals(right); + + public static bool operator !=(Rotor3 left, Rotor3 right) => !left.Equals(right); + } +}