A game about forced loneliness, made by TACStudios
tacstudios.tngl.sh
1using System.Runtime.CompilerServices;
2using Unity.IL2CPP.CompilerServices;
3
4namespace Unity.Mathematics
5{
6 // SVD algorithm as described in:
7 // Computing the singular value decomposition of 3x3 matrices with minimal branching and elementary floating point operations,
8 // A.McAdams, A.Selle, R.Tamstorf, J.Teran and E.Sifakis, University of Wisconsin - Madison technical report TR1690, May 2011
9 [Il2CppEagerStaticClassConstruction]
10 static public class svd
11 {
12 [MethodImpl(MethodImplOptions.AggressiveInlining)]
13 static void condSwap(bool c, ref float x, ref float y)
14 {
15 var tmp = x;
16 x = math.select(x, y, c);
17 y = math.select(y, tmp, c);
18 }
19
20 [MethodImpl(MethodImplOptions.AggressiveInlining)]
21 static void condNegSwap(bool c, ref float3 x, ref float3 y)
22 {
23 var tmp = -x;
24 x = math.select(x, y, c);
25 y = math.select(y, tmp, c);
26 }
27
28 [MethodImpl(MethodImplOptions.AggressiveInlining)]
29 static quaternion condNegSwapQuat(bool c, quaternion q, float4 mask)
30 {
31 const float halfSqrt2 = 0.707106781186548f;
32 return math.mul(q, math.select(quaternion.identity.value, mask * halfSqrt2, c));
33 }
34
35 [MethodImpl(MethodImplOptions.AggressiveInlining)]
36 static void sortSingularValues(ref float3x3 b, ref quaternion v)
37 {
38 var l0 = math.lengthsq(b.c0);
39 var l1 = math.lengthsq(b.c1);
40 var l2 = math.lengthsq(b.c2);
41
42 var c = l0 < l1;
43 condNegSwap(c, ref b.c0, ref b.c1);
44 v = condNegSwapQuat(c, v, math.float4(0f, 0f, 1f, 1f));
45 condSwap(c, ref l0, ref l1);
46
47 c = l0 < l2;
48 condNegSwap(c, ref b.c0, ref b.c2);
49 v = condNegSwapQuat(c, v, math.float4(0f, -1f, 0f, 1f));
50 condSwap(c, ref l0, ref l2);
51
52 c = l1 < l2;
53 condNegSwap(c, ref b.c1, ref b.c2);
54 v = condNegSwapQuat(c, v, math.float4(1f, 0f, 0f, 1f));
55 }
56
57 [MethodImpl(MethodImplOptions.AggressiveInlining)]
58 static quaternion approxGivensQuat(float3 pq, float4 mask)
59 {
60 const float c8 = 0.923879532511287f; // cos(pi/8)
61 const float s8 = 0.38268343236509f; // sin(pi/8)
62 const float g = 5.82842712474619f; // 3 + 2 * sqrt(2)
63
64 var ch = 2f * (pq.x - pq.y); // approx cos(a/2)
65 var sh = pq.z; // approx sin(a/2)
66 var r = math.select(math.float4(s8, s8, s8, c8), math.float4(sh, sh, sh, ch), g * sh * sh < ch * ch) * mask;
67 return math.normalize(r);
68 }
69
70 [MethodImpl(MethodImplOptions.AggressiveInlining)]
71 static quaternion qrGivensQuat(float2 pq, float4 mask)
72 {
73 var l = math.sqrt(pq.x * pq.x + pq.y * pq.y);
74 var sh = math.select(0f, pq.y, l > k_EpsilonNormalSqrt);
75 var ch = math.abs(pq.x) + math.max(l, k_EpsilonNormalSqrt);
76 condSwap(pq.x < 0f, ref sh, ref ch);
77
78 return math.normalize(math.float4(sh, sh, sh, ch) * mask);
79 }
80
81 [MethodImpl(MethodImplOptions.AggressiveInlining)]
82 static quaternion givensQRFactorization(float3x3 b, out float3x3 r)
83 {
84 var u = qrGivensQuat(math.float2(b.c0.x, b.c0.y), math.float4(0f, 0f, 1f, 1f));
85 var qmt = math.float3x3(math.conjugate(u));
86 r = math.mul(qmt, b);
87
88 var q = qrGivensQuat(math.float2(r.c0.x, r.c0.z), math.float4(0f, -1f, 0f, 1f));
89 u = math.mul(u, q);
90 qmt = math.float3x3(math.conjugate(q));
91 r = math.mul(qmt, r);
92
93 q = qrGivensQuat(math.float2(r.c1.y, r.c1.z), math.float4(1f, 0f, 0f, 1f));
94 u = math.mul(u, q);
95 qmt = math.float3x3(math.conjugate(q));
96 r = math.mul(qmt, r);
97
98 return u;
99 }
100
101 static quaternion jacobiIteration(ref float3x3 s, int iterations = 5)
102 {
103 float3x3 qm;
104 quaternion q;
105 quaternion v = quaternion.identity;
106
107 for (int i = 0; i < iterations; ++i)
108 {
109 q = approxGivensQuat(math.float3(s.c0.x, s.c1.y, s.c0.y), math.float4(0f, 0f, 1f, 1f));
110 v = math.mul(v, q);
111 qm = math.float3x3(q);
112 s = math.mul(math.mul(math.transpose(qm), s), qm);
113
114 q = approxGivensQuat(math.float3(s.c1.y, s.c2.z, s.c1.z), math.float4(1f, 0f, 0f, 1f));
115 v = math.mul(v, q);
116 qm = math.float3x3(q);
117 s = math.mul(math.mul(math.transpose(qm), s), qm);
118
119 q = approxGivensQuat(math.float3(s.c2.z, s.c0.x, s.c2.x), math.float4(0f, 1f, 0f, 1f));
120 v = math.mul(v, q);
121 qm = math.float3x3(q);
122 s = math.mul(math.mul(math.transpose(qm), s), qm);
123 }
124
125 return v;
126 }
127
128 [MethodImpl(MethodImplOptions.AggressiveInlining)]
129 static float3 singularValuesDecomposition(float3x3 a, out quaternion u, out quaternion v)
130 {
131 u = quaternion.identity;
132 v = quaternion.identity;
133
134 var s = math.mul(math.transpose(a), a);
135 v = jacobiIteration(ref s);
136 var b = math.float3x3(v);
137 b = math.mul(a, b);
138 sortSingularValues(ref b, ref v);
139 u = givensQRFactorization(b, out var e);
140
141 return math.float3(e.c0.x, e.c1.y, e.c2.z);
142 }
143
144 public const float k_EpsilonDeterminant = 1e-6f;
145 public const float k_EpsilonRCP = 1e-9f;
146 public const float k_EpsilonNormalSqrt = 1e-15f;
147 public const float k_EpsilonNormal = 1e-30f;
148
149 [MethodImpl(MethodImplOptions.AggressiveInlining)]
150 static float3 rcpsafe(float3 x, float epsilon = k_EpsilonRCP) =>
151 math.select(math.rcp(x), float3.zero, math.abs(x) < epsilon);
152
153 [MethodImpl(MethodImplOptions.AggressiveInlining)]
154 public static float3x3 svdInverse(float3x3 a)
155 {
156 var e = singularValuesDecomposition(a, out var u, out var v);
157 var um = math.float3x3(u);
158 var vm = math.float3x3(v);
159
160 return math.mul(vm, math.scaleMul(rcpsafe(e, k_EpsilonDeterminant), math.transpose(um)));
161 }
162
163 [MethodImpl(MethodImplOptions.AggressiveInlining)]
164 public static quaternion svdRotation(float3x3 a)
165 {
166 singularValuesDecomposition(a, out var u, out var v);
167 return math.mul(u, math.conjugate(v));
168 }
169 }
170}