Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add starting point for SOA 4-wide quaternion slerp and nlerp. #40

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Add starting point for SOA 4-wide quaternion slerp and nlerp. #40

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
115 changes: 115 additions & 0 deletions src/Unity.Mathematics.Tests/Tests/Shared/TestQuat4.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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);
}
}
}
1 change: 1 addition & 0 deletions src/Unity.Mathematics.Tests/Unity.Mathematics.Tests.csproj
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,7 @@
<Compile Include="Tests\Shared\TestInt4x4.gen.cs" />
<Compile Include="Tests\Shared\TestMath.cs" />
<Compile Include="Tests\Shared\TestMatrix.cs" />
<Compile Include="Tests\Shared\TestQuat4.cs" />
<Compile Include="Tests\Shared\TestQuaternion.cs" />
<Compile Include="Tests\Shared\TestRigidTransform.cs" />
<Compile Include="Tests\TestRandom.cs" />
Expand Down
1 change: 1 addition & 0 deletions src/Unity.Mathematics/Unity.Mathematics.csproj
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -128,6 +128,7 @@
<Compile Include="Noise\psrdnoise2D.cs" />
<Compile Include="Properties\AssemblyInfo.cs" />
<Compile Include="quaternion.cs" />
<Compile Include="quaternion4.cs" />
<Compile Include="rigid_transform.cs" />
<Compile Include="random.cs" />
<Compile Include="uint2.gen.cs" />
Expand Down
88 changes: 88 additions & 0 deletions src/Unity.Mathematics/quaternion4.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,88 @@
using Unity.Mathematics;

namespace Unity.Mathematics.SOA
{
/// <summary>
/// SIMD quaternion helper functions for working with 4-wide SOA quaternion data.
/// </summary>
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);
}
}
}