public static void Concatenate(ref QuaternionWide oa, ref QuaternionWide ob, out QuaternionWide result)
{
var tempA = oa;
var tempB = ob;
ConcatenateWithoutOverlap(ref tempA, ref tempB, out result);
}
public static void ConcatenateWithoutOverlap(ref QuaternionWide a, ref QuaternionWide b, out QuaternionWide result)
{
result.X = a.W * b.X + a.X * b.W + a.Z * b.Y - a.Y * b.Z;
result.Y = a.W * b.Y + a.Y * b.W + a.X * b.Z - a.Z * b.X;
result.Z = a.W * b.Z + a.Z * b.W + a.Y * b.X - a.X * b.Y;
result.W = a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z;
}
public static void Conjugate(ref QuaternionWide quaternion, out QuaternionWide result)
{
result.X = -quaternion.X;
result.Y = -quaternion.Y;
result.Z = -quaternion.Z;
result.W = quaternion.W;
}
public static void Negate(ref QuaternionWide q, out QuaternionWide negated)
{
negated.X = -q.X;
negated.Y = -q.Y;
negated.Z = -q.Z;
negated.W = -q.W;
}
public static void Add(ref QuaternionWide a, ref QuaternionWide b, out QuaternionWide result)
{
result.X = a.X + b.X;
result.Y = a.Y + b.Y;
result.Z = a.Z + b.Z;
result.W = a.W + b.W;
}
public static void Scale(ref QuaternionWide q, ref Vector <float> scale, out QuaternionWide result)
{
result.X = q.X * scale;
result.Y = q.Y * scale;
result.Z = q.Z * scale;
result.W = q.W * scale;
}
public static void CreateFromQuaternion(ref QuaternionWide quaternion, out Matrix3x3Wide result)
{
var qX2 = quaternion.X + quaternion.X;
var qY2 = quaternion.Y + quaternion.Y;
var qZ2 = quaternion.Z + quaternion.Z;
var YY = qY2 * quaternion.Y;
var ZZ = qZ2 * quaternion.Z;
result.X.X = Vector <float> .One - YY - ZZ;
var XY = qX2 * quaternion.Y;
var ZW = qZ2 * quaternion.W;
result.X.Y = XY + ZW;
var XZ = qX2 * quaternion.Z;
var YW = qY2 * quaternion.W;
result.X.Z = XZ - YW;
var XX = qX2 * quaternion.X;
result.Y.X = XY - ZW;
result.Y.Y = Vector <float> .One - XX - ZZ;
var XW = qX2 * quaternion.W;
var YZ = qY2 * quaternion.Z;
result.Y.Z = YZ + XW;
result.Z.X = XZ + YW;
result.Z.Y = YZ - XW;
result.Z.Z = Vector <float> .One - XX - YY;
}
public static void Normalize(ref QuaternionWide q, out QuaternionWide normalized)
{
var inverseNorm = Vector <float> .One / Vector.SquareRoot(q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W);
normalized.X = q.X * inverseNorm;
normalized.Y = q.Y * inverseNorm;
normalized.Z = q.Z * inverseNorm;
normalized.W = q.W * inverseNorm;
}
public static void TransformUnitX(ref QuaternionWide rotation, out Vector3Wide result)
{
var y2 = rotation.Y + rotation.Y;
var z2 = rotation.Z + rotation.Z;
var xy2 = rotation.X * y2;
var xz2 = rotation.X * z2;
var yy2 = rotation.Y * y2;
var zz2 = rotation.Z * z2;
var wy2 = rotation.W * y2;
var wz2 = rotation.W * z2;
result.X = Vector <float> .One - yy2 - zz2;
result.Y = xy2 + wz2;
result.Z = xz2 - wy2;
}
/// <summary>
/// Constructs a quaternion from a rotation matrix.
/// </summary>
/// <param name="r">Rotation matrix to create the quaternion from.</param>
/// <param name="q">Quaternion based on the rotation matrix.</param>
public static void CreateFromRotationMatrix(ref Matrix3x3Wide r, out QuaternionWide q)
{
//Since we can't branch, we're going to end up calculating the possible states of all branches.
//This requires doing more ALU work than the branching implementation, but there are a lot of common terms across the branches, and (random-ish) branches aren't free.
//Overall, this turns out to be about 2x-2.5x more expensive per call than the scalar version, but it handles multiple lanes, so it's a net win.
var oneAddX = Vector <float> .One + r.X.X;
var oneSubX = Vector <float> .One - r.X.X;
var yAddZ = r.Y.Y + r.Z.Z;
var ySubZ = r.Y.Y - r.Z.Z;
var tX = oneAddX - yAddZ;
var tY = oneSubX + ySubZ;
var tZ = oneSubX - ySubZ;
var tW = oneAddX + yAddZ;
//There are two layers of conditions- inner, and outer. We have to first select each of the two inner halves- upper, and lower-
//and then we will select which of the two inners to use for the outer.
var useUpper = Vector.LessThan(r.Z.Z, Vector <float> .Zero);
var useUpperUpper = Vector.GreaterThan(r.X.X, r.Y.Y);
var useLowerUpper = Vector.LessThan(r.X.X, -r.Y.Y);
var t = Vector.ConditionalSelect(useUpper,
Vector.ConditionalSelect(useUpperUpper, tX, tY),
Vector.ConditionalSelect(useLowerUpper, tZ, tW));
var xyAddYx = r.X.Y + r.Y.X;
var yzSubZy = r.Y.Z - r.Z.Y;
var zxAddXz = r.Z.X + r.X.Z;
q.X = Vector.ConditionalSelect(useUpper,
Vector.ConditionalSelect(useUpperUpper, tX, xyAddYx),
Vector.ConditionalSelect(useLowerUpper, zxAddXz, yzSubZy));
var yzAddZy = r.Y.Z + r.Z.Y;
var zxSubXz = r.Z.X - r.X.Z;
q.Y = Vector.ConditionalSelect(useUpper,
Vector.ConditionalSelect(useUpperUpper, xyAddYx, tY),
Vector.ConditionalSelect(useLowerUpper, yzAddZy, zxSubXz));
var xySubYx = r.X.Y - r.Y.X;
q.Z = Vector.ConditionalSelect(useUpper,
Vector.ConditionalSelect(useUpperUpper, zxAddXz, yzAddZy),
Vector.ConditionalSelect(useLowerUpper, tZ, xySubYx));
q.W = Vector.ConditionalSelect(useUpper,
Vector.ConditionalSelect(useUpperUpper, yzSubZy, zxSubXz),
Vector.ConditionalSelect(useLowerUpper, xySubYx, tW));
var scale = new Vector <float>(0.5f) / Vector.SquareRoot(t);
Scale(ref q, ref scale, out q);
}
public static void TransformUnitY(ref QuaternionWide rotation, out Vector3Wide result)
{
var x2 = rotation.X + rotation.X;
var y2 = rotation.Y + rotation.Y;
var z2 = rotation.Z + rotation.Z;
var xx2 = rotation.X * x2;
var xy2 = rotation.X * y2;
var yz2 = rotation.Y * z2;
var zz2 = rotation.Z * z2;
var wx2 = rotation.W * x2;
var wz2 = rotation.W * z2;
result.X = xy2 - wz2;
result.Y = Vector <float> .One - xx2 - zz2;
result.Z = yz2 + wx2;
}
public static void TransformUnitZ(ref QuaternionWide rotation, out Vector3Wide result)
{
var x2 = rotation.X + rotation.X;
var y2 = rotation.Y + rotation.Y;
var z2 = rotation.Z + rotation.Z;
var xx2 = rotation.X * x2;
var xz2 = rotation.X * z2;
var yy2 = rotation.Y * y2;
var yz2 = rotation.Y * z2;
var wx2 = rotation.W * x2;
var wy2 = rotation.W * y2;
result.X = xz2 + wy2;
result.Y = yz2 - wx2;
result.Z = Vector <float> .One - xx2 - yy2;
}
public static void TransformUnitXY(ref QuaternionWide rotation, out Vector3Wide x, out Vector3Wide y)
{
var x2 = rotation.X + rotation.X;
var y2 = rotation.Y + rotation.Y;
var z2 = rotation.Z + rotation.Z;
var xx2 = rotation.X * x2;
var xy2 = rotation.X * y2;
var xz2 = rotation.X * z2;
var yy2 = rotation.Y * y2;
var yz2 = rotation.Y * z2;
var zz2 = rotation.Z * z2;
var wx2 = rotation.W * x2;
var wy2 = rotation.W * y2;
var wz2 = rotation.W * z2;
x.X = Vector <float> .One - yy2 - zz2;
x.Y = xy2 + wz2;
x.Z = xz2 - wy2;
y.X = xy2 - wz2;
y.Y = Vector <float> .One - xx2 - zz2;
y.Z = yz2 + wx2;
}
public static void TransformWithoutOverlap(ref Vector3Wide v, ref QuaternionWide rotation, out Vector3Wide result)
{
//This operation is an optimized-down version of v' = q * v * q^-1.
//The expanded form would be to treat v as an 'axis only' quaternion
//and perform standard quaternion multiplication. Assuming q is normalized,
//q^-1 can be replaced by a conjugation.
var x2 = rotation.X + rotation.X;
var y2 = rotation.Y + rotation.Y;
var z2 = rotation.Z + rotation.Z;
var xx2 = rotation.X * x2;
var xy2 = rotation.X * y2;
var xz2 = rotation.X * z2;
var yy2 = rotation.Y * y2;
var yz2 = rotation.Y * z2;
var zz2 = rotation.Z * z2;
var wx2 = rotation.W * x2;
var wy2 = rotation.W * y2;
var wz2 = rotation.W * z2;
result.X = v.X * (Vector <float> .One - yy2 - zz2) + v.Y * (xy2 - wz2) + v.Z * (xz2 + wy2);
result.Y = v.X * (xy2 + wz2) + v.Y * (Vector <float> .One - xx2 - zz2) + v.Z * (yz2 - wx2);
result.Z = v.X * (xz2 - wy2) + v.Y * (yz2 + wx2) + v.Z * (Vector <float> .One - xx2 - yy2);
}
public static void GetLength(ref QuaternionWide q, out Vector <float> length)
{
length = Vector.SquareRoot(q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W);
}
public static void GetLengthSquared(ref QuaternionWide q, out Vector <float> lengthSquared)
{
lengthSquared = q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W;
}
/// <summary>
/// Computes the quaternion rotation between two normalized vectors.
/// </summary>
/// <param name="v1">First unit-length vector.</param>
/// <param name="v2">Second unit-length vector.</param>
/// <param name="q">Quaternion representing the rotation from v1 to v2.</param>
public static void GetQuaternionBetweenNormalizedVectors(ref Vector3Wide v1, ref Vector3Wide v2, out QuaternionWide q)
{
Vector3Wide.Dot(ref v1, ref v2, out var dot);
//For non-normal vectors, the multiplying the axes length squared would be necessary:
//float w = dot + Sqrt(v1.LengthSquared() * v2.LengthSquared());
//There exists an ambiguity at dot == -1. If the directions point away from each other, there are an infinite number of shortest paths.
//One must be chosen arbitrarily. Here, we choose one by projecting onto the plane whose normal is associated with the smallest magnitude.
//Since this is a SIMD operation, the special case is always executed and its result is conditionally selected.
Vector3Wide.CrossWithoutOverlap(ref v1, ref v2, out var cross);
var useNormalCase = Vector.GreaterThan(dot, new Vector <float>(-0.999999f));
var absX = Vector.Abs(v1.X);
var absY = Vector.Abs(v1.Y);
var absZ = Vector.Abs(v1.Z);
var xIsSmallest = Vector.BitwiseAnd(Vector.LessThan(absX, absY), Vector.LessThan(absX, absZ));
var yIsSmaller = Vector.LessThan(absY, absZ);
q.X = Vector.ConditionalSelect(useNormalCase, cross.X, Vector.ConditionalSelect(xIsSmallest, Vector <float> .Zero, Vector.ConditionalSelect(yIsSmaller, -v1.Z, -v1.Y)));
q.Y = Vector.ConditionalSelect(useNormalCase, cross.Y, Vector.ConditionalSelect(xIsSmallest, -v1.Z, Vector.ConditionalSelect(yIsSmaller, Vector <float> .Zero, v1.X)));
q.Z = Vector.ConditionalSelect(useNormalCase, cross.Z, Vector.ConditionalSelect(xIsSmallest, v1.Y, Vector.ConditionalSelect(yIsSmaller, v1.X, Vector <float> .Zero)));
q.W = Vector.ConditionalSelect(useNormalCase, dot + Vector <float> .One, Vector <float> .Zero);
Normalize(ref q, out q);
}
public static void Transform(ref Vector3Wide v, ref QuaternionWide rotation, out Vector3Wide result)
{
var tempV = v;
TransformWithoutOverlap(ref tempV, ref rotation, out result);
}