Skip to content

Commit 8fd17c4

Browse files
unpacklounpacklo
authored
Copy SVD implementation from Unity.Animation (Unity-Technologies#210)
1 parent 2f043ee commit 8fd17c4

File tree

15 files changed

+726
-1
lines changed

15 files changed

+726
-1
lines changed

‎src/CHANGELOG.md‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,9 @@
88
* Added `math.chgsign` for float, float2, float3, and float4.
99
* Added `math.Euler` to convert a quaternion to Euler angles.
1010
* Added `math.angle` to compute the angle between two unit quaternions.
11+
* Added `math.rotation` to extract a quaternion rotation from a float3x3 (that may have scale).
12+
* Added `math.mulScale` to scale columns of a float3x3 with scaling coefficients in a float3.
13+
* Added `math.scaleMul` to scale rows of a float3x3 with scaling coefficients in a float3.
1114
### Changed
1215
* `asfloat(uint)`, `asuint(float)`, `asint(float)` and other related methods are now faster in mono without Burst. Other methods which use these will see a performance improvement.
1316
* Modified `quaternion.nlerp` to be branchless.

‎src/Tests/Tests/Shared/TestMath.cs‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4078,5 +4078,49 @@ static quaternion TestRefEulerToQuat(float3 angle, math.RotationOrder order)
40784078
return quaternion(float4(1f));
40794079
}
40804080
}
4081+
4082+
[TestCompiler]
4083+
public static void mulScale()
4084+
{
4085+
var tolerance = 1e-5f;
4086+
4087+
// Random matrix.
4088+
var m = new float3x3(
4089+
0.891724169254302978516f, 0.156217902898788452148f, 0.492261469364166259766f,
4090+
0.562758803367614746094f, 0.00122839550022035837173f, 0.437942296266555786133f,
4091+
0.2576503753662109375f, 0.200372591614723205566f, 0.515525519847869873047f);
4092+
4093+
// Random scale.
4094+
var scale = new float3(0.235540181398391723633f, 0.215966641902923583984f, 0.533130943775177001953f);
4095+
var actual = math.mulScale(m, scale);
4096+
var expected = new float3x3(
4097+
0.210036873817443847656f, 0.0337378568947315216064f, 0.262439817190170288086f,
4098+
0.132552310824394226074f, 0.000265292444964870810509f, 0.233480587601661682129f,
4099+
0.0606870166957378387451f, 0.043273795396089553833f, 0.274842619895935058594f);
4100+
4101+
TestUtils.AreEqual(expected, actual, 1e-5);
4102+
}
4103+
4104+
[TestCompiler]
4105+
public static void scaleMul()
4106+
{
4107+
var tolerance = 1e-5f;
4108+
4109+
// Random matrix, same as in mulScale test.
4110+
var m = new float3x3(
4111+
0.891724169254302978516f, 0.156217902898788452148f, 0.492261469364166259766f,
4112+
0.562758803367614746094f, 0.00122839550022035837173f, 0.437942296266555786133f,
4113+
0.2576503753662109375f, 0.200372591614723205566f, 0.515525519847869873047f);
4114+
4115+
// Random scale, same as in mulScale test.
4116+
var scale = new float3(0.235540181398391723633f, 0.215966641902923583984f, 0.533130943775177001953f);
4117+
var actual = math.scaleMul(scale, m);
4118+
var expected = new float3x3(
4119+
0.210036873817443847656f, 0.0367955937981605529785f, 0.115947358310222625732f,
4120+
0.121537126600742340088f, 0.000265292444964870810509f, 0.0945809260010719299316f,
4121+
0.137361392378807067871f, 0.106824830174446105957f, 0.274842619895935058594f);
4122+
4123+
TestUtils.AreEqual(expected, actual, tolerance);
4124+
}
40814125
}
40824126
}

‎src/Tests/Tests/Shared/TestMatrix.cs‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1169,5 +1169,46 @@ public static void float3x3_from_float4x4_without_new()
11691169

11701170
TestUtils.AreEqual(expected, float3x3(f4x4));
11711171
}
1172+
1173+
[TestCompiler]
1174+
public static void float3x3_pseudoinverse_non_singular()
1175+
{
1176+
const float kTolerance = 1e-5f;
1177+
1178+
// A random 3x3 matrix, non-singular, generated from octave.
1179+
var a = new float3x3(
1180+
0.806710600852966308594f, 0.985506594181060791016f, 0.593669593334197998047f,
1181+
0.0869068726897239685059f, 0.754145503044128417969f, 0.222692146897315979004f,
1182+
0.917483031749725341797f, 0.443894535303115844727f, 0.138771042227745056152f);
1183+
1184+
// The regular inverse of a, as computed by octave.
1185+
var expected = new float3x3(
1186+
-0.0299495235085487365723f, -0.654392063617706298828f, 1.17825806140899658203f,
1187+
-0.992458224296569824219f, 2.23384380340576171875f, 0.661037027835845947266f,
1188+
3.37264156341552734375f, -2.81901407241821289063f, -2.69841933250427246094f);
1189+
1190+
TestUtils.AreEqual(expected, pseudoinverse(a), kTolerance);
1191+
TestUtils.AreEqual(expected, inverse(a), kTolerance);
1192+
}
1193+
1194+
[TestCompiler]
1195+
public static void float3x3_pseudoinverse_singular()
1196+
{
1197+
const float kTolerance = 1e-5f;
1198+
1199+
// A singular matrix.
1200+
var a = new float3x3(
1201+
0.0548239313066005706787f, 0.462397903203964233398f, 0.0501357726752758026123f,
1202+
0.0548239313066005706787f, 0.462397903203964233398f, 0.0501357726752758026123f,
1203+
0.938165545463562011719f, 0.542225778102874755859f, 0.10684439539909362793f);
1204+
1205+
// The pseudoinverse computed by octave.
1206+
var expected = new float3x3(
1207+
-0.675357639789581298828f, -0.675357699394226074219f, 1.14166188240051269531f,
1208+
1.15268361568450927734f, 1.15268361568450927734f, -0.140613287687301635742f,
1209+
0.0803283303976058959961f, 0.0803283303976058959961f, 0.0484490357339382171631f);
1210+
1211+
TestUtils.AreEqual(expected, pseudoinverse(a), kTolerance);
1212+
}
11721213
}
11731214
}

‎src/Tests/Tests/Shared/TestQuaternion.cs‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -380,5 +380,85 @@ public static void quaternion_angle()
380380
angle = math.angle(q, nq);
381381
TestUtils.AreEqual(0f, angle);
382382
}
383+
384+
[TestCompiler]
385+
public static void quaternion_rotation_from_3x3_identity()
386+
{
387+
const float tolerance = 1e-5f;
388+
var q = rotation(float3x3.identity);
389+
TestUtils.AreEqual(quaternion.identity, q, tolerance);
390+
}
391+
392+
[TestCompiler]
393+
public static void quaternion_rotation_from_3x3_with_uniform_scale()
394+
{
395+
const float tolerance = 1e-5f;
396+
var random = new Random(6173u);
397+
var expectedQuaternion = random.NextQuaternionRotation();
398+
var m = new float3x3(expectedQuaternion);
399+
m = mul(m, float3x3.Scale(5.18f));
400+
var actualQuaternion = rotation(m);
401+
TestUtils.AreEqual(0.0f, angle(actualQuaternion, expectedQuaternion) % PI * 2.0f, tolerance);
402+
}
403+
404+
[TestCompiler]
405+
public static void quaternion_rotation_from_3x3_with_nonuniform_scale()
406+
{
407+
const float tolerance = 1e-5f;
408+
var m = new float3x3(
409+
2.0f, 0.0f, 0.0f,
410+
0.0f, 1.5f, 0.0f,
411+
0.0f, 0.0f, 3.1f);
412+
var q = rotation(m);
413+
TestUtils.AreEqual(quaternion.identity, q, tolerance);
414+
}
415+
416+
[TestCompiler]
417+
public static void quaternion_rotation_from_3x3_with_negative_scale_x()
418+
{
419+
const float tolerance = 1e-5f;
420+
var random = new Random(281u);
421+
var expectedQuaternion = random.NextQuaternionRotation();
422+
var m = new float3x3(expectedQuaternion);
423+
m = mul(m, float3x3.Scale(-1.5f, 0.2f, 51.281f));
424+
var actualQuaternion = rotation(m);
425+
TestUtils.AreEqual(0.0f, angle(actualQuaternion, expectedQuaternion) % PI * 2.0f, tolerance);
426+
}
427+
428+
[TestCompiler]
429+
public static void quaternion_rotation_from_3x3_with_negative_scale_y()
430+
{
431+
const float tolerance = 1e-5f;
432+
var random = new Random(10000u);
433+
var expectedQuaternion = random.NextQuaternionRotation();
434+
var m = new float3x3(expectedQuaternion);
435+
m = mul(m, float3x3.Scale(1.5f, -0.2f, 51.281f));
436+
var actualQuaternion = rotation(m);
437+
TestUtils.AreEqual(0.0f, angle(actualQuaternion, expectedQuaternion) % PI * 2.0f, tolerance);
438+
}
439+
440+
[TestCompiler]
441+
public static void quaternion_rotation_from_3x3_with_negative_scale_z()
442+
{
443+
const float tolerance = 1e-5f;
444+
var random = new Random(9891712u);
445+
var expectedQuaternion = random.NextQuaternionRotation();
446+
var m = new float3x3(expectedQuaternion);
447+
m = mul(m, float3x3.Scale(1.5f, 0.2f, -51.281f));
448+
var actualQuaternion = rotation(m);
449+
TestUtils.AreEqual(0.0f, angle(actualQuaternion, expectedQuaternion) % PI * 2.0f, tolerance);
450+
}
451+
452+
[TestCompiler]
453+
public static void quaternion_rotation_from_3x3_with_negative_nonuniform_scale()
454+
{
455+
const float tolerance = 1e-5f;
456+
var random = new Random(912u);
457+
var expectedQuaternion = random.NextQuaternionRotation();
458+
var m = new float3x3(expectedQuaternion);
459+
m = mul(m, float3x3.Scale(-1.5f, -0.2f, -51.281f));
460+
var actualQuaternion = rotation(m);
461+
TestUtils.AreEqual(0.0f, angle(actualQuaternion, expectedQuaternion) % PI * 2.0f, tolerance);
462+
}
383463
}
384464
}

‎src/Tests/Tests/Shared/TestSvd.cs‎

Lines changed: 185 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,185 @@
1+
using System.Collections.Generic;
2+
using NUnit.Framework;
3+
using Burst.Compiler.IL.Tests;
4+
5+
namespace Unity.Mathematics.Tests
6+
{
7+
[TestFixture]
8+
public class TestSvd
9+
{
10+
const float k_SVDTolerance = 1e-4f;
11+
12+
static bool QuaternionEquals(quaternion expected, quaternion actual, float tolerance) =>
13+
(math.lengthsq(expected) == 0f && math.lengthsq(actual) == 0f) || math.abs(math.dot(expected, actual)) > (1f - tolerance);
14+
15+
// Validate Penrose condition that [aba = a]
16+
static void ValidatePenrose1(in float3x3 a, in float3x3 b)
17+
{
18+
var testA = math.mul(math.mul(a, b), a);
19+
20+
TestUtils.AreEqual(a.c0.x, testA.c0.x, k_SVDTolerance);
21+
TestUtils.AreEqual(a.c0.y, testA.c0.y, k_SVDTolerance);
22+
TestUtils.AreEqual(a.c0.z, testA.c0.z, k_SVDTolerance);
23+
TestUtils.AreEqual(a.c1.x, testA.c1.x, k_SVDTolerance);
24+
TestUtils.AreEqual(a.c1.y, testA.c1.y, k_SVDTolerance);
25+
TestUtils.AreEqual(a.c1.z, testA.c1.z, k_SVDTolerance);
26+
TestUtils.AreEqual(a.c2.x, testA.c2.x, k_SVDTolerance);
27+
TestUtils.AreEqual(a.c2.y, testA.c2.y, k_SVDTolerance);
28+
TestUtils.AreEqual(a.c2.z, testA.c2.z, k_SVDTolerance);
29+
}
30+
31+
// Validate Penrose condition that [transpose(ab) = ab]
32+
static void ValidatePenrose2(in float3x3 a, in float3x3 b)
33+
{
34+
var ab = math.mul(a, b);
35+
var testAB = math.transpose(ab);
36+
37+
TestUtils.AreEqual(ab.c0.x, testAB.c0.x, k_SVDTolerance);
38+
TestUtils.AreEqual(ab.c0.y, testAB.c0.y, k_SVDTolerance);
39+
TestUtils.AreEqual(ab.c0.z, testAB.c0.z, k_SVDTolerance);
40+
TestUtils.AreEqual(ab.c1.x, testAB.c1.x, k_SVDTolerance);
41+
TestUtils.AreEqual(ab.c1.y, testAB.c1.y, k_SVDTolerance);
42+
TestUtils.AreEqual(ab.c1.z, testAB.c1.z, k_SVDTolerance);
43+
TestUtils.AreEqual(ab.c2.x, testAB.c2.x, k_SVDTolerance);
44+
TestUtils.AreEqual(ab.c2.y, testAB.c2.y, k_SVDTolerance);
45+
TestUtils.AreEqual(ab.c2.z, testAB.c2.z, k_SVDTolerance);
46+
}
47+
48+
static void ValidateSingular(in float3x3 a) =>
49+
TestUtils.AreEqual(0.0f, math.determinant(a), k_SVDTolerance);
50+
51+
[TestCompiler]
52+
public static void CanSVDInverseNonSingularFloat3x3()
53+
{
54+
var mat = math.float3x3(
55+
math.float3(9f, 1f, 2f),
56+
math.float3(3f, 8f, 4f),
57+
math.float3(5f, 6f, 7f)
58+
);
59+
60+
var inv = svd.svdInverse(mat);
61+
var testIdentity = math.mul(mat, inv);
62+
63+
TestUtils.AreEqual(1f, testIdentity.c0.x, k_SVDTolerance);
64+
TestUtils.AreEqual(0f, testIdentity.c0.y, k_SVDTolerance);
65+
TestUtils.AreEqual(0f, testIdentity.c0.z, k_SVDTolerance);
66+
TestUtils.AreEqual(0f, testIdentity.c1.x, k_SVDTolerance);
67+
TestUtils.AreEqual(1f, testIdentity.c1.y, k_SVDTolerance);
68+
TestUtils.AreEqual(0f, testIdentity.c1.z, k_SVDTolerance);
69+
TestUtils.AreEqual(0f, testIdentity.c2.x, k_SVDTolerance);
70+
TestUtils.AreEqual(0f, testIdentity.c2.y, k_SVDTolerance);
71+
TestUtils.AreEqual(1f, testIdentity.c2.z, k_SVDTolerance);
72+
}
73+
74+
[TestCompiler]
75+
public static void CanSVDInverseFloat3x3With_NullColumn()
76+
{
77+
var mat = math.float3x3(
78+
math.float3(9f, 1f, 0f),
79+
math.float3(3f, 8f, 0f),
80+
math.float3(5f, 6f, 0f)
81+
);
82+
83+
ValidateSingular(mat);
84+
var inv = svd.svdInverse(mat);
85+
86+
ValidatePenrose1(mat, inv);
87+
ValidatePenrose1(inv, mat);
88+
ValidatePenrose2(mat, inv);
89+
ValidatePenrose2(inv, mat);
90+
}
91+
92+
[TestCompiler]
93+
public static void CanSVDInverseFloat3x3With_NullRow()
94+
{
95+
var mat = math.float3x3(
96+
math.float3(9f, 1f, 2f),
97+
math.float3(0f, 0f, 0f),
98+
math.float3(5f, 6f, 7f)
99+
);
100+
101+
ValidateSingular(mat);
102+
var inv = svd.svdInverse(mat);
103+
104+
ValidatePenrose1(mat, inv);
105+
ValidatePenrose1(inv, mat);
106+
ValidatePenrose2(mat, inv);
107+
ValidatePenrose2(inv, mat);
108+
}
109+
110+
[TestCompiler]
111+
public static void CanSVDInverseFloat3x3With_LinearDependentColumn()
112+
{
113+
var mat = math.float3x3(
114+
math.float3(9f, 4f, 2f),
115+
math.float3(3f, 8f, 4f),
116+
math.float3(5f, 14f, 7f)
117+
);
118+
119+
ValidateSingular(mat);
120+
var inv = svd.svdInverse(mat);
121+
122+
ValidatePenrose1(mat, inv);
123+
ValidatePenrose1(inv, mat);
124+
ValidatePenrose2(mat, inv);
125+
ValidatePenrose2(inv, mat);
126+
}
127+
128+
[TestCompiler]
129+
public static void CanSVDInverseFloat3x3With_LinearDependentRow()
130+
{
131+
var mat = math.float3x3(
132+
math.float3(9f, 1f, 2f),
133+
math.float3(10f, 12f, 14f),
134+
math.float3(5f, 6f, 7f)
135+
);
136+
137+
ValidateSingular(mat);
138+
var inv = svd.svdInverse(mat);
139+
140+
ValidatePenrose1(mat, inv);
141+
ValidatePenrose1(inv, mat);
142+
ValidatePenrose2(mat, inv);
143+
ValidatePenrose2(inv, mat);
144+
}
145+
146+
[TestCompiler]
147+
public static void CanSVDInverseFloat3x3With_RotatedZeroScale()
148+
{
149+
var m102030 = math.float3x3(quaternion.Euler(math.radians(10f), math.radians(20f), math.radians(30f)));
150+
var parent = math.mulScale(m102030, math.float3(1f, 1f, 0f));
151+
var mat = math.mul(parent, m102030);
152+
153+
ValidateSingular(mat);
154+
var inv = svd.svdInverse(mat);
155+
156+
ValidatePenrose1(mat, inv);
157+
ValidatePenrose1(inv, mat);
158+
ValidatePenrose2(mat, inv);
159+
ValidatePenrose2(inv, mat);
160+
}
161+
162+
// Case 928598: The errors appear, when GameObject has a child with ParticleSystem which is rotated along the y-axis to -180 and is moved
163+
[TestCompiler]
164+
public static void CanExtractSVDRotationFromFloat3x3With_X180_Y0_Z181()
165+
{
166+
var q = quaternion.Euler(math.radians(180f), math.radians(0f), math.radians(181f));
167+
var qSVD = svd.svdRotation(math.float3x3(q));
168+
169+
TestUtils.IsTrue(QuaternionEquals(q, qSVD, k_SVDTolerance));
170+
}
171+
172+
// Case 938548: Assertion failed on expression: 'CompareApproximately(det, 1.0F, .005f)' when scaling system to 0 on at least 2 axes
173+
[TestCompiler]
174+
public static void CanExtractSVDRotationFromFloat3x3With_ZeroScaleXY()
175+
{
176+
var q0 = quaternion.Euler(math.radians(10f), math.radians(20f), math.radians(30f));
177+
var m0 = math.float3x3(q0);
178+
var m0Scaled = math.mulScale(m0, math.float3(1f, 0f, 0f));
179+
var q1 = svd.svdRotation(m0Scaled);
180+
var m1 = math.float3x3(q1);
181+
182+
TestUtils.AreEqual(0.0f, math.length(m0.c0 - m1.c0), k_SVDTolerance);
183+
}
184+
}
185+
}

‎src/Tests/Tests/Shared/TestSvd.cs.meta‎

Lines changed: 11 additions & 0 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

0 commit comments

Comments
 (0)