diff --git a/src/Unity.Mathematics.Tests/Tests/Shared/TestQuat4.cs b/src/Unity.Mathematics.Tests/Tests/Shared/TestQuat4.cs new file mode 100644 index 00000000..8b71dc7f --- /dev/null +++ b/src/Unity.Mathematics.Tests/Tests/Shared/TestQuat4.cs @@ -0,0 +1,115 @@ +using Unity.Mathematics.SOA; +using NUnit.Framework; +using static Unity.Mathematics.math; +using Burst.Compiler.IL.Tests; + +namespace Unity.Mathematics.Tests +{ + [TestFixture] + public class Quaternion4Tests + { + [TestCompiler] + public unsafe void TestSlerp() + { + quaternion* a_s = stackalloc quaternion[4]; + quaternion* b_s = stackalloc quaternion[4]; + quaternion* c_s = stackalloc quaternion[4]; + + for (int i = 0; i < 4; ++i) + { + a_s[i] = quaternion.AxisAngle(new float3(1.0f, 0.0f, 0.0f), i * 0.25f); + b_s[i] = quaternion.AxisAngle(new float3(0.0f, 1.0f, 0.0f), i * 0.25f); + c_s[i] = math.slerp(a_s[i], b_s[i], 0.5f); + } + + float4 ax = new float4(a_s[0].value.x, a_s[1].value.x, a_s[2].value.x, a_s[3].value.x); + float4 ay = new float4(a_s[0].value.y, a_s[1].value.y, a_s[2].value.y, a_s[3].value.y); + float4 az = new float4(a_s[0].value.z, a_s[1].value.z, a_s[2].value.z, a_s[3].value.z); + float4 aw = new float4(a_s[0].value.w, a_s[1].value.w, a_s[2].value.w, a_s[3].value.w); + float4 bx = new float4(b_s[0].value.x, b_s[1].value.x, b_s[2].value.x, b_s[3].value.x); + float4 by = new float4(b_s[0].value.y, b_s[1].value.y, b_s[2].value.y, b_s[3].value.y); + float4 bz = new float4(b_s[0].value.z, b_s[1].value.z, b_s[2].value.z, b_s[3].value.z); + float4 bw = new float4(b_s[0].value.w, b_s[1].value.w, b_s[2].value.w, b_s[3].value.w); + + float4 cx, cy, cz, cw; + + quaternion4.slerp4( + ax, ay, az, aw, + bx, by, bz, bw, + new float4(0.5f), + out cx, out cy, out cz, out cw); + + TestUtils.AreEqual(c_s[0].value.x, cx.x, 0.0001); + TestUtils.AreEqual(c_s[1].value.x, cx.y, 0.0001); + TestUtils.AreEqual(c_s[2].value.x, cx.z, 0.0001); + TestUtils.AreEqual(c_s[3].value.x, cx.w, 0.0001); + + TestUtils.AreEqual(c_s[0].value.y, cy.x, 0.0001); + TestUtils.AreEqual(c_s[1].value.y, cy.y, 0.0001); + TestUtils.AreEqual(c_s[2].value.y, cy.z, 0.0001); + TestUtils.AreEqual(c_s[3].value.y, cy.w, 0.0001); + + TestUtils.AreEqual(c_s[0].value.z, cz.x, 0.0001); + TestUtils.AreEqual(c_s[1].value.z, cz.y, 0.0001); + TestUtils.AreEqual(c_s[2].value.z, cz.z, 0.0001); + TestUtils.AreEqual(c_s[3].value.z, cz.w, 0.0001); + + TestUtils.AreEqual(c_s[0].value.w, cw.x, 0.0001); + TestUtils.AreEqual(c_s[1].value.w, cw.y, 0.0001); + TestUtils.AreEqual(c_s[2].value.w, cw.z, 0.0001); + TestUtils.AreEqual(c_s[3].value.w, cw.w, 0.0001); + } + + [TestCompiler] + public unsafe void TestNlerp() + { + quaternion* a_s = stackalloc quaternion[4]; + quaternion* b_s = stackalloc quaternion[4]; + quaternion* c_s = stackalloc quaternion[4]; + + for (int i = 0; i < 4; ++i) + { + a_s[i] = quaternion.AxisAngle(new float3(1.0f, 0.0f, 0.0f), i * 0.25f); + b_s[i] = quaternion.AxisAngle(new float3(0.0f, 0.72f, 0.72f), i * 0.25f); + c_s[i] = math.nlerp(a_s[i], b_s[i], 0.5f); + } + + float4 ax = new float4(a_s[0].value.x, a_s[1].value.x, a_s[2].value.x, a_s[3].value.x); + float4 ay = new float4(a_s[0].value.y, a_s[1].value.y, a_s[2].value.y, a_s[3].value.y); + float4 az = new float4(a_s[0].value.z, a_s[1].value.z, a_s[2].value.z, a_s[3].value.z); + float4 aw = new float4(a_s[0].value.w, a_s[1].value.w, a_s[2].value.w, a_s[3].value.w); + float4 bx = new float4(b_s[0].value.x, b_s[1].value.x, b_s[2].value.x, b_s[3].value.x); + float4 by = new float4(b_s[0].value.y, b_s[1].value.y, b_s[2].value.y, b_s[3].value.y); + float4 bz = new float4(b_s[0].value.z, b_s[1].value.z, b_s[2].value.z, b_s[3].value.z); + float4 bw = new float4(b_s[0].value.w, b_s[1].value.w, b_s[2].value.w, b_s[3].value.w); + + float4 cx, cy, cz, cw; + + quaternion4.nlerp4( + ax, ay, az, aw, + bx, by, bz, bw, + new float4(0.5f), + out cx, out cy, out cz, out cw); + + TestUtils.AreEqual(c_s[0].value.x, cx.x, 0.0001); + TestUtils.AreEqual(c_s[1].value.x, cx.y, 0.0001); + TestUtils.AreEqual(c_s[2].value.x, cx.z, 0.0001); + TestUtils.AreEqual(c_s[3].value.x, cx.w, 0.0001); + + TestUtils.AreEqual(c_s[0].value.y, cy.x, 0.0001); + TestUtils.AreEqual(c_s[1].value.y, cy.y, 0.0001); + TestUtils.AreEqual(c_s[2].value.y, cy.z, 0.0001); + TestUtils.AreEqual(c_s[3].value.y, cy.w, 0.0001); + + TestUtils.AreEqual(c_s[0].value.z, cz.x, 0.0001); + TestUtils.AreEqual(c_s[1].value.z, cz.y, 0.0001); + TestUtils.AreEqual(c_s[2].value.z, cz.z, 0.0001); + TestUtils.AreEqual(c_s[3].value.z, cz.w, 0.0001); + + TestUtils.AreEqual(c_s[0].value.w, cw.x, 0.0001); + TestUtils.AreEqual(c_s[1].value.w, cw.y, 0.0001); + TestUtils.AreEqual(c_s[2].value.w, cw.z, 0.0001); + TestUtils.AreEqual(c_s[3].value.w, cw.w, 0.0001); + } + } +} \ No newline at end of file diff --git a/src/Unity.Mathematics.Tests/Unity.Mathematics.Tests.csproj b/src/Unity.Mathematics.Tests/Unity.Mathematics.Tests.csproj index 05f9a782..9fa246e0 100644 --- a/src/Unity.Mathematics.Tests/Unity.Mathematics.Tests.csproj +++ b/src/Unity.Mathematics.Tests/Unity.Mathematics.Tests.csproj @@ -100,6 +100,7 @@ + diff --git a/src/Unity.Mathematics/Unity.Mathematics.csproj b/src/Unity.Mathematics/Unity.Mathematics.csproj index 06cee930..c23685cf 100644 --- a/src/Unity.Mathematics/Unity.Mathematics.csproj +++ b/src/Unity.Mathematics/Unity.Mathematics.csproj @@ -128,6 +128,7 @@ + diff --git a/src/Unity.Mathematics/quaternion4.cs b/src/Unity.Mathematics/quaternion4.cs new file mode 100644 index 00000000..33228620 --- /dev/null +++ b/src/Unity.Mathematics/quaternion4.cs @@ -0,0 +1,88 @@ +using Unity.Mathematics; + +namespace Unity.Mathematics.SOA +{ + /// + /// SIMD quaternion helper functions for working with 4-wide SOA quaternion data. + /// + public static class quaternion4 + { + private static void flip_q2_sign(ref float4 dt, ref float4 qx, ref float4 qy, ref float4 qz, ref float4 qw) + { + uint4 sign_bits = math.asuint(dt) & 0x80000000; + + dt = math.asfloat(math.asuint(dt) ^ sign_bits); + qx = math.asfloat(math.asuint(qx) ^ sign_bits); + qy = math.asfloat(math.asuint(qy) ^ sign_bits); + qz = math.asfloat(math.asuint(qz) ^ sign_bits); + qw = math.asfloat(math.asuint(qw) ^ sign_bits); + } + + public static void nlerp4( + float4 q1x, float4 q1y, float4 q1z, float4 q1w, + float4 q2x, float4 q2y, float4 q2z, float4 q2w, + float4 t, + out float4 oqx, out float4 oqy, out float4 oqz, out float4 oqw) + { + float4 dt = q1x * q2x + q1y * q2y + q1z * q2z + q1w * q2w; + + flip_q2_sign(ref dt, ref q2x, ref q2y, ref q2z, ref q2w); + + float4 x = math.lerp(q1x, q2x, t); + float4 y = math.lerp(q1y, q2y, t); + float4 z = math.lerp(q1z, q2z, t); + float4 w = math.lerp(q1w, q2w, t); + + float4 lensq = x * x + y * y + z * z + w * w; + float4 recip = math.rsqrt(lensq); + + oqx = x * recip; + oqy = y * recip; + oqz = z * recip; + oqw = w * recip; + } + + public static void slerp4( + float4 q1x, float4 q1y, float4 q1z, float4 q1w, + float4 q2x, float4 q2y, float4 q2z, float4 q2w, float4 t, + out float4 oqx, out float4 oqy, out float4 oqz, out float4 oqw) + { + float4 dt = q1x * q2x + q1y * q2y + q1z * q2z + q1w * q2w; + + flip_q2_sign(ref dt, ref q2x, ref q2y, ref q2z, ref q2w); + + bool4 acos_mask = dt < 0.9995f; + + float4 a_qx = default(float4); + float4 a_qy = default(float4); + float4 a_qz = default(float4); + float4 a_qw = default(float4); + + if (math.any(acos_mask)) + { + float4 angles = math.acos(dt); + float4 s = math.rsqrt(1.0f - dt * dt); // 1.0f / sin(angle) + float4 w1 = math.sin(angles * (1.0f - t)) * s; + float4 w2 = math.sin(angles * t) * s; + + a_qx = q1x * w1 + q2x * w2; + a_qy = q1y * w1 + q2y * w2; + a_qz = q1z * w1 + q2z * w2; + a_qw = q1w * w1 + q2w * w2; + } + + // if the angle is small, use linear interpolation + float4 b_qx, b_qy, b_qz, b_qw; + nlerp4( + q1x, q1y, q1z, q1w, + q2x, q2y, q2z, q2w, + t, + out b_qx, out b_qy, out b_qz, out b_qw); + + oqx = math.select(b_qx, a_qx, acos_mask); + oqy = math.select(b_qy, a_qy, acos_mask); + oqz = math.select(b_qz, a_qz, acos_mask); + oqw = math.select(b_qw, a_qw, acos_mask); + } + } +} \ No newline at end of file