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 7d270c17cc0de7..fd98190e2125d4 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 fd741e019c4d3b..f847206f9120f9 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; } } @@ -166,10 +163,12 @@ public Vector3 Translation public static Impl operator *(in Impl left, in Impl right) { Impl result; + 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; } @@ -216,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)); @@ -245,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; @@ -411,32 +408,12 @@ public static Impl CreateLookTo(in Vector3 cameraPosition, in Vector3 cameraDire 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)] @@ -449,32 +426,12 @@ 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)] @@ -696,17 +653,17 @@ 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; + Vector4 normal = Plane.Normalize(value).AsVector4(); + normal *= normal * -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.UnitX + Vector4.Create(normal.X); + result.Y = Vector4.UnitY + Vector4.Create(normal.Y); + result.Z = Vector4.UnitZ + Vector4.Create(normal.Z); + result.W = Vector4.UnitW; - return result; + return Transpose(result); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -734,8 +691,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 ] @@ -777,8 +734,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 ] @@ -820,8 +777,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 ] @@ -919,19 +876,19 @@ 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 normal = Plane.Normalize(plane).AsVector4(); + float dot = Vector4.Dot(normal, lightDirection.AsVector4()); - Vector3 normal = -p.Normal; + normal *= -lightDirection.AsVector4(); 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.Create(dot, 0, 0, 0) + normal; + result.Y = Vector4.Create(0, dot, 0, 0); + result.Z = Vector4.Create(0, 0, dot, 0); + result.W = Vector4.Create(0, 0, 0, dot); - return result; + return Transpose(result); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1015,190 +972,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; - Impl matTemp = Identity; - CanonicalBasis canonicalBasis = default; - Vector3* pCanonicalBasis = &canonicalBasis.Row0; + Vector3* canonicalBasis = stackalloc Vector3[3] { + Vector3.UnitX, + Vector3.UnitY, + Vector3.UnitZ, + }; - canonicalBasis.Row0 = Vector3.UnitX; - canonicalBasis.Row1 = Vector3.UnitY; - canonicalBasis.Row2 = Vector3.UnitZ; + translation = matrix.W.AsVector3(); - translation = matrix.W.AsVector3(); + Vector3** vectorBasis = stackalloc Vector3*[3] { + (Vector3*)&matTemp.X, + (Vector3*)&matTemp.Y, + (Vector3*)&matTemp.Z, + }; - pVectorBasis[0] = (Vector3*)&matTemp.X; - pVectorBasis[1] = (Vector3*)&matTemp.Y; - pVectorBasis[2] = (Vector3*)&matTemp.Z; + *(vectorBasis[0]) = matrix.X.AsVector3(); + *(vectorBasis[1]) = matrix.Y.AsVector3(); + *(vectorBasis[2]) = matrix.Z.AsVector3(); - *(pVectorBasis[0]) = matrix.X.AsVector3(); - *(pVectorBasis[1]) = matrix.Y.AsVector3(); - *(pVectorBasis[2]) = matrix.Z.AsVector3(); + float* scales = stackalloc float[3] { + vectorBasis[0]->Length(), + vectorBasis[1]->Length(), + vectorBasis[2]->Length(), + }; - scale.X = pVectorBasis[0]->Length(); - scale.Y = pVectorBasis[1]->Length(); - scale.Z = pVectorBasis[2]->Length(); + uint a, b, c; - uint a, b, c; + #region Ranking + float x = scales[0]; + float y = scales[1]; + float z = scales[2]; - #region Ranking - float x = pfScales[0]; - float y = pfScales[1]; - float z = pfScales[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]); + + if (scales[c] < DecomposeEpsilon) + { + *vectorBasis[c] = Vector3.Cross(*vectorBasis[a], *vectorBasis[b]); + } - *pVectorBasis[c] = Vector3.Normalize(*pVectorBasis[c]); + *vectorBasis[c] = Vector3.Normalize(*vectorBasis[c]); - det = matTemp.GetDeterminant(); + float det = matTemp.GetDeterminant(); - // 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]); + // 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 = -det; - } + det = -det; + } - det -= 1.0f; - det *= det; + det -= 1.0f; + 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)); - } + 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; } @@ -1221,7 +1175,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 @@ -1231,53 +1185,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; @@ -1285,44 +1237,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; @@ -1341,15 +1293,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()); @@ -1369,10 +1321,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(); @@ -1382,24 +1331,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 @@ -1761,21 +1692,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); + readonly bool IEquatable.Equals(Impl other) => Equals(in other); - private struct CanonicalBasis - { - public Vector3 Row0; - public Vector3 Row1; - public Vector3 Row2; - }; - - 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 55f4522800316e..d4ce5d54d3f5d4 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,12 @@ 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; - - float bx = point3.X - point1.X; - float by = point3.Y - point1.Y; - float bz = point3.Z - point1.Z; - - // 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); + Vector3 normal = Vector3.Normalize(Vector3.Cross(point2 - point1, point3 - point1)); - 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. @@ -142,19 +103,7 @@ public static Plane CreateFromVertices(Vector3 point1, Vector3 point2, Vector3 p /// 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(); - - if (float.Abs(normalLengthSquared - 1.0f) < NormalizeEpsilon) - { - // It already normalized, so we don't need to farther process. - return value; - } - - return (value.AsVector128() / float.Sqrt(normalLengthSquared)).AsPlane(); - } + public static Plane Normalize(Plane value) => Create(Vector3.Normalize(value.Normal), 0); /// Transforms a normalized plane by a 4x4 matrix. /// The normalized plane to transform. @@ -162,19 +111,7 @@ public static Plane Normalize(Plane value) /// The transformed plane. /// 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, 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 - ); - } + public static Plane Transform(Plane plane, Matrix4x4 matrix) => Vector4.Transform(plane.AsVector4(), matrix).AsPlane(); /// Transforms a normalized plane by a Quaternion rotation. /// The normalized plane to transform. @@ -182,44 +119,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 1d1bbfa9e05304..7e4e54aa41f841 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,10 +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) - { - return value1 * Inverse(value2); - } + 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. @@ -128,48 +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; + // This implementation is based on the DirectX Math Library XMQuaternionMultiply method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl - // cross(av, bv) - float cx = q1y * q2z - q1z * q2y; - float cy = q1z * q2x - q1x * q2z; - float cz = q1x * q2y - q1y * q2x; + Vector128 left = value1.AsVector128(); + Vector128 right = value2.AsVector128(); - 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; - } + 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. @@ -239,18 +206,11 @@ public float this[int index] /// 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; - (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 Create(axis * s, c); } /// Creates a quaternion from the specified rotation matrix. @@ -363,39 +323,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; - - Quaternion r = default; + Vector128 q2 = quaternion2.AsVector128(); - float dot = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + - quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W; + q2 = Vector128.ConditionalSelect( + Vector128.GreaterThanOrEqual(Vector128.Create(Dot(quaternion1, quaternion2)), Vector128.Zero), + q2, + -q2 + ); - 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. @@ -431,17 +368,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; @@ -449,28 +384,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 7eb79bf3524a7e..cd4a2c5c0358e7 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 12f9ea29046faf..ec97d4a4f5d525 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,23 +303,19 @@ public static Vector3 Create(ReadOnlySpan values) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3 Cross(Vector3 vector1, Vector3 vector2) { - if (Vector128.IsHardwareAccelerated) - { - var v1 = vector1.AsVector128(); - var v2 = vector2.AsVector128(); - return ((Vector128.Shuffle(v1, Vector128.Create(1, 2, 0, 3)) * - Vector128.Shuffle(v2, Vector128.Create(2, 0, 1, 3))) - - (Vector128.Shuffle(v1, Vector128.Create(2, 0, 1, 3)) * - Vector128.Shuffle(v2, Vector128.Create(1, 2, 0, 3)))).AsVector3(); - } - else - { - 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. @@ -363,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. @@ -424,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. @@ -452,45 +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) - { - if (Vector128.IsHardwareAccelerated) - { - var vVector = value.AsVector128(); - var rVector = Unsafe.BitCast>(rotation); - - // v + 2 * (q x (q.W * v + q x v) - return (vVector + Vector128.Create(2f) * Cross(rVector, Vector128.Shuffle(rVector, Vector128.Create(3, 3, 3, 3)) * vVector + Cross(rVector, vVector))).AsVector3(); - - static Vector128 Cross(Vector128 v1, Vector128 v2) - { - return (Vector128.Shuffle(v1, Vector128.Create(1, 2, 0, 3)) * - Vector128.Shuffle(v2, Vector128.Create(2, 0, 1, 3))) - - (Vector128.Shuffle(v1, Vector128.Create(2, 0, 1, 3)) * - Vector128.Shuffle(v2, Vector128.Create(1, 2, 0, 3))); - } - } - - 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. @@ -504,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 65ae809ba6bc30..ad72aaa593c32f 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. @@ -442,11 +445,8 @@ public static Vector4 Create(Vector3 vector, float w) internal static Vector4 Transform(Vector2 position, in Matrix4x4.Impl matrix) { Vector4 result = matrix.X * position.X; - - result += matrix.Y * position.Y; - result += matrix.W; - - return result; + 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 +454,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. @@ -488,12 +466,9 @@ public static Vector4 Transform(Vector2 value, Quaternion rotation) 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; - - return result; + 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 +476,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. @@ -535,11 +488,9 @@ public static Vector4 Transform(Vector3 value, Quaternion rotation) 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; - + 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 +501,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.