diff --git a/src/libraries/System.Numerics.Vectors/tests/QuaternionTests.cs b/src/libraries/System.Numerics.Vectors/tests/QuaternionTests.cs index 6bfaad266479f..fcf8418c6edca 100644 --- a/src/libraries/System.Numerics.Vectors/tests/QuaternionTests.cs +++ b/src/libraries/System.Numerics.Vectors/tests/QuaternionTests.cs @@ -326,7 +326,7 @@ public void QuaternionCreateFromAxisAngleTest() Vector3 axis = Vector3.Normalize(new Vector3(1.0f, 2.0f, 3.0f)); float angle = MathHelper.ToRadians(30.0f); - Quaternion expected = new Quaternion(0.0691723f, 0.1383446f, 0.207516879f, 0.9659258f); + Quaternion expected = new Quaternion(0.06917231f, 0.13834462f, 0.2075169f, 0.9659258f); Quaternion actual; actual = Quaternion.CreateFromAxisAngle(axis, angle); @@ -545,10 +545,11 @@ public void QuaternionInverseTest() public void QuaternionInverseTest1() { Quaternion a = new Quaternion(); + + Quaternion expected = Quaternion.Zero; Quaternion actual = Quaternion.Inverse(a); - Assert.True(float.IsNaN(actual.X) && float.IsNaN(actual.Y) && float.IsNaN(actual.Z) && float.IsNaN(actual.W) - , $"Quaternion.Inverse - did not return the expected value: expected {new Quaternion(float.NaN, float.NaN, float.NaN, float.NaN)} actual {actual}"); + Assert.Equal(expected, actual); } // A test for ToString () @@ -919,10 +920,10 @@ public void QuaternionZeroTest() { // A default value should be equal to a zero value. Assert.Equal(default(Quaternion), Quaternion.Zero); - + // A newly constructed value should be equal to a zero value. Assert.Equal(new Quaternion(), Quaternion.Zero); - + // A newly constructed value with (0, 0, 0, 0) should be equal to a zero value. Assert.Equal(new Quaternion(0, 0, 0, 0), Quaternion.Zero); } diff --git a/src/libraries/System.Numerics.Vectors/tests/Vector2Tests.cs b/src/libraries/System.Numerics.Vectors/tests/Vector2Tests.cs index 853ad86480b17..0472832009f4d 100644 --- a/src/libraries/System.Numerics.Vectors/tests/Vector2Tests.cs +++ b/src/libraries/System.Numerics.Vectors/tests/Vector2Tests.cs @@ -632,7 +632,7 @@ public void Vector2TransformByQuaternionTest1() { Vector2 v = new Vector2(1.0f, 2.0f); Quaternion q = new Quaternion(); - Vector2 expected = v; + Vector2 expected = Vector2.Zero; Vector2 actual = Vector2.Transform(v, q); Assert.True(MathHelper.Equal(expected, actual), "Vector2f.Transform did not return the expected value."); diff --git a/src/libraries/System.Numerics.Vectors/tests/Vector3Tests.cs b/src/libraries/System.Numerics.Vectors/tests/Vector3Tests.cs index 235d8b08c431b..4d0e97526a918 100644 --- a/src/libraries/System.Numerics.Vectors/tests/Vector3Tests.cs +++ b/src/libraries/System.Numerics.Vectors/tests/Vector3Tests.cs @@ -689,7 +689,7 @@ public void Vector3TransformByQuaternionTest1() { Vector3 v = new Vector3(1.0f, 2.0f, 3.0f); Quaternion q = new Quaternion(); - Vector3 expected = v; + Vector3 expected = Vector3.Zero; Vector3 actual = Vector3.Transform(v, q); Assert.True(MathHelper.Equal(expected, actual), "Vector3f.Transform did not return the expected value."); diff --git a/src/libraries/System.Numerics.Vectors/tests/Vector4Tests.cs b/src/libraries/System.Numerics.Vectors/tests/Vector4Tests.cs index 041663f414430..b3759a716a028 100644 --- a/src/libraries/System.Numerics.Vectors/tests/Vector4Tests.cs +++ b/src/libraries/System.Numerics.Vectors/tests/Vector4Tests.cs @@ -800,7 +800,7 @@ public void Vector4TransformVector4QuaternionTest1() { Vector4 v = new Vector4(1.0f, 2.0f, 3.0f, 0.0f); Quaternion q = new Quaternion(); - Vector4 expected = v; + Vector4 expected = Vector4.Zero; Vector4 actual = Vector4.Transform(v, q); Assert.True(MathHelper.Equal(expected, actual), "Vector4f.Transform did not return the expected value."); @@ -844,7 +844,7 @@ public void Vector4TransformVector3QuaternionTest1() { Vector3 v = new Vector3(1.0f, 2.0f, 3.0f); Quaternion q = new Quaternion(); - Vector4 expected = new Vector4(v, 1.0f); + Vector4 expected = Vector4.Zero; Vector4 actual = Vector4.Transform(v, q); Assert.True(MathHelper.Equal(expected, actual), "Vector4f.Transform did not return the expected value."); @@ -888,7 +888,7 @@ public void Vector4TransformVector2QuaternionTest1() { Vector2 v = new Vector2(1.0f, 2.0f); Quaternion q = new Quaternion(); - Vector4 expected = new Vector4(1.0f, 2.0f, 0, 1.0f); + Vector4 expected = Vector4.Zero; Vector4 actual = Vector4.Transform(v, q); Assert.True(MathHelper.Equal(expected, actual), "Vector4f.Transform did not return the expected value."); diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix3x2.Impl.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix3x2.Impl.cs index 7d270c17cc0de..fd98190e2125d 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix3x2.Impl.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix3x2.Impl.cs @@ -72,8 +72,7 @@ readonly get { ThrowHelper.ThrowArgumentOutOfRangeException(); } - - return Unsafe.Add(ref Unsafe.AsRef(in this.X), row)[column]; + return Unsafe.Add(ref Unsafe.AsRef(in X), row)[column]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -83,7 +82,7 @@ readonly get { ThrowHelper.ThrowArgumentOutOfRangeException(); } - Unsafe.Add(ref this.X, row)[column] = value; + Unsafe.Add(ref X, row)[column] = value; } } @@ -514,7 +513,7 @@ public readonly float GetDeterminant() [MethodImpl(MethodImplOptions.AggressiveInlining)] public override readonly int GetHashCode() => HashCode.Combine(X, Y, Z); - bool IEquatable.Equals(Impl other) => Equals(in other); + readonly bool IEquatable.Equals(Impl other) => Equals(in other); } } } diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.Impl.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.Impl.cs index 79e96fd05dd5d..3a489f0fc1af0 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.Impl.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.Impl.cs @@ -18,8 +18,6 @@ public partial struct Matrix4x4 // syntax. We do this because it saves roughly 8-bytes of IL per method which // in turn helps improve inlining chances. - // TODO: Vector3 is "inefficient" and we'd be better off taking Vector4 or Vector128 - internal const uint RowCount = 4; internal const uint ColumnCount = 4; @@ -92,8 +90,7 @@ readonly get { ThrowHelper.ThrowArgumentOutOfRangeException(); } - - return Unsafe.Add(ref Unsafe.AsRef(in this.X), row)[column]; + return Unsafe.Add(ref Unsafe.AsRef(in X), row)[column]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -103,7 +100,7 @@ readonly get { ThrowHelper.ThrowArgumentOutOfRangeException(); } - Unsafe.Add(ref this.X, row)[column] = value; + Unsafe.Add(ref X, row)[column] = value; } } @@ -167,29 +164,10 @@ public Vector3 Translation { Impl result; - // result.X = Transform(left.X, in right); - result.X = right.X * left.X.X; - result.X += right.Y * left.X.Y; - result.X += right.Z * left.X.Z; - result.X += right.W * left.X.W; - - // result.Y = Transform(left.Y, in right); - result.Y = right.X * left.Y.X; - result.Y += right.Y * left.Y.Y; - result.Y += right.Z * left.Y.Z; - result.Y += right.W * left.Y.W; - - // result.Z = Transform(left.Z, in right); - result.Z = right.X * left.Z.X; - result.Z += right.Y * left.Z.Y; - result.Z += right.Z * left.Z.Z; - result.Z += right.W * left.Z.W; - - // result.W = Transform(left.W, in right); - result.W = right.X * left.W.X; - result.W += right.Y * left.W.Y; - result.W += right.Z * left.W.Z; - result.W += right.W * left.W.W; + result.X = Vector4.Transform(left.X, in right); + result.Y = Vector4.Transform(left.Y, in right); + result.Z = Vector4.Transform(left.Z, in right); + result.W = Vector4.Transform(left.W, in right); return result; } @@ -237,15 +215,14 @@ public Vector3 Translation public static Impl CreateBillboard(in Vector3 objectPosition, in Vector3 cameraPosition, in Vector3 cameraUpVector, in Vector3 cameraForwardVector) { Vector3 axisZ = objectPosition - cameraPosition; - float norm = axisZ.LengthSquared(); - if (norm < BillboardEpsilon) + if (axisZ.LengthSquared() < BillboardEpsilon) { axisZ = -cameraForwardVector; } else { - axisZ = Vector3.Multiply(axisZ, 1.0f / float.Sqrt(norm)); + axisZ = Vector3.Normalize(axisZ); } Vector3 axisX = Vector3.Normalize(Vector3.Cross(cameraUpVector, axisZ)); @@ -266,15 +243,14 @@ public static Impl CreateConstrainedBillboard(in Vector3 objectPosition, in Vect { // Treat the case when object and camera positions are too close. Vector3 faceDir = objectPosition - cameraPosition; - float norm = faceDir.LengthSquared(); - if (norm < BillboardEpsilon) + if (faceDir.LengthSquared() < BillboardEpsilon) { faceDir = -cameraForwardVector; } else { - faceDir = Vector3.Multiply(faceDir, (1.0f / float.Sqrt(norm))); + faceDir = Vector3.Normalize(faceDir); } Vector3 axisY = rotateAxis; @@ -311,69 +287,8 @@ public static Impl CreateConstrainedBillboard(in Vector3 objectPosition, in Vect [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreateFromAxisAngle(in Vector3 axis, float angle) { - // a: angle - // x, y, z: unit vector for axis. - // - // Rotation matrix M can compute by using below equation. - // - // T T - // M = uu + (cos a)( I-uu ) + (sin a)S - // - // Where: - // - // u = ( x, y, z ) - // - // [ 0 -z y ] - // S = [ z 0 -x ] - // [ -y x 0 ] - // - // [ 1 0 0 ] - // I = [ 0 1 0 ] - // [ 0 0 1 ] - // - // - // [ xx+cosa*(1-xx) yx-cosa*yx-sina*z zx-cosa*xz+sina*y ] - // M = [ xy-cosa*yx+sina*z yy+cosa(1-yy) yz-cosa*yz-sina*x ] - // [ zx-cosa*zx-sina*y zy-cosa*zy+sina*x zz+cosa*(1-zz) ] - // - - float x = axis.X; - float y = axis.Y; - float z = axis.Z; - - (float sa, float ca) = float.SinCos(angle); - - float xx = x * x; - float yy = y * y; - float zz = z * z; - - float xy = x * y; - float xz = x * z; - float yz = y * z; - - Impl result; - - result.X = Vector4.Create( - xx + ca * (1.0f - xx), - xy - ca * xy + sa * z, - xz - ca * xz - sa * y, - 0 - ); - result.Y = Vector4.Create( - xy - ca * xy - sa * z, - yy + ca * (1.0f - yy), - yz - ca * yz + sa * x, - 0 - ); - result.Z = Vector4.Create( - xz - ca * xz + sa * y, - yz - ca * yz - sa * x, - zz + ca * (1.0f - zz), - 0 - ); - result.W = Vector4.UnitW; - - return result; + Quaternion q = Quaternion.CreateFromAxisAngle(axis, angle); + return CreateFromQuaternion(q); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -425,44 +340,18 @@ public static Impl CreateFromYawPitchRoll(float yaw, float pitch, float roll) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreateLookTo(in Vector3 cameraPosition, in Vector3 cameraDirection, in Vector3 cameraUpVector) { - Vector3 axisZ = Vector3.Normalize(-cameraDirection); - Vector3 axisX = Vector3.Normalize(Vector3.Cross(cameraUpVector, axisZ)); - Vector3 axisY = Vector3.Cross(axisZ, axisX); - Vector3 negativeCameraPosition = -cameraPosition; - - Impl result; - - result.X = Vector4.Create( - axisX.X, - axisY.X, - axisZ.X, - 0 - ); - result.Y = Vector4.Create( - axisX.Y, - axisY.Y, - axisZ.Y, - 0 - ); - result.Z = Vector4.Create( - axisX.Z, - axisY.Z, - axisZ.Z, - 0 - ); - result.W = Vector4.Create( - Vector3.Dot(axisX, negativeCameraPosition), - Vector3.Dot(axisY, negativeCameraPosition), - Vector3.Dot(axisZ, negativeCameraPosition), - 1 - ); + // This implementation is based on the DirectX Math Library XMMatrixLookToRH method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl - return result; + return CreateLookToLeftHanded(cameraPosition, -cameraDirection, cameraUpVector); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreateLookToLeftHanded(in Vector3 cameraPosition, in Vector3 cameraDirection, in Vector3 cameraUpVector) { + // This implementation is based on the DirectX Math Library XMMatrixLookToLH method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl + Vector3 axisZ = Vector3.Normalize(cameraDirection); Vector3 axisX = Vector3.Normalize(Vector3.Cross(cameraUpVector, axisZ)); Vector3 axisY = Vector3.Cross(axisZ, axisX); @@ -470,37 +359,20 @@ public static Impl CreateLookToLeftHanded(in Vector3 cameraPosition, in Vector3 Impl result; - result.X = Vector4.Create( - axisX.X, - axisY.X, - axisZ.X, - 0 - ); - result.Y = Vector4.Create( - axisX.Y, - axisY.Y, - axisZ.Y, - 0 - ); - result.Z = Vector4.Create( - axisX.Z, - axisY.Z, - axisZ.Z, - 0 - ); - result.W = Vector4.Create( - Vector3.Dot(axisX, negativeCameraPosition), - Vector3.Dot(axisY, negativeCameraPosition), - Vector3.Dot(axisZ, negativeCameraPosition), - 1 - ); + result.X = Vector4.Create(axisX, Vector3.Dot(axisX, negativeCameraPosition)); + result.Y = Vector4.Create(axisY, Vector3.Dot(axisY, negativeCameraPosition)); + result.Z = Vector4.Create(axisZ, Vector3.Dot(axisZ, negativeCameraPosition)); + result.W = Vector4.UnitW; - return result; + return Transpose(result); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane) { + // This implementation is based on the DirectX Math Library XMMatrixOrthographicRH method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl + float range = 1.0f / (zNearPlane - zFarPlane); Impl result; @@ -516,6 +388,9 @@ public static Impl CreateOrthographic(float width, float height, float zNearPlan [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreateOrthographicLeftHanded(float width, float height, float zNearPlane, float zFarPlane) { + // This implementation is based on the DirectX Math Library XMMatrixOrthographicLH method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl + float range = 1.0f / (zFarPlane - zNearPlane); Impl result; @@ -531,6 +406,9 @@ public static Impl CreateOrthographicLeftHanded(float width, float height, float [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane) { + // This implementation is based on the DirectX Math Library XMMatrixOrthographicOffCenterRH method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl + float reciprocalWidth = 1.0f / (right - left); float reciprocalHeight = 1.0f / (top - bottom); float range = 1.0f / (zNearPlane - zFarPlane); @@ -553,6 +431,9 @@ public static Impl CreateOrthographicOffCenter(float left, float right, float bo [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreateOrthographicOffCenterLeftHanded(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane) { + // This implementation is based on the DirectX Math Library XMMatrixOrthographicOffCenterLH method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl + float reciprocalWidth = 1.0f / (right - left); float reciprocalHeight = 1.0f / (top - bottom); float range = 1.0f / (zFarPlane - zNearPlane); @@ -575,6 +456,9 @@ public static Impl CreateOrthographicOffCenterLeftHanded(float left, float right [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreatePerspective(float width, float height, float nearPlaneDistance, float farPlaneDistance) { + // This implementation is based on the DirectX Math Library XMMatrixPerspectiveRH method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl + ArgumentOutOfRangeException.ThrowIfLessThanOrEqual(nearPlaneDistance, 0.0f); ArgumentOutOfRangeException.ThrowIfLessThanOrEqual(farPlaneDistance, 0.0f); ArgumentOutOfRangeException.ThrowIfGreaterThanOrEqual(nearPlaneDistance, farPlaneDistance); @@ -595,6 +479,9 @@ public static Impl CreatePerspective(float width, float height, float nearPlaneD [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreatePerspectiveLeftHanded(float width, float height, float nearPlaneDistance, float farPlaneDistance) { + // This implementation is based on the DirectX Math Library XMMatrixPerspectiveLH method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl + ArgumentOutOfRangeException.ThrowIfLessThanOrEqual(nearPlaneDistance, 0.0f); ArgumentOutOfRangeException.ThrowIfLessThanOrEqual(farPlaneDistance, 0.0f); ArgumentOutOfRangeException.ThrowIfGreaterThanOrEqual(nearPlaneDistance, farPlaneDistance); @@ -615,6 +502,9 @@ public static Impl CreatePerspectiveLeftHanded(float width, float height, float [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance) { + // This implementation is based on the DirectX Math Library XMMatrixPerspectiveFovRH method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl + ArgumentOutOfRangeException.ThrowIfLessThanOrEqual(fieldOfView, 0.0f); ArgumentOutOfRangeException.ThrowIfGreaterThanOrEqual(fieldOfView, float.Pi); @@ -639,6 +529,9 @@ public static Impl CreatePerspectiveFieldOfView(float fieldOfView, float aspectR [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreatePerspectiveFieldOfViewLeftHanded(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance) { + // This implementation is based on the DirectX Math Library XMMatrixPerspectiveFovLH method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl + ArgumentOutOfRangeException.ThrowIfLessThanOrEqual(fieldOfView, 0.0f); ArgumentOutOfRangeException.ThrowIfGreaterThanOrEqual(fieldOfView, float.Pi); @@ -663,6 +556,9 @@ public static Impl CreatePerspectiveFieldOfViewLeftHanded(float fieldOfView, flo [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float nearPlaneDistance, float farPlaneDistance) { + // This implementation is based on the DirectX Math Library XMMatrixPerspectiveOffCenterRH method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl + ArgumentOutOfRangeException.ThrowIfLessThanOrEqual(nearPlaneDistance, 0.0f); ArgumentOutOfRangeException.ThrowIfLessThanOrEqual(farPlaneDistance, 0.0f); ArgumentOutOfRangeException.ThrowIfGreaterThanOrEqual(nearPlaneDistance, farPlaneDistance); @@ -690,6 +586,9 @@ public static Impl CreatePerspectiveOffCenter(float left, float right, float bot [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreatePerspectiveOffCenterLeftHanded(float left, float right, float bottom, float top, float nearPlaneDistance, float farPlaneDistance) { + // This implementation is based on the DirectX Math Library XMMatrixPerspectiveOffCenterLH method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl + ArgumentOutOfRangeException.ThrowIfLessThanOrEqual(nearPlaneDistance, 0.0f); ArgumentOutOfRangeException.ThrowIfLessThanOrEqual(farPlaneDistance, 0.0f); ArgumentOutOfRangeException.ThrowIfGreaterThanOrEqual(nearPlaneDistance, farPlaneDistance); @@ -717,15 +616,18 @@ public static Impl CreatePerspectiveOffCenterLeftHanded(float left, float right, [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreateReflection(in Plane value) { - Plane p = Plane.Normalize(value); - Vector3 f = p.Normal * -2.0f; + // This implementation is based on the DirectX Math Library XMMatrixReflect method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl + + Vector4 p = Plane.Normalize(value).AsVector4(); + Vector4 s = p * -2.0f; Impl result; - result.X = (f * p.Normal.X).AsVector4() + Vector4.UnitX; - result.Y = (f * p.Normal.Y).AsVector4() + Vector4.UnitY; - result.Z = (f * p.Normal.Z).AsVector4() + Vector4.UnitZ; - result.W = Vector4.Create(f * p.D, 1); + result.X = Vector4.MultiplyAddEstimate(Vector4.Create(p.X), s, Vector4.UnitX); + result.Y = Vector4.MultiplyAddEstimate(Vector4.Create(p.Y), s, Vector4.UnitY); + result.Z = Vector4.MultiplyAddEstimate(Vector4.Create(p.Z), s, Vector4.UnitZ); + result.W = Vector4.MultiplyAddEstimate(Vector4.Create(p.W), s, Vector4.UnitW); return result; } @@ -755,8 +657,8 @@ public static Impl CreateRotationX(float radians, in Vector3 centerPoint) { (float s, float c) = float.SinCos(radians); - float y = centerPoint.Y * (1 - c) + centerPoint.Z * s; - float z = centerPoint.Z * (1 - c) - centerPoint.Y * s; + float y = float.MultiplyAddEstimate(centerPoint.Y, 1 - c, +centerPoint.Z * s); + float z = float.MultiplyAddEstimate(centerPoint.Z, 1 - c, -centerPoint.Y * s); // [ 1 0 0 0 ] // [ 0 c s 0 ] @@ -798,8 +700,8 @@ public static Impl CreateRotationY(float radians, in Vector3 centerPoint) { (float s, float c) = float.SinCos(radians); - float x = centerPoint.X * (1 - c) - centerPoint.Z * s; - float z = centerPoint.Z * (1 - c) + centerPoint.X * s; + float x = float.MultiplyAddEstimate(centerPoint.X, 1 - c, -centerPoint.Z * s); + float z = float.MultiplyAddEstimate(centerPoint.Z, 1 - c, +centerPoint.X * s); // [ c 0 -s 0 ] // [ 0 1 0 0 ] @@ -841,8 +743,8 @@ public static Impl CreateRotationZ(float radians, in Vector3 centerPoint) { (float s, float c) = float.SinCos(radians); - float x = centerPoint.X * (1 - c) + centerPoint.Y * s; - float y = centerPoint.Y * (1 - c) - centerPoint.X * s; + float x = float.MultiplyAddEstimate(centerPoint.X, 1 - c, +centerPoint.Y * s); + float y = float.MultiplyAddEstimate(centerPoint.Y, 1 - c, -centerPoint.X * s); // [ c s 0 0 ] // [ -s c 0 0 ] @@ -940,17 +842,18 @@ public static Impl CreateScale(float scale, in Vector3 centerPoint) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl CreateShadow(in Vector3 lightDirection, in Plane plane) { - Plane p = Plane.Normalize(plane); - float dot = Vector3.Dot(lightDirection, p.Normal); + Vector4 p = Plane.Normalize(plane).AsVector4(); + Vector4 l = lightDirection.AsVector4(); + float dot = Vector4.Dot(p, l); - Vector3 normal = -p.Normal; + p = -p; Impl result; - result.X = (lightDirection * normal.X).AsVector4() + Vector4.CreateScalar(dot); - result.Y = (lightDirection * normal.Y).AsVector4() + Vector4.Create(0, dot, 0, 0); - result.Z = (lightDirection * normal.Z).AsVector4() + Vector4.Create(0, 0, dot, 0); - result.W = Vector4.Create(lightDirection * -p.D, dot); + result.X = Vector4.MultiplyAddEstimate(l, Vector4.Create(p.X), Vector4.Create(dot, 0, 0, 0)); + result.Y = Vector4.MultiplyAddEstimate(l, Vector4.Create(p.Y), Vector4.Create(0, dot, 0, 0)); + result.Z = Vector4.MultiplyAddEstimate(l, Vector4.Create(p.Z), Vector4.Create(0, 0, dot, 0)); + result.W = Vector4.MultiplyAddEstimate(l, Vector4.Create(p.W), Vector4.Create(0, 0, 0, dot)); return result; } @@ -1036,190 +939,187 @@ public static Impl CreateWorld(in Vector3 position, in Vector3 forward, in Vecto [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe bool Decompose(in Impl matrix, out Vector3 scale, out Quaternion rotation, out Vector3 translation) { - bool result = true; - - fixed (Vector3* scaleBase = &scale) - { - float* pfScales = (float*)scaleBase; - float det; - - VectorBasis vectorBasis; - Vector3** pVectorBasis = (Vector3**)&vectorBasis; - - Impl matTemp = Identity; - CanonicalBasis canonicalBasis = default; - Vector3* pCanonicalBasis = &canonicalBasis.Row0; + Impl matTemp = Identity; - canonicalBasis.Row0 = Vector3.UnitX; - canonicalBasis.Row1 = Vector3.UnitY; - canonicalBasis.Row2 = Vector3.UnitZ; + Vector3* canonicalBasis = stackalloc Vector3[3] { + Vector3.UnitX, + Vector3.UnitY, + Vector3.UnitZ, + }; - translation = matrix.W.AsVector3(); + translation = matrix.W.AsVector3(); - pVectorBasis[0] = (Vector3*)&matTemp.X; - pVectorBasis[1] = (Vector3*)&matTemp.Y; - pVectorBasis[2] = (Vector3*)&matTemp.Z; + Vector3** vectorBasis = stackalloc Vector3*[3] { + (Vector3*)&matTemp.X, + (Vector3*)&matTemp.Y, + (Vector3*)&matTemp.Z, + }; - *(pVectorBasis[0]) = matrix.X.AsVector3(); - *(pVectorBasis[1]) = matrix.Y.AsVector3(); - *(pVectorBasis[2]) = matrix.Z.AsVector3(); + *(vectorBasis[0]) = matrix.X.AsVector3(); + *(vectorBasis[1]) = matrix.Y.AsVector3(); + *(vectorBasis[2]) = matrix.Z.AsVector3(); - scale.X = pVectorBasis[0]->Length(); - scale.Y = pVectorBasis[1]->Length(); - scale.Z = pVectorBasis[2]->Length(); + float* scales = stackalloc float[3] { + vectorBasis[0]->Length(), + vectorBasis[1]->Length(), + vectorBasis[2]->Length(), + }; - uint a, b, c; + uint a, b, c; - #region Ranking - float x = pfScales[0]; - float y = pfScales[1]; - float z = pfScales[2]; + #region Ranking + float x = scales[0]; + float y = scales[1]; + float z = scales[2]; - if (x < y) + if (x < y) + { + if (y < z) { - if (y < z) + a = 2; + b = 1; + c = 0; + } + else + { + a = 1; + + if (x < z) { - a = 2; - b = 1; + b = 2; c = 0; } else { - a = 1; - - if (x < z) - { - b = 2; - c = 0; - } - else - { - b = 0; - c = 2; - } + b = 0; + c = 2; } } + } + else + { + if (x < z) + { + a = 2; + b = 0; + c = 1; + } else { - if (x < z) + a = 0; + + if (y < z) { - a = 2; - b = 0; + b = 2; c = 1; } else { - a = 0; - - if (y < z) - { - b = 2; - c = 1; - } - else - { - b = 1; - c = 2; - } + b = 1; + c = 2; } } - #endregion + } + #endregion - if (pfScales[a] < DecomposeEpsilon) - { - *(pVectorBasis[a]) = pCanonicalBasis[a]; - } + if (scales[a] < DecomposeEpsilon) + { + *(vectorBasis[a]) = canonicalBasis[a]; + } - *pVectorBasis[a] = Vector3.Normalize(*pVectorBasis[a]); + *vectorBasis[a] = Vector3.Normalize(*vectorBasis[a]); - if (pfScales[b] < DecomposeEpsilon) - { - uint cc; - float fAbsX, fAbsY, fAbsZ; + if (scales[b] < DecomposeEpsilon) + { + uint cc; + float fAbsX, fAbsY, fAbsZ; - fAbsX = float.Abs(pVectorBasis[a]->X); - fAbsY = float.Abs(pVectorBasis[a]->Y); - fAbsZ = float.Abs(pVectorBasis[a]->Z); + fAbsX = float.Abs(vectorBasis[a]->X); + fAbsY = float.Abs(vectorBasis[a]->Y); + fAbsZ = float.Abs(vectorBasis[a]->Z); - #region Ranking - if (fAbsX < fAbsY) + #region Ranking + if (fAbsX < fAbsY) + { + if (fAbsY < fAbsZ) { - if (fAbsY < fAbsZ) + cc = 0; + } + else + { + if (fAbsX < fAbsZ) { cc = 0; } else { - if (fAbsX < fAbsZ) - { - cc = 0; - } - else - { - cc = 2; - } + cc = 2; } } + } + else + { + if (fAbsX < fAbsZ) + { + cc = 1; + } else { - if (fAbsX < fAbsZ) + if (fAbsY < fAbsZ) { cc = 1; } else { - if (fAbsY < fAbsZ) - { - cc = 1; - } - else - { - cc = 2; - } + cc = 2; } } - #endregion - - *pVectorBasis[b] = Vector3.Cross(*pVectorBasis[a], *(pCanonicalBasis + cc)); } + #endregion - *pVectorBasis[b] = Vector3.Normalize(*pVectorBasis[b]); + *vectorBasis[b] = Vector3.Cross(*vectorBasis[a], canonicalBasis[cc]); + } - if (pfScales[c] < DecomposeEpsilon) - { - *pVectorBasis[c] = Vector3.Cross(*pVectorBasis[a], *pVectorBasis[b]); - } + *vectorBasis[b] = Vector3.Normalize(*vectorBasis[b]); - *pVectorBasis[c] = Vector3.Normalize(*pVectorBasis[c]); + if (scales[c] < DecomposeEpsilon) + { + *vectorBasis[c] = Vector3.Cross(*vectorBasis[a], *vectorBasis[b]); + } - det = matTemp.GetDeterminant(); + *vectorBasis[c] = Vector3.Normalize(*vectorBasis[c]); - // use Kramer's rule to check for handedness of coordinate system - if (det < 0.0f) - { - // switch coordinate system by negating the scale and inverting the basis vector on the x-axis - pfScales[a] = -pfScales[a]; - *pVectorBasis[a] = -(*pVectorBasis[a]); + float det = matTemp.GetDeterminant(); - det = -det; - } + // use Kramer's rule to check for handedness of coordinate system + if (det < 0.0f) + { + // switch coordinate system by negating the scale and inverting the basis vector on the x-axis + scales[a] = -scales[a]; + *vectorBasis[a] = -(*vectorBasis[a]); - det -= 1.0f; - det *= det; + det = -det; + } - if ((DecomposeEpsilon < det)) - { - // Non-SRT matrix encountered - rotation = Quaternion.Identity; - result = false; - } - else - { - // generate the quaternion from the matrix - rotation = Quaternion.CreateFromRotationMatrix(Unsafe.As(ref matTemp)); - } + det -= 1.0f; + det *= det; + + bool result; + + if (DecomposeEpsilon < det) + { + // Non-SRT matrix encountered + rotation = Quaternion.Identity; + result = false; + } + else + { + // generate the quaternion from the matrix + rotation = Quaternion.CreateFromRotationMatrix(matTemp.AsM4x4()); + result = true; } + scale = Unsafe.ReadUnaligned(scales); return result; } @@ -1242,7 +1142,7 @@ static bool SseImpl(in Impl matrix, out Impl result) if (!Sse.IsSupported) { // Redundant test so we won't prejit remainder of this method on platforms without SSE. - throw new PlatformNotSupportedException(); + ThrowPlatformNotSupportedException(); } // Load the matrix values into rows @@ -1252,53 +1152,51 @@ static bool SseImpl(in Impl matrix, out Impl result) Vector128 row4 = matrix.W.AsVector128(); // Transpose the matrix - Vector128 vTemp1 = Sse.Shuffle(row1, row2, 0x44); //_MM_SHUFFLE(1, 0, 1, 0) - Vector128 vTemp3 = Sse.Shuffle(row1, row2, 0xEE); //_MM_SHUFFLE(3, 2, 3, 2) - Vector128 vTemp2 = Sse.Shuffle(row3, row4, 0x44); //_MM_SHUFFLE(1, 0, 1, 0) - Vector128 vTemp4 = Sse.Shuffle(row3, row4, 0xEE); //_MM_SHUFFLE(3, 2, 3, 2) - - row1 = Sse.Shuffle(vTemp1, vTemp2, 0x88); //_MM_SHUFFLE(2, 0, 2, 0) - row2 = Sse.Shuffle(vTemp1, vTemp2, 0xDD); //_MM_SHUFFLE(3, 1, 3, 1) - row3 = Sse.Shuffle(vTemp3, vTemp4, 0x88); //_MM_SHUFFLE(2, 0, 2, 0) - row4 = Sse.Shuffle(vTemp3, vTemp4, 0xDD); //_MM_SHUFFLE(3, 1, 3, 1) - - Vector128 V00 = Permute(row3, 0x50); //_MM_SHUFFLE(1, 1, 0, 0) - Vector128 V10 = Permute(row4, 0xEE); //_MM_SHUFFLE(3, 2, 3, 2) - Vector128 V01 = Permute(row1, 0x50); //_MM_SHUFFLE(1, 1, 0, 0) - Vector128 V11 = Permute(row2, 0xEE); //_MM_SHUFFLE(3, 2, 3, 2) - Vector128 V02 = Sse.Shuffle(row3, row1, 0x88); //_MM_SHUFFLE(2, 0, 2, 0) - Vector128 V12 = Sse.Shuffle(row4, row2, 0xDD); //_MM_SHUFFLE(3, 1, 3, 1) + Vector128 vTemp1 = Sse.Shuffle(row1, row2, 0b01_00_01_00); //_MM_SHUFFLE(1, 0, 1, 0) + Vector128 vTemp3 = Sse.Shuffle(row1, row2, 0b11_10_11_10); //_MM_SHUFFLE(3, 2, 3, 2) + Vector128 vTemp2 = Sse.Shuffle(row3, row4, 0b01_00_01_00); //_MM_SHUFFLE(1, 0, 1, 0) + Vector128 vTemp4 = Sse.Shuffle(row3, row4, 0b11_10_11_10); //_MM_SHUFFLE(3, 2, 3, 2) + + row1 = Sse.Shuffle(vTemp1, vTemp2, 0b10_00_10_00); //_MM_SHUFFLE(2, 0, 2, 0) + row2 = Sse.Shuffle(vTemp1, vTemp2, 0b11_01_11_01); //_MM_SHUFFLE(3, 1, 3, 1) + row3 = Sse.Shuffle(vTemp3, vTemp4, 0b10_00_10_00); //_MM_SHUFFLE(2, 0, 2, 0) + row4 = Sse.Shuffle(vTemp3, vTemp4, 0b11_01_11_01); //_MM_SHUFFLE(3, 1, 3, 1) + + Vector128 V00 = Vector128.Shuffle(row3, Vector128.Create(0, 0, 1, 1)); + Vector128 V10 = Vector128.Shuffle(row4, Vector128.Create(2, 3, 2, 3)); + Vector128 V01 = Vector128.Shuffle(row1, Vector128.Create(0, 0, 1, 1)); + Vector128 V11 = Vector128.Shuffle(row2, Vector128.Create(2, 3, 2, 3)); + Vector128 V02 = Sse.Shuffle(row3, row1, 0b10_00_10_00); //_MM_SHUFFLE(2, 0, 2, 0) + Vector128 V12 = Sse.Shuffle(row4, row2, 0b11_01_11_01); //_MM_SHUFFLE(3, 1, 3, 1) Vector128 D0 = V00 * V10; Vector128 D1 = V01 * V11; Vector128 D2 = V02 * V12; - V00 = Permute(row3, 0xEE); //_MM_SHUFFLE(3, 2, 3, 2) - V10 = Permute(row4, 0x50); //_MM_SHUFFLE(1, 1, 0, 0) - V01 = Permute(row1, 0xEE); //_MM_SHUFFLE(3, 2, 3, 2) - V11 = Permute(row2, 0x50); //_MM_SHUFFLE(1, 1, 0, 0) - V02 = Sse.Shuffle(row3, row1, 0xDD); //_MM_SHUFFLE(3, 1, 3, 1) - V12 = Sse.Shuffle(row4, row2, 0x88); //_MM_SHUFFLE(2, 0, 2, 0) + V00 = Vector128.Shuffle(row3, Vector128.Create(2, 3, 2, 3)); + V10 = Vector128.Shuffle(row4, Vector128.Create(0, 0, 1, 1)); + V01 = Vector128.Shuffle(row1, Vector128.Create(2, 3, 2, 3)); + V11 = Vector128.Shuffle(row2, Vector128.Create(0, 0, 1, 1)); + V02 = Sse.Shuffle(row3, row1, 0b11_01_11_01); //_MM_SHUFFLE(3, 1, 3, 1) + V12 = Sse.Shuffle(row4, row2, 0b10_00_10_00); //_MM_SHUFFLE(2, 0, 2, 0) - // Note: We use this expansion pattern instead of Fused Multiply Add - // in order to support older hardware - D0 -= V00 * V10; - D1 -= V01 * V11; - D2 -= V02 * V12; + D0 = Vector128.MultiplyAddEstimate(-V00, V10, D0); + D1 = Vector128.MultiplyAddEstimate(-V01, V11, D1); + D2 = Vector128.MultiplyAddEstimate(-V02, V12, D2); // V11 = D0Y,D0W,D2Y,D2Y - V11 = Sse.Shuffle(D0, D2, 0x5D); //_MM_SHUFFLE(1, 1, 3, 1) - V00 = Permute(row2, 0x49); //_MM_SHUFFLE(1, 0, 2, 1) - V10 = Sse.Shuffle(V11, D0, 0x32); //_MM_SHUFFLE(0, 3, 0, 2) - V01 = Permute(row1, 0x12); //_MM_SHUFFLE(0, 1, 0, 2) - V11 = Sse.Shuffle(V11, D0, 0x99); //_MM_SHUFFLE(2, 1, 2, 1) + V11 = Sse.Shuffle(D0, D2, 0b01_01_11_01); //_MM_SHUFFLE(1, 1, 3, 1) + V00 = Vector128.Shuffle(row2, Vector128.Create(1, 2, 0, 1)); + V10 = Sse.Shuffle(V11, D0, 0b00_11_00_10); //_MM_SHUFFLE(0, 3, 0, 2) + V01 = Vector128.Shuffle(row1, Vector128.Create(2, 0, 1, 0)); + V11 = Sse.Shuffle(V11, D0, 0b10_01_10_01); //_MM_SHUFFLE(2, 1, 2, 1) // V13 = D1Y,D1W,D2W,D2W - Vector128 V13 = Sse.Shuffle(D1, D2, 0xFD); //_MM_SHUFFLE(3, 3, 3, 1) - V02 = Permute(row4, 0x49); //_MM_SHUFFLE(1, 0, 2, 1) - V12 = Sse.Shuffle(V13, D1, 0x32); //_MM_SHUFFLE(0, 3, 0, 2) - Vector128 V03 = Permute(row3, 0x12); //_MM_SHUFFLE(0, 1, 0, 2) - V13 = Sse.Shuffle(V13, D1, 0x99); //_MM_SHUFFLE(2, 1, 2, 1) + Vector128 V13 = Sse.Shuffle(D1, D2, 0b11_11_11_01); //_MM_SHUFFLE(3, 3, 3, 1) + V02 = Vector128.Shuffle(row4, Vector128.Create(1, 2, 0, 1)); + V12 = Sse.Shuffle(V13, D1, 0b00_11_00_10); //_MM_SHUFFLE(0, 3, 0, 2) + Vector128 V03 = Vector128.Shuffle(row3, Vector128.Create(2, 0, 1, 0)); + V13 = Sse.Shuffle(V13, D1, 0b10_01_10_01); //_MM_SHUFFLE(2, 1, 2, 1) Vector128 C0 = V00 * V10; Vector128 C2 = V01 * V11; @@ -1306,44 +1204,44 @@ static bool SseImpl(in Impl matrix, out Impl result) Vector128 C6 = V03 * V13; // V11 = D0X,D0Y,D2X,D2X - V11 = Sse.Shuffle(D0, D2, 0x4); //_MM_SHUFFLE(0, 0, 1, 0) - V00 = Permute(row2, 0x9e); //_MM_SHUFFLE(2, 1, 3, 2) - V10 = Sse.Shuffle(D0, V11, 0x93); //_MM_SHUFFLE(2, 1, 0, 3) - V01 = Permute(row1, 0x7b); //_MM_SHUFFLE(1, 3, 2, 3) - V11 = Sse.Shuffle(D0, V11, 0x26); //_MM_SHUFFLE(0, 2, 1, 2) + V11 = Sse.Shuffle(D0, D2, 0b00_00_01_00); //_MM_SHUFFLE(0, 0, 1, 0) + V00 = Vector128.Shuffle(row2, Vector128.Create(2, 3, 1, 2)); + V10 = Sse.Shuffle(D0, V11, 0b10_01_00_11); //_MM_SHUFFLE(2, 1, 0, 3) + V01 = Vector128.Shuffle(row1, Vector128.Create(3, 2, 3, 1)); + V11 = Sse.Shuffle(D0, V11, 0b00_10_01_10); //_MM_SHUFFLE(0, 2, 1, 2) // V13 = D1X,D1Y,D2Z,D2Z - V13 = Sse.Shuffle(D1, D2, 0xa4); //_MM_SHUFFLE(2, 2, 1, 0) - V02 = Permute(row4, 0x9e); //_MM_SHUFFLE(2, 1, 3, 2) - V12 = Sse.Shuffle(D1, V13, 0x93); //_MM_SHUFFLE(2, 1, 0, 3) - V03 = Permute(row3, 0x7b); //_MM_SHUFFLE(1, 3, 2, 3) - V13 = Sse.Shuffle(D1, V13, 0x26); //_MM_SHUFFLE(0, 2, 1, 2) + V13 = Sse.Shuffle(D1, D2, 0b10_10_01_00); //_MM_SHUFFLE(2, 2, 1, 0) + V02 = Vector128.Shuffle(row4, Vector128.Create(2, 3, 1, 2)); + V12 = Sse.Shuffle(D1, V13, 0b10_01_00_11); //_MM_SHUFFLE(2, 1, 0, 3) + V03 = Vector128.Shuffle(row3, Vector128.Create(3, 2, 3, 1)); + V13 = Sse.Shuffle(D1, V13, 0b_00_10_01_10); //_MM_SHUFFLE(0, 2, 1, 2) - C0 -= V00 * V10; - C2 -= V01 * V11; - C4 -= V02 * V12; - C6 -= V03 * V13; + C0 = Vector128.MultiplyAddEstimate(-V00, V10, C0); + C2 = Vector128.MultiplyAddEstimate(-V01, V11, C2); + C4 = Vector128.MultiplyAddEstimate(-V02, V12, C4); + C6 = Vector128.MultiplyAddEstimate(-V03, V13, C6); - V00 = Permute(row2, 0x33); //_MM_SHUFFLE(0, 3, 0, 3) + V00 = Vector128.Shuffle(row2, Vector128.Create(3, 0, 3, 0)); // V10 = D0Z,D0Z,D2X,D2Y - V10 = Sse.Shuffle(D0, D2, 0x4A); //_MM_SHUFFLE(1, 0, 2, 2) - V10 = Permute(V10, 0x2C); //_MM_SHUFFLE(0, 2, 3, 0) - V01 = Permute(row1, 0x8D); //_MM_SHUFFLE(2, 0, 3, 1) + V10 = Sse.Shuffle(D0, D2, 0b01_00_10_10); //_MM_SHUFFLE(1, 0, 2, 2) + V10 = Vector128.Shuffle(V10, Vector128.Create(0, 3, 2, 0)); + V01 = Vector128.Shuffle(row1, Vector128.Create(1, 3, 0, 2)); // V11 = D0X,D0W,D2X,D2Y - V11 = Sse.Shuffle(D0, D2, 0x4C); //_MM_SHUFFLE(1, 0, 3, 0) - V11 = Permute(V11, 0x93); //_MM_SHUFFLE(2, 1, 0, 3) - V02 = Permute(row4, 0x33); //_MM_SHUFFLE(0, 3, 0, 3) + V11 = Sse.Shuffle(D0, D2, 0b01_00_11_00); //_MM_SHUFFLE(1, 0, 3, 0) + V11 = Vector128.Shuffle(V11, Vector128.Create(3, 0, 1, 2)); + V02 = Vector128.Shuffle(row4, Vector128.Create(3, 0, 3, 0)); // V12 = D1Z,D1Z,D2Z,D2W - V12 = Sse.Shuffle(D1, D2, 0xEA); //_MM_SHUFFLE(3, 2, 2, 2) - V12 = Permute(V12, 0x2C); //_MM_SHUFFLE(0, 2, 3, 0) - V03 = Permute(row3, 0x8D); //_MM_SHUFFLE(2, 0, 3, 1) + V12 = Sse.Shuffle(D1, D2, 0b11_10_10_10); //_MM_SHUFFLE(3, 2, 2, 2) + V12 = Vector128.Shuffle(V12, Vector128.Create(0, 3, 2, 0)); + V03 = Vector128.Shuffle(row3, Vector128.Create(1, 3, 0, 2)); // V13 = D1X,D1W,D2Z,D2W - V13 = Sse.Shuffle(D1, D2, 0xEC); //_MM_SHUFFLE(3, 2, 3, 0) - V13 = Permute(V13, 0x93); //_MM_SHUFFLE(2, 1, 0, 3) + V13 = Sse.Shuffle(D1, D2, 0b11_10_11_00); //_MM_SHUFFLE(3, 2, 3, 0) + V13 = Vector128.Shuffle(V13, Vector128.Create(3, 0, 1, 2)); V00 *= V10; V01 *= V11; @@ -1362,15 +1260,15 @@ static bool SseImpl(in Impl matrix, out Impl result) Vector128 C7 = C6 + V03; C6 -= V03; - C0 = Sse.Shuffle(C0, C1, 0xD8); //_MM_SHUFFLE(3, 1, 2, 0) - C2 = Sse.Shuffle(C2, C3, 0xD8); //_MM_SHUFFLE(3, 1, 2, 0) - C4 = Sse.Shuffle(C4, C5, 0xD8); //_MM_SHUFFLE(3, 1, 2, 0) - C6 = Sse.Shuffle(C6, C7, 0xD8); //_MM_SHUFFLE(3, 1, 2, 0) + C0 = Sse.Shuffle(C0, C1, 0b11_01_10_00); //_MM_SHUFFLE(3, 1, 2, 0) + C2 = Sse.Shuffle(C2, C3, 0b11_01_10_00); //_MM_SHUFFLE(3, 1, 2, 0) + C4 = Sse.Shuffle(C4, C5, 0b11_01_10_00); //_MM_SHUFFLE(3, 1, 2, 0) + C6 = Sse.Shuffle(C6, C7, 0b11_01_10_00); //_MM_SHUFFLE(3, 1, 2, 0) - C0 = Permute(C0, 0xD8); //_MM_SHUFFLE(3, 1, 2, 0) - C2 = Permute(C2, 0xD8); //_MM_SHUFFLE(3, 1, 2, 0) - C4 = Permute(C4, 0xD8); //_MM_SHUFFLE(3, 1, 2, 0) - C6 = Permute(C6, 0xD8); //_MM_SHUFFLE(3, 1, 2, 0) + C0 = Vector128.Shuffle(C0, Vector128.Create(0, 2, 1, 3)); + C2 = Vector128.Shuffle(C2, Vector128.Create(0, 2, 1, 3)); + C4 = Vector128.Shuffle(C4, Vector128.Create(0, 2, 1, 3)); + C6 = Vector128.Shuffle(C6, Vector128.Create(0, 2, 1, 3)); // Get the determinant float det = Vector4.Dot(C0.AsVector4(), row1.AsVector4()); @@ -1390,10 +1288,7 @@ static bool SseImpl(in Impl matrix, out Impl result) // Create Vector128 copy of the determinant and invert them. - Vector128 ones = Vector128.Create(1.0f); - Vector128 vTemp = Vector128.Create(det); - - vTemp = ones / vTemp; + Vector128 vTemp = Vector128.One / det; result.X = (C0 * vTemp).AsVector4(); result.Y = (C2 * vTemp).AsVector4(); @@ -1403,24 +1298,6 @@ static bool SseImpl(in Impl matrix, out Impl result) return true; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static Vector128 Permute(Vector128 value, [ConstantExpected] byte control) - { - if (Avx.IsSupported) - { - return Avx.Permute(value, control); - } - else if (Sse.IsSupported) - { - return Sse.Shuffle(value, value, control); - } - else - { - // Redundant test so we won't prejit remainder of this method on platforms without SSE. - throw new PlatformNotSupportedException(); - } - } - static bool SoftwareFallback(in Impl matrix, out Impl result) { // -1 @@ -1782,21 +1659,9 @@ public readonly float GetDeterminant() [MethodImpl(MethodImplOptions.AggressiveInlining)] public override readonly int GetHashCode() => HashCode.Combine(X, Y, Z, W); - bool IEquatable.Equals(Impl other) => Equals(in other); - - private struct CanonicalBasis - { - public Vector3 Row0; - public Vector3 Row1; - public Vector3 Row2; - }; + readonly bool IEquatable.Equals(Impl other) => Equals(in other); - private unsafe struct VectorBasis - { - public Vector3* Element0; - public Vector3* Element1; - public Vector3* Element2; - } + private static void ThrowPlatformNotSupportedException() => throw new PlatformNotSupportedException(); } } } diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Plane.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Plane.cs index 55f4522800316..67df1c546b0ae 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Plane.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Plane.cs @@ -14,8 +14,6 @@ namespace System.Numerics [Intrinsic] public struct Plane : IEquatable { - private const float NormalizeEpsilon = 1.192092896e-07f; // smallest such that 1.0+NormalizeEpsilon != 1.0 - /// The normal vector of the plane. public Vector3 Normal; @@ -74,49 +72,15 @@ public Plane(Vector4 value) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Plane CreateFromVertices(Vector3 point1, Vector3 point2, Vector3 point3) { - if (Vector128.IsHardwareAccelerated) - { - Vector3 a = point2 - point1; - Vector3 b = point3 - point1; - - // N = Cross(a, b) - Vector3 n = Vector3.Cross(a, b); - Vector3 normal = Vector3.Normalize(n); - - // D = - Dot(N, point1) - float d = -Vector3.Dot(normal, point1); - - return Create(normal, d); - } - else - { - float ax = point2.X - point1.X; - float ay = point2.Y - point1.Y; - float az = point2.Z - point1.Z; + // This implementation is based on the DirectX Math Library XMPlaneFromPoints method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl - float bx = point3.X - point1.X; - float by = point3.Y - point1.Y; - float bz = point3.Z - point1.Z; + Vector3 normal = Vector3.Normalize(Vector3.Cross(point2 - point1, point3 - point1)); - // N=Cross(a,b) - float nx = ay * bz - az * by; - float ny = az * bx - ax * bz; - float nz = ax * by - ay * bx; - - // Normalize(N) - float ls = nx * nx + ny * ny + nz * nz; - float invNorm = 1.0f / float.Sqrt(ls); - - Vector3 normal = Vector3.Create( - nx * invNorm, - ny * invNorm, - nz * invNorm); - - return Create( - normal, - -(normal.X * point1.X + normal.Y * point1.Y + normal.Z * point1.Z) - ); - } + return Create( + normal, + -Vector3.Dot(normal, point1) + ); } /// Calculates the dot product of a plane and a 4-dimensional vector. @@ -131,29 +95,40 @@ public static Plane CreateFromVertices(Vector3 point1, Vector3 point2, Vector3 p /// The plane. /// The 3-dimensional vector. /// The dot product. - public static float DotCoordinate(Plane plane, Vector3 value) => Vector3.Dot(plane.Normal, value) + plane.D; + public static float DotCoordinate(Plane plane, Vector3 value) + { + // This implementation is based on the DirectX Math Library XMPlaneDotCoord method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl + + return Dot(plane, Vector4.Create(value, 1.0f)); + } /// Returns the dot product of a specified three-dimensional vector and the vector of this plane. /// The plane. /// The three-dimensional vector. /// The dot product. - public static float DotNormal(Plane plane, Vector3 value) => Vector3.Dot(plane.Normal, value); + public static float DotNormal(Plane plane, Vector3 value) + { + // This implementation is based on the DirectX Math Library XMPlaneDotNormal method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl + + return Vector3.Dot(plane.Normal, value); + } /// Creates a new object whose normal vector is the source plane's normal vector normalized. /// The source plane. /// The normalized plane. - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Plane Normalize(Plane value) { - float normalLengthSquared = value.Normal.LengthSquared(); + // This implementation is based on the DirectX Math Library XMPlaneNormalize method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl - if (float.Abs(normalLengthSquared - 1.0f) < NormalizeEpsilon) - { - // It already normalized, so we don't need to farther process. - return value; - } + Vector128 lengthSquared = Vector128.Create(value.Normal.LengthSquared()); - return (value.AsVector128() / float.Sqrt(normalLengthSquared)).AsPlane(); + return Vector128.AndNot( + (value.AsVector128() / Vector128.Sqrt(lengthSquared)), + Vector128.Equals(lengthSquared, Vector128.Create(float.PositiveInfinity)) + ).AsPlane(); } /// Transforms a normalized plane by a 4x4 matrix. @@ -164,16 +139,8 @@ public static Plane Normalize(Plane value) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Plane Transform(Plane plane, Matrix4x4 matrix) { - Matrix4x4.Invert(matrix, out Matrix4x4 m); - - float x = plane.Normal.X, y = plane.Normal.Y, z = plane.Normal.Z, w = plane.D; - - return Create( - x * m.M11 + y * m.M12 + z * m.M13 + w * m.M14, - x * m.M21 + y * m.M22 + z * m.M23 + w * m.M24, - x * m.M31 + y * m.M32 + z * m.M33 + w * m.M34, - x * m.M41 + y * m.M42 + z * m.M43 + w * m.M44 - ); + Matrix4x4.Impl.Invert(matrix.AsImpl(), out Matrix4x4.Impl inverseMatrix); + return Vector4.Transform(plane.AsVector4(), Matrix4x4.Impl.Transpose(inverseMatrix)).AsPlane(); } /// Transforms a normalized plane by a Quaternion rotation. @@ -182,44 +149,7 @@ public static Plane Transform(Plane plane, Matrix4x4 matrix) /// A new plane that results from applying the Quaternion rotation. /// must already be normalized so that its vector is of unit length before this method is called. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Plane Transform(Plane plane, Quaternion rotation) - { - // Compute rotation matrix. - float x2 = rotation.X + rotation.X; - float y2 = rotation.Y + rotation.Y; - float z2 = rotation.Z + rotation.Z; - - float wx2 = rotation.W * x2; - float wy2 = rotation.W * y2; - float wz2 = rotation.W * z2; - float xx2 = rotation.X * x2; - float xy2 = rotation.X * y2; - float xz2 = rotation.X * z2; - float yy2 = rotation.Y * y2; - float yz2 = rotation.Y * z2; - float zz2 = rotation.Z * z2; - - float m11 = 1.0f - yy2 - zz2; - float m21 = xy2 - wz2; - float m31 = xz2 + wy2; - - float m12 = xy2 + wz2; - float m22 = 1.0f - xx2 - zz2; - float m32 = yz2 - wx2; - - float m13 = xz2 - wy2; - float m23 = yz2 + wx2; - float m33 = 1.0f - xx2 - yy2; - - float x = plane.Normal.X, y = plane.Normal.Y, z = plane.Normal.Z; - - return Create( - x * m11 + y * m21 + z * m31, - x * m12 + y * m22 + z * m32, - x * m13 + y * m23 + z * m33, - plane.D - ); - } + public static Plane Transform(Plane plane, Quaternion rotation) => Vector4.Transform(plane.AsVector4(), rotation).AsPlane(); /// Returns a value that indicates whether two planes are equal. /// The first plane to compare. diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs index b14a70e68106b..f52da0df94f0d 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs @@ -1,7 +1,6 @@ // Licensed to the .NET Foundation under one or more agreements. // The .NET Foundation licenses this file to you under the MIT license. -using System.Diagnostics; using System.Diagnostics.CodeAnalysis; using System.Runtime.CompilerServices; using System.Runtime.Intrinsics; @@ -14,8 +13,6 @@ namespace System.Numerics [Intrinsic] public struct Quaternion : IEquatable { - private const float SlerpEpsilon = 1e-6f; - /// The X value of the vector component of the quaternion. public float X; @@ -76,6 +73,7 @@ public float this[int index] readonly get => this.AsVector128().GetElement(index); [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] set { this = this.AsVector128().WithElement(index, value).AsQuaternion(); @@ -101,43 +99,7 @@ public float this[int index] /// The divisor. /// The quaternion that results from dividing by . /// The method defines the division operation for objects. - public static Quaternion operator /(Quaternion value1, Quaternion value2) - { - Quaternion ans; - - float q1x = value1.X; - float q1y = value1.Y; - float q1z = value1.Z; - float q1w = value1.W; - - //------------------------------------- - // Inverse part. - float ls = value2.X * value2.X + value2.Y * value2.Y + - value2.Z * value2.Z + value2.W * value2.W; - float invNorm = 1.0f / ls; - - float q2x = -value2.X * invNorm; - float q2y = -value2.Y * invNorm; - float q2z = -value2.Z * invNorm; - float q2w = value2.W * invNorm; - - //------------------------------------- - // Multiply part. - - // cross(av, bv) - float cx = q1y * q2z - q1z * q2y; - float cy = q1z * q2x - q1x * q2z; - float cz = q1x * q2y - q1y * q2x; - - float dot = q1x * q2x + q1y * q2y + q1z * q2z; - - ans.X = q1x * q2w + q2x * q1w + cx; - ans.Y = q1y * q2w + q2y * q1w + cy; - ans.Z = q1z * q2w + q2z * q1w + cz; - ans.W = q1w * q2w - dot; - - return ans; - } + public static Quaternion operator /(Quaternion value1, Quaternion value2) => value1 * Inverse(value2); /// Returns a value that indicates whether two quaternions are equal. /// The first quaternion to compare. @@ -161,47 +123,20 @@ public float this[int index] /// The second quaternion. /// The product quaternion. /// The method defines the operation of the multiplication operator for objects. + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Quaternion operator *(Quaternion value1, Quaternion value2) { - if (Vector128.IsHardwareAccelerated) - { - Vector128 left = value1.AsVector128(); - Vector128 right = value2.AsVector128(); - - Vector128 result = right * left.GetElementUnsafe(3); - result += (Vector128.Shuffle(right, Vector128.Create(3, 2, 1, 0)) * left.GetElementUnsafe(0)) * Vector128.Create(+1.0f, -1.0f, +1.0f, -1.0f); - result += (Vector128.Shuffle(right, Vector128.Create(2, 3, 0, 1)) * left.GetElementUnsafe(1)) * Vector128.Create(+1.0f, +1.0f, -1.0f, -1.0f); - result += (Vector128.Shuffle(right, Vector128.Create(1, 0, 3, 2)) * left.GetElementUnsafe(2)) * Vector128.Create(-1.0f, +1.0f, +1.0f, -1.0f); - return Unsafe.BitCast, Quaternion>(result); - } - else - { - Quaternion ans; - - float q1x = value1.X; - float q1y = value1.Y; - float q1z = value1.Z; - float q1w = value1.W; - - float q2x = value2.X; - float q2y = value2.Y; - float q2z = value2.Z; - float q2w = value2.W; - - // cross(av, bv) - float cx = q1y * q2z - q1z * q2y; - float cy = q1z * q2x - q1x * q2z; - float cz = q1x * q2y - q1y * q2x; + // This implementation is based on the DirectX Math Library XMQuaternionMultiply method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl - float dot = q1x * q2x + q1y * q2y + q1z * q2z; + Vector128 left = value1.AsVector128(); + Vector128 right = value2.AsVector128(); - ans.X = q1x * q2w + q2x * q1w + cx; - ans.Y = q1y * q2w + q2y * q1w + cy; - ans.Z = q1z * q2w + q2z * q1w + cz; - ans.W = q1w * q2w - dot; - - return ans; - } + Vector128 result = right * left.GetElement(3); + result = Vector128.MultiplyAddEstimate(Vector128.Shuffle(right, Vector128.Create(3, 2, 1, 0)) * left.GetElement(0), Vector128.Create(+1.0f, -1.0f, +1.0f, -1.0f), result); + result = Vector128.MultiplyAddEstimate(Vector128.Shuffle(right, Vector128.Create(2, 3, 0, 1)) * left.GetElement(1), Vector128.Create(+1.0f, +1.0f, -1.0f, -1.0f), result); + result = Vector128.MultiplyAddEstimate(Vector128.Shuffle(right, Vector128.Create(1, 0, 3, 2)) * left.GetElement(2), Vector128.Create(-1.0f, +1.0f, +1.0f, -1.0f), result); + return result.AsQuaternion(); } /// Returns the quaternion that results from scaling all the components of a specified quaternion by a scalar factor. @@ -241,43 +176,20 @@ public float this[int index] /// The first quaternion rotation in the series. /// The second quaternion rotation in the series. /// A new quaternion representing the concatenation of the rotation followed by the rotation. - public static Quaternion Concatenate(Quaternion value1, Quaternion value2) - { - Quaternion ans; - - // Concatenate rotation is actually q2 * q1 instead of q1 * q2. - // So that's why value2 goes q1 and value1 goes q2. - float q1x = value2.X; - float q1y = value2.Y; - float q1z = value2.Z; - float q1w = value2.W; - - float q2x = value1.X; - float q2y = value1.Y; - float q2z = value1.Z; - float q2w = value1.W; - - // cross(av, bv) - float cx = q1y * q2z - q1z * q2y; - float cy = q1z * q2x - q1x * q2z; - float cz = q1x * q2y - q1y * q2x; - - float dot = q1x * q2x + q1y * q2y + q1z * q2z; - - ans.X = q1x * q2w + q2x * q1w + cx; - ans.Y = q1y * q2w + q2y * q1w + cy; - ans.Z = q1z * q2w + q2z * q1w + cz; - ans.W = q1w * q2w - dot; - - return ans; - } + public static Quaternion Concatenate(Quaternion value1, Quaternion value2) => value2 * value1; /// Returns the conjugate of a specified quaternion. /// The quaternion. /// A new quaternion that is the conjugate of . [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Quaternion Conjugate(Quaternion value) => (value.AsVector128() * Vector128.Create(-1.0f, -1.0f, -1.0f, 1.0f)).AsQuaternion(); + public static Quaternion Conjugate(Quaternion value) + { + // This implementation is based on the DirectX Math Library XMQuaternionConjugate method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl + + return (value.AsVector128() * Vector128.Create(-1.0f, -1.0f, -1.0f, 1.0f)).AsQuaternion(); + } /// Creates a quaternion from the specified components. /// The value to assign to the X component of the quaternion. @@ -300,18 +212,14 @@ public static Quaternion Concatenate(Quaternion value1, Quaternion value2) /// The angle, in radians, to rotate around the vector. /// The newly created quaternion. /// vector must be normalized before calling this method or the resulting will be incorrect. + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle) { - Quaternion ans; + // This implementation is based on the DirectX Math Library XMQuaternionRotationNormal method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl (float s, float c) = float.SinCos(angle * 0.5f); - - ans.X = axis.X * s; - ans.Y = axis.Y * s; - ans.Z = axis.Z * s; - ans.W = c; - - return ans; + return (Vector4.Create(axis, 1) * Vector4.Create(Vector3.Create(s), c)).AsQuaternion(); } /// Creates a quaternion from the specified rotation matrix. @@ -410,11 +318,20 @@ public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float ro [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Quaternion Inverse(Quaternion value) { + // This implementation is based on the DirectX Math Library XMQuaternionInverse method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl + + const float Epsilon = 1.192092896e-7f; + // -1 ( a -v ) // q = ( ------------- ------------- ) // ( a^2 + |v|^2 , a^2 + |v|^2 ) - return (Conjugate(value).AsVector128() / value.LengthSquared()).AsQuaternion(); + Vector128 lengthSquared = Vector128.Create(value.LengthSquared()); + return Vector128.AndNot( + (Conjugate(value).AsVector128() / lengthSquared), + Vector128.LessThanOrEqual(lengthSquared, Vector128.Create(Epsilon)) + ).AsQuaternion(); } /// Performs a linear interpolation between two quaternions based on a value that specifies the weighting of the second quaternion. @@ -424,39 +341,16 @@ public static Quaternion Inverse(Quaternion value) /// The interpolated quaternion. public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount) { - float t = amount; - float t1 = 1.0f - t; + Vector128 q2 = quaternion2.AsVector128(); - Quaternion r = default; + q2 = Vector128.ConditionalSelect( + Vector128.GreaterThanOrEqual(Vector128.Create(Dot(quaternion1, quaternion2)), Vector128.Zero), + q2, + -q2 + ); - float dot = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + - quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W; - - if (dot >= 0.0f) - { - r.X = t1 * quaternion1.X + t * quaternion2.X; - r.Y = t1 * quaternion1.Y + t * quaternion2.Y; - r.Z = t1 * quaternion1.Z + t * quaternion2.Z; - r.W = t1 * quaternion1.W + t * quaternion2.W; - } - else - { - r.X = t1 * quaternion1.X - t * quaternion2.X; - r.Y = t1 * quaternion1.Y - t * quaternion2.Y; - r.Z = t1 * quaternion1.Z - t * quaternion2.Z; - r.W = t1 * quaternion1.W - t * quaternion2.W; - } - - // Normalize it. - float ls = r.X * r.X + r.Y * r.Y + r.Z * r.Z + r.W * r.W; - float invNorm = 1.0f / float.Sqrt(ls); - - r.X *= invNorm; - r.Y *= invNorm; - r.Z *= invNorm; - r.W *= invNorm; - - return r; + Vector128 result = Vector128.MultiplyAddEstimate(quaternion1.AsVector128(), Vector128.Create(1.0f - amount), q2 * amount); + return Normalize(result.AsQuaternion()); } /// Returns the quaternion that results from multiplying two quaternions together. @@ -492,17 +386,15 @@ public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, fl /// The interpolated quaternion. public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount) { - float t = amount; + const float SlerpEpsilon = 1e-6f; - float cosOmega = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + - quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W; - - bool flip = false; + float cosOmega = Dot(quaternion1, quaternion2); + float sign = 1.0f; if (cosOmega < 0.0f) { - flip = true; cosOmega = -cosOmega; + sign = -1.0f; } float s1, s2; @@ -510,28 +402,19 @@ public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, f if (cosOmega > (1.0f - SlerpEpsilon)) { // Too close, do straight linear interpolation. - s1 = 1.0f - t; - s2 = (flip) ? -t : t; + s1 = 1.0f - amount; + s2 = amount * sign; } else { float omega = float.Acos(cosOmega); float invSinOmega = 1 / float.Sin(omega); - s1 = float.Sin((1.0f - t) * omega) * invSinOmega; - s2 = (flip) - ? -float.Sin(t * omega) * invSinOmega - : float.Sin(t * omega) * invSinOmega; + s1 = float.Sin((1.0f - amount) * omega) * invSinOmega; + s2 = float.Sin(amount * omega) * invSinOmega * sign; } - Quaternion ans; - - ans.X = s1 * quaternion1.X + s2 * quaternion2.X; - ans.Y = s1 * quaternion1.Y + s2 * quaternion2.Y; - ans.Z = s1 * quaternion1.Z + s2 * quaternion2.Z; - ans.W = s1 * quaternion1.W + s2 * quaternion2.W; - - return ans; + return (quaternion1 * s1) + (quaternion2 * s2); } /// Subtracts each element in a second quaternion from its corresponding element in a first quaternion. diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector2.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector2.cs index 7eb79bf3524a7..cd4a2c5c0358e 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector2.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector2.cs @@ -89,6 +89,7 @@ public static Vector2 UnitY public float this[int index] { [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] readonly get { if ((uint)index >= Count) @@ -99,6 +100,7 @@ readonly get } [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] set { if ((uint)index >= Count) @@ -133,6 +135,7 @@ readonly get /// The result of the division. /// The method defines the division operation for objects. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector2 operator /(Vector2 value1, float value2) => (value1.AsVector128Unsafe() / value2).AsVector2(); /// Returns a value that indicates whether each pair of elements in two specified vectors is equal. @@ -166,6 +169,7 @@ readonly get /// The scaled vector. /// The method defines the multiplication operation for objects. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector2 operator *(Vector2 left, float right) => (left.AsVector128Unsafe() * right).AsVector2(); /// Multiplies the scalar value by the specified vector. @@ -190,6 +194,7 @@ readonly get /// The negated vector. /// The method defines the unary negation operation for objects. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector2 operator -(Vector2 value) => (-value.AsVector128Unsafe()).AsVector2(); /// Returns a vector whose elements are the absolute values of each of the specified vector's elements. @@ -234,6 +239,7 @@ public static Vector2 Clamp(Vector2 value1, Vector2 min, Vector2 max) /// Constructs a vector from the given . The span must contain at least 2 elements. /// The span of elements to assign to the vector. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector2 Create(ReadOnlySpan values) { if (values.Length < Count) @@ -306,7 +312,7 @@ public static Vector2 Create(ReadOnlySpan values) /// ]]> [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Lerp(Vector2 value1, Vector2 value2, float amount) => (value1 * (1.0f - amount)) + (value2 * amount); + public static Vector2 Lerp(Vector2 value1, Vector2 value2, float amount) => MultiplyAddEstimate(value1, Create(1.0f - amount), value2 * amount); /// Returns a vector whose elements are the maximum of each of the pairs of elements in two specified vectors. /// The first vector. @@ -367,7 +373,15 @@ public static Vector2 Create(ReadOnlySpan values) /// The normal of the surface being reflected off. /// The reflected vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Reflect(Vector2 vector, Vector2 normal) => vector - (2.0f * (Dot(vector, normal) * normal)); + public static Vector2 Reflect(Vector2 vector, Vector2 normal) + { + // This implementation is based on the DirectX Math Library XMVector2Reflect method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathVector.inl + + Vector2 tmp = Create(Dot(vector, normal)); + tmp += tmp; + return MultiplyAddEstimate(-tmp, normal, vector); + } /// Returns a vector whose elements are the square root of each of a specified vector's elements. /// A vector. @@ -393,11 +407,8 @@ public static Vector2 Create(ReadOnlySpan values) internal static Vector2 Transform(Vector2 position, in Matrix3x2.Impl matrix) { Vector2 result = matrix.X * position.X; - - result += matrix.Y * position.Y; - result += matrix.Z; - - return result; + result = MultiplyAddEstimate(matrix.Y, Create(position.Y), result); + return result + matrix.Z; } /// Transforms a vector by a specified 4x4 matrix. @@ -412,23 +423,7 @@ internal static Vector2 Transform(Vector2 position, in Matrix3x2.Impl matrix) /// The rotation to apply. /// The transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Transform(Vector2 value, Quaternion rotation) - { - float x2 = rotation.X + rotation.X; - float y2 = rotation.Y + rotation.Y; - float z2 = rotation.Z + rotation.Z; - - float wz2 = rotation.W * z2; - float xx2 = rotation.X * x2; - float xy2 = rotation.X * y2; - float yy2 = rotation.Y * y2; - float zz2 = rotation.Z * z2; - - return Create( - value.X * (1.0f - yy2 - zz2) + value.Y * (xy2 - wz2), - value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2) - ); - } + public static Vector2 Transform(Vector2 value, Quaternion rotation) => Vector4.Transform(value, rotation).AsVector2(); /// Transforms a vector normal by the given 3x2 matrix. /// The source vector. @@ -440,9 +435,7 @@ public static Vector2 Transform(Vector2 value, Quaternion rotation) internal static Vector2 TransformNormal(Vector2 normal, in Matrix3x2.Impl matrix) { Vector2 result = matrix.X * normal.X; - - result += matrix.Y * normal.Y; - + result = MultiplyAddEstimate(matrix.Y, Create(normal.Y), result); return result; } @@ -456,10 +449,8 @@ internal static Vector2 TransformNormal(Vector2 normal, in Matrix3x2.Impl matrix internal static Vector2 TransformNormal(Vector2 normal, in Matrix4x4.Impl matrix) { Vector4 result = matrix.X * normal.X; - - result += matrix.Y * normal.Y; - - return result.AsVector128().AsVector2(); + result = Vector4.MultiplyAddEstimate(matrix.Y, Vector4.Create(normal.Y), result); + return result.AsVector2(); } /// Copies the elements of the vector to a specified array. diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs index 29efe6d702831..ec97d4a4f5d52 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs @@ -110,6 +110,7 @@ public static Vector3 UnitZ public float this[int index] { [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] readonly get { if ((uint)index >= Count) @@ -120,6 +121,7 @@ readonly get } [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] set { if ((uint)index >= Count) @@ -154,6 +156,7 @@ readonly get /// The result of the division. /// The method defines the division operation for objects. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3 operator /(Vector3 value1, float value2) => (value1.AsVector128Unsafe() / value2).AsVector3(); /// Returns a value that indicates whether each pair of elements in two specified vectors is equal. @@ -187,6 +190,7 @@ readonly get /// The scaled vector. /// The method defines the multiplication operation for objects. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3 operator *(Vector3 left, float right) => (left.AsVector128Unsafe() * right).AsVector3(); /// Multiplies the scalar value by the specified vector. @@ -211,6 +215,7 @@ readonly get /// The negated vector. /// The method defines the unary negation operation for objects. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3 operator -(Vector3 value) => (-value.AsVector128Unsafe()).AsVector3(); /// Returns a vector whose elements are the absolute values of each of the specified vector's elements. @@ -250,6 +255,7 @@ public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max) /// The Z component. /// A new from the specified object and a Z and a W component. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3 Create(Vector2 vector, float z) { return vector.AsVector128Unsafe() @@ -268,6 +274,7 @@ public static Vector3 Create(Vector2 vector, float z) /// Constructs a vector from the given . The span must contain at least 3 elements. /// The span of elements to assign to the vector. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3 Create(ReadOnlySpan values) { if (values.Length < Count) @@ -296,11 +303,19 @@ public static Vector3 Create(ReadOnlySpan values) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3 Cross(Vector3 vector1, Vector3 vector2) { - return Create( - (vector1.Y * vector2.Z) - (vector1.Z * vector2.Y), - (vector1.Z * vector2.X) - (vector1.X * vector2.Z), - (vector1.X * vector2.Y) - (vector1.Y * vector2.X) - ); + // This implementation is based on the DirectX Math Library XMVector3Cross method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathVector.inl + + Vector128 v1 = vector1.AsVector128(); + Vector128 v2 = vector2.AsVector128(); + + Vector128 temp = Vector128.Shuffle(v1, Vector128.Create(1, 2, 0, 3)) * Vector128.Shuffle(v2, Vector128.Create(2, 0, 1, 3)); + + return Vector128.MultiplyAddEstimate( + -Vector128.Shuffle(v1, Vector128.Create(2, 0, 1, 3)), + Vector128.Shuffle(v2, Vector128.Create(1, 2, 0, 3)), + temp + ).AsVector3(); } /// Computes the Euclidean distance between the two given points. @@ -351,7 +366,7 @@ public static Vector3 Cross(Vector3 vector1, Vector3 vector2) /// The interpolated vector. [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount) => (value1 * (1.0f - amount)) + (value2 * amount); + public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount) => MultiplyAddEstimate(value1, Create(1.0f - amount), value2 * amount); /// Returns a vector whose elements are the maximum of each of the pairs of elements in two specified vectors. /// The first vector. @@ -412,7 +427,15 @@ public static Vector3 Cross(Vector3 vector1, Vector3 vector2) /// The normal of the surface being reflected off. /// The reflected vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Reflect(Vector3 vector, Vector3 normal) => vector - (2.0f * (Dot(vector, normal) * normal)); + public static Vector3 Reflect(Vector3 vector, Vector3 normal) + { + // This implementation is based on the DirectX Math Library XMVector3Reflect method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathVector.inl + + Vector3 tmp = Create(Dot(vector, normal)); + tmp += tmp; + return MultiplyAddEstimate(-tmp, normal, vector); + } /// Returns a vector whose elements are the square root of each of a specified vector's elements. /// A vector. @@ -440,28 +463,7 @@ public static Vector3 Cross(Vector3 vector1, Vector3 vector2) /// The rotation to apply. /// The transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Transform(Vector3 value, Quaternion rotation) - { - float x2 = rotation.X + rotation.X; - float y2 = rotation.Y + rotation.Y; - float z2 = rotation.Z + rotation.Z; - - float wx2 = rotation.W * x2; - float wy2 = rotation.W * y2; - float wz2 = rotation.W * z2; - float xx2 = rotation.X * x2; - float xy2 = rotation.X * y2; - float xz2 = rotation.X * z2; - float yy2 = rotation.Y * y2; - float yz2 = rotation.Y * z2; - float zz2 = rotation.Z * z2; - - return Create( - value.X * (1.0f - yy2 - zz2) + value.Y * (xy2 - wz2) + value.Z * (xz2 + wy2), - value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2) + value.Z * (yz2 - wx2), - value.X * (xz2 - wy2) + value.Y * (yz2 + wx2) + value.Z * (1.0f - xx2 - yy2) - ); - } + public static Vector3 Transform(Vector3 value, Quaternion rotation) => Vector4.Transform(value, rotation).AsVector3(); /// Transforms a vector normal by the given 4x4 matrix. /// The source vector. @@ -475,10 +477,10 @@ internal static Vector3 TransformNormal(Vector3 normal, in Matrix4x4.Impl matrix { Vector4 result = matrix.X * normal.X; - result += matrix.Y * normal.Y; - result += matrix.Z * normal.Z; + result = Vector4.MultiplyAddEstimate(matrix.Y, Vector4.Create(normal.Y), result); + result = Vector4.MultiplyAddEstimate(matrix.Z, Vector4.Create(normal.Z), result); - return result.AsVector128().AsVector3(); + return result.AsVector3(); } /// Copies the elements of the vector to a specified array. diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector4.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector4.cs index 65ae809ba6bc3..2cbc385316ea4 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector4.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector4.cs @@ -3,9 +3,7 @@ using System.Diagnostics.CodeAnalysis; using System.Globalization; -using System.Reflection; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; using System.Runtime.Intrinsics; namespace System.Numerics @@ -137,6 +135,7 @@ public float this[int index] readonly get => this.AsVector128().GetElement(index); [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] set { this = this.AsVector128().WithElement(index, value).AsVector4(); @@ -167,6 +166,7 @@ public float this[int index] /// The result of the division. /// The method defines the division operation for objects. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector4 operator /(Vector4 value1, float value2) => (value1.AsVector128() / value2).AsVector4(); /// Returns a value that indicates whether each pair of elements in two specified vectors is equal. @@ -200,6 +200,7 @@ public float this[int index] /// The scaled vector. /// The method defines the multiplication operation for objects. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector4 operator *(Vector4 left, float right) => (left.AsVector128() * right).AsVector4(); /// Multiplies the scalar value by the specified vector. @@ -265,6 +266,7 @@ public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max) /// The W component. /// A new from the specified object and a Z and a W component. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector4 Create(Vector2 vector, float z, float w) { return vector.AsVector128Unsafe() @@ -278,6 +280,7 @@ public static Vector4 Create(Vector2 vector, float z, float w) /// The W component. /// A new from the specified object and a W component. [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector4 Create(Vector3 vector, float w) { return vector.AsVector128Unsafe() @@ -362,7 +365,7 @@ public static Vector4 Create(Vector3 vector, float w) /// ]]> [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount) => (value1 * (1.0f - amount)) + (value2 * amount); + public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount) => MultiplyAddEstimate(value1, Create(1.0f - amount), value2 * amount); /// Returns a vector whose elements are the maximum of each of the pairs of elements in two specified vectors. /// The first vector. @@ -441,12 +444,12 @@ public static Vector4 Create(Vector3 vector, float w) [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static Vector4 Transform(Vector2 position, in Matrix4x4.Impl matrix) { - Vector4 result = matrix.X * position.X; + // This implementation is based on the DirectX Math Library XMVector2Transform method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathVector.inl - result += matrix.Y * position.Y; - result += matrix.W; - - return result; + Vector4 result = matrix.X * position.X; + result = MultiplyAddEstimate(matrix.Y, Create(position.Y), result); + return result + matrix.W; } /// Transforms a two-dimensional vector by the specified Quaternion rotation value. @@ -454,29 +457,7 @@ internal static Vector4 Transform(Vector2 position, in Matrix4x4.Impl matrix) /// The rotation to apply. /// The transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Transform(Vector2 value, Quaternion rotation) - { - float x2 = rotation.X + rotation.X; - float y2 = rotation.Y + rotation.Y; - float z2 = rotation.Z + rotation.Z; - - float wx2 = rotation.W * x2; - float wy2 = rotation.W * y2; - float wz2 = rotation.W * z2; - float xx2 = rotation.X * x2; - float xy2 = rotation.X * y2; - float xz2 = rotation.X * z2; - float yy2 = rotation.Y * y2; - float yz2 = rotation.Y * z2; - float zz2 = rotation.Z * z2; - - return Create( - value.X * (1.0f - yy2 - zz2) + value.Y * (xy2 - wz2), - value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2), - value.X * (xz2 - wy2) + value.Y * (yz2 + wx2), - 1.0f - ); - } + public static Vector4 Transform(Vector2 value, Quaternion rotation) => Transform(Create(value, 0.0f, 1.0f), rotation); /// Transforms a three-dimensional vector by a specified 4x4 matrix. /// The vector to transform. @@ -487,13 +468,13 @@ public static Vector4 Transform(Vector2 value, Quaternion rotation) [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static Vector4 Transform(Vector3 position, in Matrix4x4.Impl matrix) { - Vector4 result = matrix.X * position.X; - - result += matrix.Y * position.Y; - result += matrix.Z * position.Z; - result += matrix.W; + // This implementation is based on the DirectX Math Library XMVector3Transform method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathVector.inl - return result; + Vector4 result = matrix.X * position.X; + result = MultiplyAddEstimate(matrix.Y, Create(position.Y), result); + result = MultiplyAddEstimate(matrix.Z, Create(position.Z), result); + return result + matrix.W; } /// Transforms a three-dimensional vector by the specified Quaternion rotation value. @@ -501,29 +482,7 @@ internal static Vector4 Transform(Vector3 position, in Matrix4x4.Impl matrix) /// The rotation to apply. /// The transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Transform(Vector3 value, Quaternion rotation) - { - float x2 = rotation.X + rotation.X; - float y2 = rotation.Y + rotation.Y; - float z2 = rotation.Z + rotation.Z; - - float wx2 = rotation.W * x2; - float wy2 = rotation.W * y2; - float wz2 = rotation.W * z2; - float xx2 = rotation.X * x2; - float xy2 = rotation.X * y2; - float xz2 = rotation.X * z2; - float yy2 = rotation.Y * y2; - float yz2 = rotation.Y * z2; - float zz2 = rotation.Z * z2; - - return Create( - value.X * (1.0f - yy2 - zz2) + value.Y * (xy2 - wz2) + value.Z * (xz2 + wy2), - value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2) + value.Z * (yz2 - wx2), - value.X * (xz2 - wy2) + value.Y * (yz2 + wx2) + value.Z * (1.0f - xx2 - yy2), - 1.0f - ); - } + public static Vector4 Transform(Vector3 value, Quaternion rotation) => Transform(Create(value, 1.0f), rotation); /// Transforms a four-dimensional vector by a specified 4x4 matrix. /// The vector to transform. @@ -534,12 +493,13 @@ public static Vector4 Transform(Vector3 value, Quaternion rotation) [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static Vector4 Transform(Vector4 vector, in Matrix4x4.Impl matrix) { - Vector4 result = matrix.X * vector.X; - - result += matrix.Y * vector.Y; - result += matrix.Z * vector.Z; - result += matrix.W * vector.W; + // This implementation is based on the DirectX Math Library XMVector4Transform method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathVector.inl + Vector4 result = matrix.X * vector.X; + result = MultiplyAddEstimate(matrix.Y, Create(vector.Y), result); + result = MultiplyAddEstimate(matrix.Z, Create(vector.Z), result); + result = MultiplyAddEstimate(matrix.W, Create(vector.W), result); return result; } @@ -550,25 +510,12 @@ internal static Vector4 Transform(Vector4 vector, in Matrix4x4.Impl matrix) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector4 Transform(Vector4 value, Quaternion rotation) { - float x2 = rotation.X + rotation.X; - float y2 = rotation.Y + rotation.Y; - float z2 = rotation.Z + rotation.Z; - - float wx2 = rotation.W * x2; - float wy2 = rotation.W * y2; - float wz2 = rotation.W * z2; - float xx2 = rotation.X * x2; - float xy2 = rotation.X * y2; - float xz2 = rotation.X * z2; - float yy2 = rotation.Y * y2; - float yz2 = rotation.Y * z2; - float zz2 = rotation.Z * z2; - - return Create( - value.X * (1.0f - yy2 - zz2) + value.Y * (xy2 - wz2) + value.Z * (xz2 + wy2), - value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2) + value.Z * (yz2 - wx2), - value.X * (xz2 - wy2) + value.Y * (yz2 + wx2) + value.Z * (1.0f - xx2 - yy2), - value.W); + // This implementation is based on the DirectX Math Library XMVector3Rotate method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathVector.inl + + Quaternion conjuagate = Quaternion.Conjugate(rotation); + Quaternion temp = Quaternion.Concatenate(conjuagate, value.AsQuaternion()); + return Quaternion.Concatenate(temp, rotation).AsVector4(); } /// Copies the elements of the vector to a specified array.