Skip to content

Commit

Permalink
Merge remote-tracking branch 'martenf/vectorsimd' into numerics-cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
@tannergooding
tannergooding committed Jun 15, 2024
2 parents 94bb125 + 00199d2 commit ba252ac
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 94 deletions.
29 changes: 4 additions & 25 deletions src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.Impl.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -166,31 +166,10 @@ public Vector3 Translation
public static Impl operator *(in Impl left, in Impl right)
{
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;
}

Expand Down
67 changes: 3 additions & 64 deletions src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,40 +103,7 @@ public float this[int index]
/// <remarks>The <see cref="op_Division" /> method defines the division operation for <see cref="Quaternion" /> objects.</remarks>
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;
return value1 * Inverse(value2);
}

/// <summary>Returns a value that indicates whether two quaternions are equal.</summary>
Expand Down Expand Up @@ -169,6 +136,7 @@ public float this[int index]
Vector128<float> right = value2.AsVector128();

Vector128<float> 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);
Expand Down Expand Up @@ -241,36 +209,7 @@ public float this[int index]
/// <param name="value1">The first quaternion rotation in the series.</param>
/// <param name="value2">The second quaternion rotation in the series.</param>
/// <returns>A new quaternion representing the concatenation of the <paramref name="value1" /> rotation followed by the <paramref name="value2" /> rotation.</returns>
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;

/// <summary>Returns the conjugate of a specified quaternion.</summary>
/// <param name="value">The quaternion.</param>
Expand Down
39 changes: 34 additions & 5 deletions src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -296,11 +296,23 @@ public static Vector3 Create(ReadOnlySpan<float> 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)
);
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)
);
}
}

/// <summary>Computes the Euclidean distance between the two given points.</summary>
Expand Down Expand Up @@ -442,6 +454,23 @@ public static Vector3 Cross(Vector3 vector1, Vector3 vector2)
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static Vector3 Transform(Vector3 value, Quaternion rotation)
{
if (Vector128.IsHardwareAccelerated)
{
var vVector = value.AsVector128();
var rVector = Unsafe.BitCast<Quaternion, Vector128<float>>(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<float> Cross(Vector128<float> v1, Vector128<float> 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;
Expand Down

0 comments on commit ba252ac

Please sign in to comment.