From c077e094363ee8abc6d0ef8e6a97c6d3542d23c8 Mon Sep 17 00:00:00 2001 From: Leon Friedrich <60421075+ElectroJr@users.noreply.github.com> Date: Wed, 2 Feb 2022 12:03:45 +1300 Subject: [PATCH] Add explicit transform matrices (#2483) --- Robust.Shared.Maths/Matrix3.cs | 63 ++++++++++++++++--- .../Transform/TransformComponent.cs | 18 ++---- .../Shared/Maths/Matrix3_Test.cs | 30 ++++++++- 3 files changed, 85 insertions(+), 26 deletions(-) diff --git a/Robust.Shared.Maths/Matrix3.cs b/Robust.Shared.Maths/Matrix3.cs index cc3fe8ae9..c0f175f50 100644 --- a/Robust.Shared.Maths/Matrix3.cs +++ b/Robust.Shared.Maths/Matrix3.cs @@ -427,21 +427,66 @@ namespace Robust.Shared.Maths return CreateScale(scale.X, scale.Y); } - public static Matrix3 CreateTransform(in Vector2 position, in Angle rotation, in Vector2 scale) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3 CreateTransform(float posX, float posY, float angle, float scaleX = 1, float scaleY = 1) { - var sMat = CreateScale(in scale); - var rMat = CreateRotation(rotation); - var tMat = CreateTranslation(position); + // returns a matrix that is equivalent to returning CreateScale(scale) * CreateRotation(angle) * CreateTranslation(posX, posY) - return sMat * rMat * tMat; + var (sin, cos) = MathF.SinCos(angle); + + return new Matrix3 + { + R0C0 = cos * scaleX, + R0C1 = -sin * scaleY, + R0C2 = posX, + R1C0 = sin * scaleX, + R1C1 = cos * scaleY, + R1C2 = posY, + R2C2 = 1 + }; } - public static Matrix3 CreateTransform(in Vector2 position, in Angle rotation) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3 CreateInverseTransform(float posX, float posY, float angle, float scaleX = 1, float scaleY = 1) { - var rMat = CreateRotation(rotation); - var tMat = CreateTranslation(position); + // returns a matrix that is equivalent to returning CreateTranslation(-posX, -posY) * CreateRotation(-angle) * CreateScale(1/scaleX, 1/scaleY) - return rMat * tMat; + var (sin, cos) = MathF.SinCos(angle); + + return new Matrix3 + { + R0C0 = cos / scaleX, + R0C1 = sin / scaleX, + R0C2 = - (posX * cos + posY * sin) / scaleX, + R1C0 = -sin / scaleY, + R1C1 = cos / scaleY, + R1C2 = (posX * sin - posY * cos) / scaleY, + R2C2 = 1 + }; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3 CreateTransform(in Vector2 position, in Angle angle) + { + return CreateTransform(position.X, position.Y, (float)angle.Theta); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3 CreateTransform(in Vector2 position, in Angle angle, in Vector2 scale) + { + return CreateTransform(position.X, position.Y, (float)angle.Theta, scale.X, scale.Y); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3 CreateInverseTransform(in Vector2 position, in Angle angle) + { + return CreateInverseTransform(position.X, position.Y, (float)angle.Theta); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3 CreateInverseTransform(in Vector2 position, in Angle angle, in Vector2 scale) + { + return CreateInverseTransform(position.X, position.Y, (float)angle.Theta, scale.X, scale.Y); } #endregion Constructors diff --git a/Robust.Shared/GameObjects/Components/Transform/TransformComponent.cs b/Robust.Shared/GameObjects/Components/Transform/TransformComponent.cs index 2de1a0b77..92f76ece0 100644 --- a/Robust.Shared/GameObjects/Components/Transform/TransformComponent.cs +++ b/Robust.Shared/GameObjects/Components/Transform/TransformComponent.cs @@ -693,6 +693,7 @@ namespace Robust.Shared.GameObjects _entMan.EventBus.RaiseLocalEvent(Owner, ref entParentChangedMessage); // Does it even make sense to call these since this is called purely from OnRemove right now? + // > FWIW, also called pre-entity-delete and when moved outside of PVS range. RebuildMatrices(); MapIdChanged(oldMapId); Dirty(); @@ -1001,21 +1002,10 @@ namespace Robust.Shared.GameObjects if (!_parent.IsValid()) // Root Node pos = Vector2.Zero; - var rot = _localRotation.Theta; + var rot = (float) _localRotation.Theta; - var posMat = Matrix3.CreateTranslation(pos); - var rotMat = Matrix3.CreateRotation((float) rot); - - Matrix3.Multiply(ref rotMat, ref posMat, out var transMat); - - _localMatrix = transMat; - - var posImat = Matrix3.Invert(posMat); - var rotImap = Matrix3.Invert(rotMat); - - Matrix3.Multiply(ref posImat, ref rotImap, out var itransMat); - - _invLocalMatrix = itransMat; + _localMatrix = Matrix3.CreateTransform(pos.X, pos.Y, rot); + _invLocalMatrix = Matrix3.CreateInverseTransform(pos.X, pos.Y, rot); } public string GetDebugString() diff --git a/Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs b/Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs index e9b8898d9..6c1e4d361 100644 --- a/Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs +++ b/Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs @@ -56,9 +56,14 @@ namespace Robust.UnitTesting.Shared.Maths public void MultiplyTransformOrder() { var startPoint = new Vector3(1, 0, 1); - var scaleMatrix = Matrix3.CreateScale(new Vector2(2, 2)); - var rotateMatrix = Matrix3.CreateRotation((float)(System.Math.PI / 2.0)); - var translateMatrix = Matrix3.CreateTranslation(new Vector2(-5, -3)); + + Vector2 scale = new Vector2(2, 2); + Angle angle = new Angle(System.MathF.PI / 2); + Vector2 offset = new Vector2(-5, -3); + + var scaleMatrix = Matrix3.CreateScale(scale); + var rotateMatrix = Matrix3.CreateRotation(angle); + var translateMatrix = Matrix3.CreateTranslation(offset); // 1. Take the start point -> ( 1, 0) // 2. Scale it by 2 -> ( 2, 0) @@ -68,6 +73,25 @@ namespace Robust.UnitTesting.Shared.Maths Assert.That(result.X, Is.Approximately(-5f)); Assert.That(result.Y, Is.Approximately(-1f)); + + // repeat but with CreateTransform() + var transform = Matrix3.CreateTransform(offset, angle, scale); + result = transform * startPoint; + Assert.That(result.X, Is.Approximately(-5f)); + Assert.That(result.Y, Is.Approximately(-1f)); + } + + [Test] + public void InverseTransformTest() + { + Vector2 scale = new Vector2(2.32f, 2); + Angle angle = new Angle(System.MathF.PI / 2.21f); + Vector2 offset = new Vector2(-5, 3); + + var transform = Matrix3.CreateTransform(offset, angle, scale); + var invTransform = Matrix3.CreateInverseTransform(offset, angle, scale); + + Assert.That(Matrix3.Invert(transform).EqualsApprox(invTransform)); } [Test]